static void spistream_event() {
static struct AutopilotMessagePTStream msg_in;
static struct AutopilotMessagePTStream msg_out;
static uint8_t crc_valid;
spistream_read_pkg(&msg_in);
/*
uint8_t cnt;
static uint8_t pkg_size = sizeof(msg_in.pkg_data);
if(msg_out.message_cnt != 0) {
printf("Package out: Size: %3d MID: %3d PCNTD: %3d | ",
pkg_size, msg_out.message_cnt, msg_out.package_cntd);
for(cnt = 0; cnt < pkg_size; cnt++) {
printf("%3d ", msg_out.pkg_data[cnt]);
}
printf("\n");
}
*/
spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid);
/*
if(msg_in.message_cnt != 0) {
printf("PKG in (spi trx: %d): Size: %3d MID: %3d PCNTD: %3d | ", SPISTREAM_PACKAGE_SIZE,
pkg_size, msg_in.message_cnt, msg_in.package_cntd);
for(cnt = 0; cnt < pkg_size; cnt++) {
printf("%02X ", msg_in.pkg_data[cnt]);
}
printf("\n");
}
*/
spistream_write_pkg(&msg_out);
}
static void dialog_with_io_proc() {
struct AutopilotMessageCRCFrame msg_in;
struct AutopilotMessageCRCFrame msg_out;
uint8_t crc_valid;
for (uint8_t i=0; i<6; i++) msg_out.payload.msg_down.pwm_outputs_usecs[i] = otp.servos_outputs_usecs[i];
// for (uint8_t i=0; i<4; i++) msg_out.payload.msg_down.csc_servo_cmd[i] = otp.csc_servo_outputs[i];
spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid);
struct AutopilotMessagePTUp *in = &msg_in.payload.msg_up;
RATES_FLOAT_OF_BFP(otp.imu.gyro, in->gyro);
ACCELS_FLOAT_OF_BFP(otp.imu.accel, in->accel);
MAGS_FLOAT_OF_BFP(otp.imu.mag, in->mag);
otp.io_proc_msg_cnt = in->stm_msg_cnt;
otp.io_proc_err_cnt = in->stm_crc_err_cnt;
otp.rc_status = in->rc_status;
otp.baro_abs = in->pressure_absolute;
otp.baro_diff = in->pressure_differential;
}
static void main_send_to_stm(void) {
struct OVERO_LINK_MSG_UP msg_in;
struct OVERO_LINK_MSG_DOWN msg_out;
spi_link_send(&msg_out, sizeof(union AutopilotMessage), &msg_in);
// printf("spi telemetry got %d\n", msg_in.up.tw_len);
// for (int i=0; i<msg_in.up.tw_len; i++)
// printf("%02x ", msg_in.up.data[i]);
// printf("\n");
}
static void dialog_with_io_proc() {
struct AutopilotMessageCRCFrame msg_in;
struct AutopilotMessageCRCFrame msg_out;
uint8_t crc_valid;
for (uint8_t i=0; i<LISA_PWM_OUTPUT_NB; i++) msg_out.payload.msg_down.pwm_outputs_usecs[i] = blmc_calibrate.servos_outputs_usecs[i];
spi_link_send(&msg_out, sizeof(struct AutopilotMessageCRCFrame), &msg_in, &crc_valid);
}
void spi_ap_link_periodic()
{
static struct AutopilotMessageCRCFrame msg_up;
static struct AutopilotMessageCRCFrame msg_down;
uint8_t crc_valid;
passthrough_down_fill(&msg_down.payload.msg_down);
// SPI transcieve
spi_link_send(&msg_down, sizeof(struct AutopilotMessageCRCFrame), &msg_up, &crc_valid);
if(crc_valid) {
passthrough_up_parse(&msg_up.payload.msg_up);
}
}
//static uint8_t main_dialog_with_io_proc() {
static void main_dialog_with_io_proc() {
DEFINE_AutopilotMessageCRCFrame_IN_and_OUT(message);
uint8_t crc_valid;
// for (uint8_t i=0; i<6; i++) msg_out.payload.msg_down.pwm_outputs_usecs[i] = otp.servos_outputs_usecs[i];
spi_link_send(&message_out, sizeof(struct AutopilotMessageCRCFrame), &message_in, &crc_valid);
main_rawlog_dump(&message_in.payload.msg_up);
if(GPS_READY(message_in.payload.msg_up.valid_sensors)){printf("GPS!\n");}
/*struct AutopilotMessageVIUp *in = &message_in.payload.msg_up;
if(IMU_READY(in->valid_sensors)){
COPY_RATES_ACCEL_TO_IMU_FLOAT(in);
}
if(MAG_READY(in->valid_sensors)){
COPY_MAG_TO_IMU_FLOAT(in);
#if PRINT_MAG
printmag();
#endif
}
if(BARO_READY(in->valid_sensors)){
baro_measurement = in->pressure_absolute;
}
if(GPS_READY(in->valid_sensors)){
COPY_GPS_TO_IMU(in);
#if PRINT_GPS
printgps();
#endif
}
return in->valid_sensors;*/
}
int main(int argc, char *argv[]) {
uint32_t us_delay;
if(argc > 1) {
us_delay = atoi(argv[1]);
}
else {
us_delay = 1953;
}
printf("Delay: %dus\n", us_delay);
if (spi_link_init()) {
TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n");
return -1;
}
uint8_t skip_buf_check = 0;
uint8_t skip_crc_check = 0;
uint32_t buf_check_errors = 0;
while (1) {
struct AutopilotMessageCRCFrame crc_msg_out;
struct AutopilotMessageCRCFrame msg_out_prev;
struct AutopilotMessageCRCFrame crc_msg_in;
uint8_t crc_valid;
/* backup message for later comparison */
memcpy(&msg_out_prev, &crc_msg_out, sizeof(struct AutopilotMessageCRCFrame));
/* fill message with data */
fill_msg(&crc_msg_out);
/* send it over spi */
spi_link_send(&crc_msg_out, sizeof(struct AutopilotMessageCRCFrame), &crc_msg_in, &crc_valid);
/* check that received message is identical to the one previously sent */
if (!skip_buf_check && spi_link.msg_cnt > 1) {
if (memcmp(&crc_msg_in.payload, &msg_out_prev.payload, sizeof(struct OVERO_LINK_MSG_DOWN))) {
printf("Compare failed: (received != expected): \n");
print_up_msg(&crc_msg_in);
print_down_msg(&msg_out_prev);
buf_check_errors++;
}
}
/* report crc error */
if (!skip_crc_check & !crc_valid) {
printf("CRC checksum failed: received %04X != computed %04X\n",
crc_msg_in.crc,
crc_calc_block_crc8((uint8_t*)&crc_msg_in.payload, sizeof(struct OVERO_LINK_MSG_DOWN)));
}
/* report message count */
if (!(spi_link.msg_cnt % 1000))
printf("msg %d, buf err %d, CRC errors: %d\n", spi_link.msg_cnt,
buf_check_errors, spi_link.crc_err_cnt);
/* give it some rest */
if(us_delay > 0)
usleep(us_delay);
}
return 0;
}
