268 lines
8.1 KiB
C
268 lines
8.1 KiB
C
#include "xp_project.h"
|
|
#include "xp_rotation_sensor.h"
|
|
|
|
#include "xp_project.h"
|
|
|
|
|
|
T_rotation_sensor rotation_sensor = T_CDS_ROTATION_SENSOR_DEFAULTS;
|
|
|
|
//Äèñêðåòèçàöèþ, ïðè êîòîðîé ðàñ÷èòûâàåòñþ äëèòåëüíîñòü èìïóëüñîâ
|
|
#define SAMPLING_TIME_NS 1 // 16,666667ns
|
|
#define SAMPLING_TIME_MS 0 // 1,666667us
|
|
// Êîëè÷åñòâî èìïóëüñîâ, ïðè êîòîðûõ ïåðåêëþ÷àåòñó ïåðèîä äèñêðåòèçàöèè.
|
|
// Âåëè÷èíû âûáðàíû ñ "íàõë¸ñòîì" ÷òî áû íå áûëî ïîñòîþííûõ ïåðåêëþ÷åíèé
|
|
// â ðàéîíå ãðàíè÷íûõ âåëè÷èí
|
|
#define LEVEL_SWITCH_NANOSEC 327
|
|
#define LEVEL_SWITCH_MICROSEC 0xC000
|
|
|
|
|
|
static void read_in_sensor_line1(T_cds_in_rotation_sensor *rs);
|
|
static void read_in_sensor_line2(T_cds_in_rotation_sensor *rs);
|
|
static void read_command_reg(T_cds_in_rotation_sensor *rs);
|
|
static void write_command_reg(T_cds_in_rotation_sensor *rs);
|
|
static void tune_sampling_time(T_rotation_sensor *rs);
|
|
static void wait_for_registers_updated(T_cds_in_rotation_sensor *rs);
|
|
static void read_direction_in_plane(T_cds_in_rotation_sensor *rs);
|
|
|
|
void rot_sensor_set(T_rotation_sensor *rs)
|
|
{
|
|
|
|
if(rs->use_sensor1 || rs->use_sensor2)
|
|
{
|
|
rs->in_plane.set(&rs->in_plane);
|
|
}
|
|
if(rs->use_angle_plane)
|
|
{
|
|
rs->rotation_plane.set(&rs->rotation_plane);
|
|
}
|
|
}
|
|
|
|
void in_plane_set(T_cds_in_rotation_sensor* rs)
|
|
{
|
|
if(!rs->cds_in->useit)
|
|
{
|
|
return;
|
|
}
|
|
|
|
rs->cds_in->write.sbus.enabled_channels.all = rs->write.sbus.enabled_channels.all;
|
|
rs->cds_in->write.sbus.first_sensor.all = rs->write.sbus.first_sensor_inputs.all;
|
|
rs->cds_in->write.sbus.second_sensor.all = rs->write.sbus.second_sensor_inputs.all;
|
|
// rs->cds_in->write_sbus(rs->cds_in);
|
|
write_command_reg(rs);
|
|
|
|
}
|
|
|
|
|
|
void angle_plane_set(T_cds_angle_sensor *rs)
|
|
{
|
|
if((rs->cds_rs == NULL) || (!rs->cds_rs->useit))
|
|
{
|
|
return;
|
|
}
|
|
|
|
rs->cds_rs->write.sbus.config.all = rs->write.sbus.config.all;
|
|
rs->cds_rs->write_sbus(rs->cds_rs);
|
|
}
|
|
|
|
void sensor_read(T_rotation_sensor *rs)
|
|
{
|
|
if(rs->use_sensor1 || rs->use_sensor2 || rs->use_angle_plane)
|
|
{
|
|
wait_for_registers_updated(&rs->in_plane);
|
|
read_direction_in_plane(&rs->in_plane);
|
|
}
|
|
else
|
|
{
|
|
return;
|
|
}
|
|
if(rs->use_sensor1)
|
|
{
|
|
rs->in_plane.read_sensor1(&rs->in_plane);
|
|
}
|
|
if(rs->use_sensor2)
|
|
{
|
|
rs->in_plane.read_sensor2(&rs->in_plane);
|
|
}
|
|
if(rs->use_angle_plane)
|
|
{
|
|
rs->rotation_plane.read_sensor(&rs->rotation_plane);
|
|
}
|
|
#ifdef AUTO_CHANGE_SAMPLING_TIME
|
|
tune_sampling_time(rs);
|
|
#endif
|
|
}
|
|
|
|
void in_sensor_read1(T_cds_in_rotation_sensor *rs)
|
|
{
|
|
read_in_sensor_line1(rs);
|
|
#if C_PROJECT_TYPE != PROJECT_BALZAM
|
|
rs->out.Impulses1 = rs->read.regs.n_impulses_line1;
|
|
rs->out.Time1 = rs->read.regs.time_line1 / 60;
|
|
//Counter`s freq is 60ÌÃö => N/60 = time in mksec
|
|
#endif
|
|
rs->out.CountZero1 = rs->read.regs.zero_time_line1;
|
|
rs->out.CountOne1 = rs->read.regs.one_time_line1;
|
|
|
|
rs->out.counter_freq1 = rs->read.regs.comand_reg.bit.sampling_time1;
|
|
|
|
rs->out.direction1 = rs->read.pbus.direction.bit.sensor1;
|
|
}
|
|
|
|
void in_sensor_read2(T_cds_in_rotation_sensor *rs)
|
|
{
|
|
read_in_sensor_line2(rs);
|
|
#if C_PROJECT_TYPE != PROJECT_BALZAM
|
|
rs->out.Impulses2 = rs->read.regs.n_impulses_line2;
|
|
rs->out.Time2 = rs->read.regs.time_line2 / 60;
|
|
#endif
|
|
rs->out.CountZero2 = rs->read.regs.zero_time_line2;
|
|
rs->out.CountOne2 = rs->read.regs.one_time_line2;
|
|
|
|
rs->out.counter_freq2 = rs->read.regs.comand_reg.bit.sampling_time2;
|
|
|
|
rs->out.direction2 = rs->read.pbus.direction.bit.sensor2;
|
|
}
|
|
|
|
void read_in_sensor_line1(T_cds_in_rotation_sensor *rs)
|
|
{
|
|
if(!rs->read.regs.comand_reg.bit.update_registers)
|
|
{
|
|
#if C_PROJECT_TYPE != PROJECT_BALZAM
|
|
rs->read.regs.time_line1 = i_ReadMemory(ADR_SENSOR_S1_T_PERIOD);//TODO check time when turn off
|
|
rs->read.regs.n_impulses_line1 = i_ReadMemory(ADR_SENSOR_S1_COUNT_IMPULS);
|
|
#endif
|
|
rs->read.regs.zero_time_line1 = i_ReadMemory(ADR_SENSOR_S1_T_PERIOD_LOW_ONE_IMPULS);
|
|
rs->read.regs.one_time_line1 = i_ReadMemory(ADR_SENSOR_S1_T_PERIOD_HIGH_ONE_IMPULS);
|
|
}
|
|
}
|
|
|
|
void read_in_sensor_line2(T_cds_in_rotation_sensor *rs)
|
|
{
|
|
if(!rs->read.regs.comand_reg.bit.update_registers)
|
|
{
|
|
#if C_PROJECT_TYPE != PROJECT_BALZAM
|
|
rs->read.regs.time_line2 = i_ReadMemory(ADR_SENSOR_S2_T_PERIOD);
|
|
rs->read.regs.n_impulses_line2 = i_ReadMemory(ADR_SENSOR_S2_COUNT_IMPULS);
|
|
#endif
|
|
rs->read.regs.zero_time_line2 = i_ReadMemory(ADR_SENSOR_S2_T_PERIOD_LOW_ONE_IMPULS);
|
|
rs->read.regs.one_time_line2 = i_ReadMemory(ADR_SENSOR_S2_T_PERIOD_HIGH_ONE_IMPULS);
|
|
}
|
|
}
|
|
|
|
void write_command_reg(T_cds_in_rotation_sensor *rs)
|
|
{
|
|
WriteMemory(ADR_SENSOR_CMD, rs->write.regs.comand_reg.all);
|
|
}
|
|
|
|
void read_command_reg(T_cds_in_rotation_sensor *rs)
|
|
{
|
|
rs->read.regs.comand_reg.all = i_ReadMemory(ADR_SENSOR_CMD);
|
|
}
|
|
|
|
void update_sensors_data_r(T_rotation_sensor *rs)
|
|
{
|
|
rs->in_plane.write.regs.comand_reg.bit.update_registers = 1;
|
|
write_command_reg(&rs->in_plane);
|
|
// rs->in_plane.write.regs.comand_reg.bit.update_registers = 0;
|
|
}
|
|
|
|
void read_direction_in_plane(T_cds_in_rotation_sensor *rs)
|
|
{
|
|
/*
|
|
rs->read.pbus.direction.bit.sensor1 = rs->cds_in->read.pbus.direction_in.bit.dir0 == 2 ? 1 :
|
|
rs->cds_in->read.pbus.direction_in.bit.dir0 == 1 ? -1 :
|
|
0;
|
|
rs->read.pbus.direction.bit.sensor2 = rs->cds_in->read.pbus.direction_in.bit.dir1 == 2 ? 1 :
|
|
rs->cds_in->read.pbus.direction_in.bit.dir1 == 1 ? -1 :
|
|
0;
|
|
rs->read.pbus.direction.bit.sens_err1 = rs->cds_in->read.pbus.direction_in.bit.dir0 == 3;
|
|
rs->read.pbus.direction.bit.sens_err2 = rs->cds_in->read.pbus.direction_in.bit.dir1 == 3;
|
|
*/
|
|
//Direction changes not often. May be, it`s enough to read it in main cycle.
|
|
}
|
|
|
|
void wait_for_registers_updated(T_cds_in_rotation_sensor *rs)
|
|
{
|
|
int counter_in_while = 0;
|
|
read_command_reg(rs);
|
|
while(rs->read.regs.comand_reg.bit.update_registers)
|
|
{
|
|
read_command_reg(rs);
|
|
rs->count_wait_for_update_registers++;
|
|
counter_in_while++;
|
|
if(counter_in_while > 1000)
|
|
{
|
|
rs->error_update++;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
void tune_sampling_time(T_rotation_sensor *rs)
|
|
{
|
|
if((rs->use_sensor1 && (rs->in_plane.read.regs.zero_time_line1 < LEVEL_SWITCH_NANOSEC))
|
|
|| (rs->use_sensor2 && (rs->in_plane.read.regs.zero_time_line2 < LEVEL_SWITCH_NANOSEC)))
|
|
{
|
|
rs->in_plane.write.regs.comand_reg.bit.set_sampling_time = SAMPLING_TIME_NS;
|
|
}
|
|
|
|
if((rs->use_sensor1 && (rs->in_plane.read.regs.zero_time_line1 > LEVEL_SWITCH_MICROSEC))
|
|
|| (rs->use_sensor2 && (rs->in_plane.read.regs.zero_time_line2 > LEVEL_SWITCH_MICROSEC)))
|
|
{
|
|
rs->in_plane.write.regs.comand_reg.bit.set_sampling_time = SAMPLING_TIME_MS;
|
|
}
|
|
}
|
|
|
|
void angle_sensor_read(T_cds_angle_sensor *as)
|
|
{
|
|
as->cds_rs->read_pbus(as->cds_rs);
|
|
|
|
if(as->cds_rs->read.sbus.config.bit.channel1_enable)
|
|
{
|
|
// logpar.log15 = as->cds_rs->read.pbus.sensor[0].turned_angle;
|
|
// logpar.log16 = as->cds_rs->read.pbus.sensor[0].angle;
|
|
|
|
as->out.Delta_angle1 = as->cds_rs->read.pbus.sensor[0].turned_angle;
|
|
as->out.Current_angle1 = as->cds_rs->read.pbus.sensor[0].angle << 3;
|
|
}
|
|
else
|
|
{
|
|
as->out.Delta_angle1 = 0;
|
|
as->out.Current_angle1 = 0;
|
|
}
|
|
if(as->cds_rs->read.sbus.config.bit.channel2_enable)
|
|
{
|
|
as->out.Delta_angle2 = as->cds_rs->read.pbus.sensor[1].turned_angle;
|
|
as->out.Current_angle2 = as->cds_rs->read.pbus.sensor[1].angle << 3;
|
|
}
|
|
else
|
|
{
|
|
as->out.Delta_angle2 = 0;
|
|
as->out.Current_angle2 = 0;
|
|
}
|
|
if(as->cds_rs->read.sbus.config.bit.channel3_enable)
|
|
{
|
|
as->out.Delta_angle3 = as->cds_rs->read.pbus.sensor[2].turned_angle;
|
|
as->out.Current_angle3 = as->cds_rs->read.pbus.sensor[2].angle << 3;
|
|
}
|
|
else
|
|
{
|
|
as->out.Delta_angle3 = 0;
|
|
as->out.Current_angle3 = 0;
|
|
}
|
|
if(as->cds_rs->read.sbus.config.bit.channel4_enable)
|
|
{
|
|
as->out.Delta_angle4 = as->cds_rs->read.pbus.sensor[3].turned_angle;
|
|
as->out.Current_angle4 = as->cds_rs->read.pbus.sensor[3].angle << 3;
|
|
}
|
|
else
|
|
{
|
|
as->out.Delta_angle4 = 0;
|
|
as->out.Current_angle4 = 0;
|
|
}
|
|
|
|
as->out.survey_time_mks = (as->read.sbus.config.bit.survey_time + 1) * 10;
|
|
|
|
}
|
|
|