dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

Commander: avoid RC actions during calibration

This commit is contained in:
Matthias Grob 2021-12-22 13:02:29 +01:00 committed by Daniel Agar
parent 89974c46b9
commit f68ae39840
2 changed files with 15 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
msg/vehicle_status_flags.msg
View File

@ -30,7 +30,7 @@ bool circuit_breaker_vtol_fw_arming_check # set to true if for VTOLs arming in
bool offboard_control_signal_lost bool offboard_control_signal_lost
bool rc_signal_found_once bool rc_signal_found_once
bool rc_input_blocked # set if RC input should be ignored temporarily bool rc_calibration_in_progress
bool rc_calibration_valid # set if RC calibration is valid bool rc_calibration_valid # set if RC calibration is valid
bool vtol_transition_failure # Set to true if vtol transition failed bool vtol_transition_failure # Set to true if vtol transition failed
bool usb_connected # status of the USB power supply bool usb_connected # status of the USB power supply

21
src/modules/commander/Commander.cpp
View File

@ -572,7 +572,6 @@ transition_result_t Commander::arm(arm_disarm_reason_t calling_reason, bool run_
"Arming denied: throttle above center"); "Arming denied: throttle above center");
tune_negative(true); tune_negative(true);
return TRANSITION_DENIED; return TRANSITION_DENIED;
} }
if (!_vehicle_control_mode.flag_control_climb_rate_enabled && if (!_vehicle_control_mode.flag_control_climb_rate_enabled &&
@ -1257,7 +1256,7 @@ Commander::handle_command(const vehicle_command_s &cmd)
/* RC calibration */ /* RC calibration */
answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED); answer_command(cmd, vehicle_command_s::VEHICLE_CMD_RESULT_ACCEPTED);
/* disable RC control input completely */ /* disable RC control input completely */
_status_flags.rc_input_blocked = true; _status_flags.rc_calibration_in_progress = true;
mavlink_log_info(&_mavlink_log_pub, "Calibration: Disabling RC input\t"); mavlink_log_info(&_mavlink_log_pub, "Calibration: Disabling RC input\t");
events::send(events::ID("commander_calib_rc_off"), events::Log::Info, events::send(events::ID("commander_calib_rc_off"), events::Log::Info,
"Calibration: Disabling RC input"); "Calibration: Disabling RC input");
@ -1307,9 +1306,9 @@ Commander::handle_command(const vehicle_command_s &cmd)
} else if ((int)(cmd.param4) == 0) { } else if ((int)(cmd.param4) == 0) {
/* RC calibration ended - have we been in one worth confirming? */ /* RC calibration ended - have we been in one worth confirming? */
if (_status_flags.rc_input_blocked) { if (_status_flags.rc_calibration_in_progress) {
/* enable RC control input */ /* enable RC control input */
_status_flags.rc_input_blocked = false; _status_flags.rc_calibration_in_progress = false;
mavlink_log_info(&_mavlink_log_pub, "Calibration: Restoring RC input\t"); mavlink_log_info(&_mavlink_log_pub, "Calibration: Restoring RC input\t");
events::send(events::ID("commander_calib_rc_on"), events::Log::Info, events::send(events::ID("commander_calib_rc_on"), events::Log::Info,
"Calibration: Restoring RC input"); "Calibration: Restoring RC input");
@ -1552,6 +1551,15 @@ void Commander::executeActionRequest(const action_request_s &action_request)
{ {
arm_disarm_reason_t arm_disarm_reason{}; arm_disarm_reason_t arm_disarm_reason{};
// Silently ignore RC actions during RC calibration
if (_status_flags.rc_calibration_in_progress
&& (action_request.source == action_request_s::SOURCE_RC_STICK_GESTURE
|| action_request.source == action_request_s::SOURCE_RC_SWITCH
|| action_request.source == action_request_s::SOURCE_RC_BUTTON
|| action_request.source == action_request_s::SOURCE_RC_MODE_SLOT)) {
return;
}
switch (action_request.source) { switch (action_request.source) {
case action_request_s::SOURCE_RC_STICK_GESTURE: arm_disarm_reason = arm_disarm_reason_t::rc_stick; break; case action_request_s::SOURCE_RC_STICK_GESTURE: arm_disarm_reason = arm_disarm_reason_t::rc_stick; break;
@ -2448,8 +2456,7 @@ Commander::run()
_last_valid_manual_control_setpoint = manual_control_setpoint.timestamp; _last_valid_manual_control_setpoint = manual_control_setpoint.timestamp;
} else { } else {
if (_status_flags.rc_signal_found_once && !_status.rc_signal_lost if (_status_flags.rc_signal_found_once && !_status.rc_signal_lost) {
&& !_status_flags.condition_calibration_enabled && !_status_flags.rc_input_blocked) {
mavlink_log_critical(&_mavlink_log_pub, "Manual control lost\t"); mavlink_log_critical(&_mavlink_log_pub, "Manual control lost\t");
events::send(events::ID("commander_rc_lost"), {events::Log::Critical, events::LogInternal::Info}, events::send(events::ID("commander_rc_lost"), {events::Log::Critical, events::LogInternal::Info},
"Manual control lost"); "Manual control lost");
@ -2472,7 +2479,7 @@ Commander::run()
if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING) if ((_status.vehicle_type == vehicle_status_s::VEHICLE_TYPE_ROTARY_WING)
&& !in_low_battery_failsafe && !_geofence_warning_action_on && !in_low_battery_failsafe && !_geofence_warning_action_on
&& _armed.armed && _armed.armed
&& !_status_flags.rc_input_blocked && !_status_flags.rc_calibration_in_progress
&& manual_control_setpoint.valid && manual_control_setpoint.valid
&& manual_control_setpoint.sticks_moving && manual_control_setpoint.sticks_moving
&& override_enabled) { && override_enabled) {