Merge ada295f0eb into acbab53d13

src/main/cli/settings.c

@@ -1800,6 +1800,7 @@ const clivalue_t valueTable[] = {
 // PG_RANGEFINDER_CONFIG
 #ifdef USE_RANGEFINDER
     { "rangefinder_hardware", VAR_UINT8 | MASTER_VALUE | MODE_LOOKUP, .config.lookup = { TABLE_RANGEFINDER_HARDWARE }, PG_RANGEFINDER_CONFIG, offsetof(rangefinderConfig_t, rangefinder_hardware) },
+    { "rangefinder_trust"   , VAR_UINT8 | MASTER_VALUE,               .config.minmaxUnsigned = { 0, 100 }            , PG_RANGEFINDER_CONFIG, offsetof(rangefinderConfig_t, rangefinder_trust) },
 #endif
 
 // PG_OPTICALFLOW_CONFIG

src/main/flight/position.c

@@ -44,6 +44,10 @@
 #include "sensors/sensors.h"
 #include "sensors/barometer.h"
 
+#ifdef USE_RANGEFINDER
+#include "sensors/rangefinder.h"
+#endif
+
 #include "pg/pg.h"
 #include "pg/pg_ids.h"
 
@@ -127,6 +131,10 @@ void calculateEstimatedAltitude(void)
     }
 #endif
 
+#ifdef USE_RANGEFINDER
+    float rangefinderAltCm = rangefinderGetLatestAltitude();;
+    float rangefinderTrust = (float)(rangefinderConfig()->rangefinder_trust) / 100.0f;
+#endif
     //  ***  DISARMED  ***
     if (!ARMING_FLAG(ARMED)) {
         if (wasArmed) { // things to run once, on disarming, after being armed
@@ -183,6 +191,12 @@ void calculateEstimatedAltitude(void)
             zeroedAltitudeCm = baroAltCm; // use Baro if no GPS data, or we want Baro only
         }
     }
+#ifdef USE_RANGEFINDER
+    if (rangefinderAltCm > 0) {
+    // mix it with the range finder 
+        zeroedAltitudeCm = (rangefinderTrust * rangefinderAltCm) + ((1.0 - rangefinderTrust)  * zeroedAltitudeCm);
+    }   
+#endif
 
     zeroedAltitudeCm = pt2FilterApply(&altitudeLpf, zeroedAltitudeCm);
     // NOTE: this filter must receive 0 as its input, for the whole disarmed time, to ensure correct zeroed values on arming

src/main/sensors/rangefinder.c

@@ -70,10 +70,11 @@ rangefinder_t rangefinder;
 #define RANGEFINDER_DYNAMIC_THRESHOLD           600     //Used to determine max. usable rangefinder disatance
 #define RANGEFINDER_DYNAMIC_FACTOR              75
 
-PG_REGISTER_WITH_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, PG_RANGEFINDER_CONFIG, 0);
+PG_REGISTER_WITH_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig, PG_RANGEFINDER_CONFIG, 1);
 
 PG_RESET_TEMPLATE(rangefinderConfig_t, rangefinderConfig,
     .rangefinder_hardware = RANGEFINDER_NONE,
+    .rangefinder_trust = 0,
 );
 
 #ifdef USE_RANGEFINDER_HCSR04

src/main/sensors/rangefinder.h

@@ -40,6 +40,7 @@ typedef enum {
 
 typedef struct rangefinderConfig_s {
     uint8_t rangefinder_hardware;
+    uint8_t rangefinder_trust;
 } rangefinderConfig_t;
 
 PG_DECLARE(rangefinderConfig_t, rangefinderConfig);