forked from Archive/PX4-Autopilot
commander: handle manual switches separately from setpoint
This commit is contained in:
Daniel Agar
parent
8c475c24b4
commit
d8fcef054c
|
|
@ -1627,6 +1627,9 @@ Commander::run()
|
|||
|
||||
_offboard_available.set_hysteresis_time_from(true, _param_com_of_loss_t.get() * 1e6f);
|
||||
|
||||
_arm_button_hysteresis.set_hysteresis_time_from(true, _param_rc_arm_hyst.get() * 1_ms);
|
||||
_arm_button_hysteresis.set_hysteresis_time_from(false, _param_rc_arm_hyst.get() * 1_ms);
|
||||
|
||||
param_init_forced = false;
|
||||
}
|
||||
|
||||
|
|
@ -1740,9 +1743,7 @@ Commander::run()
|
|||
}
|
||||
|
||||
/* update safety topic */
|
||||
const bool safety_updated = _safety_sub.updated();
|
||||
|
||||
if (safety_updated) {
|
||||
if (_safety_sub.updated()) {
|
||||
const bool previous_safety_off = _safety.safety_off;
|
||||
|
||||
if (_safety_sub.copy(&_safety)) {
|
||||
|
|
@ -1778,6 +1779,18 @@ Commander::run()
|
|||
|
||||
_status_changed = true;
|
||||
}
|
||||
|
||||
if (_safety.override_available && _safety.override_enabled) {
|
||||
const transition_result_t res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags,
|
||||
&_internal_state);
|
||||
|
||||
if (res == TRANSITION_CHANGED) {
|
||||
_status_changed = true;
|
||||
|
||||
// play tune on mode change only if armed, blink LED always
|
||||
tune_positive(_armed.armed);
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -2034,7 +2047,8 @@ Commander::run()
|
|||
|
||||
// reset if no longer in LOITER or if manually switched to LOITER
|
||||
const bool in_loiter_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_LOITER;
|
||||
const bool manual_loiter_switch_on = _manual_control_switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON;
|
||||
const bool manual_loiter_switch_on = _last_manual_control_switches.loiter_switch ==
|
||||
manual_control_switches_s::SWITCH_POS_ON;
|
||||
|
||||
if (!in_loiter_mode || manual_loiter_switch_on) {
|
||||
_geofence_loiter_on = false;
|
||||
|
|
@ -2043,7 +2057,8 @@ Commander::run()
|
|||
|
||||
// reset if no longer in RTL or if manually switched to RTL
|
||||
const bool in_rtl_mode = _internal_state.main_state == commander_state_s::MAIN_STATE_AUTO_RTL;
|
||||
const bool manual_return_switch_on = _manual_control_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON;
|
||||
const bool manual_return_switch_on = _last_manual_control_switches.return_switch ==
|
||||
manual_control_switches_s::SWITCH_POS_ON;
|
||||
|
||||
if (!in_rtl_mode || manual_return_switch_on) {
|
||||
_geofence_rtl_on = false;
|
||||
|
|
@ -2110,14 +2125,14 @@ Commander::run()
|
|||
|
||||
_status.rc_signal_lost = false;
|
||||
|
||||
const bool rc_arming_enabled = (_status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF);
|
||||
if ((_status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF)
|
||||
&& (_last_manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_NONE)) {
|
||||
|
||||
if (rc_arming_enabled) {
|
||||
if (_manual_control.wantsDisarm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) {
|
||||
if (_manual_control.wantsDisarm(_vehicle_control_mode, _status, _land_detector.landed)) {
|
||||
disarm(arm_disarm_reason_t::RC_STICK);
|
||||
}
|
||||
|
||||
if (_manual_control.wantsArm(_vehicle_control_mode, _status, _manual_control_switches, _land_detector.landed)) {
|
||||
if (_manual_control.wantsArm(_vehicle_control_mode, _status, _land_detector.landed)) {
|
||||
if (_vehicle_control_mode.flag_control_manual_enabled) {
|
||||
arm(arm_disarm_reason_t::RC_STICK);
|
||||
|
||||
|
|
@ -2139,21 +2154,40 @@ Commander::run()
|
|||
_status_changed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
if (_manual_control_switches_sub.update(&_manual_control_switches) || safety_updated) {
|
||||
if (!_manual_control.isRCAvailable()) {
|
||||
// set RC lost
|
||||
if (_status_flags.rc_signal_found_once && !_status.rc_signal_lost) {
|
||||
// ignore RC lost during calibration
|
||||
if (!_status_flags.condition_calibration_enabled && !_status_flags.rc_input_blocked) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Manual control lost");
|
||||
_status.rc_signal_lost = true;
|
||||
_rc_signal_lost_timestamp = _manual_control.getLastRcTimestamp();
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, _status);
|
||||
_status_changed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
if (_manual_control_switches_sub.updated()) {
|
||||
manual_control_switches_s manual_control_switches;
|
||||
|
||||
if (_manual_control_switches_sub.copy(&manual_control_switches)) {
|
||||
|
||||
// handle landing gear switch if configured and in a manual mode
|
||||
if ((_vehicle_control_mode.flag_control_manual_enabled) &&
|
||||
(_manual_control_switches.gear_switch != manual_control_switches_s::SWITCH_POS_NONE) &&
|
||||
(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)) {
|
||||
(manual_control_switches.gear_switch != _last_manual_control_switches.gear_switch)) {
|
||||
// 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) {
|
||||
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) {
|
||||
} 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;
|
||||
|
|
@ -2171,56 +2205,94 @@ Commander::run()
|
|||
}
|
||||
}
|
||||
|
||||
// evaluate the main state machine according to mode switches
|
||||
if (set_main_state(&_status_changed) == TRANSITION_CHANGED) {
|
||||
// play tune on mode change only if armed, blink LED always
|
||||
tune_positive(_armed.armed);
|
||||
_status_changed = true;
|
||||
|
||||
// evaluate the main state machine according to mode switches (unless in manual override)
|
||||
if (!_safety.override_available || !_safety.override_enabled) {
|
||||
if (set_main_state(manual_control_switches) == TRANSITION_CHANGED) {
|
||||
// play tune on mode change only if armed, blink LED always
|
||||
tune_positive(_armed.armed);
|
||||
_status_changed = true;
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
/* check throttle kill switch */
|
||||
if (_manual_control_switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
/* set lockdown flag */
|
||||
if (!_armed.manual_lockdown) {
|
||||
const char kill_switch_string[] = "Kill-switch engaged";
|
||||
|
||||
if (_land_detector.landed) {
|
||||
mavlink_log_info(&_mavlink_log_pub, kill_switch_string);
|
||||
// check throttle kill switch
|
||||
if (manual_control_switches.kill_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
// set lockdown flag
|
||||
if (!_armed.manual_lockdown) {
|
||||
static constexpr const char kill_switch_string[] = "Kill-switch engaged";
|
||||
|
||||
} else {
|
||||
mavlink_log_critical(&_mavlink_log_pub, kill_switch_string);
|
||||
if (_land_detector.landed) {
|
||||
mavlink_log_info(&_mavlink_log_pub, kill_switch_string);
|
||||
|
||||
} else {
|
||||
mavlink_log_critical(&_mavlink_log_pub, kill_switch_string);
|
||||
}
|
||||
|
||||
_status_changed = true;
|
||||
_armed.manual_lockdown = true;
|
||||
}
|
||||
|
||||
_status_changed = true;
|
||||
_armed.manual_lockdown = true;
|
||||
} else if (manual_control_switches.kill_switch == manual_control_switches_s::SWITCH_POS_OFF) {
|
||||
if (_armed.manual_lockdown) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Kill-switch disengaged");
|
||||
_status_changed = true;
|
||||
_armed.manual_lockdown = false;
|
||||
}
|
||||
}
|
||||
|
||||
} else if (_manual_control_switches.kill_switch == manual_control_switches_s::SWITCH_POS_OFF) {
|
||||
if (_armed.manual_lockdown) {
|
||||
mavlink_log_info(&_mavlink_log_pub, "Kill-switch disengaged");
|
||||
_status_changed = true;
|
||||
_armed.manual_lockdown = false;
|
||||
}
|
||||
}
|
||||
|
||||
/* no else case: do not change lockdown flag in unconfigured case */
|
||||
}
|
||||
// arm_switch
|
||||
if (manual_control_switches.arm_switch != manual_control_switches_s::SWITCH_POS_NONE) {
|
||||
if (_param_com_arm_swisbtn.get() == 0) {
|
||||
// COM_ARM_SWISBTN 0 - arm on switch transition
|
||||
|
||||
if (!_manual_control.isRCAvailable()) {
|
||||
// set RC lost
|
||||
if (_status_flags.rc_signal_found_once && !_status.rc_signal_lost) {
|
||||
// ignore RC lost during calibration
|
||||
if (!_status_flags.condition_calibration_enabled && !_status_flags.rc_input_blocked) {
|
||||
mavlink_log_critical(&_mavlink_log_pub, "Manual control lost");
|
||||
_status.rc_signal_lost = true;
|
||||
_rc_signal_lost_timestamp = _manual_control.getLastRcTimestamp();
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, _status);
|
||||
_status_changed = true;
|
||||
if (!_armed.armed && (_last_manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF)
|
||||
&& (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON)) {
|
||||
// arm_switch OFF -> ON (arm)
|
||||
arm(arm_disarm_reason_t::RC_SWITCH);
|
||||
|
||||
} else if (_armed.armed && (_last_manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON)
|
||||
&& (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF)) {
|
||||
// arm_switch ON -> OFF (disarm)
|
||||
disarm(arm_disarm_reason_t::RC_SWITCH);
|
||||
}
|
||||
|
||||
} else if (_param_com_arm_swisbtn.get() == 1) {
|
||||
// COM_ARM_SWISBTN 1 - arm when momentary held down
|
||||
const bool last_arm_hysteresis = _arm_button_hysteresis.get_state();
|
||||
const bool arm_button_pressed = (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON);
|
||||
|
||||
if (!_armed.armed && arm_button_pressed) {
|
||||
// pressing button to arm
|
||||
_arm_button_hysteresis.set_state_and_update(true, hrt_absolute_time());
|
||||
|
||||
if (!last_arm_hysteresis && _arm_button_hysteresis.get_state()) {
|
||||
// disarmed and arm button pressed -> ARM
|
||||
arm(arm_disarm_reason_t::RC_SWITCH);
|
||||
}
|
||||
|
||||
} else if (_armed.armed && arm_button_pressed) {
|
||||
// pressing button disarm
|
||||
_arm_button_hysteresis.set_state_and_update(false, hrt_absolute_time());
|
||||
|
||||
if (last_arm_hysteresis && !_arm_button_hysteresis.get_state()) {
|
||||
// armed and arm button pressed again-> DISARM
|
||||
disarm(arm_disarm_reason_t::RC_SWITCH);
|
||||
}
|
||||
|
||||
} else {
|
||||
// otherwise track armed state
|
||||
_arm_button_hysteresis.set_state_and_update(_armed.armed, hrt_absolute_time());
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
_last_manual_control_switches = manual_control_switches;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
// data link checks which update the status
|
||||
data_link_check();
|
||||
|
||||
|
|
@ -2748,58 +2820,29 @@ Commander::control_status_leds(bool changed, const uint8_t battery_warning)
|
|||
}
|
||||
|
||||
transition_result_t
|
||||
Commander::set_main_state(bool *changed)
|
||||
Commander::set_main_state(const manual_control_switches_s &manual_control_switches)
|
||||
{
|
||||
if (_safety.override_available && _safety.override_enabled) {
|
||||
return set_main_state_override_on(changed);
|
||||
|
||||
} else {
|
||||
return set_main_state_rc();
|
||||
}
|
||||
}
|
||||
|
||||
transition_result_t
|
||||
Commander::set_main_state_override_on(bool *changed)
|
||||
{
|
||||
const transition_result_t res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags,
|
||||
&_internal_state);
|
||||
*changed = (res == TRANSITION_CHANGED);
|
||||
|
||||
return res;
|
||||
}
|
||||
|
||||
transition_result_t
|
||||
Commander::set_main_state_rc()
|
||||
{
|
||||
if ((_manual_control_switches.timestamp == 0)
|
||||
|| (_manual_control_switches.timestamp == _last_manual_control_switches.timestamp)) {
|
||||
|
||||
// no manual control or no update -> nothing changed
|
||||
return TRANSITION_NOT_CHANGED;
|
||||
}
|
||||
|
||||
// Note: even if _status_flags.offboard_control_set_by_command is set
|
||||
// we want to allow rc mode change to take precedence. This is a safety
|
||||
// feature, just in case offboard control goes crazy.
|
||||
|
||||
// only switch mode based on RC switch if necessary to also allow mode switching via MAVLink
|
||||
bool should_evaluate_rc_mode_switch =
|
||||
(_last_manual_control_switches.offboard_switch != _manual_control_switches.offboard_switch)
|
||||
|| (_last_manual_control_switches.return_switch != _manual_control_switches.return_switch)
|
||||
|| (_last_manual_control_switches.mode_switch != _manual_control_switches.mode_switch)
|
||||
|| (_last_manual_control_switches.acro_switch != _manual_control_switches.acro_switch)
|
||||
|| (_last_manual_control_switches.posctl_switch != _manual_control_switches.posctl_switch)
|
||||
|| (_last_manual_control_switches.loiter_switch != _manual_control_switches.loiter_switch)
|
||||
|| (_last_manual_control_switches.mode_slot != _manual_control_switches.mode_slot)
|
||||
|| (_last_manual_control_switches.stab_switch != _manual_control_switches.stab_switch)
|
||||
|| (_last_manual_control_switches.man_switch != _manual_control_switches.man_switch);
|
||||
(_last_manual_control_switches.offboard_switch != manual_control_switches.offboard_switch)
|
||||
|| (_last_manual_control_switches.return_switch != manual_control_switches.return_switch)
|
||||
|| (_last_manual_control_switches.mode_switch != manual_control_switches.mode_switch)
|
||||
|| (_last_manual_control_switches.acro_switch != manual_control_switches.acro_switch)
|
||||
|| (_last_manual_control_switches.posctl_switch != manual_control_switches.posctl_switch)
|
||||
|| (_last_manual_control_switches.loiter_switch != manual_control_switches.loiter_switch)
|
||||
|| (_last_manual_control_switches.mode_slot != manual_control_switches.mode_slot)
|
||||
|| (_last_manual_control_switches.stab_switch != manual_control_switches.stab_switch)
|
||||
|| (_last_manual_control_switches.man_switch != manual_control_switches.man_switch);
|
||||
|
||||
|
||||
if (_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
|
||||
// if already armed don't evaluate first time RC
|
||||
if (_last_manual_control_switches.timestamp == 0) {
|
||||
should_evaluate_rc_mode_switch = false;
|
||||
_last_manual_control_switches = _manual_control_switches;
|
||||
}
|
||||
|
||||
} else {
|
||||
|
|
@ -2821,8 +2864,6 @@ Commander::set_main_state_rc()
|
|||
return TRANSITION_NOT_CHANGED;
|
||||
}
|
||||
|
||||
_last_manual_control_switches = _manual_control_switches;
|
||||
|
||||
// reset the position and velocity validity calculation to give the best change of being able to select
|
||||
// the desired mode
|
||||
reset_posvel_validity();
|
||||
|
|
@ -2831,7 +2872,7 @@ Commander::set_main_state_rc()
|
|||
transition_result_t res = TRANSITION_NOT_CHANGED;
|
||||
|
||||
/* offboard switch overrides main switch */
|
||||
if (_manual_control_switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
if (manual_control_switches.offboard_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
res = main_state_transition(_status, commander_state_s::MAIN_STATE_OFFBOARD, _status_flags, &_internal_state);
|
||||
|
||||
if (res == TRANSITION_DENIED) {
|
||||
|
|
@ -2845,7 +2886,7 @@ Commander::set_main_state_rc()
|
|||
}
|
||||
|
||||
/* RTL switch overrides main switch */
|
||||
if (_manual_control_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
if (manual_control_switches.return_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_RTL, _status_flags, &_internal_state);
|
||||
|
||||
if (res == TRANSITION_DENIED) {
|
||||
|
|
@ -2864,7 +2905,7 @@ Commander::set_main_state_rc()
|
|||
}
|
||||
|
||||
/* Loiter switch overrides main switch */
|
||||
if (_manual_control_switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
if (manual_control_switches.loiter_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
res = main_state_transition(_status, commander_state_s::MAIN_STATE_AUTO_LOITER, _status_flags, &_internal_state);
|
||||
|
||||
if (res == TRANSITION_DENIED) {
|
||||
|
|
@ -2876,14 +2917,14 @@ Commander::set_main_state_rc()
|
|||
}
|
||||
|
||||
/* we know something has changed - check if we are in mode slot operation */
|
||||
if (_manual_control_switches.mode_slot != manual_control_switches_s::MODE_SLOT_NONE) {
|
||||
if (manual_control_switches.mode_slot != manual_control_switches_s::MODE_SLOT_NONE) {
|
||||
|
||||
if (_manual_control_switches.mode_slot > manual_control_switches_s::MODE_SLOT_NUM) {
|
||||
if (manual_control_switches.mode_slot > manual_control_switches_s::MODE_SLOT_NUM) {
|
||||
PX4_WARN("m slot overflow");
|
||||
return TRANSITION_DENIED;
|
||||
}
|
||||
|
||||
int new_mode = _flight_mode_slots[_manual_control_switches.mode_slot - 1];
|
||||
int new_mode = _flight_mode_slots[manual_control_switches.mode_slot - 1];
|
||||
|
||||
if (new_mode < 0) {
|
||||
/* slot is unused */
|
||||
|
|
@ -3013,21 +3054,21 @@ Commander::set_main_state_rc()
|
|||
|
||||
return res;
|
||||
|
||||
} else if (_manual_control_switches.mode_switch != manual_control_switches_s::SWITCH_POS_NONE) {
|
||||
} else if (manual_control_switches.mode_switch != manual_control_switches_s::SWITCH_POS_NONE) {
|
||||
/* offboard and RTL switches off or denied, check main mode switch */
|
||||
switch (_manual_control_switches.mode_switch) {
|
||||
switch (manual_control_switches.mode_switch) {
|
||||
case manual_control_switches_s::SWITCH_POS_NONE:
|
||||
res = TRANSITION_NOT_CHANGED;
|
||||
break;
|
||||
|
||||
case manual_control_switches_s::SWITCH_POS_OFF: // MANUAL
|
||||
if (_manual_control_switches.stab_switch == manual_control_switches_s::SWITCH_POS_NONE &&
|
||||
_manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_NONE) {
|
||||
if (manual_control_switches.stab_switch == manual_control_switches_s::SWITCH_POS_NONE &&
|
||||
manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_NONE) {
|
||||
/*
|
||||
* Legacy mode:
|
||||
* Acro switch being used as stabilized switch in FW.
|
||||
*/
|
||||
if (_manual_control_switches.acro_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
if (manual_control_switches.acro_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
/* manual mode is stabilized already for multirotors, so switch to acro
|
||||
* for any non-manual mode
|
||||
*/
|
||||
|
|
@ -3050,16 +3091,16 @@ Commander::set_main_state_rc()
|
|||
* - Acro is Acro
|
||||
* - Manual is not default anymore when the manual switch is assigned
|
||||
*/
|
||||
if (_manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
if (manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state);
|
||||
|
||||
} else if (_manual_control_switches.acro_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
} else if (manual_control_switches.acro_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
res = main_state_transition(_status, commander_state_s::MAIN_STATE_ACRO, _status_flags, &_internal_state);
|
||||
|
||||
} else if (_manual_control_switches.stab_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
} else if (manual_control_switches.stab_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
res = main_state_transition(_status, commander_state_s::MAIN_STATE_STAB, _status_flags, &_internal_state);
|
||||
|
||||
} else if (_manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_NONE) {
|
||||
} else if (manual_control_switches.man_switch == manual_control_switches_s::SWITCH_POS_NONE) {
|
||||
// default to MANUAL when no manual switch is set
|
||||
res = main_state_transition(_status, commander_state_s::MAIN_STATE_MANUAL, _status_flags, &_internal_state);
|
||||
|
||||
|
|
@ -3073,7 +3114,7 @@ Commander::set_main_state_rc()
|
|||
break;
|
||||
|
||||
case manual_control_switches_s::SWITCH_POS_MIDDLE: // ASSIST
|
||||
if (_manual_control_switches.posctl_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
if (manual_control_switches.posctl_switch == manual_control_switches_s::SWITCH_POS_ON) {
|
||||
res = main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, &_internal_state);
|
||||
|
||||
if (res != TRANSITION_DENIED) {
|
||||
|
|
@ -3090,7 +3131,7 @@ Commander::set_main_state_rc()
|
|||
break; // changed successfully or already in this mode
|
||||
}
|
||||
|
||||
if (_manual_control_switches.posctl_switch != manual_control_switches_s::SWITCH_POS_ON) {
|
||||
if (manual_control_switches.posctl_switch != manual_control_switches_s::SWITCH_POS_ON) {
|
||||
print_reject_mode("ALTITUDE CONTROL");
|
||||
}
|
||||
|
||||
|
|
|
|||
|
|
@ -173,14 +173,8 @@ private:
|
|||
|
||||
void UpdateEstimateValidity();
|
||||
|
||||
// Set the main system state based on RC and override device inputs
|
||||
transition_result_t set_main_state(bool *changed);
|
||||
|
||||
// Enable override (manual reversion mode) on the system
|
||||
transition_result_t set_main_state_override_on(bool *changed);
|
||||
|
||||
// Set the system main state based on the current RC inputs
|
||||
transition_result_t set_main_state_rc();
|
||||
// Set the main system state based on manual switches
|
||||
transition_result_t set_main_state(const manual_control_switches_s &manual_control_switches);
|
||||
|
||||
bool shutdown_if_allowed();
|
||||
|
||||
|
|
@ -234,6 +228,9 @@ private:
|
|||
(ParamFloat<px4::params::COM_EF_C2T>) _param_ef_current2throttle_thres,
|
||||
(ParamFloat<px4::params::COM_EF_TIME>) _param_ef_time_thres,
|
||||
|
||||
(ParamInt<px4::params::COM_RC_ARM_HYST>) _param_rc_arm_hyst,
|
||||
(ParamBool<px4::params::COM_ARM_SWISBTN>) _param_com_arm_swisbtn,
|
||||
|
||||
(ParamBool<px4::params::COM_ARM_WO_GPS>) _param_arm_without_gps,
|
||||
(ParamBool<px4::params::COM_ARM_MIS_REQ>) _param_arm_mission_required,
|
||||
(ParamBool<px4::params::COM_ARM_AUTH_REQ>) _param_arm_auth_required,
|
||||
|
|
@ -335,6 +332,7 @@ private:
|
|||
uint8_t _battery_warning{battery_status_s::BATTERY_WARNING_NONE};
|
||||
float _battery_current{0.0f};
|
||||
|
||||
Hysteresis _arm_button_hysteresis{false};
|
||||
Hysteresis _auto_disarm_landed{false};
|
||||
Hysteresis _auto_disarm_killed{false};
|
||||
Hysteresis _offboard_available{false};
|
||||
|
|
@ -349,7 +347,6 @@ private:
|
|||
|
||||
unsigned int _leds_counter{0};
|
||||
|
||||
manual_control_switches_s _manual_control_switches{};
|
||||
manual_control_switches_s _last_manual_control_switches{};
|
||||
ManualControl _manual_control{this};
|
||||
hrt_abstime _rc_signal_lost_timestamp{0}; ///< Time at which the RC reception was lost
|
||||
|
|
|
|||
|
|
@ -88,88 +88,44 @@ bool ManualControl::wantsOverride(const vehicle_control_mode_s &vehicle_control_
|
|||
}
|
||||
|
||||
bool ManualControl::wantsDisarm(const vehicle_control_mode_s &vehicle_control_mode,
|
||||
const vehicle_status_s &vehicle_status,
|
||||
manual_control_switches_s &manual_control_switches, const bool landed)
|
||||
const vehicle_status_s &vehicle_status, const bool landed)
|
||||
{
|
||||
bool ret = false;
|
||||
|
||||
const bool use_stick = manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_NONE;
|
||||
const bool use_button = !use_stick && _param_com_arm_swisbtn.get();
|
||||
const bool use_switch = !use_stick && !use_button;
|
||||
|
||||
const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
|
||||
const bool stick_in_lower_left = use_stick
|
||||
&& isThrottleLow()
|
||||
&& _manual_control_setpoint.r < -.9f;
|
||||
const bool arm_button_pressed = (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON)
|
||||
&& use_button;
|
||||
const bool arm_switch_to_disarm_transition = use_switch
|
||||
&& (_last_manual_control_switches_arm_switch == manual_control_switches_s::SWITCH_POS_ON)
|
||||
&& (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_OFF);
|
||||
const bool stick_in_lower_left = isThrottleLow() && (_manual_control_setpoint.r < -.9f);
|
||||
|
||||
const bool mc_manual_thrust_mode = vehicle_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING
|
||||
&& vehicle_control_mode.flag_control_manual_enabled
|
||||
&& !vehicle_control_mode.flag_control_climb_rate_enabled;
|
||||
|
||||
if (armed
|
||||
&& (landed || mc_manual_thrust_mode)
|
||||
&& (stick_in_lower_left || arm_button_pressed || arm_switch_to_disarm_transition)) {
|
||||
if (armed && (landed || mc_manual_thrust_mode) && stick_in_lower_left) {
|
||||
|
||||
const bool last_disarm_hysteresis = _stick_disarm_hysteresis.get_state();
|
||||
_stick_disarm_hysteresis.set_state_and_update(true, hrt_absolute_time());
|
||||
const bool disarm_trigger = !last_disarm_hysteresis && _stick_disarm_hysteresis.get_state()
|
||||
&& !_stick_arm_hysteresis.get_state();
|
||||
|
||||
if (disarm_trigger || arm_switch_to_disarm_transition) {
|
||||
ret = true;
|
||||
if (!last_disarm_hysteresis && _stick_disarm_hysteresis.get_state() && !_stick_arm_hysteresis.get_state()) {
|
||||
return true;
|
||||
}
|
||||
|
||||
} else if (!arm_button_pressed) {
|
||||
|
||||
_stick_disarm_hysteresis.set_state_and_update(false, hrt_absolute_time());
|
||||
}
|
||||
|
||||
return ret;
|
||||
return false;
|
||||
}
|
||||
|
||||
bool ManualControl::wantsArm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status,
|
||||
const manual_control_switches_s &manual_control_switches, const bool landed)
|
||||
const bool landed)
|
||||
{
|
||||
bool ret = false;
|
||||
|
||||
const bool use_stick = manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_NONE;
|
||||
const bool use_button = !use_stick && _param_com_arm_swisbtn.get();
|
||||
const bool use_switch = !use_stick && !use_button;
|
||||
|
||||
const bool armed = (vehicle_status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
|
||||
const bool stick_in_lower_right = use_stick
|
||||
&& isThrottleLow()
|
||||
&& _manual_control_setpoint.r > .9f;
|
||||
const bool arm_button_pressed = use_button
|
||||
&& (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON);
|
||||
const bool arm_switch_to_arm_transition = use_switch
|
||||
&& (_last_manual_control_switches_arm_switch == manual_control_switches_s::SWITCH_POS_OFF)
|
||||
&& (manual_control_switches.arm_switch == manual_control_switches_s::SWITCH_POS_ON);
|
||||
|
||||
if (!armed
|
||||
&& (stick_in_lower_right || arm_button_pressed || arm_switch_to_arm_transition)) {
|
||||
const bool stick_in_lower_right = isThrottleLow() && (_manual_control_setpoint.r > .9f);
|
||||
|
||||
if (!armed && stick_in_lower_right) {
|
||||
const bool last_arm_hysteresis = _stick_arm_hysteresis.get_state();
|
||||
_stick_arm_hysteresis.set_state_and_update(true, hrt_absolute_time());
|
||||
const bool arm_trigger = !last_arm_hysteresis && _stick_arm_hysteresis.get_state()
|
||||
&& !_stick_disarm_hysteresis.get_state();
|
||||
|
||||
if (arm_trigger || arm_switch_to_arm_transition) {
|
||||
ret = true;
|
||||
if (!last_arm_hysteresis && _stick_arm_hysteresis.get_state() && !_stick_disarm_hysteresis.get_state()) {
|
||||
return true;
|
||||
}
|
||||
|
||||
} else if (!arm_button_pressed) {
|
||||
|
||||
_stick_arm_hysteresis.set_state_and_update(false, hrt_absolute_time());
|
||||
}
|
||||
|
||||
_last_manual_control_switches_arm_switch = manual_control_switches.arm_switch; // After disarm and arm check
|
||||
|
||||
return ret;
|
||||
return false;
|
||||
}
|
||||
|
||||
void ManualControl::updateParams()
|
||||
|
|
|
|||
|
|
@ -66,9 +66,9 @@ public:
|
|||
bool isRCAvailable() const { return _rc_available; }
|
||||
bool wantsOverride(const vehicle_control_mode_s &vehicle_control_mode);
|
||||
bool wantsDisarm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status,
|
||||
manual_control_switches_s &manual_control_switches, const bool landed);
|
||||
const bool landed);
|
||||
bool wantsArm(const vehicle_control_mode_s &vehicle_control_mode, const vehicle_status_s &vehicle_status,
|
||||
const manual_control_switches_s &manual_control_switches, const bool landed);
|
||||
const bool landed);
|
||||
bool isThrottleLow() const { return _last_manual_control_setpoint.z < 0.1f; }
|
||||
bool isThrottleAboveCenter() const { return _last_manual_control_setpoint.z > 0.6f; }
|
||||
hrt_abstime getLastRcTimestamp() const { return _last_manual_control_setpoint.timestamp; }
|
||||
|
|
|
|||
Loading…
Reference in New Issue