From ec01aa74b21757b20e709b73ff6eee52349fb659 Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Fri, 12 Jul 2019 20:21:45 +0200 Subject: [PATCH 1/2] [OSD] Warnings for unknown heading and unhealthy accelerometer --- src/main/io/osd.c | 13 ++++++++++++- src/main/navigation/navigation.h | 8 ++++++++ src/main/navigation/navigation_pos_estimator.c | 17 +++++++++++++++++ .../navigation_pos_estimator_private.h | 4 +++- 4 files changed, 40 insertions(+), 2 deletions(-) diff --git a/src/main/io/osd.c b/src/main/io/osd.c index efca3f7f171..0d647f0b3c8 100755 --- a/src/main/io/osd.c +++ b/src/main/io/osd.c @@ -2147,7 +2147,18 @@ static bool osdDrawSingleElement(uint8_t item) // messages to show. const char *messages[5]; unsigned messageCount = 0; - if (FLIGHT_MODE(FAILSAFE_MODE)) { + if (isUsingNavigationModes() || failsafeMayRequireNavigationMode()) { + // If we configure any navigation modes - show navigation-related warnings + uint32_t navWarn = navGetPositionEstimatorWarnings(); + + if (navWarn & NAV_WARNING_ACCELEROMETER_HEALTH) { + messages[messageCount++] = "(ACCEL UNHEALTHY)"; + } + + if (navWarn & NAV_WARNING_HEADING_UNKNOWN) { + messages[messageCount++] = "(HEADING UNHEALTHY)"; + } + } else if (FLIGHT_MODE(FAILSAFE_MODE)) { // In FS mode while being armed too const char *failsafePhaseMessage = osdFailsafePhaseMessage(); const char *failsafeInfoMessage = osdFailsafeInfoMessage(); diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index d8e81ca16d1..fbb7c7096c2 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -488,6 +488,14 @@ bool isAdjustingHeading(void); */ int32_t navigationGetHomeHeading(void); +/* Notify navigation system that accelerometer is not healthy (navigation modes may misbehave) */ +uint32_t navGetPositionEstimatorWarnings(void); + +typedef enum { + NAV_WARNING_ACCELEROMETER_HEALTH = (1 << 0), + NAV_WARNING_HEADING_UNKNOWN = (1 << 1), +} navGetPositionEstimatorWarnings_e; + /* Compatibility data */ extern navSystemStatus_t NAV_Status; diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index d190c0c041e..233654d3092 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -362,6 +362,7 @@ static void updateIMUEstimationWeight(const float dt) // and gradually restore weight back to 1.0 over time if (isAccClipped) { posEstimator.imu.accWeightFactor = 0.0f; + posEstimator.imu.lastClippingTimeMs = millis(); } else { const float relAlpha = dt / (dt + INAV_ACC_CLIPPING_RC_CONSTANT); @@ -806,6 +807,7 @@ void initializePositionEstimator(void) posEstimator.est.flowCoordinates[Y] = 0; posEstimator.imu.accWeightFactor = 0; + posEstimator.imu.lastClippingTimeMs = 0; restartGravityCalibration(); @@ -849,4 +851,19 @@ bool navIsCalibrationComplete(void) return gravityCalibrationComplete(); } +uint32_t navGetPositionEstimatorWarnings(void) +{ + uint32_t warningBits = 0; + + if (!navIsAccelerationUsable() || (millis() - posEstimator.imu.lastClippingTimeMs < INAV_ACC_CLIPPING_WARNING_TIMEOUT_MS)) { + warningBits |= NAV_WARNING_ACCELEROMETER_HEALTH; + } + + if (!navIsHeadingUsable()) { + warningBits |= NAV_WARNING_HEADING_UNKNOWN; + } + + return warningBits; +} + #endif diff --git a/src/main/navigation/navigation_pos_estimator_private.h b/src/main/navigation/navigation_pos_estimator_private.h index 7306936ff6c..0e4be554252 100644 --- a/src/main/navigation/navigation_pos_estimator_private.h +++ b/src/main/navigation/navigation_pos_estimator_private.h @@ -52,6 +52,7 @@ #define INAV_SURFACE_AVERAGE_HZ 1.0f #define INAV_ACC_CLIPPING_RC_CONSTANT (0.010f) // Reduce acc weight for ~10ms after clipping +#define INAV_ACC_CLIPPING_WARNING_TIMEOUT_MS 500 #define RANGEFINDER_RELIABILITY_RC_CONSTANT (0.47802f) #define RANGEFINDER_RELIABILITY_LIGHT_THRESHOLD (0.15f) @@ -128,7 +129,8 @@ typedef struct { } navPositionEstimatorESTIMATE_t; typedef struct { - timeUs_t lastUpdateTime; + timeUs_t lastUpdateTime; + timeMs_t lastClippingTimeMs; fpVector3_t accelNEU; fpVector3_t accelBias; float calibratedGravityCMSS; From c8a0a09dc20b96eda1de3760468678fd71034d9c Mon Sep 17 00:00:00 2001 From: "Konstantin (DigitalEntity) Sharlaimov" Date: Fri, 12 Jul 2019 20:29:51 +0200 Subject: [PATCH 2/2] [NAV] Add option to disable acc weight reduction on clipping --- src/main/fc/settings.yaml | 3 +++ src/main/navigation/navigation.h | 1 + src/main/navigation/navigation_pos_estimator.c | 15 ++++++++++----- 3 files changed, 14 insertions(+), 5 deletions(-) diff --git a/src/main/fc/settings.yaml b/src/main/fc/settings.yaml index 7d856dac02d..4f860bcf4d1 100644 --- a/src/main/fc/settings.yaml +++ b/src/main/fc/settings.yaml @@ -1196,6 +1196,9 @@ groups: type: positionEstimationConfig_t condition: USE_NAV members: + - name: inav_avoid_acc_clipping + field: avoid_accel_clipping + type: bool - name: inav_auto_mag_decl field: automatic_mag_declination type: bool diff --git a/src/main/navigation/navigation.h b/src/main/navigation/navigation.h index fbb7c7096c2..e05fa8dce81 100755 --- a/src/main/navigation/navigation.h +++ b/src/main/navigation/navigation.h @@ -110,6 +110,7 @@ typedef struct positionEstimationConfig_s { uint8_t gravity_calibration_tolerance; // Tolerance of gravity calibration (cm/s/s) uint8_t use_gps_velned; uint8_t allow_dead_reckoning; + uint8_t avoid_accel_clipping; uint16_t max_surface_altitude; diff --git a/src/main/navigation/navigation_pos_estimator.c b/src/main/navigation/navigation_pos_estimator.c index 233654d3092..5bfa76eb8ed 100755 --- a/src/main/navigation/navigation_pos_estimator.c +++ b/src/main/navigation/navigation_pos_estimator.c @@ -54,7 +54,7 @@ navigationPosEstimator_t posEstimator; -PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 4); +PG_REGISTER_WITH_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, PG_POSITION_ESTIMATION_CONFIG, 5); PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, // Inertial position estimator parameters @@ -64,6 +64,7 @@ PG_RESET_TEMPLATE(positionEstimationConfig_t, positionEstimationConfig, .gravity_calibration_tolerance = 5, // 5 cm/s/s calibration error accepted (0.5% of gravity) .use_gps_velned = 1, // "Disabled" is mandatory with gps_dyn_model = Pedestrian .allow_dead_reckoning = 0, + .avoid_accel_clipping = 1, .max_surface_altitude = 200, @@ -375,10 +376,14 @@ static void updateIMUEstimationWeight(const float dt) float navGetAccelerometerWeight(void) { - const float accWeightScaled = posEstimator.imu.accWeightFactor * positionEstimationConfig()->w_xyz_acc_p; - DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000); - - return accWeightScaled; + if (positionEstimationConfig()->avoid_accel_clipping) { + const float accWeightScaled = posEstimator.imu.accWeightFactor * positionEstimationConfig()->w_xyz_acc_p; + DEBUG_SET(DEBUG_VIBE, 5, accWeightScaled * 1000); + return accWeightScaled; + } + else { + return positionEstimationConfig()->w_xyz_acc_p; + } } static void updateIMUTopic(timeUs_t currentTimeUs)