sonar althold

Conflicts:

	src/imu.c

src/main/drivers/sonar_hcsr04.c

@@ -59,8 +59,14 @@ void ECHO_EXTI_IRQHandler(void)
             // object we take half of the distance traveled.
             //
             // 340 m/s = 0.034 cm/microsecond = 29.41176471 *2 = 58.82352941 rounded to 59
-            int32_t pulse_duration = timing_stop - timing_start;
-            *distance_ptr = pulse_duration / 59;
+            int32_t distance = (timing_stop - timing_start) / 59;
+
+            // this sonar range is up to 4meter , but 3meter is accurate enough (+tilted and roll)
+            if (distance > 300)
+                distance = -1;
+
+            *distance_ptr = distance;
+
         }
     }
 

src/main/flight/imu.c

@@ -36,6 +36,7 @@
 #include "sensors/compass.h"
 #include "sensors/acceleration.h"
 #include "sensors/barometer.h"
+#include "sensors/sonar.h"
 
 #include "config/runtime_config.h"
 
@@ -416,6 +417,10 @@ void calculateEstimatedAltitude(uint32_t currentTime)
     static float accAlt = 0.0f;
     static int32_t lastBaroAlt;
 
+    static int32_t baroAlt_offset = 0;
+    float sonarTransition;
+
+
     dTime = currentTime - previousTime;
     if (dTime < BARO_UPDATE_FREQUENCY_40HZ)
         return;
@@ -428,6 +433,20 @@ void calculateEstimatedAltitude(uint32_t currentTime)
         accAlt = 0;
     }
     BaroAlt = baroCalculateAltitude();
+    sonarAlt = sonarCalculateAltitude(sonarAlt, &inclination);
+
+	if (sonarAlt > 0 && sonarAlt < 200) {
+		baroAlt_offset = BaroAlt - sonarAlt;
+		BaroAlt = sonarAlt;
+	} else {
+		BaroAlt -= baroAlt_offset;
+		if (sonarAlt > 0) {
+			sonarTransition = (300 - sonarAlt) / 100.0f;
+			BaroAlt = sonarAlt * sonarTransition + BaroAlt * (1.0f - sonarTransition);
+		}
+	}
+
+
     dt = accTimeSum * 1e-6f; // delta acc reading time in seconds
 
     // Integrator - velocity, cm/sec
@@ -451,8 +470,11 @@ void calculateEstimatedAltitude(uint32_t currentTime)
         return;
     }
 
-
-    EstAlt = accAlt;
+    if (sonarAlt > 0 && sonarAlt < 200) {
+    	EstAlt = BaroAlt;
+	} else {
+		EstAlt = accAlt;
+    }
 
     baroVel = (BaroAlt - lastBaroAlt) * 1000000.0f / dTime;
     lastBaroAlt = BaroAlt;

src/main/sensors/barometer.c

@@ -21,6 +21,8 @@
 
 #include "platform.h"
 
+#include "common/maths.h"
+
 #include "drivers/barometer.h"
 #include "config/config.h"
 

src/main/sensors/sonar.c

@@ -20,13 +20,18 @@
 
 #include "platform.h"
 
+#include "common/maths.h"
+#include "common/axis.h"
+
 #include "drivers/sonar_hcsr04.h"
 #include "config/runtime_config.h"
 
+#include "flight/flight.h"
+
 #include "sensors/sensors.h"
 #include "sensors/sonar.h"
 
-int32_t sonarAlt;              // to think about the unit
+int32_t sonarAlt = -1;	// in cm , -1 indicate sonar is not in range
 
 #ifdef SONAR
 
@@ -42,4 +47,15 @@ void Sonar_update(void)
     hcsr04_get_distance(&sonarAlt);
 }
 
+int16_t sonarCalculateAltitude(int32_t sonarAlt, rollAndPitchInclination_t *angle)
+{
+	// calculate sonar altitude
+	int16_t sonarAnglecorrection = max(abs(angle->values.rollDeciDegrees), abs(angle->values.pitchDeciDegrees));
+	if (sonarAnglecorrection > 250)
+		return -1;
+
+	return sonarAlt * (900.0f - sonarAnglecorrection) / 900.0f;
+}
+
+
 #endif

src/main/sensors/sonar.h

@@ -17,5 +17,9 @@
 
 #pragma once
 
+extern int32_t sonarAlt;
+
 void Sonar_init(void);
 void Sonar_update(void);
+
+int16_t sonarCalculateAltitude(int32_t sonarAlt, rollAndPitchInclination_t *angle);