Mirror/betaflight
1
0
Fork 0
mirror of https://github.com/betaflight/betaflight.git synced 2025-07-21 07:15:18 +03:00
Code Issues Wiki Activity

Cleanup deprecated IMU code by using a union.

Browse source
This commit is contained in:
Dominic Clifton 2014-04-24 01:13:29 +01:00
parent a6c22d2115
commit 7af9ca4fdc
2 changed files with 22 additions and 26 deletions
Download patch file Download diff file Expand all files Collapse all files

5
src/flight_common.h
View file

@ -54,6 +54,11 @@ typedef struct fp_angles {
float roll; float roll;
float pitch; float pitch;
float yaw; float yaw;
} fp_angles_def;
typedef union {
float raw[3];
fp_angles_def angles;
} fp_angles_t; } fp_angles_t;
typedef struct int16_flightDynamicsTrims_s { typedef struct int16_flightDynamicsTrims_s {

43
src/flight_imu.c
View file

@ -141,7 +141,7 @@ void normalizeV(struct fp_vector *src, struct fp_vector *dest)
} }
// Rotate Estimated vector(s) with small angle approximation, according to the gyro data // Rotate Estimated vector(s) with small angle approximation, according to the gyro data
void rotateAnglesV(struct fp_vector *v, fp_angles_t *delta) void rotateV(struct fp_vector *v, fp_angles_t *delta)
{ {
struct fp_vector v_tmp = *v; struct fp_vector v_tmp = *v;
@ -150,12 +150,12 @@ void rotateAnglesV(struct fp_vector *v, fp_angles_t *delta)
float cosx, sinx, cosy, siny, cosz, sinz; float cosx, sinx, cosy, siny, cosz, sinz;
float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx; float coszcosx, coszcosy, sinzcosx, coszsinx, sinzsinx;
cosx = cosf(delta->roll); cosx = cosf(delta->angles.roll);
sinx = sinf(delta->roll); sinx = sinf(delta->angles.roll);
cosy = cosf(delta->pitch); cosy = cosf(delta->angles.pitch);
siny = sinf(delta->pitch); siny = sinf(delta->angles.pitch);
cosz = cosf(delta->yaw); cosz = cosf(delta->angles.yaw);
sinz = sinf(delta->yaw); sinz = sinf(delta->angles.yaw);
coszcosx = cosz * cosx; coszcosx = cosz * cosx;
coszcosy = cosz * cosy; coszcosy = cosz * cosy;
@ -178,16 +178,6 @@ void rotateAnglesV(struct fp_vector *v, fp_angles_t *delta)
v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2]; v->Z = v_tmp.X * mat[0][2] + v_tmp.Y * mat[1][2] + v_tmp.Z * mat[2][2];
} }
// deprecated - it uses legacy indices for ROLL/PITCH/YAW, see rc_alias_e - use rotateAnglesV instead
void rotateV(struct fp_vector *v, float *delta) {
fp_angles_t temp;
temp.roll = delta[FD_ROLL];
temp.pitch = delta[FD_PITCH];
temp.yaw = delta[FD_YAW];
rotateAnglesV(v, &temp);
}
int32_t applyDeadband(int32_t value, int32_t deadband) int32_t applyDeadband(int32_t value, int32_t deadband)
{ {
if (abs(value) < deadband) { if (abs(value) < deadband) {
@ -212,15 +202,15 @@ void acc_calc(uint32_t deltaT)
t_fp_vector accel_ned; t_fp_vector accel_ned;
// the accel values have to be rotated into the earth frame // the accel values have to be rotated into the earth frame
rpy.roll = -(float)anglerad[AI_ROLL]; rpy.angles.roll = -(float)anglerad[AI_ROLL];
rpy.pitch = -(float)anglerad[AI_PITCH]; rpy.angles.pitch = -(float)anglerad[AI_PITCH];
rpy.yaw = -(float)heading * RAD; rpy.angles.yaw = -(float)heading * RAD;
accel_ned.V.X = accSmooth[0]; accel_ned.V.X = accSmooth[0];
accel_ned.V.Y = accSmooth[1]; accel_ned.V.Y = accSmooth[1];
accel_ned.V.Z = accSmooth[2]; accel_ned.V.Z = accSmooth[2];
rotateAnglesV(&accel_ned.V, &rpy); rotateV(&accel_ned.V, &rpy);
if (currentProfile.acc_unarmedcal == 1) { if (currentProfile.acc_unarmedcal == 1) {
if (!f.ARMED) { if (!f.ARMED) {
@ -281,14 +271,15 @@ static void getEstimatedAttitude(void)
static uint32_t previousT; static uint32_t previousT;
uint32_t currentT = micros(); uint32_t currentT = micros();
uint32_t deltaT; uint32_t deltaT;
float scale, deltaGyroAngle[3]; float scale;
fp_angles_t deltaGyroAngle;
deltaT = currentT - previousT; deltaT = currentT - previousT;
scale = deltaT * gyro.scale; scale = deltaT * gyro.scale;
previousT = currentT; previousT = currentT;
// Initialization // Initialization
for (axis = 0; axis < 3; axis++) { for (axis = 0; axis < 3; axis++) {
deltaGyroAngle[axis] = gyroADC[axis] * scale; deltaGyroAngle.raw[axis] = gyroADC[axis] * scale;
if (currentProfile.acc_lpf_factor > 0) { if (currentProfile.acc_lpf_factor > 0) {
accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / currentProfile.acc_lpf_factor)) + accADC[axis] * (1.0f / currentProfile.acc_lpf_factor); accLPF[axis] = accLPF[axis] * (1.0f - (1.0f / currentProfile.acc_lpf_factor)) + accADC[axis] * (1.0f / currentProfile.acc_lpf_factor);
accSmooth[axis] = accLPF[axis]; accSmooth[axis] = accLPF[axis];
@ -299,11 +290,11 @@ static void getEstimatedAttitude(void)
} }
accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G); accMag = accMag * 100 / ((int32_t)acc_1G * acc_1G);
rotateV(&EstG.V, deltaGyroAngle); rotateV(&EstG.V, &deltaGyroAngle);
if (sensors(SENSOR_MAG)) { if (sensors(SENSOR_MAG)) {
rotateV(&EstM.V, deltaGyroAngle); rotateV(&EstM.V, &deltaGyroAngle);
} else { } else {
rotateV(&EstN.V, deltaGyroAngle); rotateV(&EstN.V, &deltaGyroAngle);
normalizeV(&EstN.V, &EstN.V); normalizeV(&EstN.V, &EstN.V);
} }