bool calibration_enter(void) { // If not flying if (!sys_state_is_flying()) { calibration_prev_state = sys_get_state(); calibration_prev_mode = sys_get_mode(); // Lock vehicle during calibration sys_set_mode((uint8_t)MAV_MODE_LOCKED); sys_set_state((uint8_t)MAV_STATE_CALIBRATING); debug_message_buffer("Starting calibration."); mavlink_msg_sys_status_send(MAVLINK_COMM_0, global_data.state.mav_mode, global_data.state.nav_mode, global_data.state.status, global_data.cpu_usage, global_data.battery_voltage, global_data.motor_block, communication_get_uart_drop_rate()); mavlink_msg_sys_status_send(MAVLINK_COMM_1, global_data.state.mav_mode, global_data.state.nav_mode, global_data.state.status, global_data.cpu_usage, global_data.battery_voltage, global_data.motor_block, communication_get_uart_drop_rate()); debug_message_send_one(); debug_message_send_one(); return true; } else { //Can't calibrate during flight debug_message_buffer("Can't calibrate during flight!!!"); return false; } }
void execute_action(uint8_t action) { switch (action) { case MAV_ACTION_LAUNCH: if (global_data.state.mav_mode > (uint8_t)MAV_MODE_LOCKED) { global_data.state.status = (uint8_t)MAV_STATE_ACTIVE; } break; case MAV_ACTION_MOTORS_START: if (global_data.state.mav_mode > (uint8_t)MAV_MODE_LOCKED) { global_data.state.status = (uint8_t)MAV_STATE_ACTIVE; } break; case MAV_ACTION_MOTORS_STOP: global_data.state.status = (uint8_t)MAV_STATE_STANDBY; break; case MAV_ACTION_EMCY_KILL: global_data.state.status = (uint8_t)MAV_STATE_EMERGENCY; break; case MAV_ACTION_STORAGE_READ: param_read_all(); debug_message_buffer("Started reading params from eeprom"); break; case MAV_ACTION_STORAGE_WRITE: debug_message_buffer("Started writing params to eeprom"); param_write_all(); break; case MAV_ACTION_CALIBRATE_GYRO: start_gyro_calibration(); break; case MAV_ACTION_CALIBRATE_RC: start_rc_calibration(); break; case MAV_ACTION_CALIBRATE_MAG: start_mag_calibration(); break; case MAV_ACTION_CALIBRATE_PRESSURE: start_pressure_calibration(); break; case MAV_ACTION_SET_ORIGIN: // If not flying if (!sys_state_is_flying()) { gps_set_local_origin(); altitude_set_local_origin(); } break; default: // Should never be reached, ignore unknown commands debug_message_buffer_sprintf("Rejected unknown action Number: %u", action); break; } }
/** * @brief Set the current mode of operation * @param mode the new mode */ bool sys_set_mode(uint8_t mode) { if (mode == MAV_MODE_AUTO) { global_data.state.mav_mode = MAV_MODE_AUTO; return true; } else if (mode == MAV_MODE_GUIDED) { global_data.state.mav_mode = MAV_MODE_GUIDED; return true; } else if (mode == MAV_MODE_LOCKED) { global_data.state.mav_mode = MAV_MODE_LOCKED; return true; } else if (mode == MAV_MODE_MANUAL) { global_data.state.mav_mode = MAV_MODE_MANUAL; return true; } else if (mode == MAV_MODE_READY) { global_data.state.mav_mode = MAV_MODE_READY; return true; } else if (mode == MAV_MODE_TEST1) { global_data.state.mav_mode = MAV_MODE_TEST1; return true; } else if (mode == MAV_MODE_TEST2) { global_data.state.mav_mode = MAV_MODE_TEST2; return true; } else if (mode == MAV_MODE_TEST3) { global_data.state.mav_mode = MAV_MODE_TEST3; return true; } else if (mode == MAV_MODE_RC_TRAINING) { // Only go into RC training if not flying if (! sys_state_is_flying()) { global_data.state.mav_mode = MAV_MODE_RC_TRAINING; return true; } else { debug_message_buffer("WARNING: SYSTEM IS IN FLIGHT! Denied to switch to RC mode"); return false; } } // UNINIT is not a mode that should be actively set // it will thus be rejected like any other invalid mode else { // No valid mode debug_message_buffer("WARNING: Attempted to set invalid mode"); return false; } }