From 78e44ebf3aa195e6971c908adadf3e1d678e4a52 Mon Sep 17 00:00:00 2001 From: Henry Wurzburg Date: Thu, 22 Jun 2023 18:10:42 -0500 Subject: [PATCH] SRV_Channel: allow scaled passthru to go to trim on rc failsafe --- libraries/SRV_Channel/SRV_Channel.h | 3 +++ libraries/SRV_Channel/SRV_Channel_aux.cpp | 7 ++++++- libraries/SRV_Channel/SRV_Channels.cpp | 19 ++++++++++++++----- 3 files changed, 23 insertions(+), 6 deletions(-) diff --git a/libraries/SRV_Channel/SRV_Channel.h b/libraries/SRV_Channel/SRV_Channel.h index c97098b517..ea90ce2d4a 100644 --- a/libraries/SRV_Channel/SRV_Channel.h +++ b/libraries/SRV_Channel/SRV_Channel.h @@ -507,6 +507,8 @@ public: 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 @@ private: 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 diff --git a/libraries/SRV_Channel/SRV_Channel_aux.cpp b/libraries/SRV_Channel/SRV_Channel_aux.cpp index bed75a2363..5b5fc63ef8 100644 --- a/libraries/SRV_Channel/SRV_Channel_aux.cpp +++ b/libraries/SRV_Channel/SRV_Channel_aux.cpp @@ -20,6 +20,7 @@ #include #include #include +#include #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<norm_input_dz() * 4500); + } } if (!ign_small_rcin_changes) { output_pwm = radio_in; diff --git a/libraries/SRV_Channel/SRV_Channels.cpp b/libraries/SRV_Channel/SRV_Channels.cpp index 2a84dcc130..ce1073a7b9 100644 --- a/libraries/SRV_Channel/SRV_Channels.cpp +++ b/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