From ffd806f3d68da2fa4504c4effd6cca22925d9182 Mon Sep 17 00:00:00 2001
From: YohanHadji <hadji.yohan@gmail.com>
Date: Fri, 8 Apr 2022 17:30:17 +0200
Subject: [PATCH] Fix minor glitches and improve GPS vertical speed precision

---
 .../R2Home_OBC_V1.04/main/flight_state.hpp                    | 4 ++--
 RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/gps.hpp  | 2 +-
 RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/main.ino | 4 +++-
 3 files changed, 6 insertions(+), 4 deletions(-)

diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/flight_state.hpp b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/flight_state.hpp
index 6fe055b..2448918 100644
--- a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/flight_state.hpp
+++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/flight_state.hpp
@@ -39,8 +39,8 @@ void flight_init() {
     }
 
     baroset(gps.altitude.meters(), 1); 
-
-    get_baro(1); 
+    get_baro(1);
+    cmpt_fusion(); 
 
     b_vs.reset(); 
     b_al.reset(); 
diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/gps.hpp b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/gps.hpp
index e0469aa..20c9e1d 100644
--- a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/gps.hpp
+++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/gps.hpp
@@ -46,7 +46,7 @@ void get_gps() {
   }
 
   if (gps.altitude.isUpdated()) {
-    cmpt_vertical_speed_gps(gps.altitude.meters()-prev_alt_gps, millis()-gpsB);
+    cmpt_vertical_speed_gps(gps.altitude.meters()-prev_alt_gps, (millis()-gps.altitude.age())-gpsB);
     gpsB = millis(); 
     prev_alt_gps = gps.altitude.meters(); 
     new_gps = true; 
diff --git a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/main.ino b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/main.ino
index 56f9fa8..affda11 100644
--- a/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/main.ino
+++ b/RLS_V1.0/R2Home_SOFTWARE_V1.01/R2Home_OBC_V1.04/main/main.ino
@@ -92,7 +92,9 @@ void datacmpt() {
 
   cmpt_flight_state(); 
   cmpt_data_rate(flight_mode); 
-  cmpt_fusion(); 
+  if (initialised) {
+    cmpt_fusion(); 
+  }
   cmpt_vertical_state(); 
 
   if ((millis()-sd)>=delaySD) {