From 7973cabf98e7ce73337031041e4c0556ad804860 Mon Sep 17 00:00:00 2001 From: Jaakko Laurikainen Date: Sun, 9 Apr 2017 10:56:11 +0300 Subject: [PATCH] Yaw boost Changed the way how dynamic yaw works. It is now a linear curve from 100% at 0 throttle to tri_dynamic_yaw_maxthrottle at max throttle. Added a parameter to control the yaw motor boost. This parameter is a gain that is applied to the pitch correction when tail servo is deflected. This allows to increase the yaw authority at low throttle. --- src/main/fc/cli.c | 2 +- src/main/fc/config.c | 4 +- src/main/flight/mixer_tricopter.c | 75 +++++++------------------------ src/main/flight/mixer_tricopter.h | 2 +- 4 files changed, 21 insertions(+), 62 deletions(-) diff --git a/src/main/fc/cli.c b/src/main/fc/cli.c index 4decad862b2..70b8a6cbfd4 100755 --- a/src/main/fc/cli.c +++ b/src/main/fc/cli.c @@ -665,7 +665,7 @@ const clivalue_t valueTable[] = { { "tri_servo_feedback", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &triMixerConfig()->tri_servo_feedback, .config.lookup = { TABLE_SERVO_FEEDBACK }}, { "tri_motor_acc_yaw_correction",VAR_UINT16| MASTER_VALUE, &triMixerConfig()->tri_motor_acc_yaw_correction, .config.minmax = { 0, TRI_MOTOR_ACC_CORRECTION_MAX }}, { "tri_motor_acceleration", VAR_FLOAT | MASTER_VALUE, &triMixerConfig()->tri_motor_acceleration, .config.minmax = { 0.01f, 1.0f }}, - { "tri_dynamic_yaw_minthrottle",VAR_UINT16 | MASTER_VALUE, &triMixerConfig()->tri_dynamic_yaw_minthrottle, .config.minmax = { 0, 500}}, + { "tri_dynamic_yaw_boost", VAR_UINT16 | MASTER_VALUE, &triMixerConfig()->tri_yaw_boost, .config.minmax = { 0, 700}}, { "tri_dynamic_yaw_maxthrottle",VAR_UINT16 | MASTER_VALUE, &triMixerConfig()->tri_dynamic_yaw_maxthrottle, .config.minmax = { 0, 100}}, #endif diff --git a/src/main/fc/config.c b/src/main/fc/config.c index cf70ef60c31..2f9f226c45b 100755 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -272,8 +272,8 @@ void resetTriMixerConfig(triMixerConfig_t *triMixerConfig) triMixerConfig->tri_servo_min_adc = 0; triMixerConfig->tri_tail_motor_thrustfactor = 138; triMixerConfig->tri_tail_servo_speed = 300; // Default for BMS-210DMH at 5V - triMixerConfig->tri_dynamic_yaw_minthrottle = 500; - triMixerConfig->tri_dynamic_yaw_maxthrottle = 12; + triMixerConfig->tri_yaw_boost = 300; + triMixerConfig->tri_dynamic_yaw_maxthrottle = 38; } #endif diff --git a/src/main/flight/mixer_tricopter.c b/src/main/flight/mixer_tricopter.c index e033496744c..172d25df99f 100644 --- a/src/main/flight/mixer_tricopter.c +++ b/src/main/flight/mixer_tricopter.c @@ -62,7 +62,6 @@ #define TRI_TAIL_SERVO_ANGLE_MID (900) #define TRI_YAW_FORCE_CURVE_SIZE (100) #define TRI_TAIL_SERVO_MAX_ANGLE (500) -#define TRI_YAW_PITCH_CORRECTION_GAIN (2.5f) #define TRI_SERVO_SATURATION_DPS_ERROR_LIMIT (75.0f) static const uint8_t TRI_TAIL_MOTOR_INDEX = 0; @@ -82,17 +81,16 @@ static tailMotor_t tailMotor = { .accelerationDelay_ms = 30, .decelerationDelay_ static int16_t throttleRange = 0; static float throttleHalfRange; static float throttleMidPoint; -static float dynamicYawGainAtMin; +static float pitchCorrectionGain; static float dynamicYawGainAtMax; static triMixerConfig_t *gpTriMixerConfig; +static int16_t lastMotorCorrection = 0; static void initYawForceCurve(void); static uint16_t getServoValueAtAngle(servoParam_t *servoConf, uint16_t angle); static float getPitchCorrectionAtTailAngle(float angle, float thrustFactor); static uint16_t getAngleFromYawForceCurve(int32_t force); static uint16_t getServoAngle(servoParam_t *servoConf, uint16_t servoValue); -static uint16_t getPitchCorrectionMaxPhaseShift(int16_t servoAngle, int16_t servoSetpointAngle, - int16_t motorAccelerationDelayAngle, int16_t motorDecelerationDelayAngle, int16_t motorDirectionChangeAngle); static uint16_t getLinearServoValue(servoParam_t *servoConf, float scaledPIDOutput, float pidSumLimit); static uint16_t getNormalServoValue(servoParam_t *servoConf, float constrainedPIDOutput, float pidSumLimit); static uint16_t virtualServoStep(uint16_t currentAngle, int16_t servoSpeed, float dT, servoParam_t *servoConf, @@ -118,12 +116,11 @@ void triInitMixer(servoParam_t *pTailServoConfig, int16_t *pTailServoOutput) tailServo.angleAtMin = TRI_TAIL_SERVO_ANGLE_MID - tailServo.maxDeflection; tailServo.angleAtMax = TRI_TAIL_SERVO_ANGLE_MID + tailServo.maxDeflection; tailServo.speed = gpTriMixerConfig->tri_tail_servo_speed; - tailServo.saturationRange = tailServo.maxDeflection * TRI_SERVO_SATURATION_RANGE_IN_PERCENT; tailServo.saturated = false; throttleRange = motorConfig()->maxthrottle - motorConfig()->minthrottle; throttleHalfRange = throttleRange / 2.0f; throttleMidPoint = motorConfig()->minthrottle + throttleHalfRange; - dynamicYawGainAtMin = gpTriMixerConfig->tri_dynamic_yaw_minthrottle / 100.0f; + pitchCorrectionGain = gpTriMixerConfig->tri_yaw_boost / 100.0f; dynamicYawGainAtMax = gpTriMixerConfig->tri_dynamic_yaw_maxthrottle / 100.0f; tailMotor.acceleration = (float) throttleRange / gpTriMixerConfig->tri_motor_acceleration; initYawForceCurve(); @@ -220,20 +217,11 @@ int16_t triGetMotorCorrection(uint8_t motorIndex) // Take motor speed up lag into account by shifting the phase of the curve // Not taking into account the motor braking lag (yet) const uint16_t servoAngle = triGetCurrentServoAngle(); - const uint16_t servoSetpointAngle = getServoAngle(tailServo.pConf, *tailServo.pOutput); - - const uint16_t maxPhaseShift = getPitchCorrectionMaxPhaseShift(servoAngle, servoSetpointAngle, - tailMotor.accelerationDelay_angle, tailMotor.decelerationDelay_angle, tailMotor.pitchZeroAngle); - int16_t angleDiff = servoSetpointAngle - servoAngle; - if (ABS(angleDiff) > maxPhaseShift) { - angleDiff = (int32_t) maxPhaseShift * angleDiff / ABS(angleDiff); - } - const int16_t futureServoAngle = constrain(servoAngle + angleDiff, tailServo.angleAtMin, tailServo.angleAtMax); - - correction = (throttleRange * getPitchCorrectionAtTailAngle(DEGREES_TO_RADIANS(futureServoAngle / 10.0f), tailServo.thrustFactor)) - throttleRange; + correction = (throttleRange * getPitchCorrectionAtTailAngle(DEGREES_TO_RADIANS(servoAngle / 10.0f), tailServo.thrustFactor)) - throttleRange; // Multiply the correction to get more authority - correction *= TRI_YAW_PITCH_CORRECTION_GAIN; + correction *= pitchCorrectionGain; + lastMotorCorrection = correction; } return correction; @@ -353,25 +341,6 @@ static uint16_t getServoAngle(servoParam_t *servoConf, uint16_t servoValue) return servoAngle; } -static uint16_t getPitchCorrectionMaxPhaseShift(int16_t servoAngle, int16_t servoSetpointAngle, - int16_t motorAccelerationDelayAngle, int16_t motorDecelerationDelayAngle, int16_t motorDirectionChangeAngle) -{ - uint16_t maxPhaseShift; - - if (((servoAngle > servoSetpointAngle) && (servoAngle >= (motorDirectionChangeAngle + motorAccelerationDelayAngle))) - || ((servoAngle < servoSetpointAngle) - && (servoAngle <= (motorDirectionChangeAngle - motorAccelerationDelayAngle)))) { - // Motor is braking - maxPhaseShift = ABS(servoAngle - motorDirectionChangeAngle) >= motorDecelerationDelayAngle ? - motorDecelerationDelayAngle : ABS(servoAngle - motorDirectionChangeAngle); - } else { - // Motor is accelerating - maxPhaseShift = motorAccelerationDelayAngle; - } - - return maxPhaseShift; -} - static uint16_t virtualServoStep(uint16_t currentAngle, int16_t servoSpeed, float dT, servoParam_t *servoConf, uint16_t servoValue) { @@ -748,37 +717,27 @@ static void predictGyroOnDeceleration(void) static float previousMotorSpeed = 1000.0f; const float tailMotorSpeed = tailMotor.virtualFeedBack; // Calculate how much the motor speed changed since last time - const float acceleration = (tailMotorSpeed - previousMotorSpeed); + float acceleration = (tailMotorSpeed - previousMotorSpeed); previousMotorSpeed = tailMotorSpeed; - if (acceleration < 0.0f) { + float error = 0; + if (acceleration < 0.0f) + { // Tests have shown that this is mostly needed when throttle is cut (motor decelerating), so only // set the expected gyro error in that case. // Set the expected axis error based on tail motor acceleration and configured gain - pidSetExpectedGyroError(FD_YAW, acceleration * gpTriMixerConfig->tri_motor_acc_yaw_correction); - } else { - pidSetExpectedGyroError(FD_YAW, 0); + error = acceleration * gpTriMixerConfig->tri_motor_acc_yaw_correction * 10.0f; + error *= sin_approx(DEGREES_TO_RADIANS(triGetCurrentServoAngle() / 10.0f)); } + pidSetExpectedGyroError(FD_YAW, error); } static float scalePIDBasedOnTailMotorSpeed(float scaledPidOutput, float pidSumLimit) { - float gain; - float distanceFromMid; // Always positive, [0, halfRange] - - // Select the yaw gain based on tail motor speed - const float midpoint = throttleMidPoint; - if (tailMotor.virtualFeedBack < midpoint) { - gain = dynamicYawGainAtMin; - distanceFromMid = midpoint - tailMotor.virtualFeedBack; - } else { - gain = dynamicYawGainAtMax; - distanceFromMid = tailMotor.virtualFeedBack - midpoint; - } - - // Calculate gain at current distance from mid - gain = 1 + (distanceFromMid / throttleHalfRange) * (gain - 1); - + const float motorFeedbackWoCorrection = constrain(tailMotor.virtualFeedBack - lastMotorCorrection, motorConfig()->minthrottle, motorConfig()->maxthrottle); + const float fromMin = motorFeedbackWoCorrection - motorConfig()->minthrottle; + const float percentage = fromMin / (float)throttleRange; + const float gain = 1.0f - ((1.0f - dynamicYawGainAtMax) * percentage); const float pid = constrainf(scaledPidOutput * gain, -pidSumLimit, pidSumLimit); return pid; } diff --git a/src/main/flight/mixer_tricopter.h b/src/main/flight/mixer_tricopter.h index b186fcb6118..79d4a55fff1 100644 --- a/src/main/flight/mixer_tricopter.h +++ b/src/main/flight/mixer_tricopter.h @@ -99,7 +99,7 @@ typedef struct triMixerConfig_s{ uint16_t tri_motor_acc_yaw_correction; uint16_t dummy; float tri_motor_acceleration; - uint16_t tri_dynamic_yaw_minthrottle; + uint16_t tri_yaw_boost; uint16_t tri_dynamic_yaw_maxthrottle; } triMixerConfig_t;