zrzk
/
px4_autopilot
1
0
Fork 0
Code Issues Pull Requests Projects Releases Wiki Activity
Browse Source

Commander: Improved preflight check routines. Running checks on all connected sensors. Re-run checks once GCS is connected.

sbg
Lorenz Meier 10 years ago
parent
5c44146c1b
commit
7dbb6c4fa8
4 changed files with 292 additions and 214 deletions
Whitespace
Show all changes Ignore whitespace when comparing lines Ignore changes in amount of whitespace Ignore changes in whitespace at EOL
Split View
Diff Options
Show Stats Download Patch File Download Diff File
  1. 207
      src/modules/commander/PreflightCheck.cpp
  2. 18
      src/modules/commander/PreflightCheck.h
  3. 3
      src/modules/commander/commander.cpp
  4. 4
      src/modules/commander/state_machine_helper.cpp

207
src/modules/commander/PreflightCheck.cpp
Unescape View File

@ -65,158 +65,219 @@ @@ -65,158 +65,219 @@
namespace Commander
{
static bool magnometerCheck(int mavlink_fd)
static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
{
int fd = open(MAG0_DEVICE_PATH, 0);
bool success = true;
char s[30];
sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance);
int fd = open(s, 0);
if (fd < 0) {
warn("failed to open magnetometer - start with 'hmc5883 start' or 'lsm303d start'");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: NO MAG");
if (!optional) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO MAG SENSOR #%u", instance);
}
return false;
}
int calibration_devid;
int ret;
int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
param_get(param_find("CAL_MAG0_ID"), &(calibration_devid));
sprintf(s, "CAL_MAG%u_ID", instance);
param_get(param_find(s), &(calibration_devid));
if (devid != calibration_devid) {
warnx("magnetometer calibration is for a different device - calibrate magnetometer first (dev: %d vs cal: %d)", devid, calibration_devid);
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID");
return false;
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u UNCALIBRATED (NO ID)", instance);
success = false;
goto out;
}
int ret = ioctl(fd, MAGIOCSELFTEST, 0);
ret = ioctl(fd, MAGIOCSELFTEST, 0);
if (ret != OK) {
warnx("magnetometer calibration missing or bad - calibrate magnetometer first");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CHECK/CAL");
return false;
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
success = false;
goto out;
}
out:
close(fd);
return true;
return success;
}
static bool accelerometerCheck(int mavlink_fd)
static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional)
{
int fd = open(ACCEL0_DEVICE_PATH, O_RDONLY);
int ret = ioctl(fd, ACCELIOCSELFTEST, 0);
bool success = true;
int calibration_devid;
int devid = ioctl(fd, DEVIOCGDEVICEID,0);
param_get(param_find("CAL_ACC0_ID"), &(calibration_devid));
if (devid != calibration_devid){
warnx("accelerometer calibration is for a different device - calibrate accelerometer first");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACC CAL ID");
return false;
char s[30];
sprintf(s, "%s%u", ACCEL_BASE_DEVICE_PATH, instance);
int fd = open(s, O_RDONLY);
if (fd < 0) {
if (!optional) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO ACCEL SENSOR #%u", instance);
}
if (ret != OK) {
warnx("accel self test failed");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL CHECK/CAL");
return false;
}
// check measurement result range
struct accel_report acc;
ret = read(fd, &acc, sizeof(acc));
if (ret == sizeof(acc)) {
// evaluate values
float accel_magnitude = sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z);
int calibration_devid;
int ret;
int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
sprintf(s, "CAL_ACC%u_ID", instance);
param_get(param_find(s), &(calibration_devid));
// evaluate values
if (accel_magnitude > 30.0f) { //m/s^2
warnx("accel with spurious values");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: |ACCEL| > 30 m/s^2");
//this is frickin' fatal
return false;
if (devid != calibration_devid) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED (NO ID)", instance);
success = false;
goto out;
}
} else {
warnx("accel read failed");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL READ");
//this is frickin' fatal
return false;
ret = ioctl(fd, ACCELIOCSELFTEST, 0);
if (ret != OK) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u SELFTEST FAILED", instance);
success = false;
goto out;
}
out:
close(fd);
return true;
return success;
}
static bool gyroCheck(int mavlink_fd)
static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
{
int fd = open(GYRO0_DEVICE_PATH, 0);
bool success = true;
char s[30];
sprintf(s, "%s%u", GYRO_BASE_DEVICE_PATH, instance);
int fd = open(s, 0);
if (fd < 0) {
if (!optional) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO GYRO SENSOR #%u", instance);
}
return false;
}
int calibration_devid;
int ret;
int devid = ioctl(fd, DEVIOCGDEVICEID, 0);
param_get(param_find("CAL_GYRO0_ID"), &(calibration_devid));
sprintf(s, "CAL_GYRO%u_ID", instance);
param_get(param_find(s), &(calibration_devid));
if (devid != calibration_devid) {
warnx("gyro calibration is for a different device - calibrate gyro first");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CAL ID");
return false;
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u UNCALIBRATED (NO ID)", instance);
success = false;
goto out;
}
int ret = ioctl(fd, GYROIOCSELFTEST, 0);
ret = ioctl(fd, GYROIOCSELFTEST, 0);
if (ret != OK) {
warnx("gyro self test failed");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: GYRO CHECK/CAL");
return false;
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
success = false;
goto out;
}
out:
close(fd);
return true;
return success;
}
static bool baroCheck(int mavlink_fd)
static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
{
int fd = open(BARO0_DEVICE_PATH, 0);
bool success = true;
char s[30];
sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance);
int fd = open(s, 0);
if (fd < 0) {
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: Barometer");
if (!optional) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
}
return false;
}
close(fd);
return true;
return success;
}
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkRC)
{
//give the system some time to sample the sensors in the background
usleep(150000);
bool failed = false;
//Magnetometer
if (checkMag) {
if(!magnometerCheck(mavlink_fd)) {
return false;
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_mag_count; i++) {
bool required = (i < max_mandatory_mag_count);
if (!magnometerCheck(mavlink_fd, i, !required) && required) {
failed = true;
}
}
}
//Accelerometer
if (checkAcc) {
if(!accelerometerCheck(mavlink_fd)) {
return false;
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_accel_count; i++) {
bool required = (i < max_mandatory_accel_count);
if (!accelerometerCheck(mavlink_fd, i, !required) && required) {
failed = true;
}
}
}
// ---- GYRO ----
if (checkGyro) {
if(!gyroCheck(mavlink_fd)) {
return false;
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_gyro_count; i++) {
bool required = (i < max_mandatory_gyro_count);
if (!gyroCheck(mavlink_fd, i, !required) && required) {
failed = true;
}
}
}
// ---- BARO ----
if (checkBaro) {
if(!baroCheck(mavlink_fd)) {
return false;
/* check all sensors, but fail only for mandatory ones */
for (unsigned i = 0; i < max_optional_baro_count; i++) {
bool required = (i < max_mandatory_baro_count);
if (!baroCheck(mavlink_fd, i, !required) && required) {
failed = true;
}
}
}
// ---- RC CALIBRATION ----
if (checkRC) {
if (rc_calibration_check(mavlink_fd) != OK) {
return false;
failed = true;
}
}
//All is good!
return true;
// Report status
return !failed;
}
}

18
src/modules/commander/PreflightCheck.h
Unescape View File

@ -44,8 +44,11 @@ @@ -44,8 +44,11 @@
namespace Commander
{
/**
* @brief
* Runs a preflight check on all sensors to see if they are properly calibrated and healthy
*
* The function won't fail the test if optional sensors are not found, however,
* it will fail the test if optional sensors are found but not in working condition.
*
* @param mavlink_fd
* Mavlink output file descriptor for feedback when a sensor fails
* @param checkMag
@ -60,4 +63,17 @@ namespace Commander @@ -60,4 +63,17 @@ namespace Commander
* true if the Remote Controller should be checked
**/
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkRC);
const unsigned max_mandatory_gyro_count = 1;
const unsigned max_optional_gyro_count = 3;
const unsigned max_mandatory_accel_count = 1;
const unsigned max_optional_accel_count = 3;
const unsigned max_mandatory_mag_count = 1;
const unsigned max_optional_mag_count = 3;
const unsigned max_mandatory_baro_count = 1;
const unsigned max_optional_baro_count = 1;
}

3
src/modules/commander/commander.cpp
Unescape View File

@ -1302,7 +1302,8 @@ int commander_thread_main(int argc, char *argv[]) @@ -1302,7 +1302,8 @@ int commander_thread_main(int argc, char *argv[])
telemetry.heartbeat_time > 0 &&
hrt_elapsed_time(&telemetry.heartbeat_time) < datalink_loss_timeout * 1e6) {
(void)rc_calibration_check(mavlink_fd);
/* provide RC and sensor status feedback to the user */
(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, true);
}
telemetry_last_heartbeat[i] = telemetry.heartbeat_time;

4
src/modules/commander/state_machine_helper.cpp
Unescape View File

@ -212,10 +212,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s @@ -212,10 +212,10 @@ arming_state_transition(struct vehicle_status_s *status, ///< current vehicle s
/* Sensors need to be initialized for STANDBY state */
if (new_arming_state == vehicle_status_s::ARMING_STATE_STANDBY && !status->condition_system_sensors_initialized) {
mavlink_log_critical(mavlink_fd, "NOT ARMING: Sensors not operational.");
mavlink_log_critical(mavlink_fd, "Not ready to fly: Sensors not operational.");
feedback_provided = true;
valid_transition = false;
new_arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
status->arming_state = vehicle_status_s::ARMING_STATE_STANDBY_ERROR;
}
// Finish up the state transition

Write Preview
Loading…
Cancel
Save