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

commander: Fix new-style mag calibration, tested.

Browse Source
This commit is contained in:
Lorenz Meier
2015-02-09 22:56:25 +01:00
parent 4ca3c44b60
commit 8fde33bcf7
1 changed files with 23 additions and 20 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/commander/mag_calibration.cpp
+23 -20
View File
@@ -88,10 +88,9 @@ int do_mag_calibration(int mavlink_fd)
char str[30]; char str[30];
for (unsigned s = 0; s < max_mags; s++) { unsigned calibrated_ok = 0;
mavlink_log_info(mavlink_fd, "Magnetometer #%u", s); for (unsigned s = 0; s < max_mags; s++) {
sleep(3);
/* erase old calibration */ /* erase old calibration */
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s);
@@ -101,6 +100,9 @@ int do_mag_calibration(int mavlink_fd)
continue; continue;
} }
mavlink_log_info(mavlink_fd, "Calibrating magnetometer #%u..", s);
sleep(3);
device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0);
/* ensure all scale fields are initialized tha same as the first struct */ /* ensure all scale fields are initialized tha same as the first struct */
@@ -127,9 +129,26 @@ int do_mag_calibration(int mavlink_fd)
if (res == OK) { if (res == OK) {
res = calibrate_instance(mavlink_fd, s, device_id[s]); res = calibrate_instance(mavlink_fd, s, device_id[s]);
if (res == OK) {
calibrated_ok++;
}
} }
} }
if (calibrated_ok) {
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
/* auto-save to EEPROM */
res = param_save_default();
if (res != OK) {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
}
} else {
mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
}
return res; return res;
} }
@@ -147,7 +166,7 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id)
float *z = NULL; float *z = NULL;
char str[30]; char str[30];
int res = ERROR; int res = OK;
/* allocate memory */ /* allocate memory */
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20); mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
@@ -316,26 +335,10 @@ int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id)
mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90); mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90);
} }
if (res == OK) {
/* auto-save to EEPROM */
res = param_save_default();
if (res != OK) {
mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
}
}
mavlink_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset, mavlink_log_info(mavlink_fd, "mag off: x:%.2f y:%.2f z:%.2f Ga", (double)mscale.x_offset,
(double)mscale.y_offset, (double)mscale.z_offset); (double)mscale.y_offset, (double)mscale.z_offset);
mavlink_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, mavlink_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale,
(double)mscale.y_scale, (double)mscale.z_scale); (double)mscale.y_scale, (double)mscale.z_scale);
if (res == OK) {
mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
} else {
mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
}
} }
return res; return res;