From efec142671b8bc8a1a7e5dc6536886b89aed0023 Mon Sep 17 00:00:00 2001 From: Jaakko Laurikainen Date: Wed, 6 Apr 2016 17:58:56 +0300 Subject: [PATCH] Dynamic YAW output + motor acceleration correction Yaw PID output bypasses the servo mixer and is passed directly to tricopter mixer. New CLI params: tri_motor_acceleration (float) - Time in seconds it takes for motor to accelerate from min_throttle to max_throttle. tri_dynamic_yaw_maxthrottle - Dynamic yaw gain (0-500%) when motor is at max_throttle tri_dynamic_yaw_minthrottle - Dynamic yaw gain (0-500%) when motor is at in_throttle The gains in between are linearly interpolated. tri_motor_acc_yaw_correction - The correction that is applied as expected yaw DPS error when motor is decelerating or accelerating. More detailed description will follow in release notes. --- src/main/config/config.c | 17 ++- src/main/config/config.h | 1 + src/main/flight/mixer.c | 137 ++++++++++----------- src/main/flight/mixer.h | 2 + src/main/flight/mixer_tricopter.c | 192 +++++++++++++++++++++++++++--- src/main/flight/mixer_tricopter.h | 5 +- src/main/flight/pid.c | 27 ++++- src/main/flight/pid.h | 3 +- src/main/io/rc_controls.h | 5 +- src/main/io/serial_cli.c | 6 +- src/main/mw.c | 31 +---- 11 files changed, 295 insertions(+), 131 deletions(-) diff --git a/src/main/config/config.c b/src/main/config/config.c index dc7fc82e32e..7839b192792 100755 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -137,7 +137,7 @@ static uint32_t activeFeaturesLatch = 0; static uint8_t currentControlRateProfileIndex = 0; controlRateConfig_t *currentControlRateProfile; -static const uint8_t EEPROM_CONF_VERSION = 112; +static const uint8_t EEPROM_CONF_VERSION = 113; void resetPidProfile(pidProfile_t *pidProfile) { @@ -300,10 +300,10 @@ static void resetControlRateConfig(controlRateConfig_t *controlRateConfig) { controlRateConfig->rcExpo8 = 85; controlRateConfig->thrMid8 = 50; controlRateConfig->dynThrPID = 0; - controlRateConfig->tpa_yaw_rate = 90; controlRateConfig->rcYawExpo8 = 83; controlRateConfig->tpa_breakpoint = 1500; - controlRateConfig->tpa_yaw_breakpoint = 1500; + controlRateConfig->tri_dynamic_yaw_minthrottle = 300; // 3x YAW gain at min throttle + controlRateConfig->tri_dynamic_yaw_maxthrottle = 100; // no reduction by default controlRateConfig->rates[FD_PITCH] = 55; controlRateConfig->rates[FD_ROLL] = 55; @@ -335,8 +335,10 @@ static void resetMixerConfig(mixerConfig_t *mixerConfig) { mixerConfig->tri_servo_feedback = TRI_SERVO_FB_RSSI; #else mixerConfig->tri_servo_feedback = TRI_SERVO_FB_VIRTUAL; -#endif -#endif +#endif //RCE + mixerConfig->tri_motor_acc_yaw_correction = 6; + mixerConfig->tri_motor_acceleration = 0.20f; +#endif //USE_SERVOS } uint8_t getCurrentProfile(void) @@ -369,6 +371,11 @@ uint16_t getCurrentMinthrottle(void) return masterConfig.escAndServoConfig.minthrottle; } +uint16_t getCurrentMaxthrottle(void) +{ + return masterConfig.escAndServoConfig.maxthrottle; +} + // Default settings STATIC_UNIT_TESTED void resetConf(void) { diff --git a/src/main/config/config.h b/src/main/config/config.h index 26b00729d25..bbd97222a02 100644 --- a/src/main/config/config.h +++ b/src/main/config/config.h @@ -75,3 +75,4 @@ void changeControlRateProfile(uint8_t profileIndex); bool canSoftwareSerialBeUsed(void); uint16_t getCurrentMinthrottle(void); +uint16_t getCurrentMaxthrottle(void); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 55add840639..a7358712a57 100755 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -691,80 +691,81 @@ STATIC_UNIT_TESTED void servoMixer(void) static int16_t currentOutput[MAX_SERVO_RULES]; uint8_t i; - if (FLIGHT_MODE(PASSTHRU_MODE)) { - // Direct passthru from RX - input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL]; - input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH]; - input[INPUT_STABILIZED_YAW] = rcCommand[YAW]; - } else { - // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui - input[INPUT_STABILIZED_ROLL] = axisPID[ROLL]; - input[INPUT_STABILIZED_PITCH] = axisPID[PITCH]; - input[INPUT_STABILIZED_YAW] = axisPID[YAW]; - - // Reverse yaw servo when inverted in 3D mode - if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) { - input[INPUT_STABILIZED_YAW] *= -1; - } + if ((currentMixerMode == MIXER_TRI) || (currentMixerMode == MIXER_CUSTOM_TRI)) + { + triServoMixer(axisPID[YAW]); } - - input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500); - input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500); - - input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500] - - // center the RC input value around the RC middle value - // by subtracting the RC middle value from the RC input value, we get: - // data - middle = input - // 2000 - 1500 = +500 - // 1500 - 1500 = 0 - // 1000 - 1500 = -500 - input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig->midrc; - input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig->midrc; - input[INPUT_RC_YAW] = rcData[YAW] - rxConfig->midrc; - input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc; - input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig->midrc; - input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig->midrc; - input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig->midrc; - input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig->midrc; - - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) - servo[i] = 0; - - // mix servos according to rules - for (i = 0; i < servoRuleCount; i++) { - // consider rule if no box assigned or box is active - if (currentServoMixer[i].box == 0 || IS_RC_MODE_ACTIVE(BOXSERVO1 + currentServoMixer[i].box - 1)) { - uint8_t target = currentServoMixer[i].targetChannel; - uint8_t from = currentServoMixer[i].inputSource; - uint16_t servo_width = servoConf[target].max - servoConf[target].min; - int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2; - int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2; - - if (currentServoMixer[i].speed == 0) - currentOutput[i] = input[from]; - else { - if (currentOutput[i] < input[from]) - currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]); - else if (currentOutput[i] > input[from]) - currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]); - } - - servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max); + else + { + if (FLIGHT_MODE(PASSTHRU_MODE)) { + // Direct passthru from RX + input[INPUT_STABILIZED_ROLL] = rcCommand[ROLL]; + input[INPUT_STABILIZED_PITCH] = rcCommand[PITCH]; + input[INPUT_STABILIZED_YAW] = rcCommand[YAW]; } else { - currentOutput[i] = 0; + // Assisted modes (gyro only or gyro+acc according to AUX configuration in Gui + input[INPUT_STABILIZED_ROLL] = axisPID[ROLL]; + input[INPUT_STABILIZED_PITCH] = axisPID[PITCH]; + input[INPUT_STABILIZED_YAW] = axisPID[YAW]; + + // Reverse yaw servo when inverted in 3D mode + if (feature(FEATURE_3D) && (rcData[THROTTLE] < rxConfig->midrc)) { + input[INPUT_STABILIZED_YAW] *= -1; + } } - } - for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { - servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L; - servo[i] += determineServoMiddleOrForwardFromChannel(i); + input[INPUT_GIMBAL_PITCH] = scaleRange(attitude.values.pitch, -1800, 1800, -500, +500); + input[INPUT_GIMBAL_ROLL] = scaleRange(attitude.values.roll, -1800, 1800, -500, +500); + + input[INPUT_STABILIZED_THROTTLE] = motor[0] - 1000 - 500; // Since it derives from rcCommand or mincommand and must be [-500:+500] + + // center the RC input value around the RC middle value + // by subtracting the RC middle value from the RC input value, we get: + // data - middle = input + // 2000 - 1500 = +500 + // 1500 - 1500 = 0 + // 1000 - 1500 = -500 + input[INPUT_RC_ROLL] = rcData[ROLL] - rxConfig->midrc; + input[INPUT_RC_PITCH] = rcData[PITCH] - rxConfig->midrc; + input[INPUT_RC_YAW] = rcData[YAW] - rxConfig->midrc; + input[INPUT_RC_THROTTLE] = rcData[THROTTLE] - rxConfig->midrc; + input[INPUT_RC_AUX1] = rcData[AUX1] - rxConfig->midrc; + input[INPUT_RC_AUX2] = rcData[AUX2] - rxConfig->midrc; + input[INPUT_RC_AUX3] = rcData[AUX3] - rxConfig->midrc; + input[INPUT_RC_AUX4] = rcData[AUX4] - rxConfig->midrc; + + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) + servo[i] = 0; + + // mix servos according to rules + for (i = 0; i < servoRuleCount; i++) { + // consider rule if no box assigned or box is active + if (currentServoMixer[i].box == 0 || IS_RC_MODE_ACTIVE(BOXSERVO1 + currentServoMixer[i].box - 1)) { + uint8_t target = currentServoMixer[i].targetChannel; + uint8_t from = currentServoMixer[i].inputSource; + uint16_t servo_width = servoConf[target].max - servoConf[target].min; + int16_t min = currentServoMixer[i].min * servo_width / 100 - servo_width / 2; + int16_t max = currentServoMixer[i].max * servo_width / 100 - servo_width / 2; + + if (currentServoMixer[i].speed == 0) + currentOutput[i] = input[from]; + else { + if (currentOutput[i] < input[from]) + currentOutput[i] = constrain(currentOutput[i] + currentServoMixer[i].speed, currentOutput[i], input[from]); + else if (currentOutput[i] > input[from]) + currentOutput[i] = constrain(currentOutput[i] - currentServoMixer[i].speed, input[from], currentOutput[i]); + } - } + servo[target] += servoDirection(target, from) * constrain(((int32_t)currentOutput[i] * currentServoMixer[i].rate) / 100, min, max); + } else { + currentOutput[i] = 0; + } + } - if (currentMixerMode == MIXER_TRI || currentMixerMode == MIXER_CUSTOM_TRI) - { - triServoMixer(); + for (i = 0; i < MAX_SUPPORTED_SERVOS; i++) { + servo[i] = ((int32_t)servoConf[i].rate * servo[i]) / 100L; + servo[i] += determineServoMiddleOrForwardFromChannel(i); + } } } #endif diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index c01a61b787e..2aebd50c17d 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -89,6 +89,8 @@ typedef struct mixerConfig_s { uint16_t tri_servo_min_adc; uint16_t tri_servo_mid_adc; uint16_t tri_servo_max_adc; + uint16_t tri_motor_acc_yaw_correction; + float tri_motor_acceleration; #endif } mixerConfig_t; diff --git a/src/main/flight/mixer_tricopter.c b/src/main/flight/mixer_tricopter.c index 19c59ef7273..0daddaeab2c 100644 --- a/src/main/flight/mixer_tricopter.c +++ b/src/main/flight/mixer_tricopter.c @@ -74,6 +74,7 @@ #include "flight/failsafe.h" #include "flight/imu.h" #include "flight/navigation.h" +#include "flight/pid.h" #include "config/runtime_config.h" #include "config/config.h" @@ -82,10 +83,16 @@ #include "mw.h" + #ifdef USE_SERVOS -#define TRI_TAIL_SERVO_ANGLE_MID (900) -#define TRI_YAW_FORCE_CURVE_SIZE (100) -#define TRI_TAIL_SERVO_MAX_ANGLE (500) +#define USE_AUX_CHANNEL_TUNING (0) +#define TRI_TAIL_SERVO_ANGLE_MID (900) +#define TRI_YAW_FORCE_CURVE_SIZE (100) +#define TRI_TAIL_SERVO_MAX_ANGLE (500) + +#define TRI_TAIL_MOTOR_INDEX (0) + +#define TRI_YAW_FORCE_PRECISION (1000) #define IsDelayElapsed_us(timestamp_us, delay_us) ((uint32_t)(micros() - timestamp_us) >= delay_us) #define IsDelayElapsed_ms(timestamp_ms, delay_ms) ((uint32_t)(millis() - timestamp_ms) >= delay_ms) @@ -109,13 +116,21 @@ static int16_t tailMotorDecelerationDelay_ms = 100; static int16_t tailMotorAccelerationDelay_angle; static int16_t tailMotorDecelerationDelay_angle; +//! Virtual tail motor speed feedback +static float tailMotorVirtual = 1000.0f; +//! Configured output throttle range (max - min) +static int16_t throttleRange = 0; +//! Motor acceleration in output units (us) / second +static float motorAcceleration = 0; +//! Reset the I term when tail motor deceleration has lasted (ms) +static uint16_t resetITermDecelerationLasted_ms = 0; + static servoParam_t * gpTailServoConf; static int16_t *gpTailServo; static mixerConfig_t *gpMixerConfig; static uint16_t tailServoADC = 0; static AdcChannel tailServoADCChannel = ADC_EXTERNAL1; - static void initCurves(); static uint16_t getServoValueAtAngle(servoParam_t * servoConf, uint16_t angle); static float getPitchCorrectionAtTailAngle(float angle, float thrustFactor); @@ -126,7 +141,7 @@ static uint16_t getPitchCorrectionMaxPhaseShift(int16_t servoAngle, int16_t motorAccelerationDelayAngle, int16_t motorDecelerationDelayAngle, int16_t motorDirectionChangeAngle); -static uint16_t getLinearServoValue(servoParam_t *servoConf, uint16_t servoValue); +static uint16_t getLinearServoValue(servoParam_t *servoConf, int16_t constrainedPIDOutput); static uint16_t virtualServoStep(uint16_t currentAngle, int16_t servoSpeed, float dT, servoParam_t *servoConf, uint16_t servoValue); static uint16_t feedbackServoStep(mixerConfig_t *mixerConf, uint16_t tailServoADC); STATIC_UNIT_TESTED void tailTuneModeThrustTorque(thrustTorque_t *pTT, const bool isThrottleHigh); @@ -134,6 +149,12 @@ static void tailTuneModeServoSetup(struct servoSetup_t *pSS, servoParam_t *pServ static void triTailTuneStep(servoParam_t *pServoConf, int16_t *pServoVal); static void updateServoAngle(void); static void updateServoFeedbackADCChannel(uint8_t tri_servo_feedback); +static void checkMotorAcceleration(void); +#if USE_AUX_CHANNEL_TUNING +static int16_t scaleAUXChannel(u8 channel, int16_t scale); +#endif +static int16_t dynamicYaw(int16_t PIDoutput); +static void tailMotorStep(int16_t setpoint, float dT); #endif void triInitMixer(servoParam_t *pTailServoConfig, @@ -147,6 +168,12 @@ void triInitMixer(servoParam_t *pTailServoConfig, tailServoSpeed = pMixerConfig->tri_tail_servo_speed; gpMixerConfig = pMixerConfig; + throttleRange = getCurrentMaxthrottle() - getCurrentMinthrottle(); + motorAcceleration = (float)throttleRange / gpMixerConfig->tri_motor_acceleration; + + // Reset the I term when motor deceleration has lasted 35% of the min to max time + resetITermDecelerationLasted_ms = gpMixerConfig->tri_motor_acceleration * 1000.0f * 0.35f; + initCurves(); updateServoFeedbackADCChannel(gpMixerConfig->tri_servo_feedback); } @@ -169,7 +196,7 @@ static void initCurves() for (int32_t i = 0; i < TRI_YAW_FORCE_CURVE_SIZE; i++) { const float angleRad = DEGREES_TO_RADIANS(angle / 10.0f); - yawForceCurve[i] = 1000.0f * (-tailServoThrustFactor * cosf(angleRad) - sinf(angleRad)) * getPitchCorrectionAtTailAngle(angleRad, tailServoThrustFactor); + yawForceCurve[i] = TRI_YAW_FORCE_PRECISION * (-tailServoThrustFactor * cosf(angleRad) - sinf(angleRad)) * getPitchCorrectionAtTailAngle(angleRad, tailServoThrustFactor); // Only calculate the top forces in the configured angle range if ((angle >= minAngle) && (angle <= maxAngle)) { @@ -187,20 +214,20 @@ uint16_t triGetCurrentServoAngle() return tailServoAngle; } -static uint16_t getLinearServoValue(servoParam_t *servoConf, uint16_t servoValue) +static uint16_t getLinearServoValue(servoParam_t *servoConf, int16_t constrainedPIDOutput) { - const int16_t servoMid = servoConf->middle; - // First find the yaw force at given servo value from a linear curve - const int16_t servoRange = (servoValue < servoMid) ? servoMid - servoConf->min : servoConf->max - servoMid; - const int32_t linearYawForceAtValue = (int32_t)(tailServoMaxYawForce) * (servoValue - servoMid) / servoRange; + const int32_t linearYawForceAtValue = tailServoMaxYawForce * constrainedPIDOutput / TRI_YAW_FORCE_PRECISION; const int16_t correctedAngle = getAngleFromYawCurveAtForce(linearYawForceAtValue); - debug[2] = servoValue; - debug[3] = tailServoMaxYawForce; return getServoValueAtAngle(servoConf, correctedAngle); } -void triServoMixer() +void triServoMixer(int16_t PIDoutput) { + // Dynamic yaw expects input [-1000, 1000] + PIDoutput = constrain(PIDoutput, -1000, 1000); + // Scale the PID output based on tail motor speed (thrust) + PIDoutput = dynamicYaw(PIDoutput); + static filterStatePt1_t feedbackFilter; if (gpMixerConfig->tri_servo_feedback != TRI_SERVO_FB_VIRTUAL) { @@ -211,18 +238,24 @@ void triServoMixer() // Linear servo logic only in armed state if (ARMING_FLAG(ARMED)) { - *gpTailServo = getLinearServoValue(gpTailServoConf, *gpTailServo); + *gpTailServo = getLinearServoValue(gpTailServoConf, PIDoutput); } triTailTuneStep(gpTailServoConf, gpTailServo); updateServoAngle(); + + // Update the tail motor virtual feedback + tailMotorStep(motor[TRI_TAIL_MOTOR_INDEX], dT); + + // Check the tail motor acceleration + checkMotorAcceleration(); } int16_t triGetMotorCorrection(uint8_t motorIndex) { uint16_t correction = 0; - if (motorIndex == 0) + if (motorIndex == TRI_TAIL_MOTOR_INDEX) { // Adjust tail motor speed based on servo angle. Check how much to adjust speed from pitch force curve based on servo angle. // Take motor speed up lag into account by shifting the phase of the curve @@ -239,7 +272,15 @@ int16_t triGetMotorCorrection(uint8_t motorIndex) } const int16_t futureServoAngle = constrain(servoAngle + angleDiff, TRI_TAIL_SERVO_ANGLE_MID - tailServoMaxAngle, TRI_TAIL_SERVO_ANGLE_MID + tailServoMaxAngle); - const uint16_t throttleMotorOutput = rcCommand[THROTTLE] - getCurrentMinthrottle(); + uint16_t throttleMotorOutput = tailMotorVirtual - getCurrentMinthrottle(); + /* Increased yaw authority at min throttle, always calculate the pitch + * correction on at least half motor output. This produces a little bit + * more forward pitch, but tested to be negligible. + * + * TODO: this is not the best way to achieve this, but how could the min_throttle + * pitch correction be calculated, as the thrust is zero? + */ + throttleMotorOutput = constrain(throttleMotorOutput, throttleRange / 2, 1000); correction = (throttleMotorOutput * getPitchCorrectionAtTailAngle(DEGREES_TO_RADIANS(futureServoAngle / 10.0f), tailServoThrustFactor)) - throttleMotorOutput; } @@ -767,3 +808,120 @@ static void updateServoFeedbackADCChannel(uint8_t tri_servo_feedback) break; } } + +#if USE_AUX_CHANNEL_TUNING +static int16_t scaleAUXChannel(u8 channel, int16_t scale) +{ + int16_t constrained = constrain(rcData[channel], 1000, 2000); + constrained -= 1000; + + return constrained * scale / 1000; +} +#endif + +static void checkMotorAcceleration(void) +{ + static float previousMotorSpeed = 1000.0f; + static uint32_t accelerationStartedAt_ms = 0; + static bool accelerating = false; + + float tailMotorSpeed = tailMotorVirtual; + // Calculate how much the motor speed changed since last time + float acceleration = (tailMotorSpeed - previousMotorSpeed); + previousMotorSpeed = tailMotorSpeed; + + // Check if acceleration has changed to deceleration and vice versa + if (acceleration > 0.0f) + { + if (!accelerating) + { + // Take a timestamp when direction changes + accelerationStartedAt_ms = millis(); + accelerating = true; + } + } + else + { + if (accelerating) + { + accelerationStartedAt_ms = millis(); + accelerating = false; + } + } + + /* Reset the I term in a dynamic situation where the motor is causing yaw error for longer period of time. + * This helps especially when throttle is cut, the copter is quickly losing yaw authority and we can't + * have the slow integrator resisting the correction. + * + * Tests have shown that this is mostly needed when throttle is cut (motor decelerating), so only + * reset I term in that case. + */ + if (!accelerating && IsDelayElapsed_ms(accelerationStartedAt_ms, resetITermDecelerationLasted_ms)) + { + pidResetErrorGyroAxis(FD_YAW); + } + + // Set the expected axis error based on tail motor acceleration and configured gain + pidSetExpectedGyroError(FD_YAW, acceleration * gpMixerConfig->tri_motor_acc_yaw_correction / 10); +} + +static int16_t dynamicYaw(int16_t PIDoutput) +{ + const int16_t halfRange = throttleRange / 2; + const int16_t midpoint = getCurrentMinthrottle() + halfRange; + + uint16_t gain; + // Select the yaw gain based on tail motor speed + if (tailMotorVirtual < midpoint) + { + /* Below midpoint, gain is increasing the output. + * e.g. 150 (%) increases the yaw output at min throttle by 150 % (1.5x) + * e.g. 250 (%) increases the yaw output at min throttle by 250 % (2.5x) + */ + gain = currentControlRateProfile->tri_dynamic_yaw_minthrottle - 100; + } + else + { + /* Above midpoint, gain is decreasing the output. + * e.g. 75 (%) reduces the yaw output at max throttle by 25 % (0.75x) + * e.g. 20 (%) reduces the yaw output at max throttle by 80 % (0.2x) + */ + gain = 100 - currentControlRateProfile->tri_dynamic_yaw_maxthrottle; + } + + const int16_t distanceFromMid = (tailMotorVirtual - midpoint); + const int16_t scaledPIDoutput = (PIDoutput - distanceFromMid * gain * PIDoutput / (halfRange * 100)); + return constrain(scaledPIDoutput, -1000, 1000); +} + +static void tailMotorStep(int16_t setpoint, float dT) +{ + static float current = 1000; + float dS; // Max change of an speed since last check + + dS = dT * motorAcceleration; + + if ( ABS(current - setpoint) < dS ) + { + // At set-point after this moment + current = setpoint; + } + else if (current < setpoint) + { + current += dS; + } + else + { + current -= dS; + } + + // Use a PT1 low-pass filter to add "slowness" to the virtual motor feedback. + /* Cut-off to delay: + * 2 Hz -> 25 ms + * 5 Hz -> 14 ms + * 10 Hz -> 9 ms + */ + static filterStatePt1_t motorFilter; + tailMotorVirtual = filterApplyPt1(current, &motorFilter, 20, dT); +} + diff --git a/src/main/flight/mixer_tricopter.h b/src/main/flight/mixer_tricopter.h index a33b53269ba..19d082e2ca1 100644 --- a/src/main/flight/mixer_tricopter.h +++ b/src/main/flight/mixer_tricopter.h @@ -24,6 +24,8 @@ #define TAIL_THRUST_FACTOR_MIN_FLOAT (TAIL_THRUST_FACTOR_MIN / 10.0f) #define TAIL_THRUST_FACTOR_MAX_FLOAT (TAIL_THRUST_FACTOR_MAX / 10.0f) +#define TRI_MOTOR_ACC_CORRECTION_MAX (200) + /** @brief Servo feedback sources. */ typedef enum { TRI_SERVO_FB_VIRTUAL = 0, ///< Virtual servo, no physical feedback signal from servo @@ -52,9 +54,10 @@ uint16_t triGetCurrentServoAngle(); /** @brief Perform tricopter mixer actions. * + * @param PIDoutput output from PID controller, in scale of [-1000, 1000]. * @return Void. */ -void triServoMixer(); +void triServoMixer(int16_t PIDoutput); /** @brief Get amount of motor correction that must be applied * for given motor. diff --git a/src/main/flight/pid.c b/src/main/flight/pid.c index a1fff9326cc..f42915282b1 100644 --- a/src/main/flight/pid.c +++ b/src/main/flight/pid.c @@ -51,9 +51,11 @@ #include "config/runtime_config.h" #include "config/config_unittest.h" +#include "debug.h" + //! Integrator is disabled when rate error exceeds this limit -#define LUXFLOAT_INTEGRATOR_DISABLE_LIMIT_DPS (40.0f) -#define REWRITE_INTEGRATOR_DISABLE_LIMIT_DPS (160) +#define LUXFLOAT_INTEGRATOR_DISABLE_LIMIT_DPS (50.0f) +#define REWRITE_INTEGRATOR_DISABLE_LIMIT_DPS (50 * 41 / 10) extern uint8_t motorCount; extern float dT; @@ -72,6 +74,8 @@ static float errorGyroIf[3]; static int32_t errorAngleI[2]; static float errorAngleIf[2]; +static int16_t expectedGyroError[3] = {0}; + static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *controlRateConfig, uint16_t max_angle_inclination, rollAndPitchTrims_t *angleTrim, rxConfig_t *rxConfig); @@ -98,6 +102,17 @@ void pidResetErrorGyro(void) } } +void pidResetErrorGyroAxis(flight_dynamics_index_t axis) +{ + errorGyroI[axis] = 0; + errorGyroIf[axis] = 0.0f; +} + +void pidSetExpectedGyroError(flight_dynamics_index_t axis, int16_t error) +{ + expectedGyroError[axis] = error; +} + float scaleItermToRcInput(int axis) { float rcCommandDeflection = (float)rcCommand[axis] / 500.0f; static float iTermScaler[3] = {1.0f, 1.0f, 1.0f}; @@ -209,14 +224,14 @@ static void pidLuxFloat(pidProfile_t *pidProfile, controlRateConfig_t *controlRa // Used in stand-alone mode for ACRO, controlled by higher level regulators in other modes // -----calculate scaled error.AngleRates // multiplication of rcCommand corresponds to changing the sticks scaling here - RateError = AngleRate - gyroRate; + RateError = AngleRate - gyroRate + (float)expectedGyroError[axis]; // -----calculate P component PTerm = RateError * pidProfile->P_f[axis] * PIDweight[axis] / 100; const float newIntegral = constrainf(errorGyroIf[axis] + RateError * dT * pidProfile->I_f[axis] * 10 * Iweigth[axis] / 100, -PID_LUX_FLOAT_MAX_I, PID_LUX_FLOAT_MAX_I); - if (fabsf(RateError) < LUXFLOAT_INTEGRATOR_DISABLE_LIMIT_DPS) + if (fabsf(gyroRate) < LUXFLOAT_INTEGRATOR_DISABLE_LIMIT_DPS) { errorGyroIf[axis] = newIntegral; } @@ -496,7 +511,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co // -----calculate scaled error.AngleRates // multiplication of rcCommand corresponds to changing the sticks scaling here gyroRate = gyroADC[axis] / 4; - RateError = AngleRateTmp - gyroRate; + RateError = AngleRateTmp - gyroRate + (expectedGyroError[axis] * 41); // -----calculate P component PTerm = (RateError * pidProfile->P8[axis] * PIDweight[axis] / 100) >> 7; @@ -511,7 +526,7 @@ static void pidMultiWiiRewrite(pidProfile_t *pidProfile, controlRateConfig_t *co // I coefficient (I8) moved before integration to make limiting independent from PID settings newIntegral = constrain(newIntegral, (int32_t) - GYRO_I_MAX << 13, (int32_t) + GYRO_I_MAX << 13); - if (ABS(RateError) < REWRITE_INTEGRATOR_DISABLE_LIMIT_DPS) + if (ABS(gyroRate) < REWRITE_INTEGRATOR_DISABLE_LIMIT_DPS) { errorGyroI[axis] = newIntegral; } diff --git a/src/main/flight/pid.h b/src/main/flight/pid.h index ccf406b1c07..e40ce027987 100644 --- a/src/main/flight/pid.h +++ b/src/main/flight/pid.h @@ -86,4 +86,5 @@ extern int32_t axisPID_P[3], axisPID_I[3], axisPID_D[3]; void pidSetController(pidControllerType_e type); void pidResetErrorAngle(void); void pidResetErrorGyro(void); - +void pidResetErrorGyroAxis(flight_dynamics_index_t axis); +void pidSetExpectedGyroError(flight_dynamics_index_t axis, int16_t error); diff --git a/src/main/io/rc_controls.h b/src/main/io/rc_controls.h index 7db68fd66d3..31a5c2cbc60 100644 --- a/src/main/io/rc_controls.h +++ b/src/main/io/rc_controls.h @@ -114,7 +114,6 @@ typedef enum { #define CONTROL_RATE_CONFIG_YAW_RATE_MAX 255 #define CONTROL_RATE_CONFIG_TPA_MAX 100 -#define CONTROL_RATE_CONFIG_YAW_TPA_MAX 255 // steps are 25 apart // a value of 0 corresponds to a channel value of 900 or less @@ -140,10 +139,10 @@ typedef struct controlRateConfig_s { uint8_t thrExpo8; uint8_t rates[3]; uint8_t dynThrPID; - uint8_t tpa_yaw_rate; uint8_t rcYawExpo8; uint16_t tpa_breakpoint; // Breakpoint where TPA is activated - uint16_t tpa_yaw_breakpoint; + uint16_t tri_dynamic_yaw_minthrottle; + uint16_t tri_dynamic_yaw_maxthrottle; } controlRateConfig_t; extern int16_t rcCommand[4]; diff --git a/src/main/io/serial_cli.c b/src/main/io/serial_cli.c index 17d16739c8a..f9d4897f357 100644 --- a/src/main/io/serial_cli.c +++ b/src/main/io/serial_cli.c @@ -608,6 +608,8 @@ const clivalue_t valueTable[] = { { "tri_tail_motor_thrustfactor",VAR_INT16 | MASTER_VALUE, &masterConfig.mixerConfig.tri_tail_motor_thrustfactor, .config.minmax = { TAIL_THRUST_FACTOR_MIN, TAIL_THRUST_FACTOR_MAX } }, { "tri_tail_servo_speed", VAR_INT16 | MASTER_VALUE, &masterConfig.mixerConfig.tri_tail_servo_speed, .config.minmax = { 0, 1000 } }, { "tri_servo_feedback", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, &masterConfig.mixerConfig.tri_servo_feedback, .config.lookup = { TABLE_SERVO_FEEDBACK } }, + { "tri_motor_acc_yaw_correction",VAR_UINT16| MASTER_VALUE, &masterConfig.mixerConfig.tri_motor_acc_yaw_correction, .config.minmax = { 0, TRI_MOTOR_ACC_CORRECTION_MAX } }, + { "tri_motor_acceleration", VAR_FLOAT | MASTER_VALUE, &masterConfig.mixerConfig.tri_motor_acceleration, .config.minmax = { 0.01f, 1.0f } }, #endif { "default_rate_profile", VAR_UINT8 | PROFILE_VALUE , &masterConfig.profile[0].defaultRateProfileIndex, .config.minmax = { 0, MAX_CONTROL_RATE_PROFILE_COUNT - 1 } }, @@ -621,8 +623,8 @@ const clivalue_t valueTable[] = { { "yaw_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].rates[FD_YAW], .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_RATE_MAX } }, { "tpa_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].dynThrPID, .config.minmax = { 0, CONTROL_RATE_CONFIG_TPA_MAX} }, { "tpa_breakpoint", VAR_UINT16 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].tpa_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} }, - { "tpa_yaw_rate", VAR_UINT8 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].tpa_yaw_rate, .config.minmax = { 0, CONTROL_RATE_CONFIG_YAW_TPA_MAX} }, - { "tpa_yaw_breakpoint", VAR_UINT16 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].tpa_yaw_breakpoint, .config.minmax = { PWM_RANGE_MIN, PWM_RANGE_MAX} }, + { "tri_dynamic_yaw_minthrottle",VAR_UINT16 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].tri_dynamic_yaw_minthrottle, .config.minmax = { 0, 500} }, + { "tri_dynamic_yaw_maxthrottle",VAR_UINT16 | CONTROL_RATE_VALUE, &masterConfig.controlRateProfiles[0].tri_dynamic_yaw_maxthrottle, .config.minmax = { 0, 100} }, { "failsafe_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_delay, .config.minmax = { 0, 200 } }, { "failsafe_off_delay", VAR_UINT8 | MASTER_VALUE, &masterConfig.failsafeConfig.failsafe_off_delay, .config.minmax = { 0, 200 } }, diff --git a/src/main/mw.c b/src/main/mw.c index 437781f43d4..33d9b17792f 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -173,7 +173,7 @@ bool isCalibrating() void annexCode(void) { int32_t tmp, tmp2; - int32_t axis, prop1 = 0, prop2, yawPDWeigth, yawIWeigth; + int32_t axis, prop1 = 0, prop2, yawIWeigth; // PITCH & ROLL dynamic PID adjustment, depending on throttle value if (rcData[THROTTLE] < currentControlRateProfile->tpa_breakpoint) { @@ -186,31 +186,6 @@ void annexCode(void) } } - // YAW dynamic PID adjustment - if (rcData[THROTTLE] < currentControlRateProfile->tpa_yaw_breakpoint) { - yawPDWeigth = 100; - } else { - if (rcData[THROTTLE] < 2000) { - if (currentControlRateProfile->tpa_yaw_rate >= 100) - { - yawPDWeigth = 100 + (uint16_t)(currentControlRateProfile->tpa_yaw_rate - 100) * (rcData[THROTTLE] - currentControlRateProfile->tpa_yaw_breakpoint) / (2000 - currentControlRateProfile->tpa_yaw_breakpoint); - } - else - { - yawPDWeigth = 100 - (uint16_t)currentControlRateProfile->tpa_yaw_rate * (rcData[THROTTLE] - currentControlRateProfile->tpa_yaw_breakpoint) / (2000 - currentControlRateProfile->tpa_yaw_breakpoint); - } - } else { - if (currentControlRateProfile->tpa_yaw_rate >= 100) - { - yawPDWeigth = currentControlRateProfile->tpa_yaw_rate; - } - else - { - yawPDWeigth = 100 - currentControlRateProfile->tpa_yaw_rate; - } - } - } - // YAW dynamic integral adjustment // Attenuate integral term proportional to the stick position, 100% attenuation starting at full stick position int32_t yawCommandFromCenter = ABS(rcData[YAW] - 1500); @@ -250,7 +225,7 @@ void annexCode(void) // non coupled PID reduction scaler used in PID controller 1 and PID controller 2. YAW TPA disabled. 100 means 100% of the pids if (axis == YAW) { - PIDweight[axis] = yawPDWeigth; + PIDweight[axis] = 100; Iweigth[axis] = yawIWeigth; } else { @@ -955,4 +930,4 @@ bool isRcAxisWithinDeadband(int32_t axis) } return ret; -} \ No newline at end of file +}