diff options
author | hauptmech <hauptmech@gmail.com> | 2015-01-08 12:38:33 +1300 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-01-17 18:41:43 +0100 |
commit | e18e5ca5b5d82a140d9a525cc3ba9bfddf4dcb7f (patch) | |
tree | 6b836df30fe6936ecddb44bf3f213f6fef967aca /src/systemcmds/preflight_check | |
parent | 6899a516b3d3981599827c72ce02046cb1418cdf (diff) | |
download | px4-firmware-e18e5ca5b5d82a140d9a525cc3ba9bfddf4dcb7f.tar.gz px4-firmware-e18e5ca5b5d82a140d9a525cc3ba9bfddf4dcb7f.tar.bz2 px4-firmware-e18e5ca5b5d82a140d9a525cc3ba9bfddf4dcb7f.zip |
Add SENS_MAG_ID parameter
Record devid to SENS_MAG_ID during mag calibration
Verify devid matches SENS_MAG_ID during preflight_check
Diffstat (limited to 'src/systemcmds/preflight_check')
-rw-r--r-- | src/systemcmds/preflight_check/preflight_check.c | 11 |
1 files changed, 11 insertions, 0 deletions
diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c index 86e4ff545..bbd90b961 100644 --- a/src/systemcmds/preflight_check/preflight_check.c +++ b/src/systemcmds/preflight_check/preflight_check.c @@ -84,6 +84,7 @@ int preflight_check_main(int argc, char *argv[]) /* open text message output path */ int mavlink_fd = open(MAVLINK_LOG_DEVICE, 0); int ret; + int32_t mag_devid,mag_calibration_devid; /* give the system some time to sample the sensors in the background */ usleep(150000); @@ -96,6 +97,16 @@ int preflight_check_main(int argc, char *argv[]) system_ok = false; goto system_eval; } + + mag_devid = ioctl(fd, DEVIOCGDEVICEID,0); + param_get(param_find("SENS_MAG_ID"), &(mag_calibration_devid)); + if (mag_devid != mag_calibration_devid){ + warnx("magnetometer calibration is for a different device - calibrate magnetometer first"); + mavlink_log_critical(mavlink_fd, "SENSOR FAIL: MAG CAL ID"); + system_ok = false; + goto system_eval; + } + ret = ioctl(fd, MAGIOCSELFTEST, 0); if (ret != OK) { |