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/modules/commander/mag_calibration.cpp | |
| 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/modules/commander/mag_calibration.cpp')
| -rw-r--r-- | src/modules/commander/mag_calibration.cpp | 7 |
1 files changed, 7 insertions, 0 deletions
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 7be8de9c6..9d2abd3ce 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -65,6 +65,7 @@ static const char *sensor_name = "mag"; int do_mag_calibration(int mavlink_fd) { + int32_t device_id; mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); mavlink_log_info(mavlink_fd, "don't move system"); @@ -88,6 +89,9 @@ int do_mag_calibration(int mavlink_fd) /* erase old calibration */ int fd = open(MAG_DEVICE_PATH, O_RDONLY); + + device_id = ioctl(fd, DEVIOCGDEVICEID, 0); + res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); if (res != OK) { @@ -253,6 +257,9 @@ int do_mag_calibration(int mavlink_fd) if (res == OK) { /* set parameters */ + if (param_set(param_find("SENS_MAG_ID"), &(device_id))) { + res = ERROR; + } if (param_set(param_find("SENS_MAG_XOFF"), &(mscale.x_offset))) { res = ERROR; } |