diff --git a/libraries/AP_Motors/AP_MotorsMulticopter.h b/libraries/AP_Motors/AP_MotorsMulticopter.h index ebc4619acc..9651a8258b 100644 --- a/libraries/AP_Motors/AP_MotorsMulticopter.h +++ b/libraries/AP_Motors/AP_MotorsMulticopter.h @@ -87,7 +87,7 @@ public: // output a thrust to all motors that match a given motor // mask. This is used to control tiltrotor motors in forward // flight. Thrust is in the range 0 to 1 - void output_motor_mask(float thrust, uint8_t mask); + virtual void output_motor_mask(float thrust, uint8_t mask); // get minimum or maximum pwm value that can be output to motors int16_t get_pwm_output_min() const; diff --git a/libraries/AP_Motors/AP_MotorsTri.cpp b/libraries/AP_Motors/AP_MotorsTri.cpp index 1713031740..48772c290f 100644 --- a/libraries/AP_Motors/AP_MotorsTri.cpp +++ b/libraries/AP_Motors/AP_MotorsTri.cpp @@ -320,3 +320,15 @@ void AP_MotorsTri::thrust_compensation(void) _thrust_rear = thrust[3]; } } + +/* + override tricopter tail servo output in output_motor_mask + */ +void AP_MotorsTri::output_motor_mask(float thrust, uint8_t mask) +{ + // normal multicopter output + AP_MotorsMulticopter::output_motor_mask(thrust, mask); + + // and override yaw servo + rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo->get_trim()); +} diff --git a/libraries/AP_Motors/AP_MotorsTri.h b/libraries/AP_Motors/AP_MotorsTri.h index 1efd27d4cb..fa893ea603 100644 --- a/libraries/AP_Motors/AP_MotorsTri.h +++ b/libraries/AP_Motors/AP_MotorsTri.h @@ -47,6 +47,11 @@ public: // this can be used to ensure other pwm outputs (i.e. for servos) do not conflict virtual uint16_t get_motor_mask(); + // output a thrust to all motors that match a given motor + // mask. This is used to control tiltrotor motors in forward + // flight. Thrust is in the range 0 to 1 + void output_motor_mask(float thrust, uint8_t mask) override; + protected: // output - sends commands to the motors void output_armed_stabilizing();