diff options
-rw-r--r-- | src/modules/commander/commander.cpp | 153 |
1 files changed, 83 insertions, 70 deletions
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp index 7917bc9f2..3307c4bfc 100644 --- a/src/modules/commander/commander.cpp +++ b/src/modules/commander/commander.cpp @@ -2201,18 +2201,6 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; - case NAVIGATION_STATE_ACRO: - control_mode.flag_control_manual_enabled = true; - control_mode.flag_control_auto_enabled = false; - 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; - control_mode.flag_control_termination_enabled = false; - break; - case NAVIGATION_STATE_ALTCTL: control_mode.flag_control_manual_enabled = true; control_mode.flag_control_auto_enabled = false; @@ -2225,64 +2213,6 @@ 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_FORCE: - control_mode.flag_control_rates_enabled = true; - control_mode.flag_control_attitude_enabled = false; - control_mode.flag_control_force_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_LOCAL_NED: - case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED: - case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED: - case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED: - 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; - //XXX: the flags could depend on sp_offboard.ignore - 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; @@ -2323,6 +2253,19 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_ACRO: + control_mode.flag_control_manual_enabled = true; + control_mode.flag_control_auto_enabled = false; + 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; + control_mode.flag_control_termination_enabled = false; + break; + + case NAVIGATION_STATE_LAND: control_mode.flag_control_manual_enabled = false; control_mode.flag_control_auto_enabled = true; @@ -2336,6 +2279,19 @@ set_control_mode() control_mode.flag_control_termination_enabled = false; break; + case NAVIGATION_STATE_DESCEND: + /* TODO: check if this makes sense */ + control_mode.flag_control_manual_enabled = false; + control_mode.flag_control_auto_enabled = true; + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = true; + control_mode.flag_control_position_enabled = false; + control_mode.flag_control_velocity_enabled = false; + control_mode.flag_control_altitude_enabled = false; + control_mode.flag_control_climb_rate_enabled = true; + control_mode.flag_control_termination_enabled = false; + break; + case NAVIGATION_STATE_TERMINATION: /* disable all controllers on termination */ control_mode.flag_control_manual_enabled = false; @@ -2349,6 +2305,63 @@ set_control_mode() control_mode.flag_control_termination_enabled = true; 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_FORCE: + control_mode.flag_control_rates_enabled = true; + control_mode.flag_control_attitude_enabled = false; + control_mode.flag_control_force_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_LOCAL_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_LOCAL_OFFSET_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_BODY_NED: + case OFFBOARD_CONTROL_MODE_DIRECT_BODY_OFFSET_NED: + 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; + //XXX: the flags could depend on sp_offboard.ignore + 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; + default: break; } |