Razvalyaev
7e0063eee0
Все основные файлы подтянуты без изменений Изменены (только папка main_matlab): - заглушки для ненужных функций (main_matlab.c) - iq библиотека (IQmathLib_matlab.c) - библиотеки DSP281x
397 lines
12 KiB
C
397 lines
12 KiB
C
#include "xp_project.h"
|
|
#include "xp_inc_sensor.h"
|
|
|
|
#include "xp_project.h"
|
|
|
|
|
|
T_inc_sensor inc_sensor = T_INC_SENSOR_DEFAULT;
|
|
|
|
//Äèñêðåòèçàöèþ, ïðè êîòîðîé ðàñ÷èòûâàåòñþ äëèòåëüíîñòü èìïóëüñîâ
|
|
#define SAMPLING_TIME_NS 1 // 16,666667ns
|
|
#define SAMPLING_TIME_MS 0 // 1,666667us
|
|
// Êîëè÷åñòâî èìïóëüñîâ, ïðè êîòîðûõ ïåðåêëþ÷àåòñó ïåðèîä äèñêðåòèçàöèè.
|
|
// Âåëè÷èíû âûáðàíû ñ "íàõë¸ñòîì" ÷òî áû íå áûëî ïîñòîþííûõ ïåðåêëþ÷åíèé
|
|
// â ðàéîíå ãðàíè÷íûõ âåëè÷èí
|
|
#define LEVEL_SWITCH_NANOSEC 300
|
|
#define LEVEL_SWITCH_MICROSEC 40000
|
|
|
|
|
|
static void read_in_sensor_line1(T_inc_sensor *inc_s);
|
|
static void read_in_sensor_line2(T_inc_sensor *inc_s);
|
|
static void read_command_reg(T_inc_sensor *inc_s);
|
|
static void write_command_reg(T_inc_sensor *inc_s);
|
|
static void tune_sampling_time(T_inc_sensor *inc_s);
|
|
static void wait_for_registers_updated(T_inc_sensor *inc_s);
|
|
static void read_direction_in_plane(T_inc_sensor *inc_s);
|
|
static void detect_break_sensor_1_2(T_inc_sensor *inc_s);
|
|
|
|
void sensor_set(T_inc_sensor *inc_s)
|
|
{
|
|
/*
|
|
if(inc_s->use_sensor1 || inc_s->use_sensor2)
|
|
{
|
|
inc_s->in_plane.set(&inc_s->in_plane);
|
|
}
|
|
if(inc_s->use_angle_plane)
|
|
{
|
|
inc_s->rotation_plane.set(&inc_s->rotation_plane);
|
|
}
|
|
*/
|
|
}
|
|
|
|
void inc_sensor_set(T_inc_sensor *inc_s)
|
|
{
|
|
/*
|
|
if(!inc_s->cds_in->useit)
|
|
{
|
|
return;
|
|
}
|
|
|
|
inc_s->cds_in->write.sbus.enabled_channels.all = inc_s->write.sbus.enabled_channels.all;
|
|
inc_s->cds_in->write.sbus.first_sensor.all = inc_s->write.sbus.first_sensor_inputs.all;
|
|
inc_s->cds_in->write.sbus.second_sensor.all = inc_s->write.sbus.second_sensor_inputs.all;
|
|
// inc_s->cds_in->write_sbus(inc_s->cds_in);
|
|
write_command_reg(inc_s);
|
|
*/
|
|
|
|
write_command_reg(inc_s);
|
|
|
|
}
|
|
|
|
|
|
void inc_sensor_read(T_inc_sensor *inc_s)
|
|
{
|
|
if (inc_s->use_sensor1 || inc_s->use_sensor2)
|
|
{
|
|
wait_for_registers_updated(inc_s);
|
|
read_direction_in_plane(inc_s);
|
|
}
|
|
else
|
|
{
|
|
return;
|
|
}
|
|
|
|
if (inc_s->use_sensor1)
|
|
{
|
|
inc_s->read_sensor1(inc_s);
|
|
}
|
|
|
|
if (inc_s->use_sensor2)
|
|
{
|
|
inc_s->read_sensor2(inc_s);
|
|
}
|
|
|
|
detect_break_sensor_1_2(inc_s);
|
|
|
|
#ifdef AUTO_CHANGE_SAMPLING_TIME
|
|
tune_sampling_time(inc_s);
|
|
#endif
|
|
}
|
|
|
|
////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////
|
|
#define MAX_COUNT_OVERFULL_DISCRET_3 150
|
|
#pragma CODE_SECTION(inc_sensor_read1,".fast_run");
|
|
void inc_sensor_read1(T_inc_sensor *inc_s)
|
|
{
|
|
read_in_sensor_line1(inc_s);
|
|
|
|
//#if C_PROJECT_TYPE != PROJECT_BALZAM
|
|
inc_s->data.Impulses1 = inc_s->pm67regs.n_impulses_line1;
|
|
inc_s->data.Time1 = inc_s->pm67regs.time_line1/60;
|
|
//Counter`s freq is 60ÌÃö => N/60 = time in mksec
|
|
|
|
if (inc_s->pm67regs.n_impulses_line1>=2)
|
|
{
|
|
inc_s->data.TimeCalcFromImpulses1 = (unsigned long)inc_s->pm67regs.time_line1*1000/inc_s->pm67regs.n_impulses_line1;
|
|
inc_s->data.TimeCalcFromImpulses1 /= 60;
|
|
}
|
|
else
|
|
inc_s->data.TimeCalcFromImpulses1 = 0;
|
|
|
|
|
|
//#endif
|
|
//inc_s->data.CountZero1 = inc_s->pm67regs.zero_time_line1;
|
|
|
|
if (inc_s->pm67regs.zero_time_line1==0)
|
|
{
|
|
if (inc_s->data.countCountZero1==MAX_COUNT_OVERFULL_DISCRET_3)
|
|
{
|
|
inc_s->data.prev_CountZero1 = inc_s->data.CountZero1 = 0;
|
|
}
|
|
else
|
|
{
|
|
inc_s->data.CountZero1 = inc_s->data.prev_CountZero1;
|
|
inc_s->data.countCountZero1++;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
inc_s->data.countCountZero1 = 0;
|
|
inc_s->data.CountZero1 = inc_s->pm67regs.zero_time_line1;
|
|
inc_s->data.prev_CountZero1 = inc_s->pm67regs.zero_time_line1;
|
|
}
|
|
|
|
// inc_s->data.CountOne1 = inc_s->pm67regs.one_time_line1;
|
|
if (inc_s->pm67regs.one_time_line1==0)
|
|
{
|
|
if (inc_s->data.countCountOne1==MAX_COUNT_OVERFULL_DISCRET_3)
|
|
{
|
|
inc_s->data.prev_CountOne1 = inc_s->data.CountOne1 = 0;
|
|
}
|
|
else
|
|
{
|
|
inc_s->data.CountOne1 = inc_s->data.prev_CountOne1;
|
|
inc_s->data.countCountOne1++;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
inc_s->data.countCountOne1 = 0;
|
|
inc_s->data.CountOne1 = inc_s->pm67regs.one_time_line1;
|
|
inc_s->data.prev_CountOne1 = inc_s->pm67regs.one_time_line1;
|
|
}
|
|
|
|
|
|
inc_s->data.counter_freq1 = inc_s->pm67regs.read_comand_reg.bit.sampling_time1;
|
|
|
|
// inc_s->data.direction1 = inc_s->read.pbus.direction.bit.sensor1;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////
|
|
#pragma CODE_SECTION(inc_sensor_read2,".fast_run");
|
|
void inc_sensor_read2(T_inc_sensor *inc_s)
|
|
{
|
|
read_in_sensor_line2(inc_s);
|
|
|
|
//#if C_PROJECT_TYPE != PROJECT_BALZAM
|
|
inc_s->data.Impulses2 = inc_s->pm67regs.n_impulses_line2;
|
|
inc_s->data.Time2 = inc_s->pm67regs.time_line2 / 60;
|
|
//Counter`s freq is 60ÌÃö => N/60 = time in mksec
|
|
|
|
if (inc_s->pm67regs.n_impulses_line2>=2)
|
|
{
|
|
inc_s->data.TimeCalcFromImpulses2 = (unsigned long)inc_s->pm67regs.time_line2*1000/inc_s->pm67regs.n_impulses_line2;
|
|
inc_s->data.TimeCalcFromImpulses2 /= 60;
|
|
}
|
|
else
|
|
inc_s->data.TimeCalcFromImpulses2 = 0;
|
|
|
|
//#endif
|
|
//inc_s->data.CountZero1 = inc_s->pm67regs.zero_time_line1;
|
|
|
|
if (inc_s->pm67regs.zero_time_line2==0)
|
|
{
|
|
if (inc_s->data.countCountZero2==MAX_COUNT_OVERFULL_DISCRET_3)
|
|
{
|
|
inc_s->data.prev_CountZero2 = inc_s->data.CountZero2 = 0;
|
|
}
|
|
else
|
|
{
|
|
inc_s->data.CountZero2 = inc_s->data.prev_CountZero2;
|
|
inc_s->data.countCountZero2++;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
inc_s->data.countCountZero2 = 0;
|
|
inc_s->data.CountZero2 = inc_s->pm67regs.zero_time_line2;
|
|
inc_s->data.prev_CountZero2 = inc_s->pm67regs.zero_time_line2;
|
|
}
|
|
|
|
// inc_s->data.CountOne1 = inc_s->pm67regs.one_time_line1;
|
|
if (inc_s->pm67regs.one_time_line2==0)
|
|
{
|
|
if (inc_s->data.countCountOne2==MAX_COUNT_OVERFULL_DISCRET_3)
|
|
{
|
|
inc_s->data.prev_CountOne2 = inc_s->data.CountOne2 = 0;
|
|
}
|
|
else
|
|
{
|
|
inc_s->data.CountOne2 = inc_s->data.prev_CountOne2;
|
|
inc_s->data.countCountOne2++;
|
|
}
|
|
}
|
|
else
|
|
{
|
|
inc_s->data.countCountOne2 = 0;
|
|
inc_s->data.CountOne2 = inc_s->pm67regs.one_time_line2;
|
|
inc_s->data.prev_CountOne2 = inc_s->pm67regs.one_time_line2;
|
|
}
|
|
|
|
inc_s->data.counter_freq2 = inc_s->pm67regs.read_comand_reg.bit.sampling_time2;
|
|
|
|
// inc_s->data.direction2 = inc_s->read.pbus.direction.bit.sensor2;
|
|
|
|
}
|
|
|
|
////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////
|
|
|
|
|
|
void read_in_sensor_line1(T_inc_sensor *inc_s)
|
|
{
|
|
|
|
if(!inc_s->pm67regs.read_comand_reg.bit.update_registers)
|
|
{
|
|
|
|
inc_s->pm67regs.time_line1 = i_ReadMemory(ADR_SENSOR_S1_T_PERIOD);//TODO check time when turn off
|
|
inc_s->pm67regs.n_impulses_line1 = i_ReadMemory(ADR_SENSOR_S1_COUNT_IMPULS);
|
|
|
|
inc_s->pm67regs.zero_time_line1 = i_ReadMemory(ADR_SENSOR_S1_T_PERIOD_LOW_ONE_IMPULS);
|
|
inc_s->pm67regs.one_time_line1 = i_ReadMemory(ADR_SENSOR_S1_T_PERIOD_HIGH_ONE_IMPULS);
|
|
}
|
|
|
|
}
|
|
|
|
////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////
|
|
|
|
|
|
void read_in_sensor_line2(T_inc_sensor *inc_s)
|
|
{
|
|
if(!inc_s->pm67regs.read_comand_reg.bit.update_registers)
|
|
{
|
|
|
|
inc_s->pm67regs.time_line2 = i_ReadMemory(ADR_SENSOR_S2_T_PERIOD);//TODO check time when turn off
|
|
inc_s->pm67regs.n_impulses_line2 = i_ReadMemory(ADR_SENSOR_S2_COUNT_IMPULS);
|
|
|
|
inc_s->pm67regs.zero_time_line2 = i_ReadMemory(ADR_SENSOR_S2_T_PERIOD_LOW_ONE_IMPULS);
|
|
inc_s->pm67regs.one_time_line2 = i_ReadMemory(ADR_SENSOR_S2_T_PERIOD_HIGH_ONE_IMPULS);
|
|
}
|
|
}
|
|
|
|
////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////
|
|
|
|
|
|
void write_command_reg(T_inc_sensor *inc_s)
|
|
{
|
|
WriteMemory(ADR_SENSOR_CMD, inc_s->pm67regs.write_comand_reg.all);
|
|
}
|
|
|
|
////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////
|
|
|
|
|
|
void read_command_reg(T_inc_sensor *inc_s)
|
|
{
|
|
inc_s->pm67regs.read_comand_reg.all = i_ReadMemory(ADR_SENSOR_CMD);
|
|
}
|
|
|
|
////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////
|
|
|
|
|
|
void update_sensors_data_s(T_inc_sensor *inc_s)
|
|
{
|
|
inc_s->pm67regs.write_comand_reg.bit.update_registers = 1;
|
|
write_command_reg(inc_s);
|
|
// inc_s->in_plane.write.regs.comand_reg.bit.update_registers = 0;
|
|
}
|
|
|
|
////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////
|
|
|
|
|
|
void read_direction_in_plane(T_inc_sensor *inc_s)
|
|
{
|
|
/*
|
|
inc_s->read.pbus.direction.bit.sensor1 = inc_s->cds_in->read.pbus.direction_in.bit.dir0 == 2 ? 1 :
|
|
inc_s->cds_in->read.pbus.direction_in.bit.dir0 == 1 ? -1 :
|
|
0;
|
|
inc_s->read.pbus.direction.bit.sensor2 = inc_s->cds_in->read.pbus.direction_in.bit.dir1 == 2 ? 1 :
|
|
inc_s->cds_in->read.pbus.direction_in.bit.dir1 == 1 ? -1 :
|
|
0;
|
|
inc_s->read.pbus.direction.bit.sens_err1 = inc_s->cds_in->read.pbus.direction_in.bit.dir0 == 3;
|
|
inc_s->read.pbus.direction.bit.sens_err2 = inc_s->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_inc_sensor *inc_s)
|
|
{
|
|
int counter_in_while = 0;
|
|
read_command_reg(inc_s);
|
|
while(inc_s->pm67regs.read_comand_reg.bit.update_registers)
|
|
{
|
|
read_command_reg(inc_s);
|
|
inc_s->count_wait_for_update_registers++;
|
|
counter_in_while++;
|
|
if(counter_in_while > 1000)
|
|
{
|
|
inc_s->error_update++;
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
////////////////////////////////////////////////////////
|
|
void detect_break_sensor_1_2(T_inc_sensor *inc_s)
|
|
{
|
|
unsigned int f1 = (inc_s->data.CountOne1 || inc_s->data.CountZero1);
|
|
unsigned int f2 = (inc_s->data.CountOne2 || inc_s->data.CountZero2);
|
|
|
|
|
|
if (f1 && f2==0)
|
|
{
|
|
inc_s->break_sensor1 = 0;
|
|
inc_s->break_sensor2 = 1;
|
|
}
|
|
|
|
if (f1==0 && f2)
|
|
{
|
|
inc_s->break_sensor1 = 1;
|
|
inc_s->break_sensor2 = 0;
|
|
}
|
|
|
|
if ((f1==0 && f2==0) || (f1 && f2))
|
|
{
|
|
inc_s->break_sensor1 = 0;
|
|
inc_s->break_sensor2 = 0;
|
|
}
|
|
|
|
|
|
}
|
|
////////////////////////////////////////////////////////
|
|
|
|
|
|
void tune_sampling_time(T_inc_sensor *inc_s)
|
|
{
|
|
|
|
// ñíà÷àëà ïðîâåðßåì íà ìàêñèìóì, ò.ê. åñëè äàò÷èê îòâàëèëñß, òî îí ïîêàæåò = 0.
|
|
|
|
if(
|
|
(inc_s->use_sensor1 && inc_s->break_sensor1==0 && (inc_s->data.CountOne1 > LEVEL_SWITCH_MICROSEC) && (inc_s->data.CountZero1 > LEVEL_SWITCH_MICROSEC) )
|
|
|| (inc_s->use_sensor2 && inc_s->break_sensor2==0 && (inc_s->data.CountOne2 > LEVEL_SWITCH_MICROSEC) && (inc_s->data.CountZero2 > LEVEL_SWITCH_MICROSEC) )
|
|
)
|
|
{
|
|
inc_s->pm67regs.write_comand_reg.bit.set_sampling_time = SAMPLING_TIME_MS;
|
|
return;
|
|
}
|
|
|
|
// ïðîâåðêà íà ìèíèìóì
|
|
if(
|
|
(inc_s->use_sensor1 && inc_s->break_sensor1==0 && (inc_s->data.CountOne1 < LEVEL_SWITCH_NANOSEC) && (inc_s->data.CountZero1 < LEVEL_SWITCH_NANOSEC) )
|
|
|| (inc_s->use_sensor2 && inc_s->break_sensor2==0 && (inc_s->data.CountOne2 < LEVEL_SWITCH_NANOSEC) && (inc_s->data.CountZero2 < LEVEL_SWITCH_NANOSEC) )
|
|
)
|
|
{
|
|
inc_s->pm67regs.write_comand_reg.bit.set_sampling_time = SAMPLING_TIME_NS;
|
|
}
|
|
|
|
}
|
|
|
|
|
|
|
|
////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////
|
|
////////////////////////////////////////////////////////
|