Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-19 22:35:23 +03:00
Code Issues Wiki Activity

Fix throttle angle correction when smoothing throttle; reduce processing overhead

Browse source 
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.
This commit is contained in:
Bruce Luckcuck 2018-06-28 10:39:21 -04:00
parent e260c37be3
commit 5cd886017d
7 changed files with 41 additions and 25 deletions
Download patch file Download diff file Expand all files Collapse all files

45
src/main/flight/imu.c
View file

@ -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) {