Advertise NAV modes through MSP even when mag is powered off (MR)

iNavFlight · Mar 29, 2018 · 413cb2e · 413cb2e
1 parent 3a5e34e
commit 413cb2e
Show file tree

Hide file tree

Showing 4 changed files with 11 additions and 2 deletions.
diff --git a/src/main/fc/fc_msp_box.c b/src/main/fc/fc_msp_box.c
@@ -30,6 +30,7 @@
 #include "fc/fc_msp_box.h"
 #include "fc/runtime_config.h"
 
+#include "sensors/compass.h"
 #include "sensors/diagnostics.h"
 #include "sensors/sensors.h"
 
@@ -179,7 +180,7 @@ void initActiveBoxIds(void)
         activeBoxIds[activeBoxIdCount++] = BOXSURFACE;
     }
 
-    if ((feature(FEATURE_GPS) && sensors(SENSOR_MAG) && sensors(SENSOR_ACC)) || (STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS))) {
+    if ((feature(FEATURE_GPS) && compassIsConfigured() && sensors(SENSOR_ACC)) || (STATE(FIXED_WING) && sensors(SENSOR_ACC) && feature(FEATURE_GPS))) {
         activeBoxIds[activeBoxIdCount++] = BOXNAVPOSHOLD;
         activeBoxIds[activeBoxIdCount++] = BOXNAVRTH;
         activeBoxIds[activeBoxIdCount++] = BOXNAVWP;

diff --git a/src/main/navigation/navigation.c b/src/main/navigation/navigation.c
@@ -51,7 +51,9 @@
 
 #include "sensors/sensors.h"
 #include "sensors/acceleration.h"
+#include "sensors/compass.h"
 #include "sensors/boardalignment.h"
+#include "sensors/diagnostics.h"
 
 
 // Multirotors:
@@ -2485,7 +2487,7 @@ bool navigationBlockArming(void)
         return false;
 
     // Apply extra arming safety only if pilot has any of GPS modes configured
-    if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !((posControl.flags.estPosStatue >= EST_USABLE) && STATE(GPS_FIX_HOME))) {
+    if ((isUsingNavigationModes() || failsafeMayRequireNavigationMode()) && !(((STATE(FIXED_WING) && !compassIsConfigured()) || (getHwCompassStatus() == HW_SENSOR_OK)) && (posControl.flags.estPosStatue >= EST_USABLE) && STATE(GPS_FIX_HOME))) {
         shouldBlockArming = true;
     }
 

diff --git a/src/main/sensors/compass.c b/src/main/sensors/compass.c
@@ -82,6 +82,11 @@ PG_RESET_TEMPLATE(compassConfig_t, compassConfig,
 
 static uint8_t magUpdatedAtLeastOnce = 0;
 
+bool compassIsConfigured()
+{
+    return ((compassConfig()->mag_hardware != MAG_NONE) && (compassConfig()->mag_hardware != MAG_AUTODETECT));
+}
+
 bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse)
 {
     magSensor_e magHardware = MAG_NONE;

diff --git a/src/main/sensors/compass.h b/src/main/sensors/compass.h
@@ -65,6 +65,7 @@ typedef struct compassConfig_s {
 
 PG_DECLARE(compassConfig_t, compassConfig);
 
+bool compassIsConfigured();
 bool compassDetect(magDev_t *dev, magSensor_e magHardwareToUse);
 bool compassInit(void);
 void compassUpdate(timeUs_t currentTimeUs);