fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 00:50:36 +08:00
Code Issues Packages Projects Releases Wiki Activity

commander: handle RC loss timeout if manual_control_setpoint stops publishing (#19680)

Browse Source 
Co-authored-by: Matthias Grob <maetugr@gmail.com>
This commit is contained in:
Daniel Agar
2022-05-20 19:37:30 -04:00
committed by GitHub
parent 2bdeb36ea3
commit c40631c136
3 changed files with 122 additions and 93 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/Commander.cpp
+118 -93
View File
@@ -837,7 +837,6 @@ Commander::Commander() :
_status_flags.offboard_control_signal_lost = true;
_status_flags.power_input_valid = true;
_status_flags.rc_calibration_valid = true;
// default for vtol is rotary wing
_vtol_status.vehicle_vtol_state = vtol_vehicle_status_s::VEHICLE_VTOL_STATE_MC;
@@ -2653,98 +2652,7 @@ Commander::run()
}
}
manual_control_setpoint_s manual_control_setpoint;
if (_manual_control_setpoint_sub.update(&manual_control_setpoint)) {
if (manual_control_setpoint.valid) {
if (!_status_flags.rc_signal_found_once) {
_status_flags.rc_signal_found_once = true;
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true,
_status_flags.rc_calibration_valid, _status);
_status_changed = true;
} else {
if (_status.rc_signal_lost) {
if (_last_valid_manual_control_setpoint > 0) {
float elapsed = hrt_elapsed_time(&_last_valid_manual_control_setpoint) * 1e-6f;
mavlink_log_info(&_mavlink_log_pub, "Manual control regained after %.1fs\t", (double)elapsed);
events::send<float>(events::ID("commander_rc_regained"), events::Log::Info,
"Manual control regained after {1:.1} s", elapsed);
}
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true,
_status_flags.rc_calibration_valid, _status);
_status_changed = true;
}
}
const bool is_mavlink = manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC;
if (!_arm_state_machine.isArmed() && (is_mavlink || !_mode_switch_mapped)
&& (_internal_state.main_state_changes == 0)) {
// if there's never been a mode change force position control as initial state
_internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
_internal_state.main_state_changes++;
}
_status.rc_signal_lost = false;
_is_throttle_above_center = manual_control_setpoint.z > 0.6f;
_is_throttle_low = manual_control_setpoint.z < 0.1f;
_last_valid_manual_control_setpoint = manual_control_setpoint.timestamp;
} else {
if (_status_flags.rc_signal_found_once && !_status.rc_signal_lost) {
mavlink_log_critical(&_mavlink_log_pub, "Manual control lost\t");
events::send(events::ID("commander_rc_lost"), {events::Log::Critical, events::LogInternal::Info},
"Manual control lost");
_status.rc_signal_lost = true;
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true,
false, _status);
_status_changed = true;
}
}
const bool override_enabled =
((_param_com_rc_override.get() & static_cast<int32_t>(RcOverrideBits::AUTO_MODE_BIT))
&& _vehicle_control_mode.flag_control_auto_enabled)
|| ((_param_com_rc_override.get() & static_cast<int32_t>(RcOverrideBits::OFFBOARD_MODE_BIT))
&& _vehicle_control_mode.flag_control_offboard_enabled);
// Abort autonomous mode and switch to position mode if sticks are moved significantly
// but only if actually in air.
if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
&& !in_low_battery_failsafe_delay && !_geofence_warning_action_on
&& _arm_state_machine.isArmed()
&& !_status_flags.rc_calibration_in_progress
&& manual_control_setpoint.valid
&& manual_control_setpoint.sticks_moving
&& override_enabled) {
const transition_result_t posctl_result =
main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state);
if (posctl_result == TRANSITION_CHANGED) {
tune_positive(true);
mavlink_log_info(&_mavlink_log_pub, "Pilot took over position control using sticks\t");
events::send(events::ID("commander_rc_override_pos"), events::Log::Info,
"Pilot took over position control using sticks");
_status_changed = true;
} else if (posctl_result == TRANSITION_DENIED) {
// If transition to POSCTL was denied, then we can try again with ALTCTL.
const transition_result_t altctl_result =
main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, _internal_state);
if (altctl_result == TRANSITION_CHANGED) {
tune_positive(true);
mavlink_log_info(&_mavlink_log_pub, "Pilot took over altitude control using sticks\t");
events::send(events::ID("commander_rc_override_alt"), events::Log::Info,
"Pilot took over altitude control using sticks");
_status_changed = true;
}
}
}
}
manual_control_check();
// data link checks which update the status
data_link_check();
@@ -4342,6 +4250,123 @@ void Commander::estimator_check()
}
}
void Commander::manual_control_check()
{
manual_control_setpoint_s manual_control_setpoint;
const bool manual_control_updated = _manual_control_setpoint_sub.update(&manual_control_setpoint);
if (manual_control_updated && manual_control_setpoint.valid) {
if (!_status_flags.rc_signal_found_once) {
_status_flags.rc_signal_found_once = true;
} else if (_status.rc_signal_lost) {
if (_last_valid_manual_control_setpoint > 0) {
float elapsed = hrt_elapsed_time(&_last_valid_manual_control_setpoint) * 1e-6f;
mavlink_log_info(&_mavlink_log_pub, "Manual control regained after %.1fs\t", (double)elapsed);
events::send<float>(events::ID("commander_rc_regained"), events::Log::Info,
"Manual control regained after {1:.1} s", elapsed);
}
}
if (_status.rc_signal_lost) {
_status.rc_signal_lost = false;
_status_changed = true;
}
_last_valid_manual_control_setpoint = manual_control_setpoint.timestamp;
_is_throttle_above_center = (manual_control_setpoint.z > 0.6f);
_is_throttle_low = (manual_control_setpoint.z < 0.1f);
const bool is_mavlink = (manual_control_setpoint.data_source > manual_control_setpoint_s::SOURCE_RC);
if (is_mavlink) {
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, true, _status);
} else {
// if not mavlink also report valid RC calibration for health
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, _status_flags.rc_calibration_valid, _status);
}
if (_arm_state_machine.isArmed()) {
// Abort autonomous mode and switch to position mode if sticks are moved significantly
// but only if actually in air.
if (manual_control_setpoint.sticks_moving
&& !_vehicle_control_mode.flag_control_manual_enabled
&& (_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
) {
bool override_enabled = false;
if (_vehicle_control_mode.flag_control_auto_enabled) {
if (_param_com_rc_override.get() & static_cast<int32_t>(RcOverrideBits::AUTO_MODE_BIT)) {
override_enabled = true;
}
}
if (_vehicle_control_mode.flag_control_offboard_enabled) {
if (_param_com_rc_override.get() & static_cast<int32_t>(RcOverrideBits::OFFBOARD_MODE_BIT)) {
override_enabled = true;
}
}
const bool in_low_battery_failsafe_delay = (_battery_failsafe_timestamp != 0);
if (override_enabled && !in_low_battery_failsafe_delay && !_geofence_warning_action_on) {
const transition_result_t posctl_result =
main_state_transition(_status, commander_state_s::MAIN_STATE_POSCTL, _status_flags, _internal_state);
if (posctl_result == TRANSITION_CHANGED) {
tune_positive(true);
mavlink_log_info(&_mavlink_log_pub, "Pilot took over position control using sticks\t");
events::send(events::ID("commander_rc_override_pos"), events::Log::Info,
"Pilot took over position control using sticks");
_status_changed = true;
} else if (posctl_result == TRANSITION_DENIED) {
// If transition to POSCTL was denied, then we can try again with ALTCTL.
const transition_result_t altctl_result =
main_state_transition(_status, commander_state_s::MAIN_STATE_ALTCTL, _status_flags, _internal_state);
if (altctl_result == TRANSITION_CHANGED) {
tune_positive(true);
mavlink_log_info(&_mavlink_log_pub, "Pilot took over altitude control using sticks\t");
events::send(events::ID("commander_rc_override_alt"), events::Log::Info,
"Pilot took over altitude control using sticks");
_status_changed = true;
}
}
}
}
} else {
// disarmed
// if there's never been a mode change force position control as initial state
if (_internal_state.main_state_changes == 0) {
if (is_mavlink || !_mode_switch_mapped) {
_internal_state.main_state = commander_state_s::MAIN_STATE_POSCTL;
_internal_state.main_state_changes++;
}
}
}
} else if ((manual_control_updated && !manual_control_setpoint.valid)
|| hrt_elapsed_time(&_last_valid_manual_control_setpoint) > _param_com_rc_loss_t.get() * 1_s) {
// prohibit stick use in case of reported invalidity or data timeout
if (!_status.rc_signal_lost) {
_status.rc_signal_lost = true;
_status_changed = true;
mavlink_log_critical(&_mavlink_log_pub, "Manual control lost\t");
events::send(events::ID("commander_rc_lost"), {events::Log::Critical, events::LogInternal::Info},
"Manual control lost");
set_health_flags(subsystem_info_s::SUBSYSTEM_TYPE_RCRECEIVER, true, true, false, _status);
}
}
}
void
Commander::offboard_control_update()
{
src/modules/commander/Commander.hpp
+3
View File
@@ -151,6 +151,8 @@ private:
void estimator_check();
void manual_control_check();
bool handle_command(const vehicle_command_s &cmd);
unsigned handle_command_motor_test(const vehicle_command_s &cmd);
@@ -195,6 +197,7 @@ private:
(ParamInt<px4::params::COM_HLDL_LOSS_T>) _param_com_hldl_loss_t,
(ParamInt<px4::params::COM_HLDL_REG_T>) _param_com_hldl_reg_t,
(ParamFloat<px4::params::COM_RC_LOSS_T>) _param_com_rc_loss_t,
(ParamInt<px4::params::NAV_RCL_ACT>) _param_nav_rcl_act,
(ParamFloat<px4::params::COM_RCL_ACT_T>) _param_com_rcl_act_t,
(ParamInt<px4::params::COM_RCL_EXCEPT>) _param_com_rcl_except,
src/modules/manual_control/ManualControlSelector.cpp
+1
View File
@@ -53,6 +53,7 @@ void ManualControlSelector::updateWithNewInputSample(uint64_t now, const manual_
if (isInputValid(input, now) && (update_existing_input || start_using_new_input)) {
_setpoint = input;
_setpoint.valid = true;
_setpoint.timestamp = now; // timestamp_sample is preserved
_instance = instance;
if (_first_valid_source == manual_control_setpoint_s::SOURCE_UNKNOWN) {