AP_BoardConfig: add allocation failure handling loop

libraries/AP_BoardConfig/AP_BoardConfig.cpp

@@ -355,11 +355,11 @@ void AP_BoardConfig::init_safety()
 /*
   notify user of a fatal startup error related to available sensors. 
 */
-bool AP_BoardConfig::_in_sensor_config_error;
+bool AP_BoardConfig::_in_error_loop;
 
-void AP_BoardConfig::config_error(const char *fmt, ...)
+void AP_BoardConfig::throw_error(const char *err_type, const char *fmt, va_list arg)
 {
-    _in_sensor_config_error = true;
+    _in_error_loop = true;
     /*
       to give the user the opportunity to connect to USB we keep
       repeating the error.  The mavlink delay callback is initialised
@@ -371,18 +371,12 @@ void AP_BoardConfig::config_error(const char *fmt, ...)
         uint32_t now = AP_HAL::millis();
         if (now - last_print_ms >= 5000) {
             last_print_ms = now;
-            va_list arg_list;
             char printfmt[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+2];
-            hal.util->snprintf(printfmt, sizeof(printfmt), "Config error: %s\n", fmt);
-            va_start(arg_list, fmt);
-            vprintf(printfmt, arg_list);
-            va_end(arg_list);
+            hal.util->snprintf(printfmt, sizeof(printfmt), "%s: %s\n", err_type, fmt);
+            vprintf(printfmt, arg);
 #if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN) && !defined(HAL_BUILD_AP_PERIPH)
-            char taggedfmt[MAVLINK_MSG_STATUSTEXT_FIELD_TEXT_LEN+1];
-            hal.util->snprintf(taggedfmt, sizeof(taggedfmt), "Config error: %s", fmt);
-            va_start(arg_list, fmt);
-            gcs().send_textv(MAV_SEVERITY_CRITICAL, taggedfmt, arg_list);
-            va_end(arg_list);
+            hal.util->snprintf(printfmt, sizeof(printfmt), "%s: %s", err_type, fmt);
+            gcs().send_textv(MAV_SEVERITY_CRITICAL, printfmt, arg);
 #endif
         }
 #if !APM_BUILD_TYPE(APM_BUILD_UNKNOWN) && !defined(HAL_BUILD_AP_PERIPH)
@@ -394,6 +388,22 @@ void AP_BoardConfig::config_error(const char *fmt, ...)
     }
 }
 
+void AP_BoardConfig::allocation_error(const char *fmt, ...)
+{
+    va_list arg_list;
+    va_start(arg_list, fmt);
+    throw_error("Allocation Error", fmt, arg_list);
+    va_end(arg_list);
+}
+
+void AP_BoardConfig::config_error(const char *fmt, ...)
+{
+    va_list arg_list;
+    va_start(arg_list, fmt);
+    throw_error("Config Error", fmt, arg_list);
+    va_end(arg_list);
+}
+
 /*
   handle logic for safety state button press. This should be called at
   10Hz when the button is pressed. The button can either be directly

libraries/AP_BoardConfig/AP_BoardConfig.h

@@ -64,11 +64,14 @@ public:
     static const struct AP_Param::GroupInfo var_info[];
 
     // notify user of a fatal startup error related to available sensors. 
-    static void config_error(const char *reason, ...);
+    static void config_error(const char *reason, ...) FMT_PRINTF(1, 2) NORETURN;
+
+    // notify user of a non-fatal startup error related to allocation failures.
+    static void allocation_error(const char *reason, ...) FMT_PRINTF(1, 2) NORETURN;
 
     // permit other libraries (in particular, GCS_MAVLink) to detect
     // that we're never going to boot properly:
-    static bool in_config_error(void) { return _in_sensor_config_error; }
+    static bool in_config_error(void) { return _in_error_loop; }
 
     // valid types for BRD_TYPE: these values need to be in sync with the
     // values from the param description
@@ -236,7 +239,10 @@ private:
     void board_setup_sbus(void);
     void board_setup(void);
 
-    static bool _in_sensor_config_error;
+    // common method to throw errors
+    static void throw_error(const char *err_str, const char *fmt, va_list arg) NORETURN;
+
+    static bool _in_error_loop;
 
 #if HAL_HAVE_IMU_HEATER
     struct {