Diff: run.cpp

Revision:

80:162ae3ce4874

Parent:

77:2bf856e3eca4

--- a/run.cpp	Tue Sep 07 08:22:39 2021 +0000
+++ b/run.cpp	Mon Sep 13 07:14:44 2021 +0000
@@ -38,7 +38,15 @@
     LoopTicker PIDtick;
     PIDtick.attach(calcServoOut,PID_dt);
     
-
+    //Kozuka_change
+    float prs_t_sub = 0.0f;
+    float alt_sub = 0.0f;
+    int prsCount = 0;
+    BME280 prs_sensor(PB_9, PB_8);
+    
+    //for test
+    Serial pc(USBTX, USBRX);
+    //Kozuka_change_finish
     
     while(1)
     {
@@ -68,6 +76,29 @@
             obsCount += 1;
         }
         
+        //Kozuka_change
+        if(prsCount == 10){
+            //_t.start();
+            //float tstart = _t.read();
+            float temp = prs_sensor.getTemperature();
+            float press = prs_sensor.getPressure();
+//            pc.printf("pressure: %f \n", press);
+            float alt = getAltitude(temp, press); //??no float-> error why?
+            pc.printf("altitude: %f \n",alt);
+            float prs_t= _t.read();
+            float v_speed = (alt-alt_sub)/(prs_t - prs_t_sub);
+            alt_sub = alt;
+            prs_t_sub = prs_t;
+            
+            //for test
+//            pc.printf("v_speed: %f \n",v_speed);
+            
+            prsCount = 0;
+        }else{
+            prsCount += 1;
+        }
+        //Kozuka_change_finish
+    
         ekf.updateGyroBiasConstraints(gyro);
         //ekf.updateVelocityConstraints();
         //ekf.updateMagMeasures(mag);
@@ -82,6 +113,16 @@
     }
 }
 
+// Kozuka_change
+float getAltitude(float temp, float press) {
+    float seaPress = 1013.25f ;
+    float press1 = pow(seaPress/press, 1.0f / 5.257f ) - 1.0f ;
+    float temp1 = temp + 273.15f ;
+    float alt = press1 * temp1 / 0.0065f ;
+    return alt ;
+}
+//Kozuka_change_finish
+
     /*
     if(serialParamSource){
         while(1){