Fixing minor bugs

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

@@ -112,22 +112,11 @@ 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); 
-      baroB = millis(); 
-      prev_alt_baro = alt_baro; 
-      new_baro = true;
-    } 
-  }
-}
 
+    new_baro = true;
+  }
+} 
+ 
 
 String baro_text() {
   String alt_baro_text = String(alt_baro, 3);

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

@@ -61,14 +61,12 @@ void send_data() {
   }
 }
 
-void save_data(bool initialised) { 
+void save_data() { 
-  if (initialised == true) {
+  if (sd_ok == true and SD_WRITING == true) {
-    if (sd_ok == true and SD_WRITING == true) {
+    if (DEBUG) { Serial.println("Saving Data"); }
-      if (DEBUG) { Serial.println("Saving Data"); }
+    dataFile = SD.open(namebuff, FILE_WRITE);
-      dataFile = SD.open(namebuff, FILE_WRITE);
+    dataFile.println(mainSD); 
-      dataFile.println(mainSD); 
+    dataFile.close();
-      dataFile.close();
-    }
   } 
 }
     
@@ -91,8 +89,10 @@ void cmpt_string_data(int flight_mode, bool initialised, bool deployed, bool win
   String packet_count_text = String(packet_count);
   
   String status_text = time_text+","+packet_count_text+","+flight_mode_text+","+initialised_text+","+deployed_text+","+wing_opened_text+","+gps_ok_text+","+cog_ok_text+","+spiral_text+","+failsafe_text+","+vbatt_text+","+loop_rate_text;  
-  
+
-  mainSD = status_text+","+gps_text()+","+baro_text()+","+nav_text()+","+rc_text()+","+servo_text()+","+servo_max_cmd_text();
+  if (initialised) {
+    mainSD = status_text+","+gps_text()+","+baro_text()+","+nav_text()+","+rc_text()+","+servo_text()+","+servo_max_cmd_text();
+  }
   mainTLM = "/*"+status_text+","+gps_text()+","+baro_text()+","+servo_text()+"/*";
 }
 
@@ -111,7 +111,7 @@ void newfile() {
     dataFile = SD.open(namebuff, FILE_WRITE);
     delay(10); 
     if (dataFile) {  
-      dataFile.println("time (ms), Packet_Count (text), Mode (text), Initialised (text), Deployed (text), Wing_Opened (text), GPS_Ok (text), COG_Ok (text), Spiral (text), FailSafe (text), Vbatt (V), Loop_rate (Hz), GPS-date, GPS-time, lat (deg), lon (deg), alt (m), CoG (deg), Speed (m/s), Sat_in_use (text), HDOP (text), Position_Age (text), Fix_type (text), Baro_Alt (m), Pressure (hpa), Baro_Vspeed (m/s), Altitude (m), Baro_Weight, GPS_Weight, Baro_Vspeed_AVG (m/s), GPS_Vspeed_AVG (m/s), VDOWN (m/s), SetPoint_Home (deg), Err_Home (deg), LatB (deg), LonB (deg), WaypointNumber (text), Distance (m), Ch 0 (us), Ch 1 (us), Ch 2 (us), Ch 3 (us), Ch 4 (us), Ch 5 (us), Ch 6 (us), PWM_L (us), PWM_R (us), PWM_D (us), PWM_X (us), MAX_M_W, MAX_C_W, MAC_M, MAX_C");
+      dataFile.println("time (ms), Packet_Count (text), Mode (text), Initialised (text), Deployed (text), Wing_Opened (text), GPS_Ok (text), COG_Ok (text), Spiral (text), FailSafe (text), Vbatt (V), Loop_rate (Hz), GPS-date, GPS-time, lat (deg), lon (deg), alt (m), CoG (deg), Speed (m/s), Sat_in_use (text), HDOP (text), Position_Age (text), Fix_type (text), Baro_Alt (m), Pressure (hpa), Baro_Vspeed (m/s), Altitude (m), Merged_Vspeed (m/s), Baro_Weight, GPS_Weight, Baro_Vspeed_AVG (m/s), GPS_Vspeed_AVG (m/s), VDOWN (m/s), SetPoint_Home (deg), Err_Home (deg), LatB (deg), LonB (deg), WaypointNumber (text), Distance (m), Ch 0 (us), Ch 1 (us), Ch 2 (us), Ch 3 (us), Ch 4 (us), Ch 5 (us), Ch 6 (us), PWM_L (us), PWM_R (us), PWM_D (us), PWM_X (us), MAX_M_W, MAX_C_W, MAC_M, MAX_C");
       dataFile.close();
     }
     if (CONFIG_FILE_SV) {

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

@@ -39,13 +39,13 @@ void flight_init() {
       current_waypoint = waypoint[waypoint_number]; 
     }
 
-    baroset(gps.altitude.meters(), 1); 
-    get_baro(1);
-    cmpt_fusion(); 
-
     b_vs.reset(); 
     b_al.reset(); 
     g_vs.reset();
+    
+    baroset(gps.altitude.meters(), 1); 
+    get_baro(1);
+    merged_alt = gps.altitude.meters(); 
 
     cmpt_string_data(flight_mode, initialised, deployed, wing_opened, spiral);
 
@@ -81,7 +81,7 @@ void ready_steady() {
     setcam(1, 20, 600); 
   }
    
-  if (is_descent(v_down(VDOWN), 0)) {  
+  if (is_descent(v_down(VDOWN), 1)) {  
     flight_mode = 3; 
     EasyBuzzer.beep(3000,100,50,3,500,1); 
     setcam(1, 60, 600); 
@@ -159,7 +159,7 @@ void flight_gliding_auto() {
     myPID.SetMode(MANUAL);
   }
 
-  if (is_descent(v_down(-5), 0)) {
+  if (is_descent(v_down(-5), 1)) {
     spiral = true; 
     spiral_time = millis();
   }
@@ -244,9 +244,9 @@ void flight_gliding_no_gps() {
 
 void cmpt_flight_state() {
 
-  if (flight_mode!=prev_flight_mode) {
+  if (flight_mode!=prev_flight_mode and prev_flight_mode !=0) {
       cmpt_string_data(flight_mode, initialised, deployed, wing_opened, spiral);
-      save_data(initialised); 
+      save_data(); 
       send_data();
       prev_flight_mode = flight_mode; 
   }

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

@@ -52,6 +52,8 @@ void get_gps() {
     new_gps = true; 
   }
 }
+
+
  
 void sendPacket(byte *packet, byte len){
     for (byte i = 0; i < len; i++) { GPS_PORT.write(packet[i]); }

RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/main.ino

@@ -92,15 +92,18 @@ void datacmpt() {
 
   cmpt_flight_state(); 
   cmpt_data_rate(flight_mode); 
+  
   if (initialised) {
+    cmpt_vertical_state();
     cmpt_fusion(); 
   }
-  cmpt_vertical_state(); 
+   
-
   if ((millis()-sd)>=delaySD) { 
     sd = millis(); 
     cmpt_string_data(flight_mode, initialised, deployed, wing_opened, spiral);
-    save_data(initialised);  
+    if (initialised) {
+      save_data();  
+    }
   }
   
   if (millis()-tlm>=delayTLM) { 

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

@@ -46,8 +46,8 @@ void cmpt_fusion() {
     double h_dop = gps.hdop.value(); 
   
     if (gps_ok) { 
-      gps_alt_weight = 50.0/h_dop;
+      gps_alt_weight = constrain((gps.altitude.meters()/3000), 0.1, 10);
-      gps_vspeed_weight = 1; 
+      gps_vspeed_weight = constrain((gps.altitude.meters()/6000), 0.05, 10); 
     }
     else { 
       gps_alt_weight = 0; 
@@ -55,7 +55,7 @@ void cmpt_fusion() {
     }
       
     merged_alt = ((alt_baro*baro_alt_weight)+(gps.altitude.meters()*gps_alt_weight))/(baro_alt_weight+gps_alt_weight); 
-    merged_vspeed = ((baro_vspeed*baro_vspeed_weight)+(gps_vspeed*gps_vspeed_weight))/(baro_vspeed_weight+gps_vspeed_weight); 
+    merged_vspeed = ((baro_stab_factor*baro_vspeed_weight)+(gps_stab_factor*gps_vspeed_weight))/(baro_vspeed_weight+gps_vspeed_weight); 
     
   }
 }
@@ -64,7 +64,7 @@ void cmpt_vertical_state() {
   if (new_baro) {
     new_baro = false; 
     new_baro_fusion = true;
-     baro_stab_factor = (baro_v.reading(baro_vspeed*100.0))/100.0;
+    baro_stab_factor = (baro_v.reading(baro_vspeed*100.0))/100.0;
   }
   if (new_gps) {
     new_gps = false; 
@@ -102,7 +102,7 @@ bool is_descent(int v_trigger, bool mode) {
     }
   }
   else {
-    if (gps_stab_factor<v_trigger and baro_stab_factor<v_trigger) {
+    if (merged_vspeed<v_trigger) {
       return true;
     }
     else {
@@ -128,7 +128,8 @@ String pos_text() {
   String baro_stab_text = String(baro_stab_factor, 2); 
   String gps_stab_text = String(gps_stab_factor, 2); 
   String v_down_text = String(v_down(VDOWN), 2); 
-  return merged_alt_text+","+baro_weight_text+","+gps_weight_text+","+baro_stab_text+","+gps_stab_text+","+v_down_text;
+  String merged_vspeed_text = String(merged_vspeed, 2); 
+  return merged_alt_text+","+merged_vspeed_text+","+baro_weight_text+","+gps_weight_text+","+baro_stab_text+","+gps_stab_text+","+v_down_text;
 }
 
 

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

@@ -150,6 +150,7 @@ servo_cmd cmpt_servo(uint16_t channels[16], int autopilot, int flight_mode, bool
         } 
         else { steering_cmpt.aux = sw; }
       } 
+    steering_cmpt.aux_deploy = 2000;
     break; 
 
     case 2: