Move check for valid GPS heading to a function

Allows calling it from both the IMU and the wind estimator
shellixyz · May 27, 2018 · 78e62d3 · 78e62d3
1 parent c52f687
commit 78e62d3
Show file tree

Hide file tree

Showing 3 changed files with 8 additions and 1 deletion.
diff --git a/src/main/flight/imu.c b/src/main/flight/imu.c
@@ -463,7 +463,7 @@ static void imuCalculateEstimatedAttitude(float dT)
 
 #if defined(USE_GPS)
     if (STATE(FIXED_WING)) {
-        bool canUseCOG = sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 300;
+        bool canUseCOG = isGPSHeadingValid();
 
         if (canUseCOG) {
             if (gpsHeadingInitialized) {

diff --git a/src/main/io/gps.c b/src/main/io/gps.c
@@ -546,4 +546,10 @@ bool isGPSHealthy(void)
 {
     return true;
 }
+
+bool isGPSHeadingValid(void)
+{
+    return sensors(SENSOR_GPS) && STATE(GPS_FIX) && gpsSol.numSat >= 6 && gpsSol.groundSpeed >= 300;
+}
+
 #endif
diff --git a/src/main/io/gps.h b/src/main/io/gps.h
@@ -157,5 +157,6 @@ void gpsInit(void);
 void gpsThread(void);
 void updateGpsIndicator(timeUs_t currentTimeUs);
 bool isGPSHealthy(void);
+bool isGPSHeadingValid(void);
 struct serialPort_s;
 void gpsEnablePassthrough(struct serialPort_s *gpsPassthroughPort);