// dcm_calibrate is called twice during the startup sequence. // Firstly 10 seconds after startup, then immediately before the first waggle, which is 10 seconds after getting radio link. // This makes sure we get initialized when there's no radio, or when bench testing, // and 2nd time is at a time the user knows to keep the plane steady before a flight. void dcm_calibrate(void) { // Don't allow re/calibrating before the initial calibration period has finished if (dcm_flags._.calib_finished) { udb_a2d_record_offsets(); } }
void MAVLinkCommandLong(mavlink_message_t* handle_msg) // MAVLINK_MSG_ID_COMMAND_LONG { mavlink_command_long_t packet; mavlink_msg_command_long_decode(handle_msg, &packet); DPRINT("MAVLINK_MSG_ID_COMMAND_LONG %u\r\n", handle_msg->msgid); //if (mavlink_check_target(packet.target, packet.target_component) == false) break; { switch (packet.command) { case MAV_CMD_PREFLIGHT_CALIBRATION: DPRINT("MAV_CMD_PREFLIGHT_CALIBRATION %u\r\n", packet.command); if (packet.param1 == 1) { #if (USE_NV_MEMORY ==1) udb_skip_flags.skip_imu_cal = 0; #endif // (USE_NV_MEMORY == 1) udb_a2d_record_offsets(); } else if (packet.param4 == 1) //param4 = radio calibration { if (udb_flags._.radio_on == 1) { udb_servo_record_trims(); command_ack(packet.command, MAV_CMD_ACK_OK); } else command_ack(packet.command, MAV_CMD_ACK_ERR_FAIL); } else command_ack(packet.command, MAV_CMD_ACK_ERR_NOT_SUPPORTED); break; #if (USE_NV_MEMORY == 1) case MAV_CMD_PREFLIGHT_STORAGE: DPRINT("MAV_CMD_PREFLIGHT_STORAGE %u\r\n", packet.command); if (packet.param1 == MAV_PFS_CMD_WRITE_ALL) // 1 { if (packet.param2 == MAV_PFS_CMD_WRITE_ALL) data_services_save_all(STORAGE_FLAG_STORE_CALIB | STORAGE_FLAG_STORE_WAYPOINTS, &preflight_storage_complete_callback); else data_services_save_all(STORAGE_FLAG_STORE_CALIB, &preflight_storage_complete_callback); } else if (packet.param1 == MAV_PFS_CMD_READ_ALL) // 0 { if (packet.param2 == MAV_PFS_CMD_READ_ALL) data_services_load_all(STORAGE_FLAG_STORE_CALIB | STORAGE_FLAG_STORE_WAYPOINTS, &preflight_storage_complete_callback); else data_services_load_all(STORAGE_FLAG_STORE_CALIB, &preflight_storage_complete_callback); } else command_ack(packet.command, MAV_CMD_ACK_ERR_NOT_SUPPORTED); break; case MAV_CMD_PREFLIGHT_STORAGE_ADVANCED: DPRINT("MAV_CMD_PREFLIGHT_STORAGE_ADVANCED %u\r\n", packet.command); switch ((uint16_t)packet.param1) { case MAV_PFS_CMD_CLEAR_SPECIFIC: storage_clear_area(packet.param2, &preflight_storage_complete_callback); break; case MAV_PFS_CMD_WRITE_SPECIFIC: data_services_save_specific(packet.param2, &preflight_storage_complete_callback); break; case MAV_PFS_CMD_READ_SPECIFIC: data_services_load_specific(packet.param2, &preflight_storage_complete_callback); break; default: command_ack(packet.command, MAV_CMD_ACK_ERR_NOT_SUPPORTED); break; } break; #endif // (USE_NV_MEMORY == 1) case 245: // MAV_CMD_PREFLIGHT_STORAGE: switch ((uint16_t)packet.param1) { case 0: // Read DPRINT("Read (ROM)\r\n"); config_load(); break; case 1: // Write DPRINT("Write (ROM)\r\n"); config_save(); break; default: DPRINT("245 packet.param1 %f packet.param2 %f\r\n", (double)packet.param1, (double)packet.param2); break; } break; case 246: // halt DPRINT("Halt - packet.command %u\r\n", packet.command); break; case 22: // start DPRINT("Start - packet.command %u\r\n", packet.command); break; case 252: // land DPRINT("Land - packet.command %u\r\n", packet.command); break; default: DPRINT("packet.command %u\r\n", packet.command); command_ack(packet.command, MAV_CMD_ACK_ERR_NOT_SUPPORTED); break; } } }
void dcm_calibrate(void) { udb_a2d_record_offsets() ; dcm_has_calibrated = true ; return ; }
// Called every 1/40 second at low priority void udb_heartbeat_40hz_callback(void) { if (udb_heartbeat_counter % 20 != 0) return; switch (calib_countdown) { #if (BOARD_TYPE == UDB4_BOARD || BOARD_TYPE == UDB5_BOARD) case 9: udb_background_trigger(&udb_background_callback_triggered); LED_RED = LED_ON; break; case 8: LED_RED = LED_OFF; LED_GREEN = LED_ON; break; case 7: LED_GREEN = LED_OFF; LED_ORANGE = LED_ON; break; case 6: LED_ORANGE = LED_OFF; LED_BLUE = LED_ON; break; case 5: LED_BLUE = LED_OFF; LED_RED = LED_ON; break; case 4: LED_RED = LED_OFF; LED_GREEN = LED_ON; break; case 3: LED_GREEN = LED_OFF; LED_ORANGE = LED_ON; break; case 2: LED_BLUE = LED_ON; LED_ORANGE = LED_OFF; break; case 1: LED_BLUE = LED_OFF; udb_a2d_record_offsets(); break; #elif (BOARD_TYPE == AUAV3_BOARD) case 9: udb_background_trigger(&udb_background_callback_triggered); LED_BLUE = LED_ON; break; case 8: LED_BLUE = LED_OFF; LED_RED = LED_ON; break; case 7: LED_RED = LED_OFF; LED_GREEN = LED_ON; break; case 6: LED_GREEN = LED_OFF; LED_ORANGE = LED_ON; break; case 5: LED_ORANGE = LED_OFF; LED_BLUE = LED_ON; break; case 4: LED_BLUE = LED_OFF; LED_RED = LED_ON; break; case 3: LED_RED = LED_OFF; LED_GREEN = LED_ON; break; case 2: LED_GREEN = LED_OFF; LED_ORANGE = LED_ON; break; case 1: LED_ORANGE = LED_OFF; udb_a2d_record_offsets(); break; #else #error "unsupported BOARD_TYPE" #endif case 0: // after 5 sec return; } calib_countdown--; }