mirror of https://github.com/ArduPilot/ardupilot
AP_Motors: tricopter apply diffential thrust in forward flight
This commit is contained in:
parent
c26948ef59
commit
28884f60d5
|
@ -323,11 +323,29 @@ void AP_MotorsTri::thrust_compensation(void)
|
|||
/*
|
||||
override tricopter tail servo output in output_motor_mask
|
||||
*/
|
||||
void AP_MotorsTri::output_motor_mask(float thrust, uint8_t mask)
|
||||
void AP_MotorsTri::output_motor_mask(float thrust, uint8_t mask, float rudder_dt)
|
||||
{
|
||||
// normal multicopter output
|
||||
AP_MotorsMulticopter::output_motor_mask(thrust, mask);
|
||||
AP_MotorsMulticopter::output_motor_mask(thrust, mask, rudder_dt);
|
||||
|
||||
// and override yaw servo
|
||||
rc_write(AP_MOTORS_CH_TRI_YAW, _yaw_servo->get_trim());
|
||||
}
|
||||
|
||||
float AP_MotorsTri::get_roll_factor(uint8_t i)
|
||||
{
|
||||
float ret = 0.0f;
|
||||
|
||||
switch (i) {
|
||||
// right motor
|
||||
case AP_MOTORS_MOT_1:
|
||||
ret = -1.0f;
|
||||
break;
|
||||
// left motor
|
||||
case AP_MOTORS_MOT_2:
|
||||
ret = 1.0f;
|
||||
break;
|
||||
}
|
||||
|
||||
return ret;
|
||||
}
|
||||
|
|
|
@ -47,8 +47,13 @@ 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) override;
|
||||
|
||||
// rudder_dt applys diffential thrust for yaw in the range 0 to 1
|
||||
void output_motor_mask(float thrust, uint8_t mask, float rudder_dt) override;
|
||||
|
||||
// return the roll factor of any motor, this is used for tilt rotors and tail sitters
|
||||
// using copter motors for forward flight
|
||||
float get_roll_factor(uint8_t i) override;
|
||||
|
||||
protected:
|
||||
// output - sends commands to the motors
|
||||
void output_armed_stabilizing() override;
|
||||
|
|
Loading…
Reference in New Issue