diff --git a/src/main/cli/settings.c b/src/main/cli/settings.c index 81d37812fb..cb8d677aad 100644 --- a/src/main/cli/settings.c +++ b/src/main/cli/settings.c @@ -663,7 +663,9 @@ const clivalue_t valueTable[] = { { "acc_lpf_hz", VAR_UINT16 | MASTER_VALUE, .config.minmaxUnsigned = { 0, 400 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, acc_lpf_hz) }, { "acc_trim_pitch", VAR_INT16 | MASTER_VALUE, .config.minmax = { -300, 300 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accelerometerTrims.values.pitch) }, { "acc_trim_roll", VAR_INT16 | MASTER_VALUE, .config.minmax = { -300, 300 }, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accelerometerTrims.values.roll) }, - { "acc_calibration", VAR_INT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = XYZ_AXIS_COUNT, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accZero.raw) }, + + // 4 elements are output for the ACC calibration - The 3 axis values and the 4th representing whether calibration has been performed + { "acc_calibration", VAR_INT16 | MASTER_VALUE | MODE_ARRAY, .config.array.length = 4, PG_ACCELEROMETER_CONFIG, offsetof(accelerometerConfig_t, accZero.raw) }, #endif // PG_COMPASS_CONFIG diff --git a/src/main/fc/core.c b/src/main/fc/core.c index 62b59d4c80..82f6110a04 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()->accZero.values.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..57a4fc03a9 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()->accZero.values.calibrationCompleted = 1; +} + void accResetRollAndPitchTrims(void) { resetRollAndPitchTrims(&accelerometerConfigMutable()->accelerometerTrims); @@ -106,6 +111,7 @@ static void resetFlightDynamicsTrims(flightDynamicsTrims_t *accZero) accZero->values.roll = 0; accZero->values.pitch = 0; accZero->values.yaw = 0; + accZero->values.calibrationCompleted = 0; } void accResetFlightDynamicsTrims(void) @@ -124,7 +130,7 @@ void pgResetFn_accelerometerConfig(accelerometerConfig_t *instance) resetFlightDynamicsTrims(&instance->accZero); } -PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 1); +PG_REGISTER_WITH_RESET_FN(accelerometerConfig_t, accelerometerConfig, PG_ACCELEROMETER_CONFIG, 2); extern uint16_t InflightcalibratingA; extern bool AccInflightCalibrationMeasurementDone; @@ -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/compass.c b/src/main/sensors/compass.c index 5d8baa0e0b..cfee7835e4 100644 --- a/src/main/sensors/compass.c +++ b/src/main/sensors/compass.c @@ -58,7 +58,7 @@ magDev_t magDev; mag_t mag; // mag access functions -PG_REGISTER_WITH_RESET_FN(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 1); +PG_REGISTER_WITH_RESET_FN(compassConfig_t, compassConfig, PG_COMPASS_CONFIG, 2); void pgResetFn_compassConfig(compassConfig_t *compassConfig) { diff --git a/src/main/sensors/sensors.h b/src/main/sensors/sensors.h index d26463efb5..531c67c97f 100644 --- a/src/main/sensors/sensors.h +++ b/src/main/sensors/sensors.h @@ -36,10 +36,11 @@ typedef struct int16_flightDynamicsTrims_s { int16_t roll; int16_t pitch; int16_t yaw; + int16_t calibrationCompleted; } flightDynamicsTrims_def_t; typedef union flightDynamicsTrims_u { - int16_t raw[3]; + int16_t raw[4]; flightDynamicsTrims_def_t values; } flightDynamicsTrims_t;