zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

commander: use control mode flags and cleanup arm_disarm 

- keep `vehicle_control_mode` last state in commander and use appropriate flags in place of various main_state and nav_state checks
 - consolidate scattered arming requirements in `arm_disarm()`
   - there were a number of different requirements from arming via RC vs Mavlink that don't make any sense
        - if geofence enabled require valid home before arming
        - throttle requirements for manual modes
 - remove unnecessary mavlink feedback that differs between arming interfaces (mavlink vs RC)
      - let the preflight/prearm checks respond directly in most cases 

Co-authored-by: Matthias Grob <maetugr@gmail.com>
release/1.12
Daniel Agar 4 years ago committed by GitHub
parent
37f78537c3
commit
0618f048f2
No known key found for this signature in database
GPG Key ID: 4AEE18F83AFDEB23
2 changed files with 308 additions and 324 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 617
      src/modules/commander/Commander.cpp
  2. 15
      src/modules/commander/Commander.hpp

617
src/modules/commander/Commander.cpp
Unescape View File

@ -382,57 +382,113 @@ bool Commander::shutdown_if_allowed() @@ -382,57 +382,113 @@ bool Commander::shutdown_if_allowed()
hrt_elapsed_time(&_boot_timestamp), arm_disarm_reason_t::SHUTDOWN);
}
transition_result_t
Commander::arm_disarm(bool arm, bool run_preflight_checks, arm_disarm_reason_t calling_reason)
static constexpr const char *arm_disarm_reason_str(arm_disarm_reason_t calling_reason)
{
transition_result_t arming_res = TRANSITION_NOT_CHANGED;
// Transition the armed state. By passing _mavlink_log_pub to arming_state_transition it will
// output appropriate error messages if the state cannot transition.
arming_res = arming_state_transition(&_status,
_safety,
arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY,
&_armed,
run_preflight_checks,
&_mavlink_log_pub,
&_status_flags,
_arm_requirements,
hrt_elapsed_time(&_boot_timestamp), calling_reason);
switch (calling_reason) {
case arm_disarm_reason_t::TRANSITION_TO_STANDBY: return "";
if (arming_res == TRANSITION_CHANGED) {
const char *reason = "";
case arm_disarm_reason_t::RC_STICK: return "RC";
case arm_disarm_reason_t::RC_SWITCH: return "RC (switch)";
case arm_disarm_reason_t::COMMAND_INTERNAL: return "internal command";
switch (calling_reason) {
case arm_disarm_reason_t::TRANSITION_TO_STANDBY: reason = ""; break;
case arm_disarm_reason_t::COMMAND_EXTERNAL: return "external command";
case arm_disarm_reason_t::RC_STICK: reason = "RC"; break;
case arm_disarm_reason_t::MISSION_START: return "mission start";
case arm_disarm_reason_t::RC_SWITCH: reason = "RC (switch)"; break;
case arm_disarm_reason_t::SAFETY_BUTTON: return "safety button";
case arm_disarm_reason_t::COMMAND_INTERNAL: reason = "internal command"; break;
case arm_disarm_reason_t::AUTO_DISARM_LAND: return "landing";
case arm_disarm_reason_t::COMMAND_EXTERNAL: reason = "external command"; break;
case arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT: return "auto preflight disarming";
case arm_disarm_reason_t::MISSION_START: reason = "mission start"; break;
case arm_disarm_reason_t::KILL_SWITCH: return "kill-switch";
case arm_disarm_reason_t::SAFETY_BUTTON: reason = "safety button"; break;
case arm_disarm_reason_t::LOCKDOWN: return "lockdown";
case arm_disarm_reason_t::AUTO_DISARM_LAND: reason = "landing"; break;
case arm_disarm_reason_t::FAILURE_DETECTOR: return "failure detector";
case arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT: reason = "auto preflight disarming"; break;
case arm_disarm_reason_t::SHUTDOWN: return "shutdown request";
case arm_disarm_reason_t::KILL_SWITCH: reason = "kill-switch"; break;
case arm_disarm_reason_t::UNIT_TEST: return "unit tests";
}
return "";
};
transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks)
{
// allow a grace period for re-arming: preflight checks don't need to pass during that time, for example for accidential in-air disarming
if (_param_com_rearm_grace.get() && (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s)) {
run_preflight_checks = false;
}
case arm_disarm_reason_t::LOCKDOWN: reason = "lockdown"; break;
if (run_preflight_checks) {
if (_vehicle_control_mode.flag_control_manual_enabled) {
const bool throttle_above_low = (_manual_control_setpoint.z > 0.1f);
const bool throttle_above_center = (_manual_control_setpoint.z > 0.6f);
case arm_disarm_reason_t::FAILURE_DETECTOR: reason = "failure detector"; break;
if (_vehicle_control_mode.flag_control_climb_rate_enabled && throttle_above_center) {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Throttle not centered");
tune_negative(true);
return TRANSITION_DENIED;
case arm_disarm_reason_t::SHUTDOWN: reason = "shutdown request"; break;
}
case arm_disarm_reason_t::UNIT_TEST: reason = "unit tests"; break;
if (!_vehicle_control_mode.flag_control_climb_rate_enabled && throttle_above_low) {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Throttle not zero");
tune_negative(true);
return TRANSITION_DENIED;
}
}
mavlink_log_info(&_mavlink_log_pub, "%s by %s", arm ? "Armed" : "Disarmed", reason);
if ((_param_geofence_action.get() == geofence_result_s::GF_ACTION_RTL)
&& !_status_flags.condition_home_position_valid) {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Geofence RTL requires valid home");
tune_negative(true);
return TRANSITION_DENIED;
}
}
transition_result_t arming_res = arming_state_transition(&_status,
_safety,
vehicle_status_s::ARMING_STATE_ARMED,
&_armed,
run_preflight_checks,
&_mavlink_log_pub,
&_status_flags,
_arm_requirements,
hrt_elapsed_time(&_boot_timestamp), calling_reason);
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(&_mavlink_log_pub, "Armed by %s", arm_disarm_reason_str(calling_reason));
_status_changed = true;
} else if (arming_res == TRANSITION_DENIED) {
tune_negative(true);
}
return arming_res;
}
transition_result_t Commander::disarm(arm_disarm_reason_t calling_reason)
{
transition_result_t arming_res = arming_state_transition(&_status,
_safety,
vehicle_status_s::ARMING_STATE_STANDBY,
&_armed,
false,
&_mavlink_log_pub,
&_status_flags,
_arm_requirements,
hrt_elapsed_time(&_boot_timestamp), calling_reason);
if (arming_res == TRANSITION_CHANGED) {
mavlink_log_info(&_mavlink_log_pub, "Disarmed by %s", arm_disarm_reason_str(calling_reason));
_status_changed = true;
} else if (arming_res == TRANSITION_DENIED) {
tune_negative(true);
@ -651,17 +707,18 @@ Commander::handle_command(const vehicle_command_s &cmd) @@ -651,17 +707,18 @@ Commander::handle_command(const vehicle_command_s &cmd)
// Adhere to MAVLink specs, but base on knowledge that these fundamentally encode ints
// for logic state parameters
if (static_cast<int>(cmd.param1 + 0.5f) != 0 && static_cast<int>(cmd.param1 + 0.5f) != 1) {
const int param1_arm = static_cast<int>(roundf(cmd.param1));
if (param1_arm != 0 && param1_arm != 1) {
mavlink_log_critical(&_mavlink_log_pub, "Unsupported ARM_DISARM param: %.3f", (double)cmd.param1);
} else {
const bool cmd_arms = (param1_arm == 1);
bool cmd_arms = (static_cast<int>(cmd.param1 + 0.5f) == 1);
// Arm is forced (checks skipped) when param2 is set to a magic number.
const bool forced = (static_cast<int>(roundf(cmd.param2)) == 21196);
// Arm/disarm is enforced when param2 is set to a magic number.
const bool enforce = (static_cast<int>(roundf(cmd.param2)) == 21196);
if (!enforce) {
if (!forced) {
if (!(_land_detector.landed || _land_detector.maybe_landed) && !is_ground_rover(&_status)) {
if (cmd_arms) {
if (_armed.armed) {
@ -685,42 +742,28 @@ Commander::handle_command(const vehicle_command_s &cmd) @@ -685,42 +742,28 @@ Commander::handle_command(const vehicle_command_s &cmd)
if (cmd.source_system == _status.system_id && cmd.source_component == _status.component_id
&& cmd_from_io && cmd_arms) {
_status.arming_state = vehicle_status_s::ARMING_STATE_IN_AIR_RESTORE;
}
}
} else {
// Refuse to arm if preflight checks have failed
if (_status.hil_state != vehicle_status_s::HIL_STATE_ON
&& !_status_flags.condition_system_sensors_initialized) {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Preflight checks have failed");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break;
}
transition_result_t arming_res = TRANSITION_DENIED;
const bool throttle_above_low = (_manual_control_setpoint.z > 0.1f);
const bool throttle_above_center = (_manual_control_setpoint.z > 0.6f);
if (cmd_arms) {
if (cmd.from_external) {
arming_res = arm(arm_disarm_reason_t::COMMAND_EXTERNAL);
if (cmd_arms && throttle_above_center &&
(_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL ||
_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL)) {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Throttle not centered");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break;
}
} else {
arming_res = arm(arm_disarm_reason_t::COMMAND_INTERNAL, !forced);
}
if (cmd_arms && throttle_above_low &&
(_status.nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL ||
_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO ||
_status.nav_state == vehicle_status_s::NAVIGATION_STATE_STAB ||
_status.nav_state == vehicle_status_s::NAVIGATION_STATE_RATTITUDE)) {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Throttle not zero");
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_DENIED;
break;
}
} else {
if (cmd.from_external) {
arming_res = disarm(arm_disarm_reason_t::COMMAND_EXTERNAL);
} else {
arming_res = disarm(arm_disarm_reason_t::COMMAND_INTERNAL);
}
}
transition_result_t arming_res = arm_disarm(cmd_arms, !enforce,
(cmd.from_external ? arm_disarm_reason_t::COMMAND_EXTERNAL : arm_disarm_reason_t::COMMAND_INTERNAL));
if (arming_res == TRANSITION_DENIED) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED;
@ -931,7 +974,7 @@ Commander::handle_command(const vehicle_command_s &cmd) @@ -931,7 +974,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
// switch to AUTO_MISSION and ARM
if ((TRANSITION_DENIED != main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_MISSION, _status_flags,
&_internal_state))
&& (TRANSITION_DENIED != arm_disarm(true, true, arm_disarm_reason_t::MISSION_START))) {
&& (TRANSITION_DENIED != arm(arm_disarm_reason_t::MISSION_START))) {
cmd_result = vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED;
@ -1528,10 +1571,8 @@ Commander::run() @@ -1528,10 +1571,8 @@ Commander::run()
while (!should_exit()) {
transition_result_t arming_ret = TRANSITION_NOT_CHANGED;
/* update parameters */
bool params_updated = _parameter_update_sub.updated();
const bool params_updated = _parameter_update_sub.updated();
if (params_updated || param_init_forced) {
// clear update
@ -1754,9 +1795,7 @@ Commander::run() @@ -1754,9 +1795,7 @@ Commander::run()
}
if (safety_disarm_allowed) {
if (TRANSITION_CHANGED == arm_disarm(false, true, arm_disarm_reason_t::SAFETY_BUTTON)) {
_status_changed = true;
}
disarm(arm_disarm_reason_t::SAFETY_BUTTON);
}
}
@ -1848,8 +1887,12 @@ Commander::run() @@ -1848,8 +1887,12 @@ Commander::run()
}
if (_auto_disarm_landed.get_state()) {
arm_disarm(false, true,
(_have_taken_off_since_arming ? arm_disarm_reason_t::AUTO_DISARM_LAND : arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT));
if (_have_taken_off_since_arming) {
disarm(arm_disarm_reason_t::AUTO_DISARM_LAND);
} else {
disarm(arm_disarm_reason_t::AUTO_DISARM_PREFLIGHT);
}
}
}
@ -1866,12 +1909,11 @@ Commander::run() @@ -1866,12 +1909,11 @@ Commander::run()
if (_auto_disarm_killed.get_state()) {
if (_armed.manual_lockdown) {
arm_disarm(false, true, arm_disarm_reason_t::KILL_SWITCH);
disarm(arm_disarm_reason_t::KILL_SWITCH);
} else {
arm_disarm(false, true, arm_disarm_reason_t::LOCKDOWN);
disarm(arm_disarm_reason_t::LOCKDOWN);
}
}
} else {
@ -1895,15 +1937,10 @@ Commander::run() @@ -1895,15 +1937,10 @@ Commander::run()
/* If in INIT state, try to proceed to STANDBY state */
if (!_status_flags.condition_calibration_enabled && _status.arming_state == vehicle_status_s::ARMING_STATE_INIT) {
arming_ret = arming_state_transition(&_status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &_armed,
true /* fRunPreArmChecks */, &_mavlink_log_pub, &_status_flags,
_arm_requirements, hrt_elapsed_time(&_boot_timestamp),
arm_disarm_reason_t::TRANSITION_TO_STANDBY);
if (arming_ret == TRANSITION_DENIED) {
/* do not complain if not allowed into standby */
arming_ret = TRANSITION_NOT_CHANGED;
}
arming_state_transition(&_status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &_armed,
true /* fRunPreArmChecks */, &_mavlink_log_pub, &_status_flags,
_arm_requirements, hrt_elapsed_time(&_boot_timestamp),
arm_disarm_reason_t::TRANSITION_TO_STANDBY);
}
/* start mission result check */
@ -2053,35 +2090,29 @@ Commander::run() @@ -2053,35 +2090,29 @@ Commander::run()
_geofence_violated_prev = false;
}
// abort auto mode or geofence reaction if sticks are moved significantly
// but only if not in a low battery handling action
const bool is_rotary_wing = _status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING;
const bool override_auto_mode =
(_param_rc_override.get() & OVERRIDE_AUTO_MODE_BIT) &&
(_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF ||
_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND ||
_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL ||
_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION ||
_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER ||
_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_FOLLOW_TARGET ||
_internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_PRECLAND);
const bool override_offboard_mode =
(_param_rc_override.get() & OVERRIDE_OFFBOARD_MODE_BIT) &&
_internal_state.main_state == commander_state_s::MAIN_STATE_OFFBOARD;
if ((override_auto_mode || override_offboard_mode) && is_rotary_wing
&& !in_low_battery_failsafe && !_geofence_warning_action_on) {
const float minimum_stick_deflection = 0.01f * _param_com_rc_stick_ov.get();
// transition to previous state if sticks are touched
if (!_status.rc_signal_lost &&
((fabsf(_manual_control_setpoint.x) > minimum_stick_deflection) ||
(fabsf(_manual_control_setpoint.y) > minimum_stick_deflection))) {
// revert to position control in any case
main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, &_internal_state);
mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks");
// abort autonomous mode and switch to position mode if sticks are moved significantly
if ((_param_rc_override.get() != 0) && (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)) {
const bool override_auto_mode = (_param_rc_override.get() & OVERRIDE_AUTO_MODE_BIT)
&& _vehicle_control_mode.flag_control_auto_enabled;
const bool override_offboard_mode = (_param_rc_override.get() & OVERRIDE_OFFBOARD_MODE_BIT)
&& _vehicle_control_mode.flag_control_offboard_enabled;
if ((override_auto_mode || override_offboard_mode) && !in_low_battery_failsafe && !_geofence_warning_action_on) {
const float minimum_stick_deflection = 0.01f * _param_com_rc_stick_ov.get();
if (!_status.rc_signal_lost &&
((fabsf(_manual_control_setpoint.x) > minimum_stick_deflection) ||
(fabsf(_manual_control_setpoint.y) > minimum_stick_deflection))) {
if (main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags,
&_internal_state) == TRANSITION_CHANGED) {
tune_positive(true);
mavlink_log_info(&_mavlink_log_pub, "Pilot took over control using sticks");
_status_changed = true;
}
}
}
}
@ -2149,18 +2180,19 @@ Commander::run() @@ -2149,18 +2180,19 @@ Commander::run()
(_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING || _land_detector.landed) &&
(stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition)) {
const bool manual_thrust_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_MANUAL
|| _internal_state.main_state == commander_state_s::MAIN_STATE_ACRO
|| _internal_state.main_state == commander_state_s::MAIN_STATE_STAB
|| _internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE;
const bool manual_thrust_mode = _vehicle_control_mode.flag_control_manual_enabled
&& !_vehicle_control_mode.flag_control_climb_rate_enabled;
const bool rc_wants_disarm = (_stick_off_counter == rc_arm_hyst && _stick_on_counter < rc_arm_hyst)
|| arm_switch_to_disarm_transition;
if (rc_wants_disarm && (_land_detector.landed || manual_thrust_mode)) {
arming_ret = arming_state_transition(&_status, _safety, vehicle_status_s::ARMING_STATE_STANDBY, &_armed,
true /* fRunPreArmChecks */,
&_mavlink_log_pub, &_status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp),
(arm_switch_to_disarm_transition ? arm_disarm_reason_t::RC_SWITCH : arm_disarm_reason_t::RC_STICK));
if (arm_switch_to_disarm_transition) {
disarm(arm_disarm_reason_t::RC_SWITCH);
} else {
disarm(arm_disarm_reason_t::RC_STICK);
}
}
_stick_off_counter++;
@ -2197,29 +2229,16 @@ Commander::run() @@ -2197,29 +2229,16 @@ Commander::run()
* for being in manual mode only applies to manual arming actions.
* the system can be armed in auto if armed via the GCS.
*/
if ((_internal_state.main_state != commander_state_s::MAIN_STATE_MANUAL)
&& (_internal_state.main_state != commander_state_s::MAIN_STATE_ACRO)
&& (_internal_state.main_state != commander_state_s::MAIN_STATE_STAB)
&& (_internal_state.main_state != commander_state_s::MAIN_STATE_ALTCTL)
&& (_internal_state.main_state != commander_state_s::MAIN_STATE_POSCTL)
&& (_internal_state.main_state != commander_state_s::MAIN_STATE_RATTITUDE)
) {
print_reject_arm("Not arming: Switch to a manual mode first");
} else if (!_status_flags.condition_home_position_valid &&
(_param_geofence_action.get() == geofence_result_s::GF_ACTION_RTL)) {
print_reject_arm("Not arming: Geofence RTL requires valid home");
} else if (_status.arming_state == vehicle_status_s::ARMING_STATE_STANDBY) {
arming_ret = arming_state_transition(&_status, _safety, vehicle_status_s::ARMING_STATE_ARMED, &_armed,
!in_rearming_grace_period /* fRunPreArmChecks */,
&_mavlink_log_pub, &_status_flags, _arm_requirements, hrt_elapsed_time(&_boot_timestamp),
(arm_switch_to_arm_transition ? arm_disarm_reason_t::RC_SWITCH : arm_disarm_reason_t::RC_STICK));
if (arming_ret != TRANSITION_CHANGED) {
px4_usleep(100000);
print_reject_arm("Not arming: Preflight checks failed");
if (!_vehicle_control_mode.flag_control_manual_enabled) {
mavlink_log_critical(&_mavlink_log_pub, "Arming denied! Switch to a manual mode first");
tune_negative(true);
} else {
if (arm_switch_to_arm_transition) {
arm(arm_disarm_reason_t::RC_SWITCH);
} else {
arm(arm_disarm_reason_t::RC_STICK);
}
}
}
@ -2234,55 +2253,35 @@ Commander::run() @@ -2234,55 +2253,35 @@ Commander::run()
_last_manual_control_switches_arm_switch = _manual_control_switches.arm_switch;
if (arming_ret == TRANSITION_DENIED) {
/*
* the arming transition can be denied to a number of reasons:
* - pre-flight check failed (sensors not ok or not calibrated)
* - safety not disabled
* - system not in manual mode
*/
tune_negative(true);
}
if (_manual_control_switches_sub.update(&_manual_control_switches) || safety_updated) {
// handle landing gear switch if configured and in a manual mode
if ((_manual_control_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) &&
if ((_vehicle_control_mode.flag_control_manual_enabled) &&
(_manual_control_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) &&
(_last_manual_control_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) &&
(_manual_control_switches.gear_switch != _last_manual_control_switches.gear_switch)) {
// TODO: replace with vehicle_control_mode manual
if (_status.nav_state == vehicle_status_s::NAVIGATION_STATE_MANUAL ||
_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ACRO ||
_status.nav_state == vehicle_status_s::NAVIGATION_STATE_RATTITUDE ||
_status.nav_state == vehicle_status_s::NAVIGATION_STATE_STAB ||
_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ALTCTL ||
_status.nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL ||
_status.nav_state == vehicle_status_s::NAVIGATION_STATE_OFFBOARD ||
_status.nav_state == vehicle_status_s::NAVIGATION_STATE_ORBIT) {
// Only switch the landing gear up if the user switched from gear down to gear up.
int8_t gear = landing_gear_s::GEAR_KEEP;
if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_OFF) {
gear = landing_gear_s::GEAR_DOWN;
} else if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) {
// gear up ignored unless flying
if (!_land_detector.landed && !_land_detector.maybe_landed) {
gear = landing_gear_s::GEAR_UP;
// Only switch the landing gear up if the user switched from gear down to gear up.
int8_t gear = landing_gear_s::GEAR_KEEP;
} else {
mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear")
}
}
if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_OFF) {
gear = landing_gear_s::GEAR_DOWN;
} else if (_manual_control_switches.gear_switch == manual_control_switches_s::SWITCH_POS_ON) {
// gear up ignored unless flying
if (!_land_detector.landed && !_land_detector.maybe_landed) {
gear = landing_gear_s::GEAR_UP;
if (gear != landing_gear_s::GEAR_KEEP) {
landing_gear_s landing_gear{};
landing_gear.landing_gear = gear;
landing_gear.timestamp = hrt_absolute_time();
_landing_gear_pub.publish(landing_gear);
} else {
mavlink_log_critical(&_mavlink_log_pub, "Landed, unable to retract landing gear")
}
}
if (gear != landing_gear_s::GEAR_KEEP) {
landing_gear_s landing_gear{};
landing_gear.landing_gear = gear;
landing_gear.timestamp = hrt_absolute_time();
_landing_gear_pub.publish(landing_gear);
}
}
// evaluate the main state machine according to mode switches
@ -2440,7 +2439,7 @@ Commander::run() @@ -2440,7 +2439,7 @@ Commander::run()
if (_status.failure_detector_status & vehicle_status_s::FAILURE_ARM_ESC) {
// 500ms is the PWM spoolup time. Within this timeframe controllers are not affecting actuator_outputs
if (hrt_elapsed_time(&_status.armed_time) < 500_ms) {
arm_disarm(false, true, arm_disarm_reason_t::FAILURE_DETECTOR);
disarm(arm_disarm_reason_t::FAILURE_DETECTOR);
mavlink_log_critical(&_mavlink_log_pub, "ESCs did not respond to arm request");
}
}
@ -2530,8 +2529,6 @@ Commander::run() @@ -2530,8 +2529,6 @@ Commander::run()
_have_taken_off_since_arming = false;
}
_was_armed = _armed.armed;
/* now set navigation state according to failsafe and main state */
bool nav_state_changed = set_nav_state(&_status,
&_armed,
@ -2611,9 +2608,6 @@ Commander::run() @@ -2611,9 +2608,6 @@ Commander::run()
_internal_state.timestamp = hrt_absolute_time();
_commander_state_pub.publish(_internal_state);
/* publish vehicle_status_flags */
_status_flags.timestamp = hrt_absolute_time();
// Evaluate current prearm status
if (!_armed.armed && !_status_flags.condition_calibration_enabled) {
bool preflight_check_res = PreFlightCheck::preflightCheck(nullptr, _status, _status_flags, false, true, 30_s);
@ -2627,6 +2621,8 @@ Commander::run() @@ -2627,6 +2621,8 @@ Commander::run()
&& prearm_check_res), _status);
}
/* publish vehicle_status_flags */
_status_flags.timestamp = hrt_absolute_time();
_vehicle_status_flags_pub.publish(_status_flags);
}
@ -2718,6 +2714,8 @@ Commander::run() @@ -2718,6 +2714,8 @@ Commander::run()
_last_condition_local_position_valid = _status_flags.condition_local_position_valid;
_last_condition_global_position_valid = _status_flags.condition_global_position_valid;
_was_armed = _armed.armed;
arm_auth_update(now, params_updated || param_init_forced);
px4_indicate_external_reset_lockout(LockoutComponent::Commander, _armed.armed);
@ -3360,56 +3358,55 @@ Commander::check_posvel_validity(const bool data_valid, const float data_accurac @@ -3360,56 +3358,55 @@ Commander::check_posvel_validity(const bool data_valid, const float data_accurac
void
Commander::update_control_mode()
{
vehicle_control_mode_s control_mode{};
control_mode.timestamp = hrt_absolute_time();
_vehicle_control_mode = {};
/* set vehicle_control_mode according to set_navigation_state */
control_mode.flag_armed = _armed.armed;
control_mode.flag_external_manual_override_ok = (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING
&& !_status.is_vtol);
_vehicle_control_mode.flag_armed = _armed.armed;
_vehicle_control_mode.flag_external_manual_override_ok =
(_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_FIXED_WING && !_status.is_vtol);
switch (_status.nav_state) {
case vehicle_status_s::NAVIGATION_STATE_MANUAL:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_rates_enabled = stabilization_required();
control_mode.flag_control_attitude_enabled = stabilization_required();
_vehicle_control_mode.flag_control_manual_enabled = true;
_vehicle_control_mode.flag_control_rates_enabled = stabilization_required();
_vehicle_control_mode.flag_control_attitude_enabled = stabilization_required();
break;
case vehicle_status_s::NAVIGATION_STATE_STAB:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_manual_enabled = true;
_vehicle_control_mode.flag_control_rates_enabled = true;
_vehicle_control_mode.flag_control_attitude_enabled = true;
break;
case vehicle_status_s::NAVIGATION_STATE_RATTITUDE:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_rattitude_enabled = true;
_vehicle_control_mode.flag_control_manual_enabled = true;
_vehicle_control_mode.flag_control_rates_enabled = true;
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_rattitude_enabled = true;
break;
case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
control_mode.flag_control_manual_enabled = true;
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;
_vehicle_control_mode.flag_control_manual_enabled = true;
_vehicle_control_mode.flag_control_rates_enabled = true;
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_altitude_enabled = true;
_vehicle_control_mode.flag_control_climb_rate_enabled = true;
break;
case vehicle_status_s::NAVIGATION_STATE_POSCTL:
control_mode.flag_control_manual_enabled = true;
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 = !_status.in_transition_mode;
control_mode.flag_control_velocity_enabled = !_status.in_transition_mode;
_vehicle_control_mode.flag_control_manual_enabled = true;
_vehicle_control_mode.flag_control_rates_enabled = true;
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_altitude_enabled = true;
_vehicle_control_mode.flag_control_climb_rate_enabled = true;
_vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode;
_vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_RTL:
/* override is not ok for the RTL and recovery mode */
control_mode.flag_external_manual_override_ok = false;
_vehicle_control_mode.flag_external_manual_override_ok = false;
/* fallthrough */
case vehicle_status_s::NAVIGATION_STATE_AUTO_FOLLOW_TARGET:
@ -3419,109 +3416,109 @@ Commander::update_control_mode() @@ -3419,109 +3416,109 @@ Commander::update_control_mode()
case vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION:
case vehicle_status_s::NAVIGATION_STATE_AUTO_LOITER:
case vehicle_status_s::NAVIGATION_STATE_AUTO_TAKEOFF:
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_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = !_status.in_transition_mode;
control_mode.flag_control_velocity_enabled = !_status.in_transition_mode;
_vehicle_control_mode.flag_control_auto_enabled = true;
_vehicle_control_mode.flag_control_rates_enabled = true;
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_altitude_enabled = true;
_vehicle_control_mode.flag_control_climb_rate_enabled = true;
_vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode;
_vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode;
break;
case vehicle_status_s::NAVIGATION_STATE_AUTO_LANDGPSFAIL:
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
_vehicle_control_mode.flag_control_rates_enabled = true;
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_climb_rate_enabled = true;
break;
case vehicle_status_s::NAVIGATION_STATE_ACRO:
control_mode.flag_control_manual_enabled = true;
control_mode.flag_control_rates_enabled = true;
_vehicle_control_mode.flag_control_manual_enabled = true;
_vehicle_control_mode.flag_control_rates_enabled = true;
break;
case vehicle_status_s::NAVIGATION_STATE_DESCEND:
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
_vehicle_control_mode.flag_control_auto_enabled = false;
_vehicle_control_mode.flag_control_rates_enabled = true;
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_climb_rate_enabled = true;
break;
case vehicle_status_s::NAVIGATION_STATE_TERMINATION:
/* disable all controllers on termination */
control_mode.flag_control_termination_enabled = true;
_vehicle_control_mode.flag_control_termination_enabled = true;
break;
case vehicle_status_s::NAVIGATION_STATE_OFFBOARD: {
const offboard_control_mode_s &offboard_control_mode = _offboard_control_mode_sub.get();
control_mode.flag_control_offboard_enabled = true;
/*
* The control flags depend on what is ignored according to the offboard control mode topic
* Inner loop flags (e.g. attitude) also depend on outer loop ignore flags (e.g. position)
*/
control_mode.flag_control_rates_enabled =
!offboard_control_mode.ignore_bodyrate_x ||
!offboard_control_mode.ignore_bodyrate_y ||
!offboard_control_mode.ignore_bodyrate_z ||
!offboard_control_mode.ignore_attitude ||
!offboard_control_mode.ignore_position ||
!offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_acceleration_force;
control_mode.flag_control_attitude_enabled = !offboard_control_mode.ignore_attitude ||
!offboard_control_mode.ignore_position ||
!offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_acceleration_force;
_vehicle_control_mode.flag_control_offboard_enabled = true;
const offboard_control_mode_s &offboard = _offboard_control_mode_sub.get();
if (!offboard.ignore_acceleration_force) {
// OFFBOARD acceleration
_vehicle_control_mode.flag_control_acceleration_enabled = true;
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_rates_enabled = true;
} else if (!offboard.ignore_position) {
// OFFBOARD position
_vehicle_control_mode.flag_control_position_enabled = true;
_vehicle_control_mode.flag_control_velocity_enabled = true;
_vehicle_control_mode.flag_control_altitude_enabled = true;
_vehicle_control_mode.flag_control_climb_rate_enabled = true;
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_rates_enabled = true;
} else if (!offboard.ignore_velocity) {
// OFFBOARD velocity
_vehicle_control_mode.flag_control_velocity_enabled = true;
_vehicle_control_mode.flag_control_altitude_enabled = true;
_vehicle_control_mode.flag_control_climb_rate_enabled = true;
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_rates_enabled = true;
} else if (!offboard.ignore_attitude) {
// OFFBOARD attitude
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_rates_enabled = true;
} else if (!offboard.ignore_bodyrate_x || !offboard.ignore_bodyrate_y || !offboard.ignore_bodyrate_z) {
// OFFBOARD rate
_vehicle_control_mode.flag_control_rates_enabled = true;
}
// TO-DO: Add support for other modes than yawrate control
control_mode.flag_control_yawrate_override_enabled =
offboard_control_mode.ignore_bodyrate_x &&
offboard_control_mode.ignore_bodyrate_y &&
!offboard_control_mode.ignore_bodyrate_z &&
!offboard_control_mode.ignore_attitude;
control_mode.flag_control_rattitude_enabled = false;
control_mode.flag_control_acceleration_enabled = !offboard_control_mode.ignore_acceleration_force &&
!_status.in_transition_mode;
control_mode.flag_control_velocity_enabled = (!offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_position) && !_status.in_transition_mode &&
!control_mode.flag_control_acceleration_enabled;
control_mode.flag_control_climb_rate_enabled = (!offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_position);
control_mode.flag_control_position_enabled = !offboard_control_mode.ignore_position && !_status.in_transition_mode &&
!control_mode.flag_control_acceleration_enabled;
control_mode.flag_control_altitude_enabled = (!offboard_control_mode.ignore_velocity ||
!offboard_control_mode.ignore_position) && !control_mode.flag_control_acceleration_enabled;
_vehicle_control_mode.flag_control_yawrate_override_enabled = offboard.ignore_bodyrate_x && offboard.ignore_bodyrate_y
&& !offboard.ignore_bodyrate_z && !offboard.ignore_attitude;
// VTOL transition override
if (_status.in_transition_mode) {
_vehicle_control_mode.flag_control_acceleration_enabled = false;
_vehicle_control_mode.flag_control_velocity_enabled = false;
_vehicle_control_mode.flag_control_position_enabled = false;
}
}
break;
case vehicle_status_s::NAVIGATION_STATE_ORBIT:
control_mode.flag_control_manual_enabled = false;
control_mode.flag_control_auto_enabled = false;
control_mode.flag_control_rates_enabled = true;
control_mode.flag_control_attitude_enabled = true;
control_mode.flag_control_rattitude_enabled = false;
control_mode.flag_control_altitude_enabled = true;
control_mode.flag_control_climb_rate_enabled = true;
control_mode.flag_control_position_enabled = !_status.in_transition_mode;
control_mode.flag_control_velocity_enabled = !_status.in_transition_mode;
control_mode.flag_control_acceleration_enabled = false;
control_mode.flag_control_termination_enabled = false;
_vehicle_control_mode.flag_control_manual_enabled = false;
_vehicle_control_mode.flag_control_auto_enabled = false;
_vehicle_control_mode.flag_control_rates_enabled = true;
_vehicle_control_mode.flag_control_attitude_enabled = true;
_vehicle_control_mode.flag_control_rattitude_enabled = false;
_vehicle_control_mode.flag_control_altitude_enabled = true;
_vehicle_control_mode.flag_control_climb_rate_enabled = true;
_vehicle_control_mode.flag_control_position_enabled = !_status.in_transition_mode;
_vehicle_control_mode.flag_control_velocity_enabled = !_status.in_transition_mode;
_vehicle_control_mode.flag_control_acceleration_enabled = false;
_vehicle_control_mode.flag_control_termination_enabled = false;
break;
default:
break;
}
_control_mode_pub.publish(control_mode);
_vehicle_control_mode.timestamp = hrt_absolute_time();
_control_mode_pub.publish(_vehicle_control_mode);
}
bool
@ -3549,18 +3546,6 @@ Commander::print_reject_mode(const char *msg) @@ -3549,18 +3546,6 @@ Commander::print_reject_mode(const char *msg)
}
}
void
Commander::print_reject_arm(const char *msg)
{
const hrt_abstime t = hrt_absolute_time();
if (t - _last_print_mode_reject_time > PRINT_MODE_REJECT_INTERVAL) {
_last_print_mode_reject_time = t;
mavlink_log_critical(&_mavlink_log_pub, "%s", msg);
tune_negative(true);
}
}
void Commander::answer_command(const vehicle_command_s &cmd, uint8_t result)
{
switch (result) {
@ -3815,13 +3800,9 @@ void Commander::avoidance_check() @@ -3815,13 +3800,9 @@ void Commander::avoidance_check()
const bool sensor_oa_present = cp_healthy || _status_flags.avoidance_system_required || cp_enabled;
const bool auto_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_MISSION
|| _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER
|| _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL
|| _internal_state.main_state == commander_state_s::MAIN_STATE_OFFBOARD
|| _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_TAKEOFF
|| _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LAND;
const bool pos_ctl_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_POSCTL;
const bool auto_mode = _vehicle_control_mode.flag_control_auto_enabled;
const bool pos_ctl_mode = (_vehicle_control_mode.flag_control_manual_enabled
&& _vehicle_control_mode.flag_control_position_enabled);
const bool sensor_oa_enabled = ((auto_mode && _status_flags.avoidance_system_required) || (pos_ctl_mode && cp_enabled));
const bool sensor_oa_healthy = ((auto_mode && _status_flags.avoidance_system_valid) || (pos_ctl_mode && cp_healthy));

15
src/modules/commander/Commander.hpp
Unescape View File

@ -124,7 +124,8 @@ public: @@ -124,7 +124,8 @@ public:
private:
void answer_command(const vehicle_command_s &cmd, uint8_t result);
transition_result_t arm_disarm(bool arm, bool run_preflight_checks, arm_disarm_reason_t calling_reason);
transition_result_t arm(arm_disarm_reason_t calling_reason, bool run_preflight_checks = true);
transition_result_t disarm(arm_disarm_reason_t calling_reason);
void battery_status_check();
@ -153,7 +154,6 @@ private: @@ -153,7 +154,6 @@ private:
void offboard_control_update();
void print_reject_arm(const char *msg);
void print_reject_mode(const char *msg);
void reset_posvel_validity();
@ -391,16 +391,19 @@ private: @@ -391,16 +391,19 @@ private:
main_state_t _main_state_pre_offboard{commander_state_s::MAIN_STATE_MANUAL};
actuator_armed_s _armed{};
commander_state_s _internal_state{};
cpuload_s _cpuload{};
geofence_result_s _geofence_result{};
vehicle_land_detected_s _land_detector{};
safety_s _safety{};
vehicle_status_s _status{};
vehicle_status_flags_s _status_flags{};
vtol_vehicle_status_s _vtol_status{};
// commander publications
actuator_armed_s _armed{};
commander_state_s _internal_state{};
vehicle_control_mode_s _vehicle_control_mode{};
vehicle_status_s _status{};
vehicle_status_flags_s _status_flags{};
WorkerThread _worker_thread;
// Subscriptions

Write Preview
Loading…
Cancel
Save