rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-11 03:29:17 +01:00
Code Issues Releases Activity

OP-1227 Fix high CPU usage: Avoid several float division in Attitude module, change optimization for attitude estimation function

Browse Source
This commit is contained in:
Alessio Morale 2014-06-08 21:48:35 +02:00
parent d51b7ce2d4
commit 843987484b
1 changed files with 14 additions and 12 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

26
flight/modules/Attitude/attitude.c
View File

@ -561,7 +561,7 @@ static inline void apply_accel_filter(const float *raw, float *filtered)
} }
} }
static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosData) __attribute__((optimize("O3"))) static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosData)
{ {
float dT = PIOS_DELTATIME_GetAverageSeconds(&dtconfig); float dT = PIOS_DELTATIME_GetAverageSeconds(&dtconfig);
@ -602,10 +602,10 @@ static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosD
if (grot_mag < 1.0e-3f) { if (grot_mag < 1.0e-3f) {
return; return;
} }
const float invMag = 1.0f / (accel_mag * grot_mag);
accel_err[0] /= (accel_mag * grot_mag); accel_err[0] *= invMag;
accel_err[1] /= (accel_mag * grot_mag); accel_err[1] *= invMag;
accel_err[2] /= (accel_mag * grot_mag); accel_err[2] *= invMag;
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
gyro_correct_int[0] += accel_err[0] * accelKi; gyro_correct_int[0] += accel_err[0] * accelKi;
@ -614,9 +614,10 @@ static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosD
// gyro_correct_int[2] += accel_err[2] * accelKi; // gyro_correct_int[2] += accel_err[2] * accelKi;
// Correct rates based on error, integral component dealt with in updateSensors // Correct rates based on error, integral component dealt with in updateSensors
gyros[0] += accel_err[0] * accelKp / dT; const float kpInvdT = accelKp / dT;
gyros[1] += accel_err[1] * accelKp / dT; gyros[0] += accel_err[0] * kpInvdT;
gyros[2] += accel_err[2] * accelKp / dT; gyros[1] += accel_err[1] * kpInvdT;
gyros[2] += accel_err[2] * kpInvdT;
{ // scoping variables to save memory { // scoping variables to save memory
// Work out time derivative from INSAlgo writeup // Work out time derivative from INSAlgo writeup
@ -652,10 +653,11 @@ static void updateAttitude(AccelStateData *accelStateData, GyroStateData *gyrosD
q[2] = 0; q[2] = 0;
q[3] = 0; q[3] = 0;
} else { } else {
q[0] = q[0] / qmag; const float invQmag = 1.0f / qmag;
q[1] = q[1] / qmag; q[0] = q[0] * invQmag;
q[2] = q[2] / qmag; q[1] = q[1] * invQmag;
q[3] = q[3] / qmag; q[2] = q[2] * invQmag;
q[3] = q[3] * invQmag;
} }
AttitudeStateData attitudeState; AttitudeStateData attitudeState;