Skip to content

Commit

Permalink
remove gravity calibration changes
Browse files Browse the repository at this point in the history
breadoven committed Aug 13, 2024
1 parent 3a4c348 commit cc59175
Showing 1 changed file with 3 additions and 9 deletions.
12 changes: 3 additions & 9 deletions src/main/navigation/navigation_pos_estimator.c
Original file line number Diff line number Diff line change
@@ -345,11 +345,6 @@ static bool gravityCalibrationComplete(void)
return zeroCalibrationIsCompleteS(&posEstimator.imu.gravityCalibration);
}

static bool gravityCalibrationIsValid(void)
{
return gravityCalibrationComplete() || posEstimator.imu.calibratedGravityCMSS;
}

#define ACC_VIB_FACTOR_S 1.0f
#define ACC_VIB_FACTOR_E 3.0f
static void updateIMUEstimationWeight(const float dt)
@@ -417,13 +412,12 @@ static void updateIMUTopic(timeUs_t currentTimeUs)

/* When unarmed, assume that accelerometer should measure 1G. Use that to correct accelerometer gain */
if (gyroConfig()->init_gyro_cal_enabled) {
if (!ARMING_FLAG(WAS_EVER_ARMED)) {
if (!ARMING_FLAG(ARMED) && !gravityCalibrationComplete()) {
zeroCalibrationAddValueS(&posEstimator.imu.gravityCalibration, posEstimator.imu.accelNEU.z);

if (gravityCalibrationComplete()) {
zeroCalibrationGetZeroS(&posEstimator.imu.gravityCalibration, &posEstimator.imu.calibratedGravityCMSS);
setGravityCalibration(posEstimator.imu.calibratedGravityCMSS);
restartGravityCalibration(); // Repeat calibration until first arm to continuously update zero datum
LOG_DEBUG(POS_ESTIMATOR, "Gravity calibration complete (%d)", (int)lrintf(posEstimator.imu.calibratedGravityCMSS));
}
}
@@ -432,7 +426,7 @@ static void updateIMUTopic(timeUs_t currentTimeUs)
posEstimator.imu.calibratedGravityCMSS = gyroConfig()->gravity_cmss_cal;
}

if (gravityCalibrationIsValid()) {
if (gravityCalibrationComplete()) {
#ifdef USE_SIMULATOR
if (ARMING_FLAG(SIMULATOR_MODE_HITL) || ARMING_FLAG(SIMULATOR_MODE_SITL)) {
posEstimator.imu.calibratedGravityCMSS = GRAVITY_CMSS;
@@ -929,5 +923,5 @@ void updatePositionEstimator(void)

bool navIsCalibrationComplete(void)
{
return gravityCalibrationIsValid();
return gravityCalibrationComplete();
}

0 comments on commit cc59175

Please sign in to comment.