Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Avoid nullptr dereference on bad rcmap value entry #28033

Merged
9 changes: 5 additions & 4 deletions ArduCopter/radio.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -20,10 +20,11 @@ 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);
// 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);
Expand Down
11 changes: 6 additions & 5 deletions ArduPlane/radio.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -8,16 +8,17 @@
*/
void Plane::set_control_channels(void)
{
// the library gaurantees that these are non-nullptr:
Copy link
Collaborator

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

Suggested change
// the library gaurantees that these are non-nullptr:
// the library guarantees 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);
Expand Down
9 changes: 5 additions & 4 deletions Blimp/radio.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
7 changes: 4 additions & 3 deletions Rover/radio.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -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);
Expand Down
5 changes: 5 additions & 0 deletions Tools/autotest/arduplane.py
Original file line number Diff line number Diff line change
Expand Up @@ -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()
Expand Down Expand Up @@ -6284,6 +6288,7 @@ def tests(self):
self.ForceArm,
self.MAV_CMD_EXTERNAL_WIND_ESTIMATE,
self.GliderPullup,
self.BadRollChannelDefined,
])
return ret

Expand Down
19 changes: 4 additions & 15 deletions libraries/AP_MSP/AP_MSP_Telem_Backend.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -25,7 +25,6 @@
#include <AP_Notify/AP_Notify.h>
#include <AP_OpticalFlow/AP_OpticalFlow.h>
#include <AP_RangeFinder/AP_RangeFinder.h>
#include <AP_RCMapper/AP_RCMapper.h>
#include <AP_RSSI/AP_RSSI.h>
#include <AP_RTC/AP_RTC.h>
#include <GCS_MAVLink/GCS.h>
Expand Down Expand Up @@ -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;
Expand All @@ -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

Expand Down
9 changes: 9 additions & 0 deletions libraries/RC_Channel/RC_Channel.h
Original file line number Diff line number Diff line change
Expand Up @@ -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() {
Expand Down Expand Up @@ -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();
Expand Down
34 changes: 34 additions & 0 deletions libraries/RC_Channel/RC_Channels.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -30,6 +30,7 @@ extern const AP_HAL::HAL& hal;

#include <AP_Math/AP_Math.h>
#include <AP_Logger/AP_Logger.h>
#include <AP_RCMapper/AP_RCMapper.h>

#include "RC_Channel.h"

Expand Down Expand Up @@ -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;

Expand Down
Loading