400 lines
12 KiB
C
400 lines
12 KiB
C
|
#include "x_parallel_bus.h"
|
|||
|
|
#include "xp_cds_rs.h"
|
|||
|
|
|
|||
|
|
#include "x_parallel_bus.h"
|
|||
|
|
#include "x_serial_bus.h"
|
|||
|
|
#include "xp_tools.h"
|
|||
|
|
#include "xp_tools.h"
|
|||
|
|
#include "xerror.h"
|
|||
|
|
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
|
|||
|
|
void cds_rs_init(T_cds_rs *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
|
|||
|
|
{
|
|||
|
|
// <20><><EFBFBD><EFBFBD><EFBFBD> <20> <20><><EFBFBD><EFBFBD><EFBFBD><EFBFBD><EFBFBD> <20><><EFBFBD>!!!
|
|||
|
|
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_rs_read_all(T_cds_rs *v)
|
|||
|
|
{
|
|||
|
|
if (v->useit == 0)
|
|||
|
|
return 0;
|
|||
|
|
|
|||
|
|
cds_rs_read_sbus(v);
|
|||
|
|
cds_rs_read_pbus(v);
|
|||
|
|
return 0;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
|
|||
|
|
|
|||
|
|
int cds_rs_write_all(T_cds_rs *v)
|
|||
|
|
{
|
|||
|
|
if (v->useit == 0)
|
|||
|
|
return 0;
|
|||
|
|
|
|||
|
|
cds_rs_write_sbus(v);
|
|||
|
|
cds_rs_write_pbus(v);
|
|||
|
|
return 0;
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
|
|||
|
|
int cds_rs_write_sbus(T_cds_rs *v)
|
|||
|
|
{
|
|||
|
|
unsigned int old_err;
|
|||
|
|
|
|||
|
|
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
|
|||
|
|
|
|||
|
|
//8 config rs speed, master/slave and period of survey
|
|||
|
|
x_serial_bus_project.reg_addr = 8; // adr memory in plate
|
|||
|
|
x_serial_bus_project.write_data = v->write.sbus.config.all; // write data
|
|||
|
|
|
|||
|
|
if (x_serial_bus_project.write(&x_serial_bus_project)) // make write
|
|||
|
|
v->status_serial_bus.count_write_error++;
|
|||
|
|
|
|||
|
|
if (old_err == v->status_serial_bus.count_write_error)// no errors
|
|||
|
|
{
|
|||
|
|
v->status_serial_bus.count_write_ok++;
|
|||
|
|
return 0; // no errors
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return 1; // !error!
|
|||
|
|
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
|
|||
|
|
int cds_rs_write_pbus(T_cds_rs *v)
|
|||
|
|
{
|
|||
|
|
if (v->useit == 0)
|
|||
|
|
return 0;
|
|||
|
|
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
|
|||
|
|
int cds_rs_read_sbus(T_cds_rs *v)
|
|||
|
|
{
|
|||
|
|
unsigned int old_err;
|
|||
|
|
|
|||
|
|
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
|
|||
|
|
if(v->write.sbus.config.bit.channel1_enable)
|
|||
|
|
{
|
|||
|
|
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.sensor[0].direction = (x_parallel_bus_project.data_table_read >> 15) & 1;
|
|||
|
|
v->read.sbus.sensor[0].turned_angle = (x_parallel_bus_project.data_table_read & 0x7FFF) << 3;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
v->status_serial_bus.count_read_error++;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//1
|
|||
|
|
if(v->write.sbus.config.bit.channel2_enable)
|
|||
|
|
{
|
|||
|
|
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.sensor[1].direction = (x_parallel_bus_project.data_table_read >> 15) & 1;
|
|||
|
|
v->read.sbus.sensor[1].turned_angle = (x_parallel_bus_project.data_table_read & 0x7FFF) << 3;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
v->status_serial_bus.count_read_error++;
|
|||
|
|
}
|
|||
|
|
//2
|
|||
|
|
if(v->write.sbus.config.bit.channel3_enable)
|
|||
|
|
{
|
|||
|
|
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.sensor[2].direction = (x_parallel_bus_project.data_table_read >> 15) & 1;
|
|||
|
|
v->read.sbus.sensor[2].turned_angle = (x_parallel_bus_project.data_table_read & 0x7FFF) << 3;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
v->status_serial_bus.count_read_error++;
|
|||
|
|
}
|
|||
|
|
//3
|
|||
|
|
if(v->write.sbus.config.bit.channel4_enable)
|
|||
|
|
{
|
|||
|
|
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.sensor[3].direction = (x_parallel_bus_project.data_table_read >> 15) & 1;
|
|||
|
|
v->read.sbus.sensor[3].turned_angle = (x_parallel_bus_project.data_table_read & 0x7FFF) << 3;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
v->status_serial_bus.count_read_error++;
|
|||
|
|
}
|
|||
|
|
//4
|
|||
|
|
if(v->write.sbus.config.bit.channel1_enable)
|
|||
|
|
{
|
|||
|
|
x_serial_bus_project.reg_addr = 4; // 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.sensor[0].angle = x_parallel_bus_project.data_table_read << 2;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
v->status_serial_bus.count_read_error++;
|
|||
|
|
}
|
|||
|
|
//5
|
|||
|
|
if(v->write.sbus.config.bit.channel2_enable)
|
|||
|
|
{
|
|||
|
|
x_serial_bus_project.reg_addr = 5; // 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.sensor[1].angle = x_parallel_bus_project.data_table_read << 2;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
v->status_serial_bus.count_read_error++;
|
|||
|
|
}
|
|||
|
|
//6
|
|||
|
|
if(v->write.sbus.config.bit.channel3_enable)
|
|||
|
|
{
|
|||
|
|
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.sensor[2].angle = x_parallel_bus_project.data_table_read << 2;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
v->status_serial_bus.count_read_error++;
|
|||
|
|
}
|
|||
|
|
//7
|
|||
|
|
if(v->write.sbus.config.bit.channel4_enable)
|
|||
|
|
{
|
|||
|
|
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.sensor[3].angle = x_parallel_bus_project.data_table_read << 2;
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
v->status_serial_bus.count_read_error++;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
//8
|
|||
|
|
x_serial_bus_project.reg_addr = 8; // 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.config.all = x_parallel_bus_project.data_table_read;
|
|||
|
|
}
|
|||
|
|
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->read.sbus.current_status_error.all = x_serial_bus_project.read_data & 0xfffe;
|
|||
|
|
}
|
|||
|
|
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++;
|
|||
|
|
return 0; // no errors
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
return 1; // !errors!
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
|
|||
|
|
int cds_rs_read_pbus(T_cds_rs *v)
|
|||
|
|
{
|
|||
|
|
if (v->useit == 0)
|
|||
|
|
return 0;
|
|||
|
|
|
|||
|
|
if (v->status & (component_Started | component_Ready | component_Error | component_ErrorSBus))
|
|||
|
|
{
|
|||
|
|
//0
|
|||
|
|
if(v->write.sbus.config.bit.channel1_enable && 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.sensor[0].direction = (x_parallel_bus_project.data_table_read >> 15) & 1;
|
|||
|
|
v->read.pbus.sensor[0].turned_angle = (x_parallel_bus_project.data_table_read & 0x7FFF) << 3;
|
|||
|
|
}
|
|||
|
|
//1
|
|||
|
|
if(v->write.sbus.config.bit.channel2_enable && 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.sensor[1].direction = (x_parallel_bus_project.data_table_read >> 15) & 1;
|
|||
|
|
v->read.pbus.sensor[1].turned_angle = (x_parallel_bus_project.data_table_read & 0x7FFF) << 3;
|
|||
|
|
}
|
|||
|
|
//2
|
|||
|
|
if(v->write.sbus.config.bit.channel3_enable && 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.sensor[2].direction = (x_parallel_bus_project.data_table_read >> 15) & 1;
|
|||
|
|
v->read.pbus.sensor[2].turned_angle = (x_parallel_bus_project.data_table_read & 0x7FFF) << 3;
|
|||
|
|
}
|
|||
|
|
//3
|
|||
|
|
if(v->write.sbus.config.bit.channel4_enable && 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.sensor[3].direction = (x_parallel_bus_project.data_table_read >> 15) & 1;
|
|||
|
|
v->read.pbus.sensor[3].turned_angle = (x_parallel_bus_project.data_table_read & 0x7FFF) << 3;
|
|||
|
|
}
|
|||
|
|
//4
|
|||
|
|
if(v->write.sbus.config.bit.channel1_enable && 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.sensor[0].angle = x_parallel_bus_project.data_table_read << 2;
|
|||
|
|
}
|
|||
|
|
//5
|
|||
|
|
if(v->write.sbus.config.bit.channel2_enable && 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.sensor[1].angle = x_parallel_bus_project.data_table_read << 2;
|
|||
|
|
}
|
|||
|
|
//6
|
|||
|
|
if(v->write.sbus.config.bit.channel3_enable && 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.sensor[2].angle = x_parallel_bus_project.data_table_read << 2;
|
|||
|
|
}
|
|||
|
|
//7
|
|||
|
|
if(v->write.sbus.config.bit.channel4_enable && 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.sensor[3].angle = x_parallel_bus_project.data_table_read << 2;
|
|||
|
|
}
|
|||
|
|
}
|
|||
|
|
else
|
|||
|
|
{
|
|||
|
|
v->read.pbus.sensor[0].direction = 0;
|
|||
|
|
v->read.pbus.sensor[0].turned_angle = 0;
|
|||
|
|
v->read.pbus.sensor[0].angle = 0;
|
|||
|
|
|
|||
|
|
v->read.pbus.sensor[1].direction = 0;
|
|||
|
|
v->read.pbus.sensor[1].turned_angle = 0;
|
|||
|
|
v->read.pbus.sensor[1].angle = 0;
|
|||
|
|
|
|||
|
|
v->read.pbus.sensor[2].direction = 0;
|
|||
|
|
v->read.pbus.sensor[2].turned_angle = 0;
|
|||
|
|
v->read.pbus.sensor[2].angle = 0;
|
|||
|
|
|
|||
|
|
v->read.pbus.sensor[3].direction = 0;
|
|||
|
|
v->read.pbus.sensor[3].turned_angle = 0;
|
|||
|
|
v->read.pbus.sensor[3].angle = 0;
|
|||
|
|
}
|
|||
|
|
return 0;
|
|||
|
|
}
|
|||
|
|
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
///////////////////////////////////////////////
|
|||
|
|
|