int main (void) { central_data_t *central_data; boardsupport_init(); central_data=central_data_get_pointer_to_struct(); central_data->mavlink_stream.msg_available=false; int cycles=0; while (true) { mavlink_stream_receive(¢ral_data->mavlink_stream); if (central_data->mavlink_stream.msg_available) { //delay_ms(500); mavboot_run(¢ral_data->mavboot_state, ¢ral_data->mavlink_stream.rec.msg); central_data->mavlink_stream.msg_available=false; } cycles++; if ((cycles>BOOTLOADER_IDLE_WAIT_TIME)&&(central_data->mavboot_state.state==MAVBOOT_IDLE)) { mavboot_start_user_application(BOOTLOADER_USER_APPLICATION_ADDRESS); // if this returns there is no program to start (page erased) --> return to bootloader function central_data->mavboot_state.state=MAVBOOT_ACTIVE; } } }
void initialisation() { central_data = central_data_get_pointer_to_struct(); boardsupport_init(central_data); central_data_init(); mavlink_telemetry_init(); //onboard_parameters_write_parameters_to_flashc(¢ral_data->mavlink_communication.onboard_parameters); onboard_parameters_read_parameters_from_flashc(¢ral_data->mavlink_communication.onboard_parameters); central_data->state.mav_state = MAV_STATE_STANDBY; central_data->imu.calibration_level = OFF; piezo_speaker_quick_startup(); // Switch off red LED LED_Off(LED2); print_util_dbg_print("OK. Starting up.\r\n"); }