From e32e2f5b5d767ec989e94201a7378119299319e1 Mon Sep 17 00:00:00 2001
From: Andrew Tridgell <andrew@tridgell.net>
Date: Tue, 2 May 2017 12:05:47 +1000
Subject: [PATCH] AP_BoardConfig: added sensor_config_error()

used to notify user of fatal sensor setup error
---
 libraries/AP_BoardConfig/AP_BoardConfig.cpp | 20 +++++++++++++
 libraries/AP_BoardConfig/AP_BoardConfig.h   |  4 ++-
 libraries/AP_BoardConfig/px4_drivers.cpp    | 31 ++++-----------------
 3 files changed, 29 insertions(+), 26 deletions(-)

diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.cpp b/libraries/AP_BoardConfig/AP_BoardConfig.cpp
index 2efed23ed223c..0f1618b47907d 100644
--- a/libraries/AP_BoardConfig/AP_BoardConfig.cpp
+++ b/libraries/AP_BoardConfig/AP_BoardConfig.cpp
@@ -19,7 +19,9 @@
 
 #include <AP_HAL/AP_HAL.h>
 #include <AP_Common/AP_Common.h>
+#include <GCS_MAVLink/GCS.h>
 #include "AP_BoardConfig.h"
+#include <stdio.h>
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
 #include <sys/types.h>
@@ -220,3 +222,21 @@ void AP_BoardConfig::init_safety()
     px4_init_safety();
 #endif
 }
+
+/*
+  notify user of a fatal startup error related to available sensors. 
+*/
+void AP_BoardConfig::sensor_config_error(const char *reason)
+{
+    /*
+      to give the user the opportunity to connect to USB we keep
+      repeating the error.  The mavlink delay callback is initialised
+      before this, so the user can change parameters (and in
+      particular BRD_TYPE if needed)
+    */
+    while (true) {
+        printf("Sensor failure: %s\n", reason);
+        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_ERROR, "Check BRD_TYPE: %s", reason);
+        hal.scheduler->delay(3000);
+    }
+}
diff --git a/libraries/AP_BoardConfig/AP_BoardConfig.h b/libraries/AP_BoardConfig/AP_BoardConfig.h
index b90ce3631111f..5001af93d3f85 100644
--- a/libraries/AP_BoardConfig/AP_BoardConfig.h
+++ b/libraries/AP_BoardConfig/AP_BoardConfig.h
@@ -42,10 +42,12 @@ class AP_BoardConfig {
     };
 #endif
 
+    // notify user of a fatal startup error related to available sensors. 
+    static void sensor_config_error(const char *reason);
+    
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
     // public method to start a driver
     static bool px4_start_driver(main_fn_t main_function, const char *name, const char *arguments);
-    static void px4_sensor_error(const char *reason);
 
     // valid types for BRD_TYPE: these values need to be in sync with the
     // values from the param description
diff --git a/libraries/AP_BoardConfig/px4_drivers.cpp b/libraries/AP_BoardConfig/px4_drivers.cpp
index 54643b4aecb7c..ecf252b619db9 100644
--- a/libraries/AP_BoardConfig/px4_drivers.cpp
+++ b/libraries/AP_BoardConfig/px4_drivers.cpp
@@ -20,7 +20,6 @@
 #include "AP_BoardConfig.h"
 
 #if CONFIG_HAL_BOARD == HAL_BOARD_PX4 || CONFIG_HAL_BOARD == HAL_BOARD_VRBRAIN
-#include <GCS_MAVLink/GCS.h>
 #include <sys/types.h>
 #include <sys/stat.h>
 #include <fcntl.h>
@@ -316,7 +315,7 @@ void AP_BoardConfig::px4_setup_drivers(void)
     case PX4_BOARD_AEROFC:
         break;
     default:
-        px4_sensor_error("Unknown board type");
+        sensor_config_error("Unknown board type");
         break;
     }
 }
@@ -352,7 +351,7 @@ void AP_BoardConfig::px4_setup_px4io(void)
         if (px4_start_driver(px4io_main, "px4io", "start norc")) {
             printf("px4io started OK\n");
         } else {
-            px4_sensor_error("px4io start failed");
+            sensor_config_error("px4io start failed");
         }
     }
 
@@ -378,7 +377,7 @@ void AP_BoardConfig::px4_setup_px4io(void)
                 px4_tone_alarm("MSPAA");
             } else {
                 px4_tone_alarm("MNGGG");
-                px4_sensor_error("PX4IO restart failed");
+                sensor_config_error("PX4IO restart failed");
             }
         } else {
             printf("PX4IO update failed\n");
@@ -397,7 +396,7 @@ void AP_BoardConfig::px4_setup_peripherals(void)
         hal.analogin->init();
         printf("ADC started OK\n");
     } else {
-        px4_sensor_error("no ADC found");
+        sensor_config_error("no ADC found");
     }
 
 #if HAL_PX4_HAVE_PX4IO
@@ -416,7 +415,7 @@ void AP_BoardConfig::px4_setup_peripherals(void)
     if (px4_start_driver(fmu_main, "fmu", fmu_mode)) {
         printf("fmu %s started OK\n", fmu_mode);
     } else {
-        px4_sensor_error("fmu start failed");
+        sensor_config_error("fmu start failed");
     }
 
     hal.gpio->init();
@@ -493,7 +492,7 @@ void AP_BoardConfig::px4_autodetect(void)
         px4.board_type.set(PX4_BOARD_PIXHAWK);
         hal.console->printf("Detected Pixhawk\n");
     } else {
-        px4_sensor_error("Unable to detect board type");
+        sensor_config_error("Unable to detect board type");
     }
 #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V4)
     // only one choice
@@ -506,24 +505,6 @@ void AP_BoardConfig::px4_autodetect(void)
 
 }
 
-/*
-  fail startup of a required sensor
- */
-void AP_BoardConfig::px4_sensor_error(const char *reason)
-{
-    /*
-      to give the user the opportunity to connect to USB we keep
-      repeating the error.  The mavlink delay callback is initialised
-      before this, so the user can change parameters (and in
-      particular BRD_TYPE if needed)
-    */
-    while (true) {
-        printf("Sensor failure: %s\n", reason);
-        GCS_MAVLINK::send_statustext_all(MAV_SEVERITY_ERROR, "Check BRD_TYPE: %s", reason);
-        hal.scheduler->delay(3000);
-    }
-}
-
 /*
   setup px4 peripherals and drivers
  */