matlab_23550/Inu/Src/N12_Xilinx/xp_rotation_sensor.c

#include "xp_project.h"
#include "xp_rotation_sensor.h"

#include "xp_project.h"


T_rotation_sensor rotation_sensor = T_CDS_ROTATION_SENSOR_DEFAULTS;

//<2F><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#define SAMPLING_TIME_NS  1  // 16,666667ns
#define SAMPLING_TIME_MS  0  // 1,666667us
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>, <20><><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>.
// <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20> "<22><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>" <20><><EFBFBD> <20><> <20><> <20><><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
// <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD>
#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<36><30><EFBFBD> => 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;

}