Cleaning the debugging and catching a bug in the baro code

RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/barometer.hpp

@@ -60,6 +60,7 @@ void barometer_setup() {
   }
   else {
     if (DEBUG) { Serial.println("Failure"); }
+    delay(1500); 
   }
   bmp.setSampling(Adafruit_BMP280::MODE_NORMAL,     
   Adafruit_BMP280::SAMPLING_X1,   
@@ -75,9 +76,10 @@ void cmpt_vertical_speed_baro(float da, int dt) {
 }
 
 void get_baro(int mode) {
-  if ((millis()-baroA)>=10 and mode == 0) { 
+  if ((millis()-baroA)>=10 and mode == 0) {
+     
     if (DEBUG) { Serial.println("Got a Baro message"); } 
-    
+  
     baroA = millis();
     unsigned waitd = millis(); 
     
@@ -86,12 +88,12 @@ void get_baro(int mode) {
         
     alt_baro = (b_al.reading(alt_baro*100.0)/100.0);
     pressure_baro = (ps.reading(pressure_baro*100.0)/100.0);
-    
+
     if (((millis() - waitd) >= 100)) { 
       barometer_setup(); 
     } 
     baro_count = (baro_count + 1);;
-     
+    
     if (baro_count >= BARO_VS_SAMPLE) { 
       baro_count = 0; 
       cmpt_vertical_speed_baro(alt_baro-prev_alt_baro, baroA-baroB); 
@@ -101,7 +103,7 @@ void get_baro(int mode) {
     } 
   }
 
-  else {
+  else if (mode == 1) {
     baroA = millis();
     unsigned waitd = millis(); 
     

RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/position.hpp

@@ -24,7 +24,7 @@ movingAvg gps_v(GPS_SAFE_AVG);
 movingAvg baro_v(BARO_SAFE_AVG);
 movingAvg ps_p(PRE_PE_AVG);  
 
-float pressure_percentage;
+double pressure_percentage;
 
 void position_setup() {
   gps_v.begin(); 
@@ -38,6 +38,8 @@ void cmpt_fusion() {
     new_gps_fusion = false; 
      
     pressure_percentage = (ps_p.reading((pressure_baro / (baro_set*100.0))*100.0)/100.0) ;
+    pressure_percentage = constrain(pressure_percentage, 5, 100); 
+    
     baro_alt_weight = pressure_percentage;
     baro_vspeed_weight = pressure_percentage*pressure_percentage;
 
@@ -110,7 +112,8 @@ bool is_descent(int v_trigger, bool mode) {
 }
 
 float pressure_sqrt_ratio() {
-  return sqrt((baro_set*100.0)/pressure_baro);
+  double raw_ratio = sqrt((baro_set*100.0)/pressure_baro);
+  return constrain(raw_ratio, 1, 5); 
 }
 
 float v_down(int vdown) {