From 8f8283e0f0283cb9caf7b0220dcca646d914626b Mon Sep 17 00:00:00 2001 From: Peter Barker Date: Sat, 2 Jan 2021 23:09:56 +1100 Subject: [PATCH] AP_Motors: rc_map_mask -> motor_mask_to_srv_channel_mask the naming of this function precedes our rc/srv-channel split --- libraries/AP_Motors/AP_MotorsCoax.cpp | 2 +- libraries/AP_Motors/AP_MotorsHeli_Single.cpp | 2 +- libraries/AP_Motors/AP_MotorsMatrix.cpp | 2 +- libraries/AP_Motors/AP_MotorsSingle.cpp | 2 +- libraries/AP_Motors/AP_MotorsTri.cpp | 2 +- libraries/AP_Motors/AP_Motors_Class.cpp | 4 ++-- libraries/AP_Motors/AP_Motors_Class.h | 9 ++++++++- 7 files changed, 15 insertions(+), 8 deletions(-) diff --git a/libraries/AP_Motors/AP_MotorsCoax.cpp b/libraries/AP_Motors/AP_MotorsCoax.cpp index dad88a9d35..2c6f7c238c 100644 --- a/libraries/AP_Motors/AP_MotorsCoax.cpp +++ b/libraries/AP_Motors/AP_MotorsCoax.cpp @@ -107,7 +107,7 @@ uint16_t AP_MotorsCoax::get_motor_mask() 1U << AP_MOTORS_MOT_4 | 1U << AP_MOTORS_MOT_5 | 1U << AP_MOTORS_MOT_6; - uint16_t mask = rc_map_mask(motor_mask); + uint16_t mask = motor_mask_to_srv_channel_mask(motor_mask); // add parent's mask mask |= AP_MotorsMulticopter::get_motor_mask(); diff --git a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp index c47980752f..e1a2b5c8a6 100644 --- a/libraries/AP_Motors/AP_MotorsHeli_Single.cpp +++ b/libraries/AP_Motors/AP_MotorsHeli_Single.cpp @@ -373,7 +373,7 @@ uint16_t AP_MotorsHeli_Single::get_motor_mask() mask |= 1U << AP_MOTORS_HELI_SINGLE_TAILRSC; } - return rc_map_mask(mask); + return motor_mask_to_srv_channel_mask(mask); } // update_motor_controls - sends commands to motor controllers diff --git a/libraries/AP_Motors/AP_MotorsMatrix.cpp b/libraries/AP_Motors/AP_MotorsMatrix.cpp index 0a7241e6c6..948fd4567c 100644 --- a/libraries/AP_Motors/AP_MotorsMatrix.cpp +++ b/libraries/AP_Motors/AP_MotorsMatrix.cpp @@ -117,7 +117,7 @@ uint16_t AP_MotorsMatrix::get_motor_mask() motor_mask |= 1U << i; } } - uint16_t mask = rc_map_mask(motor_mask); + uint16_t mask = motor_mask_to_srv_channel_mask(motor_mask); // add parent's mask mask |= AP_MotorsMulticopter::get_motor_mask(); diff --git a/libraries/AP_Motors/AP_MotorsSingle.cpp b/libraries/AP_Motors/AP_MotorsSingle.cpp index b2db48f370..b30d0b3171 100644 --- a/libraries/AP_Motors/AP_MotorsSingle.cpp +++ b/libraries/AP_Motors/AP_MotorsSingle.cpp @@ -111,7 +111,7 @@ uint16_t AP_MotorsSingle::get_motor_mask() 1U << AP_MOTORS_MOT_5 | 1U << AP_MOTORS_MOT_6; - uint16_t mask = rc_map_mask(motor_mask); + uint16_t mask = motor_mask_to_srv_channel_mask(motor_mask); // add parent's mask mask |= AP_MotorsMulticopter::get_motor_mask(); diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 5eda792826..bf5f34fbc9 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -129,7 +129,7 @@ uint16_t AP_MotorsTri::get_motor_mask() (1U << AP_MOTORS_MOT_2) | (1U << AP_MOTORS_MOT_4) | (1U << AP_MOTORS_CH_TRI_YAW); - uint16_t mask = rc_map_mask(motor_mask); + uint16_t mask = motor_mask_to_srv_channel_mask(motor_mask); // add parent's mask mask |= AP_MotorsMulticopter::get_motor_mask(); diff --git a/libraries/AP_Motors/AP_Motors_Class.cpp b/libraries/AP_Motors/AP_Motors_Class.cpp index b4642d85aa..9a689a6e56 100644 --- a/libraries/AP_Motors/AP_Motors_Class.cpp +++ b/libraries/AP_Motors/AP_Motors_Class.cpp @@ -102,7 +102,7 @@ void AP_Motors::rc_set_freq(uint32_t mask, uint16_t freq_hz) _motor_fast_mask |= mask; } - mask = rc_map_mask(mask); + mask = motor_mask_to_srv_channel_mask(mask); hal.rcout->set_freq(mask, freq_hz); switch (pwm_type(_pwm_type.get())) { @@ -142,7 +142,7 @@ void AP_Motors::rc_set_freq(uint32_t mask, uint16_t freq_hz) SERVOn_FUNCTION mappings, and allowing for multiple outputs per motor number */ -uint32_t AP_Motors::rc_map_mask(uint32_t mask) const +uint32_t AP_Motors::motor_mask_to_srv_channel_mask(uint32_t mask) const { uint32_t mask2 = 0; for (uint8_t i = 0; i < 32; i++) { diff --git a/libraries/AP_Motors/AP_Motors_Class.h b/libraries/AP_Motors/AP_Motors_Class.h index cad2e76646..f421d38b4f 100644 --- a/libraries/AP_Motors/AP_Motors_Class.h +++ b/libraries/AP_Motors/AP_Motors_Class.h @@ -210,7 +210,14 @@ protected: virtual void rc_write(uint8_t chan, uint16_t pwm); virtual void rc_write_angle(uint8_t chan, int16_t angle_cd); virtual void rc_set_freq(uint32_t mask, uint16_t freq_hz); - virtual uint32_t rc_map_mask(uint32_t mask) const; + + + /* + map an internal motor mask to real motor mask, accounting for + SERVOn_FUNCTION mappings, and allowing for multiple outputs per + motor number + */ + uint32_t motor_mask_to_srv_channel_mask(uint32_t mask) const; // add a motor to the motor map void add_motor_num(int8_t motor_num);