From 5cd886017db11f5b09f1a7a651c042997fe1b6c0 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Thu, 28 Jun 2018 10:39:21 -0400 Subject: [PATCH] Fix throttle angle correction when smoothing throttle; reduce processing overhead Change the logic to not modify rcCommand directly and instead apply the additional throttle directly in the mixer. Also move the logic to the attitude task instead of having it calculate in the PID loop. The logic relies on an angle that's only updated in the attitude task so there was no point in running the calculation every PID loop. --- src/main/fc/config.c | 2 +- src/main/fc/fc_core.c | 4 ---- src/main/flight/imu.c | 45 ++++++++++++++++++++++-------------- src/main/flight/imu.h | 3 +-- src/main/flight/mixer.c | 9 +++++++- src/main/flight/mixer.h | 2 ++ src/test/unit/flight_imu_unittest.cc | 1 + 7 files changed, 41 insertions(+), 25 deletions(-) diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 421af8c31..2295e8fd6 100644 --- a/src/main/fc/config.c +++ b/src/main/fc/config.c @@ -136,7 +136,7 @@ static void activateConfig(void) setAccelerationTrims(&accelerometerConfigMutable()->accZero); accInitFilters(); - imuConfigure(throttleCorrectionConfig()->throttle_correction_angle); + imuConfigure(throttleCorrectionConfig()->throttle_correction_angle, throttleCorrectionConfig()->throttle_correction_value); #endif // USE_OSD_SLAVE #ifdef USE_LED_STRIP diff --git a/src/main/fc/fc_core.c b/src/main/fc/fc_core.c index 8e378fe12..52bcb70a4 100644 --- a/src/main/fc/fc_core.c +++ b/src/main/fc/fc_core.c @@ -986,10 +986,6 @@ static FAST_CODE_NOINLINE void subTaskRcCommand(timeUs_t currentTimeUs) resetYawAxis(); } - if (throttleCorrectionConfig()->throttle_correction_value && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE))) { - rcCommand[THROTTLE] += calculateThrottleAngleCorrection(throttleCorrectionConfig()->throttle_correction_value); - } - processRcCommand(); #if defined(USE_GPS_RESCUE) diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index 7785ec47d..6c92a1b59 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -92,6 +92,7 @@ float accVelScale; bool canUseGPSHeading = true; static float throttleAngleScale; +static int throttleAngleValue; static float fc_acc; static float smallAngleCosZ = 0; @@ -153,7 +154,7 @@ static float calculateThrottleAngleScale(uint16_t throttle_correction_angle) return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle); } -void imuConfigure(uint16_t throttle_correction_angle) +void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correction_value) { imuRuntimeConfig.dcm_kp = imuConfig()->dcm_kp / 10000.0f; imuRuntimeConfig.dcm_ki = imuConfig()->dcm_ki / 10000.0f; @@ -162,6 +163,8 @@ void imuConfigure(uint16_t throttle_correction_angle) fc_acc = calculateAccZLowPassFilterRCTimeConstant(5.0f); // Set to fix value throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle); + + throttleAngleValue = throttle_correction_value; } void imuInit(void) @@ -503,6 +506,22 @@ static void imuCalculateEstimatedAttitude(timeUs_t currentTimeUs) #endif } +int calculateThrottleAngleCorrection(void) +{ + /* + * Use 0 as the throttle angle correction if we are inverted, vertical or with a + * small angle < 0.86 deg + * TODO: Define this small angle in config. + */ + if (rMat[2][2] <= 0.015f) { + return 0; + } + int angle = lrintf(acos_approx(rMat[2][2]) * throttleAngleScale); + if (angle > 900) + angle = 900; + return lrintf(throttleAngleValue * sin_approx(angle / (900.0f * M_PIf / 2.0f))); +} + void imuUpdateAttitude(timeUs_t currentTimeUs) { if (sensors(SENSOR_ACC) && acc.isAccelUpdatedAtLeastOnce) { @@ -516,6 +535,14 @@ void imuUpdateAttitude(timeUs_t currentTimeUs) #endif imuCalculateEstimatedAttitude(currentTimeUs); IMU_UNLOCK; + + // Update the throttle correction for angle and supply it to the mixer + int throttleAngleCorrection = 0; + if (throttleAngleValue && (FLIGHT_MODE(ANGLE_MODE) || FLIGHT_MODE(HORIZON_MODE)) && ARMING_FLAG(ARMED)) { + throttleAngleCorrection = calculateThrottleAngleCorrection(); + } + mixerSetThrottleAngleCorrection(throttleAngleCorrection); + } else { acc.accADC[X] = 0; acc.accADC[Y] = 0; @@ -549,22 +576,6 @@ void getQuaternion(quaternion *quat) quat->z = q.z; } -int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value) -{ - /* - * Use 0 as the throttle angle correction if we are inverted, vertical or with a - * small angle < 0.86 deg - * TODO: Define this small angle in config. - */ - if (rMat[2][2] <= 0.015f) { - return 0; - } - int angle = lrintf(acos_approx(rMat[2][2]) * throttleAngleScale); - if (angle > 900) - angle = 900; - return lrintf(throttle_correction_value * sin_approx(angle / (900.0f * M_PIf / 2.0f))); -} - void imuComputeQuaternionFromRPY(quaternionProducts *quatProd, int16_t initialRoll, int16_t initialPitch, int16_t initialYaw) { if (initialRoll > 1800) { diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 5bec32410..5f39d13bc 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -78,12 +78,11 @@ typedef struct imuRuntimeConfig_s { accDeadband_t accDeadband; } imuRuntimeConfig_t; -void imuConfigure(uint16_t throttle_correction_angle); +void imuConfigure(uint16_t throttle_correction_angle, uint8_t throttle_correction_value); float getCosTiltAngle(void); void getQuaternion(quaternion * q); void imuUpdateAttitude(timeUs_t currentTimeUs); -int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); void imuResetAccelerationSum(void); void imuInit(void); diff --git a/src/main/flight/mixer.c b/src/main/flight/mixer.c index 70b4d5841..29f2733bb 100644 --- a/src/main/flight/mixer.c +++ b/src/main/flight/mixer.c @@ -125,6 +125,8 @@ float motor_disarmed[MAX_SUPPORTED_MOTORS]; mixerMode_e currentMixerMode; static motorMixer_t currentMixer[MAX_SUPPORTED_MOTORS]; +static FAST_RAM_ZERO_INIT int throttleAngleCorrection; + static const motorMixer_t mixerQuadX[] = { { 1.0f, -1.0f, 1.0f, -1.0f }, // REAR_R @@ -606,7 +608,7 @@ static void calculateThrottleAndCurrentMotorEndpoints(timeUs_t currentTimeUs) pidResetITerm(); } } else { - throttle = rcCommand[THROTTLE] - rxConfig()->mincheck; + throttle = rcCommand[THROTTLE] - rxConfig()->mincheck + throttleAngleCorrection; currentThrottleInputRange = rcCommandThrottleRange; motorRangeMin = motorOutputLow; motorRangeMax = motorOutputHigh; @@ -888,3 +890,8 @@ uint16_t convertMotorToExternal(float motorValue) return externalValue; } + +void mixerSetThrottleAngleCorrection(int correctionValue) +{ + throttleAngleCorrection = correctionValue; +} diff --git a/src/main/flight/mixer.h b/src/main/flight/mixer.h index b0f493959..0cf059938 100644 --- a/src/main/flight/mixer.h +++ b/src/main/flight/mixer.h @@ -134,3 +134,5 @@ void stopPwmAllMotors(void); float convertExternalToMotor(uint16_t externalValue); uint16_t convertMotorToExternal(float motorValue); bool mixerIsTricopter(void); + +void mixerSetThrottleAngleCorrection(int correctionValue); diff --git a/src/test/unit/flight_imu_unittest.cc b/src/test/unit/flight_imu_unittest.cc index c3975cbf2..ce751678a 100644 --- a/src/test/unit/flight_imu_unittest.cc +++ b/src/test/unit/flight_imu_unittest.cc @@ -243,4 +243,5 @@ void performBaroCalibrationCycle(void) {} int32_t baroCalculateAltitude(void) { return 0; } bool gyroGetAccumulationAverage(float *) { return false; } bool accGetAccumulationAverage(float *) { return false; } +void mixerSetThrottleAngleCorrection(int) {}; } -- 2.11.4.GIT