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. 445
      src/modules/commander/PreflightCheck.cpp
  2. 50
      src/modules/commander/PreflightCheck.h
  3. 3
      src/modules/commander/commander.cpp
  4. 8
      src/modules/commander/state_machine_helper.cpp

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

@ -1,44 +1,44 @@ @@ -1,44 +1,44 @@
/****************************************************************************
*
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
*
* Copyright (c) 2012-2015 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file PreflightCheck.cpp
*
* Preflight check for main system components
*
* @author Lorenz Meier <lorenz@px4.io>
* @author Johan Jansen <jnsn.johan@gmail.com>
*/
* @file PreflightCheck.cpp
*
* Preflight check for main system components
*
* @author Lorenz Meier <lorenz@px4.io>
* @author Johan Jansen <jnsn.johan@gmail.com>
*/
#include <nuttx/config.h>
#include <unistd.h>
@ -65,158 +65,219 @@ @@ -65,158 +65,219 @@
namespace Commander
{
static bool magnometerCheck(int mavlink_fd)
{
int fd = open(MAG0_DEVICE_PATH, 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");
return false;
}
int calibration_devid;
int devid = ioctl(fd, DEVIOCGDEVICEID,0);
param_get(param_find("CAL_MAG0_ID"), &(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;
}
int 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;
}
close(fd);
return true;
}
static bool accelerometerCheck(int mavlink_fd)
{
int fd = open(ACCEL0_DEVICE_PATH, O_RDONLY);
int ret = ioctl(fd, ACCELIOCSELFTEST, 0);
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;
}
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);
// 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;
}
} else {
warnx("accel read failed");
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL READ");
//this is frickin' fatal
return false;
}
close(fd);
return true;
}
static bool gyroCheck(int mavlink_fd)
{
int fd = open(GYRO0_DEVICE_PATH, 0);
int calibration_devid;
int devid = ioctl(fd, DEVIOCGDEVICEID,0);
param_get(param_find("CAL_GYRO0_ID"), &(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;
}
int 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;
}
close(fd);
return true;
}
static bool baroCheck(int mavlink_fd)
{
int fd = open(BARO0_DEVICE_PATH, 0);
if(fd < 0) {
mavlink_log_critical(mavlink_fd, "SENSOR FAIL: Barometer");
return false;
}
close(fd);
return true;
}
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);
//Magnetometer
if(checkMag) {
if(!magnometerCheck(mavlink_fd)) {
return false;
}
}
//Accelerometer
if(checkAcc) {
if(!accelerometerCheck(mavlink_fd)) {
return false;
}
}
// ---- GYRO ----
if(checkGyro) {
if(!gyroCheck(mavlink_fd)) {
return false;
}
}
// ---- BARO ----
if(checkBaro) {
if(!baroCheck(mavlink_fd)) {
return false;
}
}
// ---- RC CALIBRATION ----
if(checkRC) {
if(rc_calibration_check(mavlink_fd) != OK) {
return false;
}
}
//All is good!
return true;
}
static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
{
bool success = true;
char s[30];
sprintf(s, "%s%u", MAG_BASE_DEVICE_PATH, instance);
int fd = open(s, 0);
if (fd < 0) {
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);
sprintf(s, "CAL_MAG%u_ID", instance);
param_get(param_find(s), &(calibration_devid));
if (devid != calibration_devid) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u UNCALIBRATED (NO ID)", instance);
success = false;
goto out;
}
ret = ioctl(fd, MAGIOCSELFTEST, 0);
if (ret != OK) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: MAG #%u SELFTEST FAILED", instance);
success = false;
goto out;
}
out:
close(fd);
return success;
}
static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional)
{
bool success = true;
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);
}
return false;
}
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));
if (devid != calibration_devid) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED (NO ID)", instance);
success = false;
goto out;
}
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 success;
}
static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
{
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);
sprintf(s, "CAL_GYRO%u_ID", instance);
param_get(param_find(s), &(calibration_devid));
if (devid != calibration_devid) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u UNCALIBRATED (NO ID)", instance);
success = false;
goto out;
}
ret = ioctl(fd, GYROIOCSELFTEST, 0);
if (ret != OK) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: GYRO #%u SELFTEST FAILED", instance);
success = false;
goto out;
}
out:
close(fd);
return success;
}
static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
{
bool success = true;
char s[30];
sprintf(s, "%s%u", BARO_BASE_DEVICE_PATH, instance);
int fd = open(s, 0);
if (fd < 0) {
if (!optional) {
mavlink_and_console_log_critical(mavlink_fd,
"PREFLIGHT FAIL: NO BARO SENSOR #%u", instance);
}
return false;
}
close(fd);
return success;
}
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkRC)
{
bool failed = false;
//Magnetometer
if (checkMag) {
/* 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) {
/* 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) {
/* 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) {
/* 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) {
failed = true;
}
}
// Report status
return !failed;
}
}

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

@ -43,21 +43,37 @@ @@ -43,21 +43,37 @@
namespace Commander
{
/**
* @brief
* Runs a preflight check on all sensors to see if they are properly calibrated and healthy
* @param mavlink_fd
* Mavlink output file descriptor for feedback when a sensor fails
* @param checkMag
* true if the magneteometer should be checked
* @param checkAcc
* true if the accelerometers should be checked
* @param checkGyro
* true if the gyroscopes should be checked
* @param checkBaro
* true if the barometer should be checked
* @param checkRC
* true if the Remote Controller should be checked
**/
bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro, bool checkBaro, bool checkRC);
/**
* 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
* true if the magneteometer should be checked
* @param checkAcc
* true if the accelerometers should be checked
* @param checkGyro
* true if the gyroscopes should be checked
* @param checkBaro
* true if the barometer should be checked
* @param checkRC
* 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;

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

@ -77,8 +77,8 @@ static const int ERROR = -1; @@ -77,8 +77,8 @@ static const int ERROR = -1;
// though the transition is marked as true additional checks must be made. See arming_state_transition
// code for those checks.
static const bool arming_transitions[vehicle_status_s::ARMING_STATE_MAX][vehicle_status_s::ARMING_STATE_MAX] = {
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, true, false, false },
// INIT, STANDBY, ARMED, ARMED_ERROR, STANDBY_ERROR, REBOOT, IN_AIR_RESTORE
{ /* vehicle_status_s::ARMING_STATE_INIT */ true, true, false, false, true, false, false },
{ /* vehicle_status_s::ARMING_STATE_STANDBY */ true, true, true, true, false, false, false },
{ /* vehicle_status_s::ARMING_STATE_ARMED */ false, true, true, false, false, false, true },
{ /* vehicle_status_s::ARMING_STATE_ARMED_ERROR */ false, false, true, true, false, false, false },
@ -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