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