diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 14163bee80..0528a7f5bc 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -80,6 +80,14 @@ void AP_MotorsTri::output_min() hal.rcout->write(pgm_read_byte(&_motor_to_channel_map[AP_MOTORS_CH_TRI_YAW]), _rc_yaw.radio_trim); } +// get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used) +// this can be used to ensure other pwm outputs (i.e. for servos) do not conflict +uint16_t AP_MotorsTri::get_motor_mask() +{ + // tri copter uses channels 1,2,4 and 7 + return (1U << 0 | 1U << 1 | 1U << 3 | 1U << AP_MOTORS_CH_TRI_YAW); +} + // output_armed - sends commands to the motors void AP_MotorsTri::output_armed() { diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h index c7522d6751..f457069a9e 100644 --- a/libraries/AP_Motors/AP_MotorsTri.h +++ b/libraries/AP_Motors/AP_MotorsTri.h @@ -41,6 +41,10 @@ public: // output_min - sends minimum values out to the motors virtual void output_min(); + // get_motor_mask - returns a bitmask of which outputs are being used for motors or servos (1 means being used) + // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict + virtual uint16_t get_motor_mask(); + protected: // output - sends commands to the motors virtual void output_armed();