aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

AP_BoardConfig: added sensor_config_error()

Browse Source 
used to notify user of fatal sensor setup error
This commit is contained in:
Andrew Tridgell 2017-05-02 12:05:47 +10:00
parent bffc5daeb0
commit e32e2f5b5d
3 changed files with 29 additions and 26 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

20
libraries/AP_BoardConfig/AP_BoardConfig.cpp
View File

@ -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);
}
}

4
libraries/AP_BoardConfig/AP_BoardConfig.h
View File

@ -42,10 +42,12 @@ public:
};
#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

31
libraries/AP_BoardConfig/px4_drivers.cpp
View File

@ -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
*/