aboutsummaryrefslogtreecommitdiff
path: root/src/modules/commander/mag_calibration.cpp
diff options
context:
space:
mode:
Diffstat (limited to 'src/modules/commander/mag_calibration.cpp')
-rw-r--r--src/modules/commander/mag_calibration.cpp194
1 files changed, 98 insertions, 96 deletions
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 198acb027..8a8d85818 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/src/modules/commander/mag_calibration.cpp
@@ -65,8 +65,7 @@ static const int ERROR = -1;
static const char *sensor_name = "mag";
static const unsigned max_mags = 3;
-int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
-int mag_calibration_worker(detect_orientation_return orientation, void* worker_data);
+calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
/// Data passed to calibration worker routine
typedef struct {
@@ -86,7 +85,7 @@ typedef struct {
int do_mag_calibration(int mavlink_fd)
{
- mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+ mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
struct mag_scale mscale_null = {
0.0f,
@@ -113,7 +112,7 @@ int do_mag_calibration(int mavlink_fd)
(void)sprintf(str, "CAL_MAG%u_ID", cur_mag);
result = param_set_no_notification(param_find(str), &(device_ids[cur_mag]));
if (result != OK) {
- mavlink_and_console_log_info(mavlink_fd, "Unabled to reset CAL_MAG%u_ID", cur_mag);
+ mavlink_and_console_log_info(mavlink_fd, "[cal] Unable to reset CAL_MAG%u_ID", cur_mag);
break;
}
@@ -131,15 +130,15 @@ int do_mag_calibration(int mavlink_fd)
result = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null);
if (result != OK) {
- mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, cur_mag);
+ mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_RESET_CAL_MSG, cur_mag);
}
+ /* calibrate range */
if (result == OK) {
- /* calibrate range */
result = ioctl(fd, MAGIOCCALIBRATE, fd);
if (result != OK) {
- mavlink_and_console_log_info(mavlink_fd, "Skipped scale calibration, sensor %u", cur_mag);
+ mavlink_and_console_log_info(mavlink_fd, "[cal] Skipped scale calibration, sensor %u", cur_mag);
/* this is non-fatal - mark it accordingly */
result = OK;
}
@@ -148,39 +147,52 @@ int do_mag_calibration(int mavlink_fd)
close(fd);
}
+ // Calibrate all mags at the same time
if (result == OK) {
- // Calibrate all mags at the same time
- result = mag_calibrate_all(mavlink_fd, device_ids);
- }
-
- if (result == OK) {
- /* auto-save to EEPROM */
- result = param_save_default();
- if (result != OK) {
- mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
+ switch (mag_calibrate_all(mavlink_fd, device_ids)) {
+ case calibrate_return_cancelled:
+ // Cancel message already displayed, we're done here
+ result = ERROR;
+ break;
+
+ case calibrate_return_ok:
+ /* auto-save to EEPROM */
+ result = param_save_default();
+
+ /* if there is a any preflight-check system response, let the barrage of messages through */
+ usleep(200000);
+
+ if (result == OK) {
+ mavlink_and_console_log_info(mavlink_fd, CAL_QGC_PROGRESS_MSG, 100);
+ mavlink_and_console_log_info(mavlink_fd, CAL_QGC_DONE_MSG, sensor_name);
+ break;
+ } else {
+ mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SAVE_PARAMS_MSG);
+ }
+ // Fall through
+
+ default:
+ mavlink_and_console_log_critical(mavlink_fd, CAL_QGC_FAILED_MSG, sensor_name);
+ break;
}
}
-
- if (result == OK) {
- mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100);
- mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
- } else {
- mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
- }
+ /* give this message enough time to propagate */
+ usleep(600000);
+
return result;
}
-int mag_calibration_worker(detect_orientation_return orientation, void* data)
+static calibrate_return mag_calibration_worker(detect_orientation_return orientation, int cancel_sub, void* data)
{
- int result = OK;
+ calibrate_return result = calibrate_return_ok;
unsigned int calibration_counter_side;
mag_worker_data_t* worker_data = (mag_worker_data_t*)(data);
- mavlink_and_console_log_info(worker_data->mavlink_fd, "Rotate vehicle around the detected orientation");
- mavlink_and_console_log_info(worker_data->mavlink_fd, "Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);
+ mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Rotate vehicle around the detected orientation");
+ mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] Continue rotation for %u seconds", worker_data->calibration_interval_perside_seconds);
sleep(2);
uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds;
@@ -191,6 +203,11 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data)
while (hrt_absolute_time() < calibration_deadline &&
calibration_counter_side < worker_data->calibration_points_perside) {
+ if (calibrate_cancel_check(worker_data->mavlink_fd, cancel_sub)) {
+ result = calibrate_return_cancelled;
+ break;
+ }
+
// Wait clocking for new data on all mags
struct pollfd fds[max_mags];
size_t fd_count = 0;
@@ -222,8 +239,7 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data)
// Progress indicator for side
mavlink_and_console_log_info(worker_data->mavlink_fd,
- "%s %s side calibration: progress <%u>",
- sensor_name,
+ "[cal] %s side calibration: progress <%u>",
detect_orientation_str(orientation),
(unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside)));
} else {
@@ -231,50 +247,25 @@ int mag_calibration_worker(detect_orientation_return orientation, void* data)
}
if (poll_errcount > worker_data->calibration_points_perside * 3) {
- result = ERROR;
- mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_FAILED_SENSOR_MSG);
+ result = calibrate_return_error;
+ mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_ERROR_SENSOR_MSG);
break;
}
}
- // Mark the opposite side as collected as well. No need to collect opposite side since it
- // would generate similar points.
- detect_orientation_return alternateOrientation = orientation;
- switch (orientation) {
- case DETECT_ORIENTATION_TAIL_DOWN:
- alternateOrientation = DETECT_ORIENTATION_NOSE_DOWN;
- break;
- case DETECT_ORIENTATION_NOSE_DOWN:
- alternateOrientation = DETECT_ORIENTATION_TAIL_DOWN;
- break;
- case DETECT_ORIENTATION_LEFT:
- alternateOrientation = DETECT_ORIENTATION_RIGHT;
- break;
- case DETECT_ORIENTATION_RIGHT:
- alternateOrientation = DETECT_ORIENTATION_LEFT;
- break;
- case DETECT_ORIENTATION_UPSIDE_DOWN:
- alternateOrientation = DETECT_ORIENTATION_RIGHTSIDE_UP;
- break;
- case DETECT_ORIENTATION_RIGHTSIDE_UP:
- alternateOrientation = DETECT_ORIENTATION_UPSIDE_DOWN;
- break;
- case DETECT_ORIENTATION_ERROR:
- warnx("Invalid orientation in mag_calibration_worker");
- break;
+ if (result == calibrate_return_ok) {
+ mavlink_and_console_log_info(worker_data->mavlink_fd, "[cal] %s side done, rotate to a different side", detect_orientation_str(orientation));
+
+ worker_data->done_count++;
+ mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_QGC_PROGRESS_MSG, 34 * worker_data->done_count);
}
- worker_data->side_data_collected[alternateOrientation] = true;
- mavlink_and_console_log_info(worker_data->mavlink_fd, "%s side done, rotate to a different side", detect_orientation_str(alternateOrientation));
-
- worker_data->done_count++;
- mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 34 * worker_data->done_count);
return result;
}
-int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
+calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
{
- int result = OK;
+ calibrate_return result = calibrate_return_ok;
mag_worker_data_t worker_data;
@@ -285,10 +276,13 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
worker_data.calibration_interval_perside_seconds = 20;
worker_data.calibration_interval_perside_useconds = worker_data.calibration_interval_perside_seconds * 1000 * 1000;
- // Initialize to collect all sides
- for (size_t cur_side=0; cur_side<6; cur_side++) {
- worker_data.side_data_collected[cur_side] = false;
- }
+ // Collect: Right-side up, Left Side, Nose down
+ worker_data.side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = false;
+ worker_data.side_data_collected[DETECT_ORIENTATION_LEFT] = false;
+ worker_data.side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = false;
+ worker_data.side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true;
+ worker_data.side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true;
+ worker_data.side_data_collected[DETECT_ORIENTATION_RIGHT] = true;
for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) {
// Initialize to no subscription
@@ -310,21 +304,21 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
worker_data.y[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
worker_data.z[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
if (worker_data.x[cur_mag] == NULL || worker_data.y[cur_mag] == NULL || worker_data.z[cur_mag] == NULL) {
- mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory");
- result = ERROR;
+ mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: out of memory");
+ result = calibrate_return_error;
}
}
// Setup subscriptions to mag sensors
- if (result == OK) {
+ if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available
worker_data.sub_mag[cur_mag] = orb_subscribe_multi(ORB_ID(sensor_mag), cur_mag);
if (worker_data.sub_mag[cur_mag] < 0) {
- mavlink_and_console_log_critical(mavlink_fd, "Mag #%u not found, abort", cur_mag);
- result = ERROR;
+ mavlink_and_console_log_critical(mavlink_fd, "[cal] Mag #%u not found, abort", cur_mag);
+ result = calibrate_return_error;
break;
}
}
@@ -332,7 +326,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
}
// Limit update rate to get equally spaced measurements over time (in ms)
- if (result == OK) {
+ if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available
@@ -344,8 +338,18 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
}
}
-
- result = calibrate_from_orientation(mavlink_fd, worker_data.side_data_collected, mag_calibration_worker, &worker_data);
+
+ if (result == calibrate_return_ok) {
+ int cancel_sub = calibrate_cancel_subscribe();
+
+ result = calibrate_from_orientation(mavlink_fd, // Mavlink fd to write output
+ cancel_sub, // Subscription to vehicle_command for cancel support
+ worker_data.side_data_collected, // Sides to calibrate
+ mag_calibration_worker, // Calibration worker
+ &worker_data, // Opaque data for calibration worked
+ true); // true: lenient still detection
+ calibrate_cancel_unsubscribe(cancel_sub);
+ }
// Close subscriptions
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
@@ -363,7 +367,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
float sphere_radius[max_mags];
// Sphere fit the data to get calibration values
- if (result == OK) {
+ if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
// Mag in this slot is available and we should have values for it to calibrate
@@ -375,8 +379,8 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
&sphere_radius[cur_mag]);
if (!isfinite(sphere_x[cur_mag]) || !isfinite(sphere_y[cur_mag]) || !isfinite(sphere_z[cur_mag])) {
- mavlink_and_console_log_info(mavlink_fd, "ERROR: NaN in sphere fit for mag #%u", cur_mag);
- result = ERROR;
+ mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: NaN in sphere fit for mag #%u", cur_mag);
+ result = calibrate_return_error;
}
}
}
@@ -389,7 +393,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
free(worker_data.z[cur_mag]);
}
- if (result == OK) {
+ if (result == calibrate_return_ok) {
for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
if (device_ids[cur_mag] != 0) {
int fd_mag = -1;
@@ -400,27 +404,25 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
(void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag);
fd_mag = open(str, 0);
if (fd_mag < 0) {
- mavlink_and_console_log_info(mavlink_fd, "ERROR: unable to open mag device #%u", cur_mag);
- result = ERROR;
+ mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: unable to open mag device #%u", cur_mag);
+ result = calibrate_return_error;
}
- if (result == OK) {
- result = ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale);
- if (result != OK) {
- mavlink_and_console_log_info(mavlink_fd, "ERROR: failed to get current calibration #%u", cur_mag);
- result = ERROR;
+ if (result == calibrate_return_ok) {
+ if (ioctl(fd_mag, MAGIOCGSCALE, (long unsigned int)&mscale) != OK) {
+ mavlink_and_console_log_critical(mavlink_fd, "[cal] ERROR: failed to get current calibration #%u", cur_mag);
+ result = calibrate_return_error;
}
}
- if (result == OK) {
+ if (result == calibrate_return_ok) {
mscale.x_offset = sphere_x[cur_mag];
mscale.y_offset = sphere_y[cur_mag];
mscale.z_offset = sphere_z[cur_mag];
- result = ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale);
- if (result != OK) {
- mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG, cur_mag);
- result = ERROR;
+ if (ioctl(fd_mag, MAGIOCSSCALE, (long unsigned int)&mscale) != OK) {
+ mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_APPLY_CAL_MSG, cur_mag);
+ result = calibrate_return_error;
}
}
@@ -429,7 +431,7 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
close(fd_mag);
}
- if (result == OK) {
+ if (result == calibrate_return_ok) {
bool failed = false;
/* set parameters */
@@ -449,13 +451,13 @@ int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags])
failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_scale)));
if (failed) {
- mavlink_and_console_log_info(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG, cur_mag);
- result = ERROR;
+ mavlink_and_console_log_critical(mavlink_fd, CAL_ERROR_SET_PARAMS_MSG, cur_mag);
+ result = calibrate_return_error;
} else {
- mavlink_and_console_log_info(mavlink_fd, "mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
+ mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u off: x:%.2f y:%.2f z:%.2f Ga",
cur_mag,
(double)mscale.x_offset, (double)mscale.y_offset, (double)mscale.z_offset);
- mavlink_and_console_log_info(mavlink_fd, "mag #%u scale: x:%.2f y:%.2f z:%.2f",
+ mavlink_and_console_log_info(mavlink_fd, "[cal] mag #%u scale: x:%.2f y:%.2f z:%.2f",
cur_mag,
(double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale);
}
generated by cgit v1.2.3 (git 2.39.1) at 2024-05-17 01:56:38 +0000