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; }