mirror of
https://gitee.com/mirrors_PX4/PX4-Autopilot.git
synced 2026-07-18 05:10:35 +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
@@ -45,12 +45,12 @@ bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const boo
|
||||
|
||||
uORB::SubscriptionData<manual_control_setpoint_s> manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||
manual_control_setpoint_sub.update();
|
||||
const manual_control_setpoint_s &manual_control = manual_control_setpoint_sub.get();
|
||||
const manual_control_setpoint_s &manual_control_setpoint = manual_control_setpoint_sub.get();
|
||||
|
||||
if (hrt_elapsed_time(&manual_control.timestamp) < 1_s) {
|
||||
if (hrt_elapsed_time(&manual_control_setpoint.timestamp) < 1_s) {
|
||||
|
||||
//check action switches
|
||||
if (manual_control.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
if (manual_control_setpoint.return_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
success = false;
|
||||
|
||||
if (report_fail) {
|
||||
@@ -58,7 +58,7 @@ bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const boo
|
||||
}
|
||||
}
|
||||
|
||||
if (manual_control.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
if (manual_control_setpoint.kill_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
success = false;
|
||||
|
||||
if (report_fail) {
|
||||
@@ -66,7 +66,7 @@ bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const boo
|
||||
}
|
||||
}
|
||||
|
||||
if (manual_control.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
if (manual_control_setpoint.gear_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
success = false;
|
||||
|
||||
if (report_fail) {
|
||||
@@ -74,7 +74,7 @@ bool PreFlightCheck::manualControlCheck(orb_advert_t *mavlink_log_pub, const boo
|
||||
}
|
||||
}
|
||||
|
||||
if (manual_control.transition_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
if (manual_control_setpoint.transition_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
|
||||
success = false;
|
||||
|
||||
if (report_fail) {
|
||||
|
||||
@@ -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");
|
||||
}
|
||||
|
||||
|
||||
@@ -343,11 +343,11 @@ private:
|
||||
|
||||
unsigned int _leds_counter{0};
|
||||
|
||||
manual_control_setpoint_s _sp_man{}; ///< the current manual control setpoint
|
||||
manual_control_setpoint_s _last_sp_man{}; ///< the manual control setpoint valid at the last mode switch
|
||||
manual_control_setpoint_s _manual_control_setpoint{}; ///< the current manual control setpoint
|
||||
manual_control_setpoint_s _last_manual_control_setpoint{}; ///< the manual control setpoint valid at the last mode switch
|
||||
hrt_abstime _rc_signal_lost_timestamp{0}; ///< Time at which the RC reception was lost
|
||||
int32_t _flight_mode_slots[manual_control_setpoint_s::MODE_SLOT_NUM] {};
|
||||
uint8_t _last_sp_man_arm_switch{0};
|
||||
uint8_t _last_manual_control_setpoint_arm_switch{0};
|
||||
float _min_stick_change{};
|
||||
uint32_t _stick_off_counter{0};
|
||||
uint32_t _stick_on_counter{0};
|
||||
@@ -400,7 +400,7 @@ private:
|
||||
uORB::Subscription _land_detector_sub{ORB_ID(vehicle_land_detected)};
|
||||
uORB::Subscription _parameter_update_sub{ORB_ID(parameter_update)};
|
||||
uORB::Subscription _safety_sub{ORB_ID(safety)};
|
||||
uORB::Subscription _sp_man_sub{ORB_ID(manual_control_setpoint)};
|
||||
uORB::Subscription _manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||
uORB::Subscription _subsys_sub{ORB_ID(subsystem_info)};
|
||||
uORB::Subscription _system_power_sub{ORB_ID(system_power)};
|
||||
uORB::Subscription _telemetry_status_sub{ORB_ID(telemetry_status)};
|
||||
|
||||
@@ -52,17 +52,17 @@
|
||||
|
||||
int do_trim_calibration(orb_advert_t *mavlink_log_pub)
|
||||
{
|
||||
uORB::Subscription sub_man{ORB_ID(manual_control_setpoint)};
|
||||
uORB::Subscription manual_control_setpoint_sub{ORB_ID(manual_control_setpoint)};
|
||||
px4_usleep(400000);
|
||||
manual_control_setpoint_s sp{};
|
||||
bool changed = sub_man.updated();
|
||||
manual_control_setpoint_s manual_control_setpoint{};
|
||||
bool changed = manual_control_setpoint_sub.updated();
|
||||
|
||||
if (!changed) {
|
||||
mavlink_log_critical(mavlink_log_pub, "no inputs, aborting");
|
||||
return PX4_ERROR;
|
||||
}
|
||||
|
||||
sub_man.copy(&sp);
|
||||
manual_control_setpoint_sub.copy(&manual_control_setpoint);
|
||||
|
||||
/* load trim values which are active */
|
||||
float roll_trim_active;
|
||||
@@ -83,15 +83,15 @@ int do_trim_calibration(orb_advert_t *mavlink_log_pub)
|
||||
/* set parameters: the new trim values are the combination of active trim values
|
||||
and the values coming from the remote control of the user
|
||||
*/
|
||||
float p = sp.y * roll_scale + roll_trim_active;
|
||||
float p = manual_control_setpoint.y * roll_scale + roll_trim_active;
|
||||
int p1r = param_set(param_find("TRIM_ROLL"), &p);
|
||||
/*
|
||||
we explicitly swap sign here because the trim is added to the actuator controls
|
||||
which are moving in an inverse sense to manual pitch inputs
|
||||
*/
|
||||
p = -sp.x * pitch_scale + pitch_trim_active;
|
||||
p = -manual_control_setpoint.x * pitch_scale + pitch_trim_active;
|
||||
int p2r = param_set(param_find("TRIM_PITCH"), &p);
|
||||
p = sp.r * yaw_scale + yaw_trim_active;
|
||||
p = manual_control_setpoint.r * yaw_scale + yaw_trim_active;
|
||||
int p3r = param_set(param_find("TRIM_YAW"), &p);
|
||||
|
||||
if (p1r != 0 || p2r != 0 || p3r != 0) {
|
||||
|
||||
Reference in New Issue
Block a user