dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

commander: Fix new-style mag calibration, tested.

This commit is contained in:
Lorenz Meier 2015-02-09 07:32:53 +01:00
parent 4ca3c44b60
commit 8fde33bcf7
1 changed files with 23 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
src/modules/commander/mag_calibration.cpp
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;