From 328fff858563323bd7db88e170a1e193a955dfa2 Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Sat, 14 Sep 2019 10:56:36 +1000 Subject: [PATCH] Copter: fixed RC failsafe handling for no RC receiver this stops us using uninitialised values in modes like circle which can operate either with or without RC input. If we didn't have a RC receiver attached then they would use a maximum yaw rate (which produces quite a spectacular result for a tuned up racing quad) --- ArduCopter/Attitude.cpp | 6 +++++- ArduCopter/mode.cpp | 6 ++++++ ArduCopter/radio.cpp | 7 ++----- 3 files changed, 13 insertions(+), 6 deletions(-) diff --git a/ArduCopter/Attitude.cpp b/ArduCopter/Attitude.cpp index 3224c2af27..b0249ab0c5 100644 --- a/ArduCopter/Attitude.cpp +++ b/ArduCopter/Attitude.cpp @@ -4,6 +4,10 @@ // returns desired yaw rate in centi-degrees per second float Copter::get_pilot_desired_yaw_rate(int16_t stick_angle) { + // throttle failsafe check + if (failsafe.radio || !ap.rc_receiver_present) { + return 0.0f; + } float yaw_request; // calculate yaw rate request @@ -76,7 +80,7 @@ void Copter::set_throttle_takeoff() float Copter::get_pilot_desired_climb_rate(float throttle_control) { // throttle failsafe check - if (failsafe.radio) { + if (failsafe.radio || !ap.rc_receiver_present) { return 0.0f; } diff --git a/ArduCopter/mode.cpp b/ArduCopter/mode.cpp index 2e63db2913..ae2e74bb4a 100644 --- a/ArduCopter/mode.cpp +++ b/ArduCopter/mode.cpp @@ -351,6 +351,12 @@ void Mode::update_navigation() // returns desired angle in centi-degrees void Mode::get_pilot_desired_lean_angles(float &roll_out, float &pitch_out, float angle_max, float angle_limit) const { + // throttle failsafe check + if (copter.failsafe.radio || !copter.ap.rc_receiver_present) { + roll_out = 0; + pitch_out = 0; + return; + } // fetch roll and pitch inputs roll_out = channel_roll->get_control_in(); pitch_out = channel_pitch->get_control_in(); diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index fee5120a13..a0840ebc9f 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -94,11 +94,8 @@ void Copter::read_radio() set_throttle_and_failsafe(channel_throttle->get_radio_in()); set_throttle_zero_flag(channel_throttle->get_control_in()); - if (!ap.rc_receiver_present) { - // RC receiver must be attached if we've just got input and - // there are no overrides - ap.rc_receiver_present = !RC_Channels::has_active_overrides(); - } + // RC receiver must be attached if we've just got input + ap.rc_receiver_present = true; // pass pilot input through to motors (used to allow wiggling servos while disarmed on heli, single, coax copters) radio_passthrough_to_motors();