diff options
-rw-r--r-- | src/modules/commander/commander.cpp | 144 | ||||
-rw-r--r-- | src/modules/commander/px4_custom_mode.h | 1 | ||||
-rw-r--r-- | src/modules/commander/state_machine_helper.cpp | 12 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink.c | 3 | ||||
-rw-r--r-- | src/modules/mavlink/mavlink_receiver.cpp | 48 | ||||
-rw-r--r-- | src/modules/sensors/sensor_params.c | 3 | ||||
-rw-r--r-- | src/modules/sensors/sensors.cpp | 29 | ||||
-rw-r--r-- | src/modules/uORB/topics/manual_control_setpoint.h | 4 | ||||
-rw-r--r-- | src/modules/uORB/topics/vehicle_status.h | 8 |
9 files changed, 201 insertions, 51 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 901f91911..e1cca8bfb 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -119,6 +119,7 @@ extern struct system_load_s system_load; #define POSITION_TIMEOUT 1000000 /**< consider the local or global position estimate invalid after 1s */ #define RC_TIMEOUT 100000 +#define OFFBOARD_TIMEOUT 200000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -432,6 +433,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_AUTO) { /* AUTO */ main_res = main_state_transition(status, MAIN_STATE_AUTO); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { + /* OFFBOARD */ + main_res = main_state_transition(status, MAIN_STATE_OFFBOARD); } } else { @@ -583,6 +588,7 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[1] = "SEATBELT"; main_states_str[2] = "EASY"; main_states_str[3] = "AUTO"; + main_states_str[4] = "OFFBOARD"; char *arming_states_str[ARMING_STATE_MAX]; arming_states_str[0] = "INIT"; @@ -712,6 +718,11 @@ int commander_thread_main(int argc, char *argv[]) struct manual_control_setpoint_s sp_man; memset(&sp_man, 0, sizeof(sp_man)); + /* Subscribe to offboard control data */ + int offboard_sp_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); + struct offboard_control_setpoint_s offboard_sp; + memset(&offboard_sp, 0, sizeof(offboard_sp)); + /* Subscribe to offboard control data */ int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); struct offboard_control_setpoint_s sp_offboard; @@ -829,6 +840,10 @@ int commander_thread_main(int argc, char *argv[]) orb_copy(ORB_ID(manual_control_setpoint), sp_man_sub, &sp_man); } + if (updated) { + orb_copy(ORB_ID(offboard_control_setpoint), offboard_sp_sub, &offboard_sp); + } + orb_check(sp_offboard_sub, &updated); if (updated) { @@ -1160,42 +1175,106 @@ int commander_thread_main(int argc, char *argv[]) status_changed = true; } - if (armed.armed) { - if (status.main_state == MAIN_STATE_AUTO) { - /* check if AUTO mode still allowed */ - transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); + /* switch to OFFBOARD mode if offboard signal available */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_OFFBOARD); - if (res == TRANSITION_DENIED) { - /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + if (res == TRANSITION_DENIED) { + /* can't switch to OFFBOARD, do normal failsafe if needed */ + if (armed.armed) { + if (status.main_state == MAIN_STATE_AUTO) { + /* check if AUTO mode still allowed */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_AUTO); if (res == TRANSITION_DENIED) { - /* LAND not allowed, set TERMINATION state */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + + if (res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } } - } - } else { - /* failsafe for manual modes */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); + } else if (status.main_state == MAIN_STATE_OFFBOARD) { + /* check if OFFBOARD mode still allowed */ + transition_result_t res = main_state_transition(&status, MAIN_STATE_OFFBOARD); + + if (res == TRANSITION_DENIED) { + /* AUTO mode denied, don't try RTL, switch to failsafe state LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + + if (res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } + } - if (res == TRANSITION_DENIED) { - /* RTL not allowed (no global position estimate), try LAND */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + } else { + /* failsafe for manual modes */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_RTL); if (res == TRANSITION_DENIED) { - /* LAND not allowed, set TERMINATION state */ - res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + /* RTL not allowed (no global position estimate), try LAND */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_LAND); + + if (res == TRANSITION_DENIED) { + /* LAND not allowed, set TERMINATION state */ + res = failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION); + } } } + + } else { + if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { + /* reset failsafe when disarmed */ + transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + } } + } + } + + /* check offboard signal */ + if (offboard_sp.timestamp != 0 && hrt_absolute_time() < offboard_sp.timestamp + OFFBOARD_TIMEOUT) { + if (!status.offboard_control_signal_found_once) { + status.offboard_control_signal_found_once = true; + mavlink_log_info(mavlink_fd, "[cmd] detected offboard signal first time"); + status_changed = true; } else { - if (status.failsafe_state != FAILSAFE_STATE_NORMAL) { - /* reset failsafe when disarmed */ - transition_result_t res = failsafe_state_transition(&status, FAILSAFE_STATE_NORMAL); + if (status.offboard_control_signal_lost) { + mavlink_log_info(mavlink_fd, "[cmd] offboard signal regained"); + status_changed = true; + } + } + + status.offboard_control_signal_lost = false; + + if (status.main_state == MAIN_STATE_OFFBOARD) { + if (offboard_sp.armed && !armed.armed) { + if (!safety.safety_switch_available || safety.safety_off) { + transition_result_t res = arming_state_transition(&status, &safety, ARMING_STATE_ARMED, &armed); + + if (res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] ARMED by offboard signal"); + } + } + + } else if (!offboard_sp.armed && armed.armed) { + arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR); + transition_result_t res = arming_state_transition(&status, &safety, new_arming_state, &armed); + + if (res == TRANSITION_CHANGED) { + mavlink_log_info(mavlink_fd, "[cmd] DISARMED by offboard signal"); + } } } + + } else { + if (!status.offboard_control_signal_lost) { + mavlink_log_critical(mavlink_fd, "[cmd[ CRITICAL: OFFBOARD SIGNAL LOST"); + status.offboard_control_signal_lost = true; + status_changed = true; + } } // TODO remove this hack @@ -1471,6 +1550,17 @@ check_mode_switches(struct manual_control_setpoint_s *sp_man, struct vehicle_sta } else { status->mission_switch = MISSION_SWITCH_MISSION; } + + /* offboard switch */ + if (!isfinite(sp_man->offboard_switch)) { + status->offboard_switch = OFFBOARD_SWITCH_NONE; + + } else if (sp_man->offboard_switch > STICK_ON_OFF_LIMIT) { + status->offboard_switch = OFFBOARD_SWITCH_OFFBOARD; + + } else { + status->offboard_switch = OFFBOARD_SWITCH_ONBOARD; + } } transition_result_t @@ -1479,6 +1569,18 @@ set_main_state_rc(struct vehicle_status_s *status) /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; + /* offboard switch overrides main switch */ + if (status->offboard_switch == OFFBOARD_SWITCH_OFFBOARD) { + res = main_state_transition(status, MAIN_STATE_OFFBOARD); + if (res == TRANSITION_DENIED) { + print_reject_mode("OFFBOARD"); + + } else { + return res; + } + } + + /* offboard switched off or denied, check mode switch */ switch (status->mode_switch) { case MODE_SWITCH_MANUAL: res = main_state_transition(status, MAIN_STATE_MANUAL); diff --git a/src/modules/commander/px4_custom_mode.h b/src/modules/commander/px4_custom_mode.h index b60a7e0b9..4843e72c3 100644 --- a/src/modules/commander/px4_custom_mode.h +++ b/src/modules/commander/px4_custom_mode.h @@ -13,6 +13,7 @@ enum PX4_CUSTOM_MAIN_MODE { PX4_CUSTOM_MAIN_MODE_SEATBELT, PX4_CUSTOM_MAIN_MODE_EASY, PX4_CUSTOM_MAIN_MODE_AUTO, + PX4_CUSTOM_MAIN_MODE_OFFBOARD, }; enum PX4_CUSTOM_SUB_MODE_AUTO { diff --git a/src/modules/commander/state_machine_helper.cpp b/src/modules/commander/state_machine_helper.cpp index 43d0e023e..23d3be55d 100644 --- a/src/modules/commander/state_machine_helper.cpp +++ b/src/modules/commander/state_machine_helper.cpp @@ -259,6 +259,18 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta } break; + + case MAIN_STATE_OFFBOARD: + + /* need global position estimate */ + if (!status->offboard_control_signal_lost) { + ret = TRANSITION_CHANGED; + } + + break; + + default: + break; } if (ret == TRANSITION_CHANGED) { diff --git a/src/modules/mavlink/mavlink.c b/src/modules/mavlink/mavlink.c index 4d975066f..9a50c059b 100644 --- a/src/modules/mavlink/mavlink.c +++ b/src/modules/mavlink/mavlink.c @@ -219,9 +219,12 @@ get_mavlink_mode_and_state(uint8_t *mavlink_state, uint8_t *mavlink_base_mode, u *mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_EASY; } else if (v_status.main_state == MAIN_STATE_AUTO) { + /* this must not happen */ *mavlink_base_mode |= MAV_MODE_FLAG_AUTO_ENABLED | MAV_MODE_FLAG_STABILIZE_ENABLED | MAV_MODE_FLAG_GUIDED_ENABLED; custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_AUTO; custom_mode.sub_mode = PX4_CUSTOM_SUB_MODE_AUTO_READY; + } else if (v_status.main_state == MAIN_STATE_OFFBOARD) { + custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_OFFBOARD; } } else { /* use navigation state when navigator is active */ diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index a371a499e..b75305406 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -97,6 +97,7 @@ static mavlink_status_t status; static struct vehicle_vicon_position_s vicon_position; static struct vehicle_command_s vcmd; static struct offboard_control_setpoint_s offboard_control_sp; +static struct vehicle_attitude_setpoint_s att_sp; struct vehicle_global_position_s hil_global_pos; struct vehicle_local_position_s hil_local_pos; @@ -125,6 +126,7 @@ static orb_advert_t cmd_pub = -1; static orb_advert_t flow_pub = -1; static orb_advert_t offboard_control_sp_pub = -1; +static orb_advert_t att_sp_pub = -1; static orb_advert_t vicon_position_pub = -1; static orb_advert_t telemetry_status_pub = -1; @@ -266,8 +268,8 @@ handle_message(mavlink_message_t *msg) /* Handle quadrotor motor setpoints */ if (msg->msgid == MAVLINK_MSG_ID_SET_QUAD_SWARM_ROLL_PITCH_YAW_THRUST) { - mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_motors_setpoint; - mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_motors_setpoint); + mavlink_set_quad_swarm_roll_pitch_yaw_thrust_t quad_swarm_setpoint; + mavlink_msg_set_quad_swarm_roll_pitch_yaw_thrust_decode(msg, &quad_swarm_setpoint); if (mavlink_system.sysid < 4) { @@ -281,7 +283,7 @@ handle_message(mavlink_message_t *msg) uint8_t ml_mode = 0; bool ml_armed = false; - switch (quad_motors_setpoint.mode) { + switch (quad_swarm_setpoint.mode) { case 0: ml_armed = false; break; @@ -307,12 +309,12 @@ handle_message(mavlink_message_t *msg) break; } - offboard_control_sp.p1 = (float)quad_motors_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p2 = (float)quad_motors_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p3 = (float)quad_motors_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; - offboard_control_sp.p4 = (float)quad_motors_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; + offboard_control_sp.p1 = (float)quad_swarm_setpoint.roll[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p2 = (float)quad_swarm_setpoint.pitch[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p3 = (float)quad_swarm_setpoint.yaw[mavlink_system.sysid - 1] / (float)INT16_MAX; + offboard_control_sp.p4 = (float)quad_swarm_setpoint.thrust[mavlink_system.sysid - 1] / (float)UINT16_MAX; - if (quad_motors_setpoint.thrust[mavlink_system.sysid - 1] == 0) { + if (quad_swarm_setpoint.thrust[mavlink_system.sysid - 1] == 0) { ml_armed = false; } @@ -326,9 +328,37 @@ handle_message(mavlink_message_t *msg) offboard_control_sp_pub = orb_advertise(ORB_ID(offboard_control_setpoint), &offboard_control_sp); } else { - /* Publish */ + /* publish */ orb_publish(ORB_ID(offboard_control_setpoint), offboard_control_sp_pub, &offboard_control_sp); } + + if (v_status.main_state == MAIN_STATE_OFFBOARD) { + /* in offboard mode also publish setpoint directly */ + switch (offboard_control_sp.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: + att_sp.timestamp = hrt_absolute_time(); + att_sp.roll_body = offboard_control_sp.p1; + att_sp.pitch_body = offboard_control_sp.p2; + att_sp.yaw_body = offboard_control_sp.p3; + att_sp.thrust = offboard_control_sp.p4; + att_sp.R_valid = false; + att_sp.q_d_valid = false; + att_sp.q_e_valid = false; + att_sp.roll_reset_integral = false; + + /* check if topic has to be advertised */ + if (att_sp_pub <= 0) { + att_sp_pub = orb_advertise(ORB_ID(vehicle_attitude_setpoint), &att_sp); + + } else { + /* publish */ + orb_publish(ORB_ID(vehicle_attitude_setpoint), att_sp_pub, &att_sp); + } + break; + default: + break; + } + } } } diff --git a/src/modules/sensors/sensor_params.c b/src/modules/sensors/sensor_params.c index 30659fd3a..999eca833 100644 --- a/src/modules/sensors/sensor_params.c +++ b/src/modules/sensors/sensor_params.c @@ -449,8 +449,7 @@ PARAM_DEFINE_INT32(RC_MAP_MODE_SW, 0); PARAM_DEFINE_INT32(RC_MAP_RETURN_SW, 0); PARAM_DEFINE_INT32(RC_MAP_ASSIST_SW, 0); PARAM_DEFINE_INT32(RC_MAP_MISSIO_SW, 0); - -//PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); +PARAM_DEFINE_INT32(RC_MAP_OFFB_SW, 0); PARAM_DEFINE_INT32(RC_MAP_FLAPS, 0); diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index ea864390d..f99e9d8bb 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -246,8 +246,7 @@ private: int rc_map_return_sw; int rc_map_assisted_sw; int rc_map_mission_sw; - -// int rc_map_offboard_ctrl_mode_sw; + int rc_map_offboard_sw; int rc_map_flaps; @@ -296,8 +295,7 @@ private: param_t rc_map_return_sw; param_t rc_map_assisted_sw; param_t rc_map_mission_sw; - -// param_t rc_map_offboard_ctrl_mode_sw; + param_t rc_map_offboard_sw; param_t rc_map_flaps; @@ -515,8 +513,7 @@ Sensors::Sensors() : /* optional mode switches, not mapped per default */ _parameter_handles.rc_map_assisted_sw = param_find("RC_MAP_ASSIST_SW"); _parameter_handles.rc_map_mission_sw = param_find("RC_MAP_MISSIO_SW"); - -// _parameter_handles.rc_map_offboard_ctrl_mode_sw = param_find("RC_MAP_OFFB_SW"); + _parameter_handles.rc_map_offboard_sw = param_find("RC_MAP_OFFB_SW"); _parameter_handles.rc_map_aux1 = param_find("RC_MAP_AUX1"); _parameter_handles.rc_map_aux2 = param_find("RC_MAP_AUX2"); @@ -669,14 +666,14 @@ Sensors::parameters_update() warnx("Failed getting mission sw chan index"); } + if (param_get(_parameter_handles.rc_map_offboard_sw, &(_parameters.rc_map_offboard_sw)) != OK) { + warnx("Failed getting offboard sw chan index"); + } + if (param_get(_parameter_handles.rc_map_flaps, &(_parameters.rc_map_flaps)) != OK) { warnx("Failed getting flaps chan index"); } -// if (param_get(_parameter_handles.rc_map_offboard_ctrl_mode_sw, &(_parameters.rc_map_offboard_ctrl_mode_sw)) != OK) { -// warnx("Failed getting offboard control mode sw chan index"); -// } - param_get(_parameter_handles.rc_map_aux1, &(_parameters.rc_map_aux1)); param_get(_parameter_handles.rc_map_aux2, &(_parameters.rc_map_aux2)); param_get(_parameter_handles.rc_map_aux3, &(_parameters.rc_map_aux3)); @@ -700,11 +697,10 @@ Sensors::parameters_update() _rc.function[RETURN] = _parameters.rc_map_return_sw - 1; _rc.function[ASSISTED] = _parameters.rc_map_assisted_sw - 1; _rc.function[MISSION] = _parameters.rc_map_mission_sw - 1; + _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_sw - 1; _rc.function[FLAPS] = _parameters.rc_map_flaps - 1; -// _rc.function[OFFBOARD_MODE] = _parameters.rc_map_offboard_ctrl_mode_sw - 1; - _rc.function[AUX_1] = _parameters.rc_map_aux1 - 1; _rc.function[AUX_2] = _parameters.rc_map_aux2 - 1; _rc.function[AUX_3] = _parameters.rc_map_aux3 - 1; @@ -1287,7 +1283,7 @@ Sensors::rc_poll() manual_control.return_switch = NAN; manual_control.assisted_switch = NAN; manual_control.mission_switch = NAN; -// manual_control.auto_offboard_input_switch = NAN; + manual_control.offboard_switch = NAN; manual_control.flaps = NAN; manual_control.aux1 = NAN; @@ -1430,9 +1426,10 @@ Sensors::rc_poll() manual_control.return_switch = limit_minus_one_to_one(_rc.chan[_rc.function[RETURN]].scaled); } -// if (_rc.function[OFFBOARD_MODE] >= 0) { -// manual_control.auto_offboard_input_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); -// } + /* offboard switch input */ + if (_rc.function[OFFBOARD_MODE] >= 0) { + manual_control.offboard_switch = limit_minus_one_to_one(_rc.chan[_rc.function[OFFBOARD_MODE]].scaled); + } /* aux functions, only assign if valid mapping is present */ if (_rc.function[AUX_1] >= 0) { diff --git a/src/modules/uORB/topics/manual_control_setpoint.h b/src/modules/uORB/topics/manual_control_setpoint.h index eac6d6e98..ab0c7a411 100644 --- a/src/modules/uORB/topics/manual_control_setpoint.h +++ b/src/modules/uORB/topics/manual_control_setpoint.h @@ -60,15 +60,13 @@ struct manual_control_setpoint_s { float return_switch; /**< land 2 position switch (mandatory): land, no effect */ float assisted_switch; /**< assisted 2 position switch (optional): seatbelt, simple */ float mission_switch; /**< mission 2 position switch (optional): mission, loiter */ + float offboard_switch; /**< offboard switch (optional): offboard, onboard */ /** * Any of the channels below may not be available and be set to NaN * to indicate that it does not contain valid data. */ - // XXX needed or parameter? - //float auto_offboard_input_switch; /**< controller setpoint source (0 = onboard, 1 = offboard) */ - float flaps; /**< flap position */ float aux1; /**< default function: camera yaw / azimuth */ diff --git a/src/modules/uORB/topics/vehicle_status.h b/src/modules/uORB/topics/vehicle_status.h index 1b3639e30..4aea1b083 100644 --- a/src/modules/uORB/topics/vehicle_status.h +++ b/src/modules/uORB/topics/vehicle_status.h @@ -66,6 +66,7 @@ typedef enum { MAIN_STATE_SEATBELT, MAIN_STATE_EASY, MAIN_STATE_AUTO, + MAIN_STATE_OFFBOARD, MAIN_STATE_MAX } main_state_t; @@ -116,6 +117,12 @@ typedef enum { MISSION_SWITCH_MISSION } mission_switch_pos_t; +typedef enum { + OFFBOARD_SWITCH_NONE = 0, + OFFBOARD_SWITCH_OFFBOARD, + OFFBOARD_SWITCH_ONBOARD +} offboard_switch_pos_t; + enum VEHICLE_MODE_FLAG { VEHICLE_MODE_FLAG_SAFETY_ARMED = 128, VEHICLE_MODE_FLAG_MANUAL_INPUT_ENABLED = 64, @@ -192,6 +199,7 @@ struct vehicle_status_s return_switch_pos_t return_switch; assisted_switch_pos_t assisted_switch; mission_switch_pos_t mission_switch; + offboard_switch_pos_t offboard_switch; bool condition_battery_voltage_valid; bool condition_system_in_air_restore; /**< true if we can restore in mid air */ |