aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
-rw-r--r--src/modules/commander/commander.cpp144
-rw-r--r--src/modules/commander/px4_custom_mode.h1
-rw-r--r--src/modules/commander/state_machine_helper.cpp12
-rw-r--r--src/modules/mavlink/mavlink.c3
-rw-r--r--src/modules/mavlink/mavlink_receiver.cpp48
-rw-r--r--src/modules/sensors/sensor_params.c3
-rw-r--r--src/modules/sensors/sensors.cpp29
-rw-r--r--src/modules/uORB/topics/manual_control_setpoint.h4
-rw-r--r--src/modules/uORB/topics/vehicle_status.h8
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 */