Diff: main.cpp

Revision:

1:b6a29c7aeaa9

Parent:

0:f6f6f699ab24

--- a/main.cpp	Wed Jun 29 12:12:24 2016 +0000
+++ b/main.cpp	Fri Aug 19 07:56:26 2016 +0000
@@ -1,22 +1,54 @@
- 
 #include "mbed.h"
 #include "Barometer.h"
 #include "math.h"
+
 Serial pc(USBTX, USBRX);
-Barometer barometer(p28, p27);
- 
-int main(void)
-{
-    float p = 0.0f, t = 0.0f, alt = 0.0f;
- 
-    while(1) {
-        barometer.update();
-        p = barometer.get_pressure();
-        t = barometer.get_temperature();
-//        alt = pow((p/101325.0),(-5.25588));  //p = 101325 (1 - 2.25577 10-5 h)^5.25588 
+
+////////////////////////////////////////////
+//    Barometer     3.3V p9(SDA) p10(SCL) //  
+////////////////////////////////////////////
+Barometer barometer(p9, p10);
+float alt, comb_alt,alt_GPS=0, alt_GPS_zero=0;
+float alt_sum=0.0f, alt_zero=0.0f;
+int count = 0, baro_ok = 0;          // for zero-calibration
+float alt_buffer[2], w_alt=0;          // weight for LPF
 
-        alt = (1.0-pow((p/101325.0),(-5.25588)))*(100000)/(2.25577);  //p = 101325 (1 - 2.25577 10-5 h)^5.25588 
-        pc.printf("Pressure: %6.2f Temperature(c): %6.2f Altitude(m): %6.2f \r\n", p, t, alt);
+void calb_alt(){
+    if (alt==0){count=0;}
+    else {
+        if (count==1){count++;}
+        else{
+            if (count<=99){alt_sum = alt_sum + alt; count++;}
+            else {
+                alt_zero = alt_sum / (float)(count-1);
+                alt_GPS_zero = alt_GPS;
+                count++;
+            }
+        }
     }
 }
- 
\ No newline at end of file
+
+void get_Baro(float*alt)
+{   
+        barometer.update();
+        *alt = barometer.get_altitude_m();
+}  
+
+
+
+int main(void)
+{     
+    while(1) {
+        if (100<count) {                
+            get_Baro(&alt);
+            alt = alt - alt_zero;
+            alt_GPS = alt_GPS - alt_GPS_zero;
+            comb_alt = 0.8*alt + 0.2*alt_GPS;
+            pc.printf("%f %f %f %f %f \r\n",alt,alt_GPS,alt_sum,alt_zero,comb_alt);
+        }
+        else {
+            get_Baro(&alt);
+            calb_alt();
+            pc.printf("%i %f %f %f %f \r\n",count,alt,alt_GPS,alt_sum,alt_zero);}
+}
+} 
\ No newline at end of file