From 3d1e42d1aab2a21a9704edeff3f2c651b7709e55 Mon Sep 17 00:00:00 2001 From: Martin Budden Date: Sun, 5 Feb 2017 20:37:20 +0000 Subject: [PATCH] Tidy of IMU header --- src/main/blackbox/blackbox.c | 1 + src/main/blackbox/blackbox_io.c | 1 + src/main/config/config_master.h | 1 + src/main/fc/fc_core.h | 8 ++ src/main/flight/imu.c | 106 +++++++++++------------- src/main/flight/imu.h | 37 +-------- src/main/flight/servos.c | 1 + src/main/io/osd.c | 1 + src/main/io/vtx.c | 2 + src/main/sensors/battery.c | 1 + src/main/sensors/esc_sensor.c | 4 +- src/main/target/COLIBRI_RACE/config.c | 10 ++- src/main/target/MULTIFLITEPICO/config.c | 1 + src/main/telemetry/crsf.c | 1 + 14 files changed, 78 insertions(+), 97 deletions(-) diff --git a/src/main/blackbox/blackbox.c b/src/main/blackbox/blackbox.c index eb9aee930..bbb9e34c0 100644 --- a/src/main/blackbox/blackbox.c +++ b/src/main/blackbox/blackbox.c @@ -32,6 +32,7 @@ #include "common/axis.h" #include "common/encoding.h" +#include "common/maths.h" #include "common/utils.h" #include "config/config_profile.h" diff --git a/src/main/blackbox/blackbox_io.c b/src/main/blackbox/blackbox_io.c index f33e73816..57852f2d1 100644 --- a/src/main/blackbox/blackbox_io.c +++ b/src/main/blackbox/blackbox_io.c @@ -15,6 +15,7 @@ #include "build/build_config.h" #include "common/encoding.h" +#include "common/maths.h" #include "common/printf.h" #include "config/config_profile.h" diff --git a/src/main/config/config_master.h b/src/main/config/config_master.h index 7f75ffb4b..2e8c2484e 100644 --- a/src/main/config/config_master.h +++ b/src/main/config/config_master.h @@ -37,6 +37,7 @@ #include "fc/rc_adjustments.h" #include "fc/rc_controls.h" +#include "fc/fc_core.h" #include "flight/failsafe.h" #include "flight/mixer.h" diff --git a/src/main/fc/fc_core.h b/src/main/fc/fc_core.h index 5e2766d60..1b83302eb 100644 --- a/src/main/fc/fc_core.h +++ b/src/main/fc/fc_core.h @@ -18,11 +18,19 @@ #pragma once #include "common/time.h" +#include "config/parameter_group.h" extern int16_t magHold; extern bool isRXDataNew; extern int16_t headFreeModeHold; +typedef struct throttleCorrectionConfig_s { + uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg + uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle. +} throttleCorrectionConfig_t; + +PG_DECLARE(throttleCorrectionConfig_t, throttleCorrectionConfig); + union rollAndPitchTrims_u; void applyAndSaveAccelerometerTrimsDelta(union rollAndPitchTrims_u *rollAndPitchTrimsDelta); void handleInflightCalibrationStickPosition(); diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index fadaf5e78..631b280f3 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -21,40 +21,40 @@ #include #include -#include "common/maths.h" - #include "platform.h" #include "build/build_config.h" #include "build/debug.h" #include "common/axis.h" +#include "common/maths.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" #include "drivers/system.h" -#include "sensors/sensors.h" -#include "sensors/gyro.h" -#include "sensors/compass.h" -#include "sensors/acceleration.h" -#include "sensors/barometer.h" -#include "sensors/sonar.h" +#include "fc/runtime_config.h" +#include "flight/imu.h" #include "flight/mixer.h" #include "flight/pid.h" -#include "flight/imu.h" #include "io/gps.h" -#include "fc/runtime_config.h" +#include "sensors/acceleration.h" +#include "sensors/barometer.h" +#include "sensors/compass.h" +#include "sensors/gyro.h" +#include "sensors/sensors.h" +#include "sensors/sonar.h" // the limit (in degrees/second) beyond which we stop integrating // omega_I. At larger spin rates the DCM PI controller can get 'dizzy' // which results in false gyro drift. See // http://gentlenav.googlecode.com/files/fastRotations.pdf + #define SPIN_RATE_LIMIT 20 int32_t accSum[XYZ_AXIS_COUNT]; @@ -63,11 +63,11 @@ uint32_t accTimeSum = 0; // keep track for integration of acc int accSumCount = 0; float accVelScale; -float throttleAngleScale; -float fc_acc; -float smallAngleCosZ = 0; +static float throttleAngleScale; +static float fc_acc; +static float smallAngleCosZ = 0; -float magneticDeclination = 0.0f; // calculated at startup from config +static float magneticDeclination = 0.0f; // calculated at startup from config static imuRuntimeConfig_t imuRuntimeConfig; static pidProfile_t *pidProfile; @@ -103,6 +103,19 @@ STATIC_UNIT_TESTED void imuComputeRotationMatrix(void) rMat[2][2] = 1.0f - 2.0f * q1q1 - 2.0f * q2q2; } +/* +* Calculate RC time constant used in the accZ lpf. +*/ +static float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff) +{ + return 0.5f / (M_PIf * accz_lpf_cutoff); +} + +static float calculateThrottleAngleScale(uint16_t throttle_correction_angle) +{ + return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle); +} + void imuConfigure( imuConfig_t *imuConfig, pidProfile_t *initialPidProfile, @@ -127,19 +140,6 @@ void imuInit(void) imuComputeRotationMatrix(); } -float calculateThrottleAngleScale(uint16_t throttle_correction_angle) -{ - return (1800.0f / M_PIf) * (900.0f / throttle_correction_angle); -} - -/* -* Calculate RC time constant used in the accZ lpf. -*/ -float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff) -{ - return 0.5f / (M_PIf * accz_lpf_cutoff); -} - void imuResetAccelerationSum(void) { accSum[0] = 0; @@ -149,14 +149,12 @@ void imuResetAccelerationSum(void) accTimeSum = 0; } -void imuTransformVectorBodyToEarth(t_fp_vector * v) +static void imuTransformVectorBodyToEarth(t_fp_vector * v) { - float x,y,z; - /* From body frame to earth frame */ - x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z; - y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z; - z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z; + const float x = rMat[0][0] * v->V.X + rMat[0][1] * v->V.Y + rMat[0][2] * v->V.Z; + const float y = rMat[1][0] * v->V.X + rMat[1][1] * v->V.Y + rMat[1][2] * v->V.Z; + const float z = rMat[2][0] * v->V.X + rMat[2][1] * v->V.Y + rMat[2][2] * v->V.Z; v->V.X = x; v->V.Y = -y; @@ -164,16 +162,15 @@ void imuTransformVectorBodyToEarth(t_fp_vector * v) } // rotate acc into Earth frame and calculate acceleration in it -void imuCalculateAcceleration(uint32_t deltaT) +static void imuCalculateAcceleration(uint32_t deltaT) { static int32_t accZoffset = 0; static float accz_smooth = 0; - float dT; - t_fp_vector accel_ned; // deltaT is measured in us ticks - dT = (float)deltaT * 1e-6f; + const float dT = (float)deltaT * 1e-6f; + t_fp_vector accel_ned; accel_ned.V.X = acc.accSmooth[X]; accel_ned.V.Y = acc.accSmooth[Y]; accel_ned.V.Z = acc.accSmooth[Z]; @@ -186,8 +183,9 @@ void imuCalculateAcceleration(uint32_t deltaT) accZoffset += accel_ned.V.Z; } accel_ned.V.Z -= accZoffset / 64; // compensate for gravitation on z-axis - } else + } else { accel_ned.V.Z -= acc.dev.acc_1G; + } accz_smooth = accz_smooth + (dT / (fc_acc + dT)) * (accel_ned.V.Z - accz_smooth); // low pass filter @@ -227,15 +225,12 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz, bool useYaw, float yawError) { static float integralFBx = 0.0f, integralFBy = 0.0f, integralFBz = 0.0f; // integral error terms scaled by Ki - float recipNorm; - float hx, hy, bx; - float ex = 0, ey = 0, ez = 0; - float qa, qb, qc; // Calculate general spin rate (rad/s) - float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz)); + const float spin_rate = sqrtf(sq(gx) + sq(gy) + sq(gz)); // Use raw heading error (from GPS or whatever else) + float ez = 0; if (useYaw) { while (yawError > M_PIf) yawError -= (2.0f * M_PIf); while (yawError < -M_PIf) yawError += (2.0f * M_PIf); @@ -244,7 +239,8 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz, } // Use measured magnetic field vector - recipNorm = sq(mx) + sq(my) + sq(mz); + float ex = 0, ey = 0; + float recipNorm = sq(mx) + sq(my) + sq(mz); if (useMag && recipNorm > 0.01f) { // Normalise magnetometer measurement recipNorm = invSqrt(recipNorm); @@ -257,12 +253,12 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz, // (hx; hy; 0) - measured mag field vector in EF (assuming Z-component is zero) // (bx; 0; 0) - reference mag field vector heading due North in EF (assuming Z-component is zero) - hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz; - hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz; - bx = sqrtf(hx * hx + hy * hy); + const float hx = rMat[0][0] * mx + rMat[0][1] * my + rMat[0][2] * mz; + const float hy = rMat[1][0] * mx + rMat[1][1] * my + rMat[1][2] * mz; + const float bx = sqrtf(hx * hx + hy * hy); // magnetometer error is cross product between estimated magnetic north and measured magnetic north (calculated in EF) - float ez_ef = -(hy * bx); + const float ez_ef = -(hy * bx); // Rotate mag error vector back to BF and accumulate ex += rMat[2][0] * ez_ef; @@ -289,7 +285,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz, if(imuRuntimeConfig.dcm_ki > 0.0f) { // Stop integrating if spinning beyond the certain limit if (spin_rate < DEGREES_TO_RADIANS(SPIN_RATE_LIMIT)) { - float dcmKiGain = imuRuntimeConfig.dcm_ki; + const float dcmKiGain = imuRuntimeConfig.dcm_ki; integralFBx += dcmKiGain * ex * dt; // integral error scaled by Ki integralFBy += dcmKiGain * ey * dt; integralFBz += dcmKiGain * ez * dt; @@ -302,7 +298,7 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz, } // Calculate kP gain. If we are acquiring initial attitude (not armed and within 20 sec from powerup) scale the kP to converge faster - float dcmKpGain = imuRuntimeConfig.dcm_kp * imuGetPGainScaleFactor(); + const float dcmKpGain = imuRuntimeConfig.dcm_kp * imuGetPGainScaleFactor(); // Apply proportional and integral feedback gx += dcmKpGain * ex + integralFBx; @@ -314,9 +310,9 @@ static void imuMahonyAHRSupdate(float dt, float gx, float gy, float gz, gy *= (0.5f * dt); gz *= (0.5f * dt); - qa = q0; - qb = q1; - qc = q2; + const float qa = q0; + const float qb = q1; + const float qc = q2; q0 += (-qb * gx - qc * gy - q3 * gz); q1 += (qa * gx + qc * gz - q3 * gy); q2 += (qa * gy - qb * gz + q3 * gx); @@ -353,10 +349,8 @@ STATIC_UNIT_TESTED void imuUpdateEulerAngles(void) static bool imuIsAccelerometerHealthy(void) { - int32_t axis; int32_t accMagnitude = 0; - - for (axis = 0; axis < 3; axis++) { + for (int axis = 0; axis < 3; axis++) { accMagnitude += (int32_t)acc.accSmooth[axis] * acc.accSmooth[axis]; } diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 0deec6c89..c28af0ada 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -18,13 +18,10 @@ #pragma once #include "common/axis.h" -#include "common/maths.h" #include "common/time.h" #include "config/parameter_group.h" -#include "sensors/acceleration.h" - // Exported symbols extern uint32_t accTimeSum; extern int accSumCount; @@ -49,13 +46,6 @@ typedef struct accDeadband_s { uint8_t z; // set the acc deadband for z-Axis, this ignores small accelerations } accDeadband_t; -typedef struct throttleCorrectionConfig_s { - uint16_t throttle_correction_angle; // the angle when the throttle correction is maximal. in 0.1 degres, ex 225 = 22.5 ,30.0, 450 = 45.0 deg - uint8_t throttle_correction_value; // the correction that will be applied at throttle_correction_angle. -} throttleCorrectionConfig_t; - -PG_DECLARE(throttleCorrectionConfig_t, throttleCorrectionConfig); - typedef struct imuConfig_s { uint16_t dcm_kp; // DCM filter proportional gain ( x 10000) uint16_t dcm_ki; // DCM filter integral gain ( x 10000) @@ -74,38 +64,13 @@ typedef struct imuRuntimeConfig_s { accDeadband_t accDeadband; } imuRuntimeConfig_t; -typedef enum { - ACCPROC_READ = 0, - ACCPROC_CHUNK_1, - ACCPROC_CHUNK_2, - ACCPROC_CHUNK_3, - ACCPROC_CHUNK_4, - ACCPROC_CHUNK_5, - ACCPROC_CHUNK_6, - ACCPROC_CHUNK_7, - ACCPROC_COPY -} accProcessorState_e; - -typedef struct accProcessor_s { - accProcessorState_e state; -} accProcessor_t; - struct pidProfile_s; -void imuConfigure( - imuConfig_t *imuConfig, - struct pidProfile_s *initialPidProfile, - uint16_t throttle_correction_angle -); +void imuConfigure(imuConfig_t *imuConfig, struct pidProfile_s *initialPidProfile, uint16_t throttle_correction_angle); float getCosTiltAngle(void); void calculateEstimatedAltitude(timeUs_t currentTimeUs); void imuUpdateAttitude(timeUs_t currentTimeUs); -float calculateThrottleAngleScale(uint16_t throttle_correction_angle); int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); -float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_hz); - -union u_fp_vector; -int16_t imuCalculateHeading(union u_fp_vector *vec); void imuResetAccelerationSum(void); void imuInit(void); diff --git a/src/main/flight/servos.c b/src/main/flight/servos.c index 87a276fe1..086b79ca8 100755 --- a/src/main/flight/servos.c +++ b/src/main/flight/servos.c @@ -27,6 +27,7 @@ #include "build/build_config.h" #include "common/filter.h" +#include "common/maths.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" diff --git a/src/main/io/osd.c b/src/main/io/osd.c index f31e4d72e..f9f627b11 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -38,6 +38,7 @@ #include "build/version.h" #include "common/printf.h" +#include "common/maths.h" #include "common/utils.h" #include "config/config_profile.h" diff --git a/src/main/io/vtx.c b/src/main/io/vtx.c index eb44e3f0b..cf43e539a 100644 --- a/src/main/io/vtx.c +++ b/src/main/io/vtx.c @@ -26,6 +26,8 @@ #include "io/osd.h" //External dependencies +#include "common/maths.h" + #include "config/config_eeprom.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" diff --git a/src/main/sensors/battery.c b/src/main/sensors/battery.c index e54a0a1ae..d942cb409 100644 --- a/src/main/sensors/battery.c +++ b/src/main/sensors/battery.c @@ -23,6 +23,7 @@ #include "build/debug.h" #include "common/filter.h" +#include "common/maths.h" #include "common/utils.h" #include "config/feature.h" diff --git a/src/main/sensors/esc_sensor.c b/src/main/sensors/esc_sensor.c index 4fe0876ba..0e4fce045 100644 --- a/src/main/sensors/esc_sensor.c +++ b/src/main/sensors/esc_sensor.c @@ -21,12 +21,15 @@ #include +#ifdef USE_DSHOT + #include "build/debug.h" #include "config/feature.h" #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "common/maths.h" #include "common/utils.h" #include "drivers/pwm_output.h" @@ -71,7 +74,6 @@ set debug_mode = DEBUG_ESC_TELEMETRY in cli */ -#ifdef USE_DSHOT enum { DEBUG_ESC_MOTOR_INDEX = 0, DEBUG_ESC_NUM_TIMEOUTS = 1, diff --git a/src/main/target/COLIBRI_RACE/config.c b/src/main/target/COLIBRI_RACE/config.c index 535da3d20..826bd10ad 100644 --- a/src/main/target/COLIBRI_RACE/config.c +++ b/src/main/target/COLIBRI_RACE/config.c @@ -15,19 +15,21 @@ * along with Cleanflight. If not, see . */ -#include #include #include #include +#include "common/maths.h" + +#include "config/config_master.h" +#include "config/feature.h" + +#include "io/ledstrip.h" #include "io/motors.h" #include "sensors/battery.h" -#include "config/config_master.h" -#include "config/feature.h" -#include "io/ledstrip.h" void targetApplyDefaultLedStripConfig(ledConfig_t *ledConfigs) { diff --git a/src/main/target/MULTIFLITEPICO/config.c b/src/main/target/MULTIFLITEPICO/config.c index 70ac3912b..8e00c8d7f 100755 --- a/src/main/target/MULTIFLITEPICO/config.c +++ b/src/main/target/MULTIFLITEPICO/config.c @@ -21,6 +21,7 @@ #include #include "common/axis.h" +#include "common/maths.h" #include "drivers/sensor.h" #include "drivers/compass.h" diff --git a/src/main/telemetry/crsf.c b/src/main/telemetry/crsf.c index baa00554a..b8bbb9f00 100644 --- a/src/main/telemetry/crsf.c +++ b/src/main/telemetry/crsf.c @@ -35,6 +35,7 @@ #include "config/parameter_group.h" #include "config/parameter_group_ids.h" +#include "common/maths.h" #include "common/streambuf.h" #include "common/utils.h"