From 565f1f4db5f52119e19c68091d4778e285f95890 Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Fri, 11 Oct 2019 16:43:25 -0400 Subject: [PATCH 1/2] Add ACC_CALIB arming disabled reason if ACC is required but not calibrated Checks various features, modes, and OSD elements to determine if ACC is needed. Generates an arming disabled warning if ACC calibration has never been completed. --- src/main/fc/core.c | 63 +++++++++++++++++++++++++++++++++ src/main/fc/runtime_config.c | 1 + src/main/fc/runtime_config.h | 3 +- src/main/osd/osd.c | 20 +++++++++++ src/main/osd/osd.h | 1 + src/main/osd/osd_elements.c | 25 +++++++++++++ src/main/osd/osd_elements.h | 1 + src/main/sensors/acceleration.c | 8 +++++ src/main/sensors/acceleration.h | 1 + 9 files changed, 122 insertions(+), 1 deletion(-) 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); From 365c6cb1f63693fa915d7a410c24d4a1f2631d0d Mon Sep 17 00:00:00 2001 From: Bruce Luckcuck Date: Mon, 21 Oct 2019 11:49:11 -0400 Subject: [PATCH 2/2] Move calibrationCompleted flag to a 4th element in the accZero structure Allows the flag indicating that calibration was completed to be output in the values for `acc_calibration`. --- src/main/cli/settings.c | 4 +++- src/main/fc/core.c | 2 +- src/main/sensors/acceleration.c | 6 +++--- src/main/sensors/acceleration.h | 1 - src/main/sensors/compass.c | 2 +- src/main/sensors/sensors.h | 3 ++- 6 files changed, 10 insertions(+), 8 deletions(-) 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 312d620874..82f6110a04 100644 --- a/src/main/fc/core.c +++ b/src/main/fc/core.c @@ -220,7 +220,7 @@ static bool accNeedsCalibration(void) if (sensors(SENSOR_ACC)) { // Check to see if the ACC has already been calibrated - if (accelerometerConfig()->calibrationCompleted || + if (accelerometerConfig()->accZero.values.calibrationCompleted || accelerometerConfig()->accZero.values.roll != 0 || accelerometerConfig()->accZero.values.pitch != 0 || accelerometerConfig()->accZero.values.yaw != 0) { diff --git a/src/main/sensors/acceleration.c b/src/main/sensors/acceleration.c index 35cffad022..57a4fc03a9 100644 --- a/src/main/sensors/acceleration.c +++ b/src/main/sensors/acceleration.c @@ -98,7 +98,7 @@ void resetRollAndPitchTrims(rollAndPitchTrims_t *rollAndPitchTrims) static void setConfigCalibrationCompleted(void) { - accelerometerConfigMutable()->calibrationCompleted = true; + accelerometerConfigMutable()->accZero.values.calibrationCompleted = 1; } void accResetRollAndPitchTrims(void) @@ -111,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,13 +125,12 @@ 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); } -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; diff --git a/src/main/sensors/acceleration.h b/src/main/sensors/acceleration.h index 8d2b98cb35..4ad1b16426 100644 --- a/src/main/sensors/acceleration.h +++ b/src/main/sensors/acceleration.h @@ -72,7 +72,6 @@ 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); 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;