From ea19907b999e461eea8fc0d6391e77fe50bdfe53 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 8 Sep 2024 00:16:37 +1000 Subject: [PATCH 1/8] autotest: add test we don't die with bad RC channel for roll --- Tools/autotest/arduplane.py | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/Tools/autotest/arduplane.py b/Tools/autotest/arduplane.py index 8506a3e478219..ea6f0a1e3827e 100644 --- a/Tools/autotest/arduplane.py +++ b/Tools/autotest/arduplane.py @@ -6151,6 +6151,10 @@ def GliderPullup(self): self.fly_home_land_and_disarm(timeout=400) + def BadRollChannelDefined(self): + '''ensure we don't die with a bad Roll channel defined''' + self.set_parameter("RCMAP_ROLL", 17) + def tests(self): '''return list of all tests''' ret = super(AutoTestPlane, self).tests() @@ -6284,6 +6288,7 @@ def tests(self): self.ForceArm, self.MAV_CMD_EXTERNAL_WIND_ESTIMATE, self.GliderPullup, + self.BadRollChannelDefined, ]) return ret From 4aedfac354326dcb8b4ad2abef8bfb39efb89f2f Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 8 Sep 2024 00:05:57 +1000 Subject: [PATCH 2/8] RC_Channel: avoid nullptr dereference on bad rcmap value entry --- libraries/RC_Channel/RC_Channel.h | 9 ++++++++ libraries/RC_Channel/RC_Channels.cpp | 34 ++++++++++++++++++++++++++++ 2 files changed, 43 insertions(+) diff --git a/libraries/RC_Channel/RC_Channel.h b/libraries/RC_Channel/RC_Channel.h index 34dfb90e92638..6e5c766e052c9 100644 --- a/libraries/RC_Channel/RC_Channel.h +++ b/libraries/RC_Channel/RC_Channel.h @@ -612,6 +612,12 @@ class RC_Channels { // get failsafe timeout in milliseconds uint32_t get_fs_timeout_ms() const { return MAX(_fs_timeout * 1000, 100); } + // methods which return RC input channels used for various axes. + RC_Channel &get_roll_channel(); + RC_Channel &get_pitch_channel(); + RC_Channel &get_yaw_channel(); + RC_Channel &get_throttle_channel(); + protected: void new_override_received() { @@ -653,6 +659,9 @@ class RC_Channels { void set_aux_cached(RC_Channel::AUX_FUNC aux_fn, RC_Channel::AuxSwitchPos pos); #endif + + RC_Channel &get_rcmap_channel_nonnull(uint8_t rcmap_number); + RC_Channel dummy_rcchannel; }; RC_Channels &rc(); diff --git a/libraries/RC_Channel/RC_Channels.cpp b/libraries/RC_Channel/RC_Channels.cpp index 9ec73c5dab365..cd32cf938ba3b 100644 --- a/libraries/RC_Channel/RC_Channels.cpp +++ b/libraries/RC_Channel/RC_Channels.cpp @@ -30,6 +30,7 @@ extern const AP_HAL::HAL& hal; #include #include +#include #include "RC_Channel.h" @@ -307,6 +308,39 @@ void RC_Channels::set_aux_cached(RC_Channel::AUX_FUNC aux_fn, RC_Channel::AuxSwi } #endif // AP_SCRIPTING_ENABLED +#if AP_RCMAPPER_ENABLED +// these methods return an RC_Channel pointers based on values from +// AP_::rcmap(). The return value is guaranteed to be not-null to +// allow use of the pointer without checking it for null-ness. If an +// invalid option has been chosen somehow then the returned channel +// will be a dummy channel. +RC_Channel &RC_Channels::get_rcmap_channel_nonnull(uint8_t rcmap_number) +{ + RC_Channel *ret = RC_Channels::rc_channel(rcmap_number-1); + if (ret != nullptr) { + return *ret; + } + return dummy_rcchannel; +} +RC_Channel &RC_Channels::get_roll_channel() +{ + return get_rcmap_channel_nonnull(AP::rcmap()->roll()); +}; +RC_Channel &RC_Channels::get_pitch_channel() +{ + return get_rcmap_channel_nonnull(AP::rcmap()->pitch()); +}; +RC_Channel &RC_Channels::get_throttle_channel() +{ + return get_rcmap_channel_nonnull(AP::rcmap()->throttle()); +}; +RC_Channel &RC_Channels::get_yaw_channel() +{ + return get_rcmap_channel_nonnull(AP::rcmap()->yaw()); +}; +#endif // AP_RCMAPPER_ENABLED + + // singleton instance RC_Channels *RC_Channels::_singleton; From 722b7eed6ce6dc5aa2f2622a4aab4a0520155004 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 8 Sep 2024 00:05:57 +1000 Subject: [PATCH 3/8] ArduPlane: avoid nullptr dereference on bad rcmap value entry --- ArduPlane/radio.cpp | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/ArduPlane/radio.cpp b/ArduPlane/radio.cpp index e601c94fde10e..3862b390b7245 100644 --- a/ArduPlane/radio.cpp +++ b/ArduPlane/radio.cpp @@ -8,16 +8,17 @@ */ void Plane::set_control_channels(void) { + // the library gaurantees that these are non-nullptr: if (g.rudder_only) { // in rudder only mode the roll and rudder channels are the // same. - channel_roll = RC_Channels::rc_channel(rcmap.yaw()-1); + channel_roll = &rc().get_yaw_channel(); } else { - channel_roll = RC_Channels::rc_channel(rcmap.roll()-1); + channel_roll = &rc().get_roll_channel(); } - channel_pitch = RC_Channels::rc_channel(rcmap.pitch()-1); - channel_throttle = RC_Channels::rc_channel(rcmap.throttle()-1); - channel_rudder = RC_Channels::rc_channel(rcmap.yaw()-1); + channel_pitch = &rc().get_pitch_channel(); + channel_throttle = &rc().get_throttle_channel(); + channel_rudder = &rc().get_yaw_channel(); // set rc channel ranges channel_roll->set_angle(SERVO_MAX); From 929595eae69ec57aae51226379b95840be421030 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 8 Sep 2024 00:20:05 +1000 Subject: [PATCH 4/8] ArduCopter: avoid nullptr dereference on bad rcmap value entry --- ArduCopter/radio.cpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index 49eeee371c90c..212763d03a7ad 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -20,10 +20,10 @@ void Copter::default_dead_zones() void Copter::init_rc_in() { - channel_roll = rc().channel(rcmap.roll()-1); - channel_pitch = rc().channel(rcmap.pitch()-1); - channel_throttle = rc().channel(rcmap.throttle()-1); - channel_yaw = rc().channel(rcmap.yaw()-1); + channel_roll = rc().get_roll_channel(); + channel_pitch = rc().get_pitch_channel(); + channel_throttle = rc().get_throttle_channel(); + channel_yaw = rc().get_yaw_channel(); // set rc channel ranges channel_roll->set_angle(ROLL_PITCH_YAW_INPUT_MAX); From b3802039edcd879998f7d08d528f92a11290a3b4 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 8 Sep 2024 00:22:48 +1000 Subject: [PATCH 5/8] ArduCopter: avoid nullptr dereference on bad rcmap value entry --- ArduCopter/radio.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/ArduCopter/radio.cpp b/ArduCopter/radio.cpp index 212763d03a7ad..b92178d0981ea 100644 --- a/ArduCopter/radio.cpp +++ b/ArduCopter/radio.cpp @@ -20,10 +20,11 @@ void Copter::default_dead_zones() void Copter::init_rc_in() { - channel_roll = rc().get_roll_channel(); - channel_pitch = rc().get_pitch_channel(); - channel_throttle = rc().get_throttle_channel(); - channel_yaw = rc().get_yaw_channel(); + // the library gaurantees that these are non-nullptr: + channel_roll = &rc().get_roll_channel(); + channel_pitch = &rc().get_pitch_channel(); + channel_throttle = &rc().get_throttle_channel(); + channel_yaw = &rc().get_yaw_channel(); // set rc channel ranges channel_roll->set_angle(ROLL_PITCH_YAW_INPUT_MAX); From c7b96492c0190e9f47bf97af1979141ce10db970 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 8 Sep 2024 00:22:49 +1000 Subject: [PATCH 6/8] Blimp: avoid nullptr dereference on bad rcmap value entry --- Blimp/radio.cpp | 9 +++++---- 1 file changed, 5 insertions(+), 4 deletions(-) diff --git a/Blimp/radio.cpp b/Blimp/radio.cpp index fddce3e914dd2..4b0c5a9a709e2 100644 --- a/Blimp/radio.cpp +++ b/Blimp/radio.cpp @@ -15,10 +15,11 @@ void Blimp::default_dead_zones() void Blimp::init_rc_in() { - channel_right = rc().channel(rcmap.roll()-1); - channel_front = rc().channel(rcmap.pitch()-1); - channel_up = rc().channel(rcmap.throttle()-1); - channel_yaw = rc().channel(rcmap.yaw()-1); + // the library gaurantees that these are non-nullptr: + channel_right = &rc().get_roll_channel(); + channel_front = &rc().get_pitch_channel(); + channel_up = &rc().get_throttle_channel(); + channel_yaw = &rc().get_yaw_channel(); // set rc channel ranges channel_right->set_angle(RC_SCALE); From 96d0f215fb388a73e97e5981cc9c7488bdba14a2 Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sun, 8 Sep 2024 00:22:49 +1000 Subject: [PATCH 7/8] Rover: avoid nullptr dereference on bad rcmap value entry --- Rover/radio.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/Rover/radio.cpp b/Rover/radio.cpp index cf1f159689c95..9fdb5c0c980f6 100644 --- a/Rover/radio.cpp +++ b/Rover/radio.cpp @@ -6,9 +6,10 @@ void Rover::set_control_channels(void) { // check change on RCMAP - channel_steer = rc().channel(rcmap.roll()-1); - channel_throttle = rc().channel(rcmap.throttle()-1); - channel_lateral = rc().channel(rcmap.yaw()-1); + // the library gaurantees that these are non-nullptr: + channel_steer = &rc().get_roll_channel(); + channel_throttle = &rc().get_throttle_channel(); + channel_lateral = &rc().get_yaw_channel(); // set rc channel ranges channel_steer->set_angle(SERVO_MAX); From 1089f6c668f7cb3f99591770ec2212a34acdb97c Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Mon, 9 Sep 2024 09:53:08 +1000 Subject: [PATCH 8/8] AP_MSP: avoid nullptr dereference on bad rcmap --- libraries/AP_MSP/AP_MSP_Telem_Backend.cpp | 19 ++++--------------- 1 file changed, 4 insertions(+), 15 deletions(-) diff --git a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp index 4f22e6e42d531..84cca6e1362c4 100644 --- a/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp +++ b/libraries/AP_MSP/AP_MSP_Telem_Backend.cpp @@ -25,7 +25,6 @@ #include #include #include -#include #include #include #include @@ -1068,17 +1067,10 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_rtc(sbuf_t *dst) #if AP_RC_CHANNEL_ENABLED MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_rc(sbuf_t *dst) { -#if AP_RCMAPPER_ENABLED - const RCMapper* rcmap = AP::rcmap(); - if (rcmap == nullptr) { - return MSP_RESULT_ERROR; - } - - // note: rcmap channels start at 1 - float roll = rc().rc_channel(rcmap->roll()-1)->norm_input_dz(); - float pitch = -rc().rc_channel(rcmap->pitch()-1)->norm_input_dz(); - float yaw = rc().rc_channel(rcmap->yaw()-1)->norm_input_dz(); - float throttle = rc().rc_channel(rcmap->throttle()-1)->norm_input_dz(); + float roll = rc().get_roll_channel().norm_input_dz(); + float pitch = -rc().get_pitch_channel().norm_input_dz(); + float yaw = rc().get_yaw_channel().norm_input_dz(); + float throttle = rc().get_throttle_channel().norm_input_dz(); const struct PACKED { uint16_t a; @@ -1095,9 +1087,6 @@ MSPCommandResult AP_MSP_Telem_Backend::msp_process_out_rc(sbuf_t *dst) sbuf_write_data(dst, &rc, sizeof(rc)); return MSP_RESULT_ACK; -#else - return MSP_RESULT_ERROR; -#endif } #endif // AP_RC_CHANNEL_ENABLED