fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-05-01 07:34:06 +08:00
Code Issues Packages Projects Releases Wiki Activity

Commander: Ensure primary sensor is present if configured

Browse Source
This commit is contained in:
Lorenz Meier 2015-10-14 20:02:02 +02:00
parent 5434d1a9ff
commit 336ca86117
1 changed files with 28 additions and 21 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

49
src/modules/commander/PreflightCheck.cpp
View File

@ -72,9 +72,9 @@
namespace Commander
{
static int check_calibration(int fd, const char* param_template);
static int check_calibration(int fd, const char* param_template, int &devid);
int check_calibration(int fd, const char* param_template)
int check_calibration(int fd, const char* param_template, int &devid)
{
bool calibration_found;
@ -82,13 +82,13 @@ int check_calibration(int fd, const char* param_template)
int ret = px4_ioctl(fd, SENSORIOCCALTEST, 0);
calibration_found = (ret == OK);
/* old style transition: check param values */
int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
char s[20];
int instance = 0;
/* old style transition: check param values */
while (!calibration_found) {
sprintf(s, param_template, instance);
param_t parm = param_find(s);
@ -131,7 +131,7 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, in
return false;
}
int ret = check_calibration(fd, "CAL_MAG%u_ID");
int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id);
if (ret) {
mavlink_and_console_log_critical(mavlink_fd,
@ -171,7 +171,7 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
return false;
}
int ret = check_calibration(fd, "CAL_ACC%u_ID");
int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id);
if (ret) {
mavlink_and_console_log_critical(mavlink_fd,
@ -236,7 +236,7 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
return false;
}
int ret = check_calibration(fd, "CAL_GYRO%u_ID");
int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id);
if (ret) {
mavlink_and_console_log_critical(mavlink_fd,
@ -276,6 +276,8 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &dev
return false;
}
device_id = -1000;
// TODO: There is no baro calibration yet, since no external baros exist
// int ret = check_calibration(fd, "CAL_BARO%u_ID");
@ -361,7 +363,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* 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);
int device_id;
int device_id = -1;
if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) {
failed = true;
@ -373,8 +375,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
}
/* check if the primary device is present */
if (!prime_found) {
//failed = true;
if (!prime_found && prime_id != 0) {
mavlink_log_critical(mavlink_fd, "warning: primary compass not operational");
failed = true;
}
}
@ -387,7 +390,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* 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);
int device_id;
int device_id = -1;
if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) {
failed = true;
@ -399,8 +402,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
}
/* check if the primary device is present */
if (!prime_found) {
//failed = true;
if (!prime_found && prime_id != 0) {
mavlink_log_critical(mavlink_fd, "warning: primary accelerometer not operational");
failed = true;
}
}
@ -413,7 +417,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool 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);
int device_id;
int device_id = -1;
if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) {
failed = true;
@ -425,8 +429,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
}
/* check if the primary device is present */
if (!prime_found) {
//failed = true;
if (!prime_found && prime_id != 0) {
mavlink_log_critical(mavlink_fd, "warning: primary gyro not operational");
failed = true;
}
}
@ -439,7 +444,7 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* 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);
int device_id;
int device_id = -1;
if (!baroCheck(mavlink_fd, i, !required, device_id) && required) {
failed = true;
@ -452,8 +457,9 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
// TODO there is no logic in place to calibrate the primary baro yet
// // check if the primary device is present
if (!prime_found) {
// failed = true;
if (!prime_found && prime_id != 0) {
mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational");
failed = true;
}
}
@ -467,12 +473,13 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
/* ---- RC CALIBRATION ---- */
if (checkRC) {
if (rc_calibration_check(mavlink_fd) != OK) {
mavlink_log_critical(mavlink_fd, "RC calibration check failed");
failed = true;
}
}
/* ---- Global Navigation Satellite System receiver ---- */
if(checkGNSS) {
if (checkGNSS) {
if(!gnssCheck(mavlink_fd)) {
failed = true;
}