diff options
Diffstat (limited to 'src/modules/commander/commander.cpp')
| -rw-r--r-- | src/modules/commander/commander.cpp | 69 |
1 files changed, 68 insertions, 1 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index b957ad9b5..fab80e325 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -124,6 +124,7 @@ extern struct system_load_s system_load; #define FAILSAFE_DEFAULT_TIMEOUT (3 * 1000 * 1000) /**< hysteresis time - the failsafe will trigger after 3 seconds in this state */ #define RC_TIMEOUT 500000 #define DL_TIMEOUT 5 * 1000* 1000 +#define OFFBOARD_TIMEOUT 500000 #define DIFFPRESS_TIMEOUT 2000000 #define PRINT_INTERVAL 5000000 @@ -164,6 +165,7 @@ static struct vehicle_status_s status; static struct actuator_armed_s armed; static struct safety_s safety; static struct vehicle_control_mode_s control_mode; +static struct offboard_control_setpoint_s sp_offboard; /* tasks waiting for low prio thread */ typedef enum { @@ -437,6 +439,10 @@ bool handle_command(struct vehicle_status_s *status, const struct safety_s *safe } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_ACRO) { /* ACRO */ main_ret = main_state_transition(status, MAIN_STATE_ACRO); + + } else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) { + /* OFFBOARD */ + main_ret = main_state_transition(status, MAIN_STATE_OFFBOARD); } } else { @@ -632,6 +638,7 @@ int commander_thread_main(int argc, char *argv[]) main_states_str[MAIN_STATE_AUTO_LOITER] = "AUTO_LOITER"; main_states_str[MAIN_STATE_AUTO_RTL] = "AUTO_RTL"; main_states_str[MAIN_STATE_ACRO] = "ACRO"; + main_states_str[MAIN_STATE_OFFBOARD] = "OFFBOARD"; char *arming_states_str[ARMING_STATE_MAX]; arming_states_str[ARMING_STATE_INIT] = "INIT"; @@ -778,7 +785,6 @@ int commander_thread_main(int argc, char *argv[]) /* Subscribe to offboard control data */ int sp_offboard_sub = orb_subscribe(ORB_ID(offboard_control_setpoint)); - struct offboard_control_setpoint_s sp_offboard; memset(&sp_offboard, 0, sizeof(sp_offboard)); /* Subscribe to telemetry status */ @@ -1589,6 +1595,18 @@ set_main_state_rc(struct vehicle_status_s *status, struct manual_control_setpoin /* set main state according to RC switches */ transition_result_t res = TRANSITION_DENIED; + /* offboard switch overrides main switch */ + if (sp_man->offboard_switch == SWITCH_POS_ON) { + res = main_state_transition(status, MAIN_STATE_OFFBOARD); + if (res == TRANSITION_DENIED) { + print_reject_mode(status, "OFFBOARD"); + + } else { + return res; + } + } + + /* offboard switched off or denied, check main mode switch */ switch (sp_man->mode_switch) { case SWITCH_POS_NONE: res = TRANSITION_NOT_CHANGED; @@ -1680,6 +1698,7 @@ set_control_mode() /* TODO: check this */ control_mode.flag_external_manual_override_ok = !status.is_rotary_wing; control_mode.flag_system_hil_enabled = status.hil_state == HIL_STATE_ON; + control_mode.flag_control_offboard_enabled = false; switch (status.nav_state) { case NAVIGATION_STATE_MANUAL: @@ -1718,6 +1737,54 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_OFFBOARD: + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = false; + control_mode.flag_control_offboard_enabled = true; + + switch (sp_offboard.mode) { + case OFFBOARD_CONTROL_MODE_DIRECT_RATES: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + case OFFBOARD_CONTROL_MODE_DIRECT_ATTITUDE: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + break; + case OFFBOARD_CONTROL_MODE_DIRECT_VELOCITY: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = true; + break; + case OFFBOARD_CONTROL_MODE_DIRECT_POSITION: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_altitude_enabled = true; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_position_enabled = true; + control_mode.flag_control_velocity_enabled = true; + break; + default: + control_mode.flag_control_rates_enabled = false; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = false; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + } + break; + case NAVIGATION_STATE_POSCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; |