mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-06-29 14:00:34 +08:00
Refactor: Name manual_control_setpoint the same way everywhere
This commit is contained in:
Matthias Grob
committed by
Daniel Agar
Daniel Agar
parent
c6dd8bfcd6
commit
e9eae1bd76
@@ -713,8 +713,8 @@ Commander::handle_command(vehicle_status_s *status_local, const vehicle_command_
|
||||
break;
|
||||
}
|
||||
|
||||
const bool throttle_above_low = (_sp_man.z > 0.1f);
|
||||
const bool throttle_above_center = (_sp_man.z > 0.6f);
|
||||
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 && throttle_above_center &&
|
||||
(status_local->nav_state == vehicle_status_s::NAVIGATION_STATE_POSCTL ||
|
||||
@@ -1428,7 +1428,7 @@ Commander::run()
|
||||
|
||||
#endif // BOARD_HAS_POWER_CONTROL
|
||||
|
||||
_sp_man_sub.update(&_sp_man);
|
||||
_manual_control_setpoint_sub.update(&_manual_control_setpoint);
|
||||
|
||||
offboard_control_update();
|
||||
|
||||
@@ -1773,7 +1773,7 @@ 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 = _sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON;
|
||||
const bool manual_loiter_switch_on = _manual_control_setpoint.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON;
|
||||
|
||||
if (!in_loiter_mode || manual_loiter_switch_on) {
|
||||
_geofence_loiter_on = false;
|
||||
@@ -1782,7 +1782,7 @@ 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 = _sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_ON;
|
||||
const bool manual_return_switch_on = _manual_control_setpoint.return_switch == manual_control_setpoint_s::SWITCH_POS_ON;
|
||||
|
||||
if (!in_rtl_mode || manual_return_switch_on) {
|
||||
_geofence_rtl_on = false;
|
||||
@@ -1819,11 +1819,11 @@ Commander::run()
|
||||
if ((override_auto_mode || override_offboard_mode) && is_rotary_wing
|
||||
&& !in_low_battery_failsafe && !_geofence_warning_action_on) {
|
||||
// transition to previous state if sticks are touched
|
||||
if ((_last_sp_man.timestamp != _sp_man.timestamp) &&
|
||||
((fabsf(_sp_man.x - _last_sp_man.x) > _min_stick_change) ||
|
||||
(fabsf(_sp_man.y - _last_sp_man.y) > _min_stick_change) ||
|
||||
(fabsf(_sp_man.z - _last_sp_man.z) > _min_stick_change) ||
|
||||
(fabsf(_sp_man.r - _last_sp_man.r) > _min_stick_change))) {
|
||||
if ((_last_manual_control_setpoint.timestamp != _manual_control_setpoint.timestamp) &&
|
||||
((fabsf(_manual_control_setpoint.x - _last_manual_control_setpoint.x) > _min_stick_change) ||
|
||||
(fabsf(_manual_control_setpoint.y - _last_manual_control_setpoint.y) > _min_stick_change) ||
|
||||
(fabsf(_manual_control_setpoint.z - _last_manual_control_setpoint.z) > _min_stick_change) ||
|
||||
(fabsf(_manual_control_setpoint.r - _last_manual_control_setpoint.r) > _min_stick_change))) {
|
||||
|
||||
// revert to position control in any case
|
||||
main_state_transition(status, commander_state_s::MAIN_STATE_POSCTL, status_flags, &_internal_state);
|
||||
@@ -1849,8 +1849,8 @@ Commander::run()
|
||||
}
|
||||
|
||||
/* RC input check */
|
||||
if (!status_flags.rc_input_blocked && _sp_man.timestamp != 0 &&
|
||||
(hrt_elapsed_time(&_sp_man.timestamp) < (_param_com_rc_loss_t.get() * 1_s))) {
|
||||
if (!status_flags.rc_input_blocked && _manual_control_setpoint.timestamp != 0 &&
|
||||
(hrt_elapsed_time(&_manual_control_setpoint.timestamp) < (_param_com_rc_loss_t.get() * 1_s))) {
|
||||
|
||||
/* handle the case where RC signal was regained */
|
||||
if (!status_flags.rc_signal_found_once) {
|
||||
@@ -1873,19 +1873,22 @@ Commander::run()
|
||||
status.rc_signal_lost = false;
|
||||
|
||||
const bool in_armed_state = (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED);
|
||||
const bool arm_switch_or_button_mapped = _sp_man.arm_switch != manual_control_setpoint_s::SWITCH_POS_NONE;
|
||||
const bool arm_switch_or_button_mapped =
|
||||
_manual_control_setpoint.arm_switch != manual_control_setpoint_s::SWITCH_POS_NONE;
|
||||
const bool arm_button_pressed = _param_arm_switch_is_button.get()
|
||||
&& (_sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON);
|
||||
&& (_manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON);
|
||||
|
||||
/* DISARM
|
||||
* check if left stick is in lower left position or arm button is pushed or arm switch has transition from arm to disarm
|
||||
* and we are in MANUAL, Rattitude, or AUTO_READY mode or (ASSIST mode and landed)
|
||||
* do it only for rotary wings in manual mode or fixed wing if landed.
|
||||
* Disable stick-disarming if arming switch or button is mapped */
|
||||
const bool stick_in_lower_left = _sp_man.r < -STICK_ON_OFF_LIMIT && (_sp_man.z < 0.1f) && !arm_switch_or_button_mapped;
|
||||
const bool stick_in_lower_left = _manual_control_setpoint.r < -STICK_ON_OFF_LIMIT
|
||||
&& (_manual_control_setpoint.z < 0.1f)
|
||||
&& !arm_switch_or_button_mapped;
|
||||
const bool arm_switch_to_disarm_transition = !_param_arm_switch_is_button.get() &&
|
||||
(_last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) &&
|
||||
(_sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF);
|
||||
(_last_manual_control_setpoint_arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) &&
|
||||
(_manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF);
|
||||
|
||||
if (in_armed_state &&
|
||||
(status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF) &&
|
||||
@@ -1907,7 +1910,8 @@ Commander::run()
|
||||
|
||||
_stick_off_counter++;
|
||||
|
||||
} else if (!(_param_arm_switch_is_button.get() && _sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) {
|
||||
} else if (!(_param_arm_switch_is_button.get()
|
||||
&& _manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) {
|
||||
/* do not reset the counter when holding the arm button longer than needed */
|
||||
_stick_off_counter = 0;
|
||||
}
|
||||
@@ -1916,7 +1920,7 @@ Commander::run()
|
||||
* check if left stick is in lower right position or arm button is pushed or arm switch has transition from disarm to arm
|
||||
* and we're in MANUAL mode.
|
||||
* Disable stick-arming if arming switch or button is mapped */
|
||||
const bool stick_in_lower_right = _sp_man.r > STICK_ON_OFF_LIMIT && _sp_man.z < 0.1f
|
||||
const bool stick_in_lower_right = _manual_control_setpoint.r > STICK_ON_OFF_LIMIT && _manual_control_setpoint.z < 0.1f
|
||||
&& !arm_switch_or_button_mapped;
|
||||
/* allow a grace period for re-arming: preflight checks don't need to pass during that time,
|
||||
* for example for accidential in-air disarming */
|
||||
@@ -1924,9 +1928,9 @@ Commander::run()
|
||||
&& (hrt_elapsed_time(&_last_disarmed_timestamp) < 5_s);
|
||||
|
||||
const bool arm_switch_to_arm_transition = !_param_arm_switch_is_button.get() &&
|
||||
(_last_sp_man_arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF) &&
|
||||
(_sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) &&
|
||||
(_sp_man.z < 0.1f || in_arming_grace_period);
|
||||
(_last_manual_control_setpoint_arm_switch == manual_control_setpoint_s::SWITCH_POS_OFF) &&
|
||||
(_manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON) &&
|
||||
(_manual_control_setpoint.z < 0.1f || in_arming_grace_period);
|
||||
|
||||
if (!in_armed_state &&
|
||||
(status.rc_input_mode != vehicle_status_s::RC_IN_MODE_OFF) &&
|
||||
@@ -1966,12 +1970,13 @@ Commander::run()
|
||||
|
||||
_stick_on_counter++;
|
||||
|
||||
} else if (!(_param_arm_switch_is_button.get() && _sp_man.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) {
|
||||
} else if (!(_param_arm_switch_is_button.get()
|
||||
&& _manual_control_setpoint.arm_switch == manual_control_setpoint_s::SWITCH_POS_ON)) {
|
||||
/* do not reset the counter when holding the arm button longer than needed */
|
||||
_stick_on_counter = 0;
|
||||
}
|
||||
|
||||
_last_sp_man_arm_switch = _sp_man.arm_switch;
|
||||
_last_manual_control_setpoint_arm_switch = _manual_control_setpoint.arm_switch;
|
||||
|
||||
if (arming_ret == TRANSITION_DENIED) {
|
||||
/*
|
||||
@@ -1984,7 +1989,7 @@ Commander::run()
|
||||
}
|
||||
|
||||
/* evaluate the main state machine according to mode switches */
|
||||
bool first_rc_eval = (_last_sp_man.timestamp == 0) && (_sp_man.timestamp > 0);
|
||||
bool first_rc_eval = (_last_manual_control_setpoint.timestamp == 0) && (_manual_control_setpoint.timestamp > 0);
|
||||
transition_result_t main_res = set_main_state(status, &_status_changed);
|
||||
|
||||
/* store last position lock state */
|
||||
@@ -2003,7 +2008,7 @@ Commander::run()
|
||||
}
|
||||
|
||||
/* check throttle kill switch */
|
||||
if (_sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
if (_manual_control_setpoint.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
/* set lockdown flag */
|
||||
if (!armed.manual_lockdown) {
|
||||
mavlink_log_emergency(&mavlink_log_pub, "Manual kill-switch engaged");
|
||||
@@ -2011,7 +2016,7 @@ Commander::run()
|
||||
armed.manual_lockdown = true;
|
||||
}
|
||||
|
||||
} else if (_sp_man.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
|
||||
} else if (_manual_control_setpoint.kill_switch == manual_control_setpoint_s::SWITCH_POS_OFF) {
|
||||
if (armed.manual_lockdown) {
|
||||
mavlink_log_emergency(&mavlink_log_pub, "Manual kill-switch disengaged");
|
||||
_status_changed = true;
|
||||
@@ -2025,7 +2030,7 @@ Commander::run()
|
||||
if (!status_flags.rc_input_blocked && !status.rc_signal_lost && status_flags.rc_signal_found_once) {
|
||||
mavlink_log_critical(&mavlink_log_pub, "Manual control lost");
|
||||
status.rc_signal_lost = true;
|
||||
_rc_signal_lost_timestamp = _sp_man.timestamp;
|
||||
_rc_signal_lost_timestamp = _manual_control_setpoint.timestamp;
|
||||
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, status);
|
||||
_status_changed = true;
|
||||
}
|
||||
@@ -2618,19 +2623,19 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
|
||||
const bool altitude_got_valid = (!_last_condition_local_altitude_valid && status_flags.condition_local_altitude_valid);
|
||||
const bool lpos_got_valid = (!_last_condition_local_position_valid && status_flags.condition_local_position_valid);
|
||||
const bool gpos_got_valid = (!_last_condition_global_position_valid && status_flags.condition_global_position_valid);
|
||||
const bool first_time_rc = (_last_sp_man.timestamp == 0);
|
||||
const bool rc_values_updated = (_last_sp_man.timestamp != _sp_man.timestamp);
|
||||
const bool first_time_rc = (_last_manual_control_setpoint.timestamp == 0);
|
||||
const bool rc_values_updated = (_last_manual_control_setpoint.timestamp != _manual_control_setpoint.timestamp);
|
||||
const bool some_switch_changed =
|
||||
(_last_sp_man.offboard_switch != _sp_man.offboard_switch)
|
||||
|| (_last_sp_man.return_switch != _sp_man.return_switch)
|
||||
|| (_last_sp_man.mode_switch != _sp_man.mode_switch)
|
||||
|| (_last_sp_man.acro_switch != _sp_man.acro_switch)
|
||||
|| (_last_sp_man.rattitude_switch != _sp_man.rattitude_switch)
|
||||
|| (_last_sp_man.posctl_switch != _sp_man.posctl_switch)
|
||||
|| (_last_sp_man.loiter_switch != _sp_man.loiter_switch)
|
||||
|| (_last_sp_man.mode_slot != _sp_man.mode_slot)
|
||||
|| (_last_sp_man.stab_switch != _sp_man.stab_switch)
|
||||
|| (_last_sp_man.man_switch != _sp_man.man_switch);
|
||||
(_last_manual_control_setpoint.offboard_switch != _manual_control_setpoint.offboard_switch)
|
||||
|| (_last_manual_control_setpoint.return_switch != _manual_control_setpoint.return_switch)
|
||||
|| (_last_manual_control_setpoint.mode_switch != _manual_control_setpoint.mode_switch)
|
||||
|| (_last_manual_control_setpoint.acro_switch != _manual_control_setpoint.acro_switch)
|
||||
|| (_last_manual_control_setpoint.rattitude_switch != _manual_control_setpoint.rattitude_switch)
|
||||
|| (_last_manual_control_setpoint.posctl_switch != _manual_control_setpoint.posctl_switch)
|
||||
|| (_last_manual_control_setpoint.loiter_switch != _manual_control_setpoint.loiter_switch)
|
||||
|| (_last_manual_control_setpoint.mode_slot != _manual_control_setpoint.mode_slot)
|
||||
|| (_last_manual_control_setpoint.stab_switch != _manual_control_setpoint.stab_switch)
|
||||
|| (_last_manual_control_setpoint.man_switch != _manual_control_setpoint.man_switch);
|
||||
|
||||
// only switch mode based on RC switch if necessary to also allow mode switching via MAVLink
|
||||
const bool should_evaluate_rc_mode_switch = first_time_rc
|
||||
@@ -2653,25 +2658,25 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
|
||||
_internal_state.main_state == commander_state_s::MAIN_STATE_RATTITUDE ||
|
||||
_internal_state.main_state == commander_state_s::MAIN_STATE_STAB)) {
|
||||
|
||||
_last_sp_man.timestamp = _sp_man.timestamp;
|
||||
_last_sp_man.x = _sp_man.x;
|
||||
_last_sp_man.y = _sp_man.y;
|
||||
_last_sp_man.z = _sp_man.z;
|
||||
_last_sp_man.r = _sp_man.r;
|
||||
_last_manual_control_setpoint.timestamp = _manual_control_setpoint.timestamp;
|
||||
_last_manual_control_setpoint.x = _manual_control_setpoint.x;
|
||||
_last_manual_control_setpoint.y = _manual_control_setpoint.y;
|
||||
_last_manual_control_setpoint.z = _manual_control_setpoint.z;
|
||||
_last_manual_control_setpoint.r = _manual_control_setpoint.r;
|
||||
}
|
||||
|
||||
/* no timestamp change or no switch change -> nothing changed */
|
||||
return TRANSITION_NOT_CHANGED;
|
||||
}
|
||||
|
||||
_last_sp_man = _sp_man;
|
||||
_last_manual_control_setpoint = _manual_control_setpoint;
|
||||
|
||||
// reset the position and velocity validity calculation to give the best change of being able to select
|
||||
// the desired mode
|
||||
reset_posvel_validity(changed);
|
||||
|
||||
/* offboard switch overrides main switch */
|
||||
if (_sp_man.offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
if (_manual_control_setpoint.offboard_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_OFFBOARD, status_flags, &_internal_state);
|
||||
|
||||
if (res == TRANSITION_DENIED) {
|
||||
@@ -2685,7 +2690,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
|
||||
}
|
||||
|
||||
/* RTL switch overrides main switch */
|
||||
if (_sp_man.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
if (_manual_control_setpoint.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_RTL, status_flags, &_internal_state);
|
||||
|
||||
if (res == TRANSITION_DENIED) {
|
||||
@@ -2704,7 +2709,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
|
||||
}
|
||||
|
||||
/* Loiter switch overrides main switch */
|
||||
if (_sp_man.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
if (_manual_control_setpoint.loiter_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_AUTO_LOITER, status_flags, &_internal_state);
|
||||
|
||||
if (res == TRANSITION_DENIED) {
|
||||
@@ -2716,14 +2721,14 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
|
||||
}
|
||||
|
||||
/* we know something has changed - check if we are in mode slot operation */
|
||||
if (_sp_man.mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE) {
|
||||
if (_manual_control_setpoint.mode_slot != manual_control_setpoint_s::MODE_SLOT_NONE) {
|
||||
|
||||
if (_sp_man.mode_slot > manual_control_setpoint_s::MODE_SLOT_NUM) {
|
||||
if (_manual_control_setpoint.mode_slot > manual_control_setpoint_s::MODE_SLOT_NUM) {
|
||||
PX4_WARN("m slot overflow");
|
||||
return TRANSITION_DENIED;
|
||||
}
|
||||
|
||||
int new_mode = _flight_mode_slots[_sp_man.mode_slot - 1];
|
||||
int new_mode = _flight_mode_slots[_manual_control_setpoint.mode_slot - 1];
|
||||
|
||||
if (new_mode < 0) {
|
||||
/* slot is unused */
|
||||
@@ -2855,19 +2860,19 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
|
||||
}
|
||||
|
||||
/* offboard and RTL switches off or denied, check main mode switch */
|
||||
switch (_sp_man.mode_switch) {
|
||||
switch (_manual_control_setpoint.mode_switch) {
|
||||
case manual_control_setpoint_s::SWITCH_POS_NONE:
|
||||
res = TRANSITION_NOT_CHANGED;
|
||||
break;
|
||||
|
||||
case manual_control_setpoint_s::SWITCH_POS_OFF: // MANUAL
|
||||
if (_sp_man.stab_switch == manual_control_setpoint_s::SWITCH_POS_NONE &&
|
||||
_sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) {
|
||||
if (_manual_control_setpoint.stab_switch == manual_control_setpoint_s::SWITCH_POS_NONE &&
|
||||
_manual_control_setpoint.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) {
|
||||
/*
|
||||
* Legacy mode:
|
||||
* Acro switch being used as stabilized switch in FW.
|
||||
*/
|
||||
if (_sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
if (_manual_control_setpoint.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
/* manual mode is stabilized already for multirotors, so switch to acro
|
||||
* for any non-manual mode
|
||||
*/
|
||||
@@ -2881,7 +2886,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
|
||||
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &_internal_state);
|
||||
}
|
||||
|
||||
} else if (_sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
} else if (_manual_control_setpoint.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
/* Similar to acro transitions for multirotors. FW aircraft don't need a
|
||||
* rattitude mode.*/
|
||||
if (status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) {
|
||||
@@ -2900,19 +2905,19 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
|
||||
* - Acro is Acro
|
||||
* - Manual is not default anymore when the manaul switch is assigned
|
||||
*/
|
||||
if (_sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
if (_manual_control_setpoint.man_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &_internal_state);
|
||||
|
||||
} else if (_sp_man.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
} else if (_manual_control_setpoint.acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_ACRO, status_flags, &_internal_state);
|
||||
|
||||
} else if (_sp_man.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
} else if (_manual_control_setpoint.rattitude_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_RATTITUDE, status_flags, &_internal_state);
|
||||
|
||||
} else if (_sp_man.stab_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
} else if (_manual_control_setpoint.stab_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_STAB, status_flags, &_internal_state);
|
||||
|
||||
} else if (_sp_man.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) {
|
||||
} else if (_manual_control_setpoint.man_switch == manual_control_setpoint_s::SWITCH_POS_NONE) {
|
||||
// default to MANUAL when no manual switch is set
|
||||
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_MANUAL, status_flags, &_internal_state);
|
||||
|
||||
@@ -2926,7 +2931,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
|
||||
break;
|
||||
|
||||
case manual_control_setpoint_s::SWITCH_POS_MIDDLE: // ASSIST
|
||||
if (_sp_man.posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
if (_manual_control_setpoint.posctl_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
res = main_state_transition(status_local, commander_state_s::MAIN_STATE_POSCTL, status_flags, &_internal_state);
|
||||
|
||||
if (res != TRANSITION_DENIED) {
|
||||
@@ -2943,7 +2948,7 @@ Commander::set_main_state_rc(const vehicle_status_s &status_local, bool *changed
|
||||
break; // changed successfully or already in this mode
|
||||
}
|
||||
|
||||
if (_sp_man.posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
if (_manual_control_setpoint.posctl_switch != manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
print_reject_mode("ALTITUDE CONTROL");
|
||||
}
|
||||
|
||||
|
||||
Reference in New Issue
Block a user