531 lines
17 KiB
C
531 lines
17 KiB
C
#include "MemoryFunctions.h"
|
|
#include "xp_cds_in.h"
|
|
|
|
#include "MemoryFunctions.h"
|
|
#include "Spartan2E_Adr.h"
|
|
#include "Spartan2E_Adr.h"
|
|
#include "TuneUpPlane.h"
|
|
#include "x_parallel_bus.h"
|
|
#include "x_serial_bus.h"
|
|
#include "xp_tools.h"
|
|
#include "xerror.h"
|
|
|
|
|
|
///////////////////////////////////////////////
|
|
///////////////////////////////////////////////
|
|
///////////////////////////////////////////////
|
|
|
|
void cds_in_init(T_cds_in *v)
|
|
{
|
|
int old_started = 0;
|
|
unsigned int i;
|
|
|
|
if (v->useit == 0)
|
|
{
|
|
clear_adr_sync_table(v->plane_address);
|
|
return ;
|
|
}
|
|
set_adr_sync_table(v->plane_address);
|
|
|
|
if (x_parallel_bus_project.flags.bit.init==0)
|
|
x_parallel_bus_project.init(&x_parallel_bus_project);
|
|
|
|
|
|
old_started = x_parallel_bus_project.flags.bit.started;
|
|
|
|
if (x_parallel_bus_project.flags.bit.started)
|
|
x_parallel_bus_project.stop(&x_parallel_bus_project);
|
|
|
|
|
|
x_parallel_bus_project.slave_addr = v->plane_address;
|
|
|
|
// for (i=0;i<v->setup_pbus.count_elements_pbus;i++)
|
|
for (i=0;i<16;i++)
|
|
{
|
|
if (v->setup_pbus.use_reg_in_pbus.all & (1<<i))
|
|
{
|
|
if (x_parallel_bus_project.check_free_table(&x_parallel_bus_project))
|
|
{
|
|
x_parallel_bus_project.reg_addr = i;
|
|
v->adr_pbus.adr_table[i] = x_parallel_bus_project.setup.size_table;
|
|
x_parallel_bus_project.add_table(&x_parallel_bus_project);
|
|
x_parallel_bus_project.reg_addr++;
|
|
x_parallel_bus_project.setup.size_table++;
|
|
}
|
|
else
|
|
{
|
|
// ìåñòà â òàáëèöå íåò!!!
|
|
xerror(xparall_bus_er_ID(1),(void *)0);
|
|
v->setup_pbus.use_reg_in_pbus.all &= (~(1<<i));
|
|
}
|
|
}
|
|
}
|
|
|
|
//
|
|
if (old_started)
|
|
x_parallel_bus_project.start(&x_parallel_bus_project);
|
|
|
|
|
|
|
|
}
|
|
|
|
///////////////////////////////////////////////
|
|
///////////////////////////////////////////////
|
|
///////////////////////////////////////////////
|
|
|
|
|
|
int cds_in_read_all(T_cds_in *v)
|
|
{
|
|
int err = 0;
|
|
|
|
if (v->useit == 0)
|
|
return 0;
|
|
|
|
err = cds_in_read_sbus(v);
|
|
err |= cds_in_read_pbus(v);
|
|
|
|
return err;
|
|
}
|
|
|
|
///////////////////////////////////////////////
|
|
///////////////////////////////////////////////
|
|
///////////////////////////////////////////////
|
|
|
|
|
|
int cds_in_write_all(T_cds_in *v)
|
|
{
|
|
int err = 0;
|
|
|
|
if (v->useit == 0)
|
|
return 0;
|
|
|
|
err = cds_in_write_sbus(v);
|
|
err |= cds_in_write_pbus(v);
|
|
|
|
return err;
|
|
}
|
|
|
|
///////////////////////////////////////////////
|
|
///////////////////////////////////////////////
|
|
///////////////////////////////////////////////
|
|
|
|
int cds_in_write_sbus(T_cds_in *v)
|
|
{
|
|
unsigned int old_err, err = 0, err_ready = 0;
|
|
|
|
if (v->useit == 0)
|
|
return 0;
|
|
|
|
old_err = v->status_serial_bus.count_write_error;
|
|
x_serial_bus_project.slave_addr = v->plane_address; // number plate
|
|
|
|
//0 enabled channels
|
|
x_serial_bus_project.reg_addr = 0; // adr memory in plate
|
|
x_serial_bus_project.write_data = v->write.sbus.enabled_channels.all;//use_invers_sensor_speed.all; // write data
|
|
|
|
if (x_serial_bus_project.write(&x_serial_bus_project)) // make write
|
|
v->status_serial_bus.count_write_error++;
|
|
|
|
//1 first sensor connection
|
|
x_serial_bus_project.reg_addr = 1; // adr memory in plate
|
|
x_serial_bus_project.write_data = v->write.sbus.first_sensor.all;//sensor_combo; // write data
|
|
|
|
if (x_serial_bus_project.write(&x_serial_bus_project)) // make write
|
|
v->status_serial_bus.count_write_error++;
|
|
|
|
//2 second sensor connection
|
|
x_serial_bus_project.reg_addr = 2; // adr memory in plate
|
|
x_serial_bus_project.write_data = v->write.sbus.second_sensor.all;//sensor_combo; // write data
|
|
|
|
if (x_serial_bus_project.write(&x_serial_bus_project)) // make write
|
|
v->status_serial_bus.count_write_error++;
|
|
|
|
if (v->type_cds_xilinx == TYPE_CDS_XILINX_SP6)
|
|
{
|
|
//3 zero sensor connection
|
|
x_serial_bus_project.reg_addr = 3; // adr memory in plate
|
|
x_serial_bus_project.write_data = v->write.sbus.zero_sensors.all;//sensor_combo; // write data
|
|
|
|
if (x_serial_bus_project.write(&x_serial_bus_project)) // make write
|
|
v->status_serial_bus.count_write_error++;
|
|
}
|
|
|
|
|
|
//6 protect_error
|
|
// if (v->read.type_cds_xilinx == TYPE_CDS_XILINX_SP6)
|
|
// v->write.sbus.protect_error.bit.enable_err_switch = 0; // äëÿ SP6 îòêëþ÷àåì îøèáêó ïî ðàçúåìó, ò.ê. åå íåò ïîêà
|
|
|
|
x_serial_bus_project.reg_addr = 6; // adr memory in plate
|
|
x_serial_bus_project.write_data = v->write.sbus.protect_error.all; // write data
|
|
|
|
if (x_serial_bus_project.write(&x_serial_bus_project)) // make write
|
|
v->status_serial_bus.count_write_error++;
|
|
|
|
|
|
//7 cmd_reset_error
|
|
/*
|
|
x_serial_bus_project.reg_addr = 7; // adr memory in plate
|
|
x_serial_bus_project.write_data = v->write.cmd_reset_error; // write data
|
|
|
|
if (x_serial_bus_project.write(&x_serial_bus_project)) // make write
|
|
v->status_serial_bus.count_error++;
|
|
*/
|
|
|
|
if (old_err == v->status_serial_bus.count_write_error)// no errors
|
|
{
|
|
v->status_serial_bus.count_write_ok++;
|
|
err = 0; // no errors
|
|
}
|
|
else
|
|
err = 1; // !errors!
|
|
|
|
err_ready = check_cds_ready_sbus( err, ITS_WRITE_BUS, &v->status_serial_bus);
|
|
set_status_cds(err_ready, &v->status);
|
|
|
|
return err_ready;
|
|
|
|
}
|
|
|
|
///////////////////////////////////////////////
|
|
///////////////////////////////////////////////
|
|
///////////////////////////////////////////////
|
|
|
|
int cds_in_write_pbus(T_cds_in *v)
|
|
{
|
|
if (v->useit == 0)
|
|
return 0;
|
|
|
|
return 0;
|
|
}
|
|
|
|
///////////////////////////////////////////////
|
|
///////////////////////////////////////////////
|
|
///////////////////////////////////////////////
|
|
|
|
int cds_in_read_sbus(T_cds_in *v)
|
|
{
|
|
unsigned int old_err, err = 0, err_ready = 0;
|
|
|
|
if (v->useit == 0)
|
|
return 0;
|
|
|
|
old_err = v->status_serial_bus.count_read_error;
|
|
|
|
x_serial_bus_project.slave_addr = v->plane_address; // number plate
|
|
|
|
//0 enabled_channels
|
|
x_serial_bus_project.reg_addr = 0; // adr memory in plate
|
|
x_serial_bus_project.read(&x_serial_bus_project); // read
|
|
|
|
if (x_serial_bus_project.flags.bit.read_error == 0) // check error
|
|
v->read.sbus.enabled_channels.all = x_serial_bus_project.read_data;
|
|
else
|
|
v->status_serial_bus.count_read_error++;
|
|
|
|
|
|
//1 first_sensor
|
|
x_serial_bus_project.reg_addr = 1; // adr memory in plate
|
|
x_serial_bus_project.read(&x_serial_bus_project); // read
|
|
|
|
if (x_serial_bus_project.flags.bit.read_error == 0) // check error
|
|
v->read.sbus.first_sensor.all = x_serial_bus_project.read_data;
|
|
else
|
|
v->status_serial_bus.count_read_error++;
|
|
|
|
//2 second_sensor
|
|
x_serial_bus_project.reg_addr = 2; // adr memory in plate
|
|
x_serial_bus_project.read(&x_serial_bus_project); // read
|
|
|
|
if (x_serial_bus_project.flags.bit.read_error == 0) // check error
|
|
v->read.sbus.second_sensor.all = x_serial_bus_project.read_data;
|
|
else
|
|
v->status_serial_bus.count_read_error++;
|
|
|
|
//15 current_status_error
|
|
x_serial_bus_project.reg_addr = 15; // adr memory in plate
|
|
x_serial_bus_project.read(&x_serial_bus_project); // read
|
|
|
|
if (x_serial_bus_project.flags.bit.read_error == 0) // check error
|
|
{
|
|
v->read.type_cds_xilinx = x_serial_bus_project.read_data & 0x1;
|
|
v->type_cds_xilinx = v->read.type_cds_xilinx;
|
|
|
|
v->read.sbus.current_status_error.all = x_serial_bus_project.read_data & 0xfffe;
|
|
}
|
|
else
|
|
v->status_serial_bus.count_read_error++;
|
|
|
|
|
|
|
|
//3 zero sensors
|
|
if (v->type_cds_xilinx == TYPE_CDS_XILINX_SP6)
|
|
{
|
|
x_serial_bus_project.reg_addr = 3; // adr memory in plate
|
|
x_serial_bus_project.read(&x_serial_bus_project); // read
|
|
|
|
if (x_serial_bus_project.flags.bit.read_error == 0) // check error
|
|
v->read.sbus.zero_sensors.all = x_serial_bus_project.read_data;
|
|
else
|
|
v->status_serial_bus.count_read_error++;
|
|
|
|
}
|
|
//6 protect_error
|
|
x_serial_bus_project.reg_addr = 6; // adr memory in plate
|
|
x_serial_bus_project.read(&x_serial_bus_project); // read
|
|
|
|
if (x_serial_bus_project.flags.bit.read_error == 0) // check error
|
|
v->read.sbus.protect_error.all = x_serial_bus_project.read_data;
|
|
else
|
|
v->status_serial_bus.count_read_error++;
|
|
|
|
//7 lock_status_error
|
|
x_serial_bus_project.reg_addr = 7; // adr memory in plate
|
|
x_serial_bus_project.read(&x_serial_bus_project); // read
|
|
|
|
if (x_serial_bus_project.flags.bit.read_error == 0) // check error
|
|
v->read.sbus.lock_status_error.all = x_serial_bus_project.read_data;
|
|
else
|
|
v->status_serial_bus.count_read_error++;
|
|
|
|
//14 id_plate
|
|
x_serial_bus_project.reg_addr = 14; // adr memory in plate
|
|
x_serial_bus_project.read(&x_serial_bus_project); // read
|
|
|
|
if (x_serial_bus_project.flags.bit.read_error == 0) // check error
|
|
v->read.sbus.id_plate.all = x_serial_bus_project.read_data;
|
|
else
|
|
v->status_serial_bus.count_read_error++;
|
|
|
|
|
|
////////////////
|
|
|
|
if (old_err == v->status_serial_bus.count_read_error)// no errors
|
|
{
|
|
v->status_serial_bus.count_read_ok++;
|
|
err = 0; // no errors
|
|
}
|
|
else
|
|
err = 1; // !errors!
|
|
|
|
err_ready = check_cds_ready_sbus( err, ITS_READ_BUS, &v->status_serial_bus);
|
|
set_status_cds(err_ready, &v->status);
|
|
|
|
return err_ready;
|
|
}
|
|
|
|
///////////////////////////////////////////////
|
|
///////////////////////////////////////////////
|
|
///////////////////////////////////////////////
|
|
|
|
int cds_in_read_pbus(T_cds_in *v)
|
|
{
|
|
unsigned long adr_pbus;
|
|
unsigned int t;
|
|
|
|
if (v->useit == 0)
|
|
return 0;
|
|
|
|
if (v->status & (component_Started | component_Ready | component_Error | component_ErrorSBus ))
|
|
{
|
|
adr_pbus = v->adr_pbus.adr_table[0] + ADR_FIRST_FREE;
|
|
|
|
if ((v->type_cds_xilinx == TYPE_CDS_XILINX_SP6) && (v->type_plate == cds_in_type_in_1))
|
|
{
|
|
read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg0,adr_pbus, t);
|
|
v->read.pbus.data_in.all = (t & 0xff00) | ((~t) & 0x00ff);
|
|
}
|
|
else
|
|
read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg0,adr_pbus,v->read.pbus.data_in.all);
|
|
|
|
read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg1,adr_pbus,v->read.pbus.ready_in.all);
|
|
read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg2,adr_pbus,v->read.pbus.direction_in.all);
|
|
read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg3,adr_pbus,v->read.pbus.SpeedS1_cnt);
|
|
read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg4,adr_pbus,v->read.pbus.SpeedS1_cnt90);
|
|
read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg5,adr_pbus,v->read.pbus.SpeedS2_cnt);
|
|
read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg6,adr_pbus,v->read.pbus.SpeedS2_cnt90);
|
|
read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg7,adr_pbus,v->read.pbus.Time_since_zero_point_S1);
|
|
read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg8,adr_pbus,v->read.pbus.Impulses_since_zero_point_Rising_S1);
|
|
read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg9,adr_pbus,v->read.pbus.Impulses_since_zero_point_Falling_S1);
|
|
read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg10,adr_pbus,v->read.pbus.Time_since_zero_point_S2);
|
|
read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg11,adr_pbus,v->read.pbus.Impulses_since_zero_point_Rising_S2);
|
|
read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg12,adr_pbus,v->read.pbus.Impulses_since_zero_point_Falling_S2);
|
|
read_pbus_value(v->setup_pbus.use_reg_in_pbus.bit.reg13,adr_pbus,v->read.pbus.channel_alive.all);
|
|
/*
|
|
//0
|
|
if (v->setup_pbus.use_reg_in_pbus.bit.reg0)
|
|
{
|
|
x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[0];
|
|
x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
|
|
v->read.pbus.data_in.all = x_parallel_bus_project.data_table_read;
|
|
}
|
|
//1
|
|
if (v->setup_pbus.use_reg_in_pbus.bit.reg1)
|
|
{
|
|
x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[1];
|
|
x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
|
|
v->read.pbus.ready_in.all = x_parallel_bus_project.data_table_read;
|
|
}
|
|
//2
|
|
if (v->setup_pbus.use_reg_in_pbus.bit.reg2)
|
|
{
|
|
x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[2];
|
|
x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
|
|
v->read.pbus.direction_in.all = x_parallel_bus_project.data_table_read;
|
|
}
|
|
//3
|
|
if (v->setup_pbus.use_reg_in_pbus.bit.reg3)
|
|
{
|
|
x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[3];
|
|
x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
|
|
v->read.pbus.SpeedS1_cnt = x_parallel_bus_project.data_table_read;
|
|
}
|
|
//4
|
|
if (v->setup_pbus.use_reg_in_pbus.bit.reg4)
|
|
{
|
|
x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[4];
|
|
x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
|
|
v->read.pbus.SpeedS1_cnt90 = x_parallel_bus_project.data_table_read;
|
|
}
|
|
//5
|
|
if (v->setup_pbus.use_reg_in_pbus.bit.reg5)
|
|
{
|
|
x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[5];
|
|
x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
|
|
v->read.pbus.SpeedS2_cnt = x_parallel_bus_project.data_table_read;
|
|
}
|
|
//6
|
|
if (v->setup_pbus.use_reg_in_pbus.bit.reg6)
|
|
{
|
|
x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[6];
|
|
x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
|
|
v->read.pbus.SpeedS2_cnt90 = x_parallel_bus_project.data_table_read;
|
|
}
|
|
//7
|
|
if (v->setup_pbus.use_reg_in_pbus.bit.reg7)
|
|
{
|
|
x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[7];
|
|
x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
|
|
v->read.pbus.Time_since_zero_point_S1 = x_parallel_bus_project.data_table_read;
|
|
}
|
|
//8
|
|
if (v->setup_pbus.use_reg_in_pbus.bit.reg8)
|
|
{
|
|
x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[8];
|
|
x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
|
|
v->read.pbus.Time_since_zero_point_Rising_S1 = x_parallel_bus_project.data_table_read;
|
|
}
|
|
//9
|
|
if (v->setup_pbus.use_reg_in_pbus.bit.reg9)
|
|
{
|
|
x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[9];
|
|
x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
|
|
v->read.pbus.Time_since_zero_point_Falling_S1 = x_parallel_bus_project.data_table_read;
|
|
}
|
|
//10
|
|
if (v->setup_pbus.use_reg_in_pbus.bit.reg10)
|
|
{
|
|
x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[10];
|
|
x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
|
|
v->read.pbus.Time_since_zero_point_S2 = x_parallel_bus_project.data_table_read;
|
|
}
|
|
//11
|
|
if (v->setup_pbus.use_reg_in_pbus.bit.reg11)
|
|
{
|
|
x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[11];
|
|
x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
|
|
v->read.pbus.Time_since_zero_point_Rising_S2 = x_parallel_bus_project.data_table_read;
|
|
}
|
|
//12
|
|
if (v->setup_pbus.use_reg_in_pbus.bit.reg12)
|
|
{
|
|
x_parallel_bus_project.adr_table_read = v->adr_pbus.adr_table[12];
|
|
x_parallel_bus_project.read_one_data(&x_parallel_bus_project);
|
|
v->read.pbus.Time_since_zero_point_Falling_S2 = x_parallel_bus_project.data_table_read;
|
|
}
|
|
|
|
*/
|
|
}
|
|
else
|
|
{
|
|
v->read.pbus.data_in.all = 0;
|
|
v->read.pbus.ready_in.all = 0;
|
|
v->read.pbus.direction_in.all = 0;
|
|
v->read.pbus.SpeedS1_cnt = 0;
|
|
v->read.pbus.SpeedS1_cnt90 = 0;
|
|
v->read.pbus.SpeedS2_cnt = 0;
|
|
v->read.pbus.SpeedS2_cnt90 = 0;
|
|
v->read.pbus.Time_since_zero_point_S1 = 0;
|
|
v->read.pbus.Impulses_since_zero_point_Rising_S1 = 0;
|
|
v->read.pbus.Impulses_since_zero_point_Falling_S1 = 0;
|
|
v->read.pbus.Time_since_zero_point_S2 = 0;
|
|
v->read.pbus.Impulses_since_zero_point_Rising_S2 = 0;
|
|
v->read.pbus.Impulses_since_zero_point_Falling_S2 = 0;
|
|
v->read.pbus.channel_alive.all = 0;
|
|
}
|
|
|
|
return 0;
|
|
}
|
|
|
|
///////////////////////////////////////////////
|
|
///////////////////////////////////////////////
|
|
///////////////////////////////////////////////
|
|
|
|
void cds_in_reset_error(T_cds_in *v)
|
|
{
|
|
if (v->useit == 0)
|
|
return ;
|
|
|
|
if (v->status == component_Error || v->status == component_ErrorSBus)
|
|
v->status = component_Started;
|
|
clear_cur_stat_sbus(&v->status_serial_bus);
|
|
clear_cur_stat_pbus(&v->status_parallel_bus);
|
|
|
|
|
|
x_serial_bus_project.slave_addr = v->plane_address; // number plate
|
|
|
|
//7 cmd_reset_error
|
|
|
|
x_serial_bus_project.reg_addr = 7; // adr memory in plate
|
|
x_serial_bus_project.write_data = v->write.sbus.cmd_reset_error; // write data
|
|
|
|
if (x_serial_bus_project.write(&x_serial_bus_project)) // make write
|
|
v->status_serial_bus.count_write_error++;
|
|
|
|
}
|
|
|
|
///////////////////////////////////////////////
|
|
///////////////////////////////////////////////
|
|
|
|
void cds_in_store_disable_error(T_cds_in *v)
|
|
{
|
|
if (v->useit == 0)
|
|
return ;
|
|
|
|
v->store_protect_error = v->write.sbus.protect_error.all;
|
|
v->write.sbus.protect_error.all = 0; // disable all error.
|
|
|
|
cds_in_write_sbus(v);
|
|
|
|
}
|
|
|
|
///////////////////////////////////////////////
|
|
///////////////////////////////////////////////
|
|
void cds_in_restore_enable_error(T_cds_in *v)
|
|
{
|
|
if (v->useit == 0)
|
|
return ;
|
|
|
|
v->write.sbus.protect_error.all = v->store_protect_error; // restore all setup error.
|
|
cds_in_write_sbus(v);
|
|
|
|
}
|
|
|
|
///////////////////////////////////////////////
|
|
///////////////////////////////////////////////
|
|
|
|
|
|
|
|
|
|
|
|
|