diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 62b59d4c80..312d620874 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -214,6 +214,61 @@ void resetArmingDisabled(void) lastArmingDisabledReason = 0; } +#ifdef USE_ACC +static bool accNeedsCalibration(void) +{ + if (sensors(SENSOR_ACC)) { + + // Check to see if the ACC has already been calibrated + if (accelerometerConfig()->calibrationCompleted || + accelerometerConfig()->accZero.values.roll != 0 || + accelerometerConfig()->accZero.values.pitch != 0 || + accelerometerConfig()->accZero.values.yaw != 0) { + + return false; + } + + // We've determined that there's a detected ACC that has not + // yet been calibrated. Check to see if anything is using the + // ACC that would be affected by the lack of calibration. + + // Check for any configured modes that use the ACC + if (isModeActivationConditionPresent(BOXANGLE) || + isModeActivationConditionPresent(BOXHORIZON) || + isModeActivationConditionPresent(BOXGPSRESCUE) || + isModeActivationConditionPresent(BOXCAMSTAB) || + isModeActivationConditionPresent(BOXCALIB) || + isModeActivationConditionPresent(BOXACROTRAINER)) { + + return true; + } + + // Launch Control only requires the ACC if a angle limit is set + if (isModeActivationConditionPresent(BOXLAUNCHCONTROL) && currentPidProfile->launchControlAngleLimit) { + return true; + } + +#ifdef USE_OSD + // Check for any enabled OSD elements that need the ACC + if (featureIsEnabled(FEATURE_OSD)) { + if (osdNeedsAccelerometer()) { + return true; + } + } +#endif + +#ifdef USE_GPS_RESCUE + // Check if failsafe will use GPS Rescue + if (failsafeConfig()->failsafe_procedure == FAILSAFE_PROCEDURE_GPS_RESCUE) { + return true; + } +#endif + } + + return false; +} +#endif + void updateArmingStatus(void) { if (ARMING_FLAG(ARMED)) { @@ -328,6 +383,14 @@ void updateArmingStatus(void) setArmingDisabled(ARMING_DISABLED_PARALYZE); } +#ifdef USE_ACC + if (accNeedsCalibration()) { + setArmingDisabled(ARMING_DISABLED_ACC_CALIBRATION); + } else { + unsetArmingDisabled(ARMING_DISABLED_ACC_CALIBRATION); + } +#endif + if (!isUsingSticksForArming()) { /* Ignore ARMING_DISABLED_CALIBRATING if we are going to calibrate gyro on first arm */ bool ignoreGyro = armingConfig()->gyro_cal_on_first_arm diff --git a/src/main/fc/runtime_config.c b/src/main/fc/runtime_config.c index aee482ec2c..c2ee31826f 100644 --- a/src/main/fc/runtime_config.c +++ b/src/main/fc/runtime_config.c @@ -57,6 +57,7 @@ const char *armingDisableFlagNames[]= { "RPMFILTER", "REBOOT_REQD", "DSHOT_BBANG", + "ACC_CALIB", "ARMSWITCH", }; diff --git a/src/main/fc/runtime_config.h b/src/main/fc/runtime_config.h index 84dfd3822a..c6243a6f5a 100644 --- a/src/main/fc/runtime_config.h +++ b/src/main/fc/runtime_config.h @@ -63,7 +63,8 @@ typedef enum { ARMING_DISABLED_RPMFILTER = (1 << 20), ARMING_DISABLED_REBOOT_REQUIRED = (1 << 21), ARMING_DISABLED_DSHOT_BITBANG = (1 << 22), - ARMING_DISABLED_ARM_SWITCH = (1 << 23), // Needs to be the last element, since it's always activated if one of the others is active when arming + ARMING_DISABLED_ACC_CALIBRATION = (1 << 23), + ARMING_DISABLED_ARM_SWITCH = (1 << 24), // Needs to be the last element, since it's always activated if one of the others is active when arming } armingDisableFlags_e; #define ARMING_DISABLE_FLAGS_COUNT (LOG2(ARMING_DISABLED_ARM_SWITCH) + 1) diff --git a/src/main/osd/osd.c b/src/main/osd/osd.c index 723581bb09..e3a4f57a3f 100644 --- a/src/main/osd/osd.c +++ b/src/main/osd/osd.c @@ -136,6 +136,10 @@ PG_REGISTER_WITH_RESET_FN(osdConfig_t, osdConfig, PG_OSD_CONFIG, 6); // be shown on the the post-flight statistics page. // If you reorder the stats it's likely that you'll need to make likewise updates // to the unit tests. + +// If adding new stats, please add to the osdStatsNeedAccelerometer() function +// if the statistic utilizes the accelerometer. +// const osd_stats_e osdStatsDisplayOrder[OSD_STAT_COUNT] = { OSD_STAT_RTC_DATE_TIME, OSD_STAT_TIMER_1, @@ -950,4 +954,20 @@ statistic_t *osdGetStats(void) { return &stats; } + +#ifdef USE_ACC +// Determine if there are any enabled stats that need +// the ACC (currently only MAX_G_FORCE). +static bool osdStatsNeedAccelerometer(void) +{ + return osdStatGetState(OSD_STAT_MAX_G_FORCE); +} + +// Check if any enabled elements or stats need the ACC +bool osdNeedsAccelerometer(void) +{ + return osdStatsNeedAccelerometer() || osdElementsNeedAccelerometer(); +} +#endif // USE_ACC + #endif // USE_OSD diff --git a/src/main/osd/osd.h b/src/main/osd/osd.h index dc1f5b9416..05066aad68 100644 --- a/src/main/osd/osd.h +++ b/src/main/osd/osd.h @@ -316,3 +316,4 @@ void changeOsdProfileIndex(uint8_t profileIndex); bool osdElementVisible(uint16_t value); bool osdGetVisualBeeperState(void); statistic_t *osdGetStats(void); +bool osdNeedsAccelerometer(void); diff --git a/src/main/osd/osd_elements.c b/src/main/osd/osd_elements.c index 2d0053d133..9e43bd6cd4 100644 --- a/src/main/osd/osd_elements.c +++ b/src/main/osd/osd_elements.c @@ -36,6 +36,8 @@ Add the mapping from the element ID added in the first step to the function created in the third step to the osdElementDrawFunction array. + If the new element utilizes the accelerometer, add it to the osdElementsNeedAccelerometer() function. + Finally add a CLI parameter for the new element in cli/settings.c. */ @@ -1779,4 +1781,27 @@ void osdUpdateAlarms(void) #endif } +#ifdef USE_ACC +static bool osdElementIsActive(osd_items_e element) +{ + for (unsigned i = 0; i < activeOsdElementCount; i++) { + if (activeOsdElementArray[i] == element) { + return true; + } + } + return false; +} + +// Determine if any active elements need the ACC +bool osdElementsNeedAccelerometer(void) +{ + return osdElementIsActive(OSD_ARTIFICIAL_HORIZON) || + osdElementIsActive(OSD_PITCH_ANGLE) || + osdElementIsActive(OSD_ROLL_ANGLE) || + osdElementIsActive(OSD_G_FORCE) || + osdElementIsActive(OSD_FLIP_ARROW); +} + +#endif // USE_ACC + #endif // USE_OSD diff --git a/src/main/osd/osd_elements.h b/src/main/osd/osd_elements.h index 8e412c5a12..ffcfc374fe 100644 --- a/src/main/osd/osd_elements.h +++ b/src/main/osd/osd_elements.h @@ -49,3 +49,4 @@ void osdAnalyzeActiveElements(void); void osdDrawActiveElements(displayPort_t *osdDisplayPort, timeUs_t currentTimeUs); void osdResetAlarms(void); void osdUpdateAlarms(void); +bool osdElementsNeedAccelerometer(void); diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index a57f8294e3..35cffad022 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -96,6 +96,11 @@ void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims) ); } +static void setConfigCalibrationCompleted(void) +{ + accelerometerConfigMutable()->calibrationCompleted = true; +} + void accResetRollAndPitchTrims(void) { resetRollAndPitchTrims(&accelerometerConfigMutable()->accelerometerTrims); @@ -119,6 +124,7 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance) .acc_lpf_hz = 10, .acc_hardware = ACC_DEFAULT, .acc_high_fsr = false, + .calibrationCompleted = false, ); resetRollAndPitchTrims(&instance->accelerometerTrims); resetFlightDynamicsTrims(&instance->accZero); @@ -405,6 +411,7 @@ static void performAcclerationCalibration(rollAndPitchTrims_t *rollAndPitchTrims accelerationTrims->raw[Z] = (a[Z] + (CALIBRATING_ACC_CYCLES / 2)) / CALIBRATING_ACC_CYCLES - acc.dev.acc_1G; resetRollAndPitchTrims(rollAndPitchTrims); + setConfigCalibrationCompleted(); saveConfigAndNotify(); } @@ -459,6 +466,7 @@ static void performInflightAccelerationCalibration(rollAndPitchTrims_t *rollAndP accelerationTrims->raw[Z] = b[Z] / 50 - acc.dev.acc_1G; // for nunchuck 200=1G resetRollAndPitchTrims(rollAndPitchTrims); + setConfigCalibrationCompleted(); saveConfigAndNotify(); } diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 4ad1b16426..8d2b98cb35 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -72,6 +72,7 @@ typedef struct accelerometerConfig_s { bool acc_high_fsr; flightDynamicsTrims_t accZero; rollAndPitchTrims_t accelerometerTrims; + bool calibrationCompleted; // has ACC calibration been performed (should not be exposed in settings) } accelerometerConfig_t; PG_DECLARE(accelerometerConfig_t, accelerometerConfig);