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