static void main_init(void) {
TRACE(TRACE_DEBUG, "%s", "Starting initialization\n");
/* Initalize our SPI link to IO processor */
if (spi_link_init()) {
TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n");
return;
}
/* Initalize the event library */
event_init();
/* Initalize our ô so accurate periodic timer */
if (fms_periodic_init(main_periodic)) {
TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n");
return;
}
/* Initialize our communications with ground segment */
fms_gs_com_init(otp.gs_gw, 4242, 4243, FALSE);
/* Initialize blaaa */
for (uint8_t i=0; i<6; i++) otp.servos_outputs_usecs[i] = 1500;
for (uint8_t i=0; i<4; i++) otp.csc_servo_outputs[i] = 1500;
TRACE(TRACE_DEBUG, "%s", "Initialization completed\n");
}
int spi_ap_link_init()
{
if (spi_link_init()) {
TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n");
return -1;
}
// Initialize IMU->Body orientation
imuFloat.body_to_imu_quat = body_to_imu_quat;
imuFloat.sample_count = 0;
#ifdef IMU_ALIGN_BENCH
// This code is for aligning body to imu rotation, turn this on, put the vehicle in hover, pointed north, read BOOZ2_AHRS_REF_QUAT as body to imu (in wing frame)
struct FloatVect3 x_axis = { 0.0, 1.0, 0.0 };
FLOAT_QUAT_OF_AXIS_ANGLE(imuFloat.body_to_imu_quat, x_axis, QUAT_SETPOINT_HOVER_PITCH);
#endif
FLOAT_QUAT_NORMALISE(imuFloat.body_to_imu_quat);
FLOAT_EULERS_OF_QUAT(imuFloat.body_to_imu_eulers, imuFloat.body_to_imu_quat);
FLOAT_RMAT_OF_QUAT(imuFloat.body_to_imu_rmat, imuFloat.body_to_imu_quat);
struct FloatRates bias0 = { 0., 0., 0.};
rdyb_mahrs_init(imuFloat.body_to_imu_quat, bias0);
return 0;
}
int main(int argc, char** argv) {
/* Initalize event library */
event_init();
if (fms_periodic_init(main_periodic)) {
TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n");
return -1;
}
if (spi_link_init()) {
TRACE(TRACE_ERROR, "%s", "failed to open SPI link\n");
return -1;
}
/* Add our datalink event */
network = network_new(LINK_HOST, LINK_PORT, DATALINK_PORT, FMS_NETWORK_BROADCAST);
struct event datalink_event;
event_set(&datalink_event, network->socket_in, EV_READ, on_datalink_event, &datalink_event);
event_add(&datalink_event, NULL);
event_dispatch();
return 0;
}
static void main_init(void) {
uint8_t byte_idx;
TRACE(TRACE_DEBUG, "%s", "Starting initialization\n");
/* Initalize our SPI link to IO processor */
if (spi_link_init()) {
TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n");
return;
}
spistream_init(&on_spistream_msg_received,
&on_spistream_msg_sent);
for(byte_idx=1; byte_idx < 123; byte_idx++) {
spistream_msg[byte_idx] = byte_idx;
}
/* Initalize the event library */
event_init();
/* Initalize our ô so accurate periodic timer */
if (fms_periodic_init(main_periodic)) {
TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n");
return;
}
signal(SIGKILL, on_kill);
signal(SIGINT, on_kill);
signal(SIGILL, on_kill);
signal(SIGHUP, on_kill);
signal(SIGQUIT, on_kill);
signal(SIGTERM, on_kill);
signal(SIGSEGV, on_kill);
signal(SIGPIPE, on_dead_pipe);
if(!open_stream()) {
fprintf(LOG_OUT, "Could not open stream, sorry\n");
exit(1);
}
TRACE(TRACE_DEBUG, "%s", "Initialization completed\n");
}
static void main_init(void) {
#if RUN_FILTER
printf("FILTER output will be in ");
#if FILTER_OUTPUT_IN_NED
printf("NED\n");
#else
printf("ECEF\n");
#endif
#else
printf("Filter wont run\n");
#endif
#if UPDATE_WITH_GRAVITY
printf("the orientation becomes UPDATED with the GRAVITY\n");
#endif
TRACE(TRACE_DEBUG, "%s", "Starting initialization\n");
/* Initalize our SPI link to IO processor */
if (spi_link_init()) {
TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n");
return;
}
/* Initalize the event library */
event_init();
/* Initalize our ô so accurate periodic timer */
if (fms_periodic_init(main_periodic)) {
TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n");
return;
}
#if RUN_FILTER
init_ins_state();
set_reference_direction();
#endif
main_rawlog_init(IMU_LOG_FILE);
}
static void main_init(void) {
/* Initalize our SPI link to IO processor */
if (spi_link_init()) {
TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n");
return;
}
printf("Starting at 2000us\n");
/* Initialize blaaa */
for (uint8_t i=0; i<LISA_PWM_OUTPUT_NB; i++) blmc_calibrate.servos_outputs_usecs[i] = 2000;
dialog_with_io_proc();
getchar();
printf("At 1000us\n");
for (uint8_t i=0; i<LISA_PWM_OUTPUT_NB; i++) blmc_calibrate.servos_outputs_usecs[i] = 1000;
dialog_with_io_proc();
getchar();
printf("At 1500us\n");
for (uint8_t i=0; i<LISA_PWM_OUTPUT_NB; i++) blmc_calibrate.servos_outputs_usecs[i] = 1500;
dialog_with_io_proc();
}
int main(int argc, char *argv[])
{
(void) signal(SIGINT, main_exit);
//set IMU neutrals
RATES_ASSIGN(imu.gyro_neutral, IMU_GYRO_P_NEUTRAL, IMU_GYRO_Q_NEUTRAL, IMU_GYRO_R_NEUTRAL);
VECT3_ASSIGN(imu.accel_neutral, IMU_ACCEL_X_NEUTRAL, IMU_ACCEL_Y_NEUTRAL, IMU_ACCEL_Z_NEUTRAL);
VECT3_ASSIGN(imu.mag_neutral, IMU_MAG_X_NEUTRAL, IMU_MAG_Y_NEUTRAL, IMU_MAG_Z_NEUTRAL);
if (spi_link_init()) {
TRACE(TRACE_ERROR, "%s", "failed to open SPI link \n");
return -1;
}
/* Initalize the event library */
event_init();
control_init();
estimator_init();
// file_logger_init("my_log.data");
gcs_com_init();
if (fms_periodic_init(main_periodic)) {
TRACE(TRACE_ERROR, "%s", "failed to start periodic generator\n");
return -1;
}
//main_parse_cmd_line(argc, argv);
event_dispatch();
//should never occur!
printf("goodbye! (%d)\n", foo);
return 0;
}
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;
}
