IMU naming cleanup.
This commit is contained in:
parent
8b0a982931
commit
01b2ce0b36
|
@ -630,7 +630,7 @@ void activateConfig(void)
|
||||||
imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;;
|
imuRuntimeConfig.acc_unarmedcal = currentProfile->acc_unarmedcal;;
|
||||||
imuRuntimeConfig.small_angle = masterConfig.small_angle;
|
imuRuntimeConfig.small_angle = masterConfig.small_angle;
|
||||||
|
|
||||||
configureIMU(
|
imuConfigure(
|
||||||
&imuRuntimeConfig,
|
&imuRuntimeConfig,
|
||||||
¤tProfile->pidProfile,
|
¤tProfile->pidProfile,
|
||||||
¤tProfile->accDeadband,
|
¤tProfile->accDeadband,
|
||||||
|
|
|
@ -294,7 +294,7 @@ void calculateEstimatedAltitude(uint32_t currentTime)
|
||||||
debug[3] = accAlt; // height
|
debug[3] = accAlt; // height
|
||||||
#endif
|
#endif
|
||||||
|
|
||||||
accSum_reset();
|
imuResetAccelerationSum();
|
||||||
|
|
||||||
#ifdef BARO
|
#ifdef BARO
|
||||||
if (!isBaroCalibrationComplete()) {
|
if (!isBaroCalibrationComplete()) {
|
||||||
|
|
|
@ -77,7 +77,7 @@ imuRuntimeConfig_t *imuRuntimeConfig;
|
||||||
pidProfile_t *pidProfile;
|
pidProfile_t *pidProfile;
|
||||||
accDeadband_t *accDeadband;
|
accDeadband_t *accDeadband;
|
||||||
|
|
||||||
void configureIMU(
|
void imuConfigure(
|
||||||
imuRuntimeConfig_t *initialImuRuntimeConfig,
|
imuRuntimeConfig_t *initialImuRuntimeConfig,
|
||||||
pidProfile_t *initialPidProfile,
|
pidProfile_t *initialPidProfile,
|
||||||
accDeadband_t *initialAccDeadband,
|
accDeadband_t *initialAccDeadband,
|
||||||
|
@ -92,7 +92,7 @@ void configureIMU(
|
||||||
throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
|
throttleAngleScale = calculateThrottleAngleScale(throttle_correction_angle);
|
||||||
}
|
}
|
||||||
|
|
||||||
void initIMU()
|
void imuInit()
|
||||||
{
|
{
|
||||||
smallAngle = lrintf(acc_1G * cosf(degreesToRadians(imuRuntimeConfig->small_angle)));
|
smallAngle = lrintf(acc_1G * cosf(degreesToRadians(imuRuntimeConfig->small_angle)));
|
||||||
accVelScale = 9.80665f / acc_1G / 10000.0f;
|
accVelScale = 9.80665f / acc_1G / 10000.0f;
|
||||||
|
@ -129,7 +129,7 @@ float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff)
|
||||||
|
|
||||||
t_fp_vector EstG;
|
t_fp_vector EstG;
|
||||||
|
|
||||||
void accSum_reset(void)
|
void imuResetAccelerationSum(void)
|
||||||
{
|
{
|
||||||
accSum[0] = 0;
|
accSum[0] = 0;
|
||||||
accSum[1] = 0;
|
accSum[1] = 0;
|
||||||
|
@ -139,7 +139,7 @@ void accSum_reset(void)
|
||||||
}
|
}
|
||||||
|
|
||||||
// rotate acc into Earth frame and calculate acceleration in it
|
// 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 int32_t accZoffset = 0;
|
||||||
static float accz_smooth = 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.
|
* //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;
|
int16_t head;
|
||||||
|
|
||||||
|
@ -234,7 +234,7 @@ int16_t calculateHeading(t_fp_vector *vec)
|
||||||
return head;
|
return head;
|
||||||
}
|
}
|
||||||
|
|
||||||
static void getEstimatedAttitude(void)
|
static void imuCalculateEstimatedAttitude(void)
|
||||||
{
|
{
|
||||||
int32_t axis;
|
int32_t axis;
|
||||||
int32_t accMag = 0;
|
int32_t accMag = 0;
|
||||||
|
@ -295,24 +295,24 @@ static void getEstimatedAttitude(void)
|
||||||
for (axis = 0; axis < 3; axis++) {
|
for (axis = 0; axis < 3; axis++) {
|
||||||
EstM.A[axis] = (EstM.A[axis] * imuRuntimeConfig->gyro_cmpfm_factor + magADC[axis]) * invGyroComplimentaryFilter_M_Factor;
|
EstM.A[axis] = (EstM.A[axis] * imuRuntimeConfig->gyro_cmpfm_factor + magADC[axis]) * invGyroComplimentaryFilter_M_Factor;
|
||||||
}
|
}
|
||||||
heading = calculateHeading(&EstM);
|
heading = imuCalculateHeading(&EstM);
|
||||||
} else {
|
} else {
|
||||||
rotateV(&EstN.V, &deltaGyroAngle);
|
rotateV(&EstN.V, &deltaGyroAngle);
|
||||||
normalizeV(&EstN.V, &EstN.V);
|
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;
|
static int16_t gyroYawSmooth = 0;
|
||||||
|
|
||||||
gyroGetADC();
|
gyroUpdate();
|
||||||
if (sensors(SENSOR_ACC)) {
|
if (sensors(SENSOR_ACC)) {
|
||||||
updateAccelerationReadings(accelerometerTrims);
|
updateAccelerationReadings(accelerometerTrims);
|
||||||
getEstimatedAttitude();
|
imuCalculateEstimatedAttitude();
|
||||||
} else {
|
} else {
|
||||||
accADC[X] = 0;
|
accADC[X] = 0;
|
||||||
accADC[Y] = 0;
|
accADC[Y] = 0;
|
||||||
|
|
|
@ -30,7 +30,7 @@ typedef struct imuRuntimeConfig_s {
|
||||||
int8_t small_angle;
|
int8_t small_angle;
|
||||||
} imuRuntimeConfig_t;
|
} imuRuntimeConfig_t;
|
||||||
|
|
||||||
void configureIMU(
|
void imuConfigure(
|
||||||
imuRuntimeConfig_t *initialImuRuntimeConfig,
|
imuRuntimeConfig_t *initialImuRuntimeConfig,
|
||||||
pidProfile_t *initialPidProfile,
|
pidProfile_t *initialPidProfile,
|
||||||
accDeadband_t *initialAccDeadband,
|
accDeadband_t *initialAccDeadband,
|
||||||
|
@ -39,11 +39,11 @@ void configureIMU(
|
||||||
);
|
);
|
||||||
|
|
||||||
void calculateEstimatedAltitude(uint32_t currentTime);
|
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);
|
float calculateThrottleAngleScale(uint16_t throttle_correction_angle);
|
||||||
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
|
int16_t calculateThrottleAngleCorrection(uint8_t throttle_correction_value);
|
||||||
float calculateAccZLowPassFilterRCTimeConstant(float accz_lpf_cutoff);
|
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);
|
||||||
|
|
|
@ -104,7 +104,7 @@ void beepcodeInit(failsafe_t *initialFailsafe);
|
||||||
void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
|
void gpsInit(serialConfig_t *serialConfig, gpsConfig_t *initialGpsConfig);
|
||||||
void navigationInit(gpsProfile_t *initialGpsProfile, pidProfile_t *pidProfile);
|
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);
|
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 displayInit(rxConfig_t *intialRxConfig);
|
||||||
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse);
|
void ledStripInit(ledConfig_t *ledConfigsToUse, hsvColor_t *colorsToUse, failsafe_t* failsafeToUse);
|
||||||
void loop(void);
|
void loop(void);
|
||||||
|
@ -267,7 +267,7 @@ void init(void)
|
||||||
LED0_OFF;
|
LED0_OFF;
|
||||||
LED1_OFF;
|
LED1_OFF;
|
||||||
|
|
||||||
initIMU();
|
imuInit();
|
||||||
mixerInit(masterConfig.mixerMode, masterConfig.customMixer);
|
mixerInit(masterConfig.mixerMode, masterConfig.customMixer);
|
||||||
|
|
||||||
#ifdef MAG
|
#ifdef MAG
|
||||||
|
|
|
@ -663,7 +663,7 @@ void loop(void)
|
||||||
if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
|
if (masterConfig.looptime == 0 || (int32_t)(currentTime - loopTime) >= 0) {
|
||||||
loopTime = currentTime + masterConfig.looptime;
|
loopTime = currentTime + masterConfig.looptime;
|
||||||
|
|
||||||
computeIMU(¤tProfile->accelerometerTrims, masterConfig.mixerMode);
|
imuUpdate(¤tProfile->accelerometerTrims, masterConfig.mixerMode);
|
||||||
|
|
||||||
// Measure loop rate just after reading the sensors
|
// Measure loop rate just after reading the sensors
|
||||||
currentTime = micros();
|
currentTime = micros();
|
||||||
|
|
|
@ -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.
|
// 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.
|
||||||
|
|
||||||
|
|
|
@ -26,6 +26,6 @@ typedef struct gyroConfig_s {
|
||||||
|
|
||||||
void useGyroConfig(gyroConfig_t *gyroConfigToUse);
|
void useGyroConfig(gyroConfig_t *gyroConfigToUse);
|
||||||
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
|
void gyroSetCalibrationCycles(uint16_t calibrationCyclesRequired);
|
||||||
void gyroGetADC(void);
|
void gyroUpdate(void);
|
||||||
bool isGyroCalibrationComplete(void);
|
bool isGyroCalibrationComplete(void);
|
||||||
|
|
||||||
|
|
Loading…
Reference in New Issue