diff --git a/src/main/fc/config.c b/src/main/fc/config.c index 421af8c316..2295e8fd6c 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 8e378fe125..52bcb70a46 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 7785ec47dc..6c92a1b59e 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 5bec324109..5f39d13bc7 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 70b4d5841f..29f2733bbe 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 b0f4939593..0cf059938b 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 c3975cbf25..ce751678a0 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) {}; }