Allow scaled passthru to go to trim on rc failsafe

ArduCopter/RC_Channel.cpp

@@ -37,9 +37,14 @@ void RC_Channel_Copter::mode_switch_changed(modeswitch_pos_t new_pos)
     }
 }
 
+bool RC_Channels_Copter::in_rc_failsafe() const
+{
+    return copter.failsafe.radio;
+}
+
 bool RC_Channels_Copter::has_valid_input() const
 {
-    if (copter.failsafe.radio) {
+    if (in_rc_failsafe()) {
         return false;
     }
     if (copter.failsafe.radio_counter != 0) {

ArduCopter/RC_Channel.h

@@ -31,6 +31,7 @@ class RC_Channels_Copter : public RC_Channels
 public:
 
     bool has_valid_input() const override;
+    bool in_rc_failsafe() const override;
 
     RC_Channel *get_arming_channel(void) const override;
 

ArduPlane/RC_Channel.cpp

@@ -16,9 +16,14 @@ int8_t RC_Channels_Plane::flight_mode_channel_number() const
     return plane.g.flight_mode_channel.get();
 }
 
+bool RC_Channels_Plane::in_rc_failsafe() const
+{
+    return (plane.rc_failsafe_active() || plane.failsafe.rc_failsafe);
+}
+
 bool RC_Channels_Plane::has_valid_input() const
 {
-    if (plane.rc_failsafe_active() || plane.failsafe.rc_failsafe) {
+    if (in_rc_failsafe()) {
         return false;
     }
     if (plane.failsafe.throttle_counter != 0) {

ArduPlane/RC_Channel.h

@@ -45,6 +45,7 @@ class RC_Channels_Plane : public RC_Channels
         return &obj_channels[chan];
     }
 
+    bool in_rc_failsafe() const override;
     bool has_valid_input() const override;
 
     RC_Channel *get_arming_channel(void) const override;

ArduSub/RC_Channel.h

@@ -25,6 +25,9 @@ class RC_Channels_Sub : public RC_Channels
         return &obj_channels[chan];
     }
 
+    // tell the gimbal code all is good with RC input:
+    bool in_rc_failsafe() const override { return false; };
+
 protected:
 
     // note that these callbacks are not presently used on Plane:

Blimp/RC_Channel.cpp

@@ -36,6 +36,11 @@ void RC_Channel_Blimp::mode_switch_changed(modeswitch_pos_t new_pos)
     }
 }
 
+bool RC_Channels_Blimp::in_rc_failsafe() const
+{
+    return blimp.failsafe.radio;
+}
+
 bool RC_Channels_Blimp::has_valid_input() const
 {
     if (blimp.failsafe.radio) {

Blimp/RC_Channel.h

@@ -30,6 +30,7 @@ class RC_Channels_Blimp : public RC_Channels
 public:
 
     bool has_valid_input() const override;
+    bool in_rc_failsafe() const override;
 
     RC_Channel *get_arming_channel(void) const override;
 

Rover/RC_Channel.cpp

@@ -62,9 +62,14 @@ void RC_Channel_Rover::init_aux_function(const aux_func_t ch_option, const AuxSw
 }
 
 
+bool RC_Channels_Rover::in_rc_failsafe() const
+{
+    return rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE;
+}
+
 bool RC_Channels_Rover::has_valid_input() const
 {
-    if (rover.failsafe.bits & FAILSAFE_EVENT_THROTTLE) {
+    if (in_rc_failsafe()) {
         return false;
     }
     return true;

Rover/RC_Channel.h

@@ -32,6 +32,7 @@ class RC_Channels_Rover : public RC_Channels
 
 public:
 
+    bool in_rc_failsafe() const override;
     bool has_valid_input() const override;
 
     RC_Channel *get_arming_channel(void) const override;

libraries/RC_Channel/RC_Channel.h

@@ -491,7 +491,7 @@ class RC_Channels {
     void reset_mode_switch();
     virtual void read_mode_switch();
 
-    // has_valid_input should be pure-virtual when Plane is converted
+    virtual bool in_rc_failsafe() const { return true; };
     virtual bool has_valid_input() const { return false; };
 
     virtual RC_Channel *get_arming_channel(void) const { return nullptr; };

libraries/SRV_Channel/SRV_Channel.h

@@ -507,6 +507,8 @@ class SRV_Channels {
     static AP_HAL::RCOutput::DshotEscType get_dshot_esc_type() { return AP_HAL::RCOutput::DshotEscType(_singleton->dshot_esc_type.get()); }
 
     static uint8_t get_dshot_rate() { return _singleton->dshot_rate.get(); }
+
+    static uint8_t get_rc_fs_mask() { return _singleton->rc_fs_mask.get(); }
 
     static SRV_Channel *srv_channel(uint8_t i) {
 #if NUM_SERVO_CHANNELS > 0
@@ -655,6 +657,7 @@ class SRV_Channels {
     AP_Int8 dshot_rate;
     AP_Int8 dshot_esc_type;
     AP_Int32 gpio_mask;
+    AP_Int32 rc_fs_mask;
 #if NUM_SERVO_CHANNELS >= 17
     AP_Int8 enable_32_channels;
 #endif

libraries/SRV_Channel/SRV_Channel_aux.cpp

@@ -20,6 +20,7 @@
 #include <AP_Math/AP_Math.h>
 #include <AP_HAL/AP_HAL.h>
 #include <RC_Channel/RC_Channel.h>
+#include <GCS_MAVLink/GCS.h>
 
 #if NUM_SERVO_CHANNELS == 0
 #pragma GCC diagnostic ignored "-Wtype-limits"
@@ -58,7 +59,11 @@ void SRV_Channel::output_ch(void)
                 // non-mapped rc passthrough
                 int16_t radio_in = c->get_radio_in();
                 if (passthrough_mapped) {
-                    radio_in = pwm_from_angle(c->norm_input_dz() * 4500);
+                    if ( ((1U<<passthrough_from) & SRV_Channels::get_rc_fs_mask()) && rc().in_rc_failsafe()) {
+                        radio_in = pwm_from_angle(0);
+                    } else {
+                        radio_in = pwm_from_angle(c->norm_input_dz() * 4500);
+                    }
                 }
                 if (!ign_small_rcin_changes) {
                     output_pwm = radio_in;

libraries/SRV_Channel/SRV_Channels.cpp

@@ -183,7 +183,7 @@ const AP_Param::GroupInfo SRV_Channels::var_info[] = {
 
     // @Param: _RATE
     // @DisplayName: Servo default output rate
-    // @Description: This sets the default output rate in Hz for all outputs.
+    // @Description: Default output rate in Hz for all PWM outputs.
     // @Range: 25 400
     // @User: Advanced
     // @Units: Hz
@@ -221,26 +221,35 @@ const AP_Param::GroupInfo SRV_Channels::var_info[] = {
 
     // @Param: _DSHOT_RATE
     // @DisplayName: Servo DShot output rate
-    // @Description: This sets the DShot output rate for all outputs as a multiple of the loop rate. 0 sets the output rate to be fixed at 1Khz for low loop rates. This value should never be set below 500Hz.
+    // @Description: DShot output rate for all outputs as a multiple of the loop rate. 0 sets the output rate to be fixed at 1Khz for low loop rates. This value should never be set below 500Hz.
     // @Values: 0:1Khz,1:loop-rate,2:double loop-rate,3:triple loop-rate,4:quadruple loop rate
     // @User: Advanced
     AP_GROUPINFO("_DSHOT_RATE",  23, SRV_Channels, dshot_rate, 0),
 
     // @Param: _DSHOT_ESC
     // @DisplayName: Servo DShot ESC type
-    // @Description: This sets the DShot ESC type for all outputs. The ESC type affects the range of DShot commands available and the bit widths used. None means that no dshot commands will be executed. Some ESC types support Extended DShot Telemetry (EDT) which allows telemetry other than RPM data to be returned when using bi-directional dshot. If you enable EDT you must install EDT capable firmware for correct operation.
+    // @Description: DShot ESC type for all outputs. The ESC type affects the range of DShot commands available and the bit widths used. None means that no dshot commands will be executed. Some ESC types support Extended DShot Telemetry (EDT) which allows telemetry other than RPM data to be returned when using bi-directional dshot. If you enable EDT you must install EDT capable firmware for correct operation.
     // @Values: 0:None,1:BLHeli32/Kiss,2:BLHeli_S,3:BLHeli32/Kiss+EDT,4:BLHeli_S+EDT
     // @User: Advanced
     AP_GROUPINFO("_DSHOT_ESC",  24, SRV_Channels, dshot_esc_type, 0),
 
     // @Param: _GPIO_MASK
     // @DisplayName: Servo GPIO mask
-    // @Description: This sets a bitmask of outputs which will be available as GPIOs. Any output with either the function set to -1 or with the corresponding bit set in this mask will be available for use as a GPIO pin
+    // @Description: Bitmask of outputs which will be available as GPIOs. Any output with either the function set to -1 or with the corresponding bit set in this mask will be available for use as a GPIO pin
     // @Bitmask: 0:Servo 1, 1:Servo 2, 2:Servo 3, 3:Servo 4, 4:Servo 5, 5:Servo 6, 6:Servo 7, 7:Servo 8, 8:Servo 9, 9:Servo 10, 10:Servo 11, 11:Servo 12, 12:Servo 13, 13:Servo 14, 14:Servo 15, 15:Servo 16, 16:Servo 17, 17:Servo 18, 18:Servo 19, 19:Servo 20, 20:Servo 21, 21:Servo 22, 22:Servo 23, 23:Servo 24, 24:Servo 25, 25:Servo 26, 26:Servo 27, 27:Servo 28, 28:Servo 29, 29:Servo 30, 30:Servo 31, 31:Servo 32
     // @User: Advanced
     // @RebootRequired: True
-    AP_GROUPINFO("_GPIO_MASK",  26, SRV_Channels, gpio_mask, 0),
-
+    AP_GROUPINFO("_GPIO_MASK",  26, SRV_Channels, gpio_mask, 0),  
+
+    // indexes 27-43 used by SERVO_32_ENABLEd group of params
+
+    // @Param: _RC_FS_MSK
+    // @DisplayName: Servo RC Failsafe Mask
+    // @Description: Bitmask of scaled passthru output function which will be set to their trim value during rc failsafe instead of holding their last position before failsafe.
+    // @Bitmask: 0:RCIN1Scaled, 1:RCIN2Scaled, 2:RCIN3Scaled, 3:RCIN4Scaled, 4:RCIN5Scaled, 5:RCIN6Scaled, 6:RCIN7Scaled, 7:RCIN8Scaled, 8:RCIN9Scaled, 9:RCIN10Scaled, 10:RCIN11Scaled, 11:SRCIN12Scaled, 12:RCIN13Scaled, 13:RCIN14Scaled, 14:RCIN15Scaled, 15:RCIN16Scaled
+    // @User: Advanced
+    AP_GROUPINFO("_RC_FS_MSK", 44, SRV_Channels, rc_fs_mask, 0),
+
 #if (NUM_SERVO_CHANNELS >= 17)
     // @Param: _32_ENABLE
     // @DisplayName: Enable outputs 17 to 31