diff --git a/src/main/config/config.c b/src/main/config/config.c index 8c49b2d79b..f38c4499b3 100644 --- a/src/main/config/config.c +++ b/src/main/config/config.c @@ -630,7 +630,7 @@ void activateConfig(void) imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;; imuRuntimeConfig.small_angle = masterConfig.small_angle; - configureIMU( + imuConfigure( &imuRuntimeConfig, ¤tProfile->pidProfile, ¤tProfile->accDeadband, diff --git a/src/main/flight/altitudehold.c b/src/main/flight/altitudehold.c index bfd7be91db..86e6ab7009 100644 --- a/src/main/flight/altitudehold.c +++ b/src/main/flight/altitudehold.c @@ -294,7 +294,7 @@ void calculateEstimatedAltitude(uint32_t currentTime) debug[3] = accAlt; // height #endif - accSum_reset(); + imuResetAccelerationSum(); #ifdef BARO if (!isBaroCalibrationComplete()) { diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c index ce3e182780..3f106a8958 100644 --- a/src/main/flight/imu.c +++ b/src/main/flight/imu.c @@ -77,7 +77,7 @@ imuRuntimeConfig_t *imuRuntimeConfig; pidProfile_t *pidProfile; accDeadband_t *accDeadband; -void configureIMU( +void imuConfigure( imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband, @@ -92,7 +92,7 @@ void configureIMU( throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle); } -void initIMU() +void imuInit() { smallAngle = lrintf(acc_1G * cosf(degreesToRadians(imuRuntimeConfig->small_angle))); accVelScale = 9.80665f / acc_1G / 10000.0f; @@ -129,7 +129,7 @@ float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff) t_fp_vector EstG; -void accSum_reset(void) +void imuResetAccelerationSum(void) { accSum[0] = 0; accSum[1] = 0; @@ -139,7 +139,7 @@ void accSum_reset(void) } // rotate acc into Earth frame and calculate acceleration in it -void acc_calc(uint32_t deltaT) +void imuCalculateAcceleration(uint32_t deltaT) { static int32_t accZoffset = 0; static float accz_smooth = 0; @@ -212,7 +212,7 @@ void acc_calc(uint32_t deltaT) * * //TODO: Add explanation for how it uses the Z dimension. */ -int16_t calculateHeading(t_fp_vector *vec) +int16_t imuCalculateHeading(t_fp_vector *vec) { int16_t head; @@ -234,7 +234,7 @@ int16_t calculateHeading(t_fp_vector *vec) return head; } -static void getEstimatedAttitude(void) +static void imuCalculateEstimatedAttitude(void) { int32_t axis; int32_t accMag = 0; @@ -295,24 +295,24 @@ static void getEstimatedAttitude(void) for (axis = 0; axis < 3; axis++) { EstM.A[axis] = (EstM.A[axis] * imuRuntimeConfig->gyro_cmpfm_factor + magADC[axis]) * invGyroComplimentaryFilter_M_Factor; } - heading = calculateHeading(&EstM); + heading = imuCalculateHeading(&EstM); } else { rotateV(&EstN.V, &deltaGyroAngle); normalizeV(&EstN.V, &EstN.V); - heading = calculateHeading(&EstN); + heading = imuCalculateHeading(&EstN); } - acc_calc(deltaT); // rotate acc vector into earth frame + imuCalculateAcceleration(deltaT); // rotate acc vector into earth frame } -void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) +void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode) { static int16_t gyroYawSmooth = 0; - gyroGetADC(); + gyroUpdate(); if (sensors(SENSOR_ACC)) { updateAccelerationReadings(accelerometerTrims); - getEstimatedAttitude(); + imuCalculateEstimatedAttitude(); } else { accADC[X] = 0; accADC[Y] = 0; diff --git a/src/main/flight/imu.h b/src/main/flight/imu.h index 45949c4e1e..a8d76e35d2 100644 --- a/src/main/flight/imu.h +++ b/src/main/flight/imu.h @@ -30,7 +30,7 @@ typedef struct imuRuntimeConfig_s { int8_t small_angle; } imuRuntimeConfig_t; -void configureIMU( +void imuConfigure( imuRuntimeConfig_t *initialImuRuntimeConfig, pidProfile_t *initialPidProfile, accDeadband_t *initialAccDeadband, @@ -39,11 +39,11 @@ void configureIMU( ); void calculateEstimatedAltitude(uint32_t currentTime); -void computeIMU(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode); +void imuUpdate(rollAndPitchTrims_t *accelerometerTrims, uint8_t mixerMode); float calculateThrottleAngleScale(uint16_t throttle_correction_angle); int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value); float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff); -int16_t calculateHeading(t_fp_vector *vec); +int16_t imuCalculateHeading(t_fp_vector *vec); -void accSum_reset(void); +void imuResetAccelerationSum(void); diff --git a/src/main/main.c b/src/main/main.c index cc8d669609..38b0ded3c6 100644 --- a/src/main/main.c +++ b/src/main/main.c @@ -104,7 +104,7 @@ void beepcodeInit(failsafe_t *initialFailsafe); void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig); void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile); bool sensorsAutodetect(sensorAlignmentConfig_t *sensorAlignmentConfig, uint16_t gyroLpf, uint8_t accHardwareToUse, int8_t magHardwareToUse, int16_t magDeclinationFromConfig); -void initIMU(void); +void imuInit(void); void displayInit(rxConfig_t *intialRxConfig); void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse); void loop(void); @@ -267,7 +267,7 @@ void init(void) LED0_OFF; LED1_OFF; - initIMU(); + imuInit(); mixerInit(masterConfig.mixerMode, masterConfig.customMixer); #ifdef MAG diff --git a/src/main/mw.c b/src/main/mw.c index 028177515a..62e7ca3e8f 100644 --- a/src/main/mw.c +++ b/src/main/mw.c @@ -663,7 +663,7 @@ void loop(void) if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) { loopTime = currentTime + masterConfig.looptime; - computeIMU(¤tProfile->accelerometerTrims, masterConfig.mixerMode); + imuUpdate(¤tProfile->accelerometerTrims, masterConfig.mixerMode); // Measure loop rate just after reading the sensors currentTime = micros(); diff --git a/src/main/sensors/gyro.c b/src/main/sensors/gyro.c index d490a91f5f..99b3a29ed5 100644 --- a/src/main/sensors/gyro.c +++ b/src/main/sensors/gyro.c @@ -108,7 +108,7 @@ static void applyGyroZero(void) } } -void gyroGetADC(void) +void gyroUpdate(void) { // FIXME When gyro.read() fails due to i2c or other error gyroZero is continually re-applied to gyroADC resulting in a old reading that gets worse over time. diff --git a/src/main/sensors/gyro.h b/src/main/sensors/gyro.h index 7acd2eb2a0..a064a80dd9 100644 --- a/src/main/sensors/gyro.h +++ b/src/main/sensors/gyro.h @@ -26,6 +26,6 @@ typedef struct gyroConfig_s { void useGyroConfig(gyroConfig_t *gyroConfigToUse); void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired); -void gyroGetADC(void); +void gyroUpdate(void); bool isGyroCalibrationComplete(void);