yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 09:28:58 +00:00
Code Issues Packages Projects Releases Wiki Activity

Commander: Finish preflight update for prime sensor IDs

Browse Source
This commit is contained in:
Lorenz Meier
2015-10-06 19:44:17 +02:00
parent e5bb1cff91
commit 53ff04e016
4 changed files with 40 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -79,13 +79,16 @@ int check_calibration(int fd, const char* param_template)
bool calibration_found;
/* new style: ask device for calibration state */
ret = px4_ioctl(fd, SENSORIOCCALTEST, 0);
int ret = px4_ioctl(fd, SENSORIOCCALTEST, 0);
calibration_found = (ret == OK);
/* old style transition: check param values */
int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
char s[20];
int instance = 0;
while (!calibration_found) {
sprintf(s, param_template, instance);
param_t parm = param_find(s);
@@ -105,6 +108,7 @@ int check_calibration(int fd, const char* param_template)
break;
}
}
instance++;
}
return !calibration_found;
@@ -150,7 +154,7 @@ out:
return success;
}
static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic)
static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id)
{
bool success = true;
@@ -215,7 +219,7 @@ out:
return success;
}
static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
{
bool success = true;
@@ -255,7 +259,7 @@ out:
return success;
}
static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
{
bool success = true;
@@ -272,6 +276,18 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
return false;
}
// TODO: There is no baro calibration yet, since no external baros exist
// int ret = check_calibration(fd, "CAL_BARO%u_ID");
// if (ret) {
// mavlink_and_console_log_critical(mavlink_fd,
// "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance);
// success = false;
// goto out;
// }
//out:
px4_close(fd);
return success;
}
@@ -371,7 +387,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;
if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) {
failed = true;
@@ -436,9 +452,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) {
if (!prime_found) {
// failed = true;
// }
}
}
/* ---- AIRSPEED ---- */