diff options
author | Don Gagne <don@thegagnes.com> | 2015-03-23 19:03:23 -0700 |
---|---|---|
committer | Lorenz Meier <lm@inf.ethz.ch> | 2015-03-28 13:09:07 -0700 |
commit | d88916d20e962ddd60915a80172e83535d526193 (patch) | |
tree | 0748b41180fa926f7949bf553afb2be6842f17fe /src/modules | |
parent | 716fb561aa24aa0e18aad64dd6fa29a0a9b9ea34 (diff) | |
download | px4-firmware-d88916d20e962ddd60915a80172e83535d526193.tar.gz px4-firmware-d88916d20e962ddd60915a80172e83535d526193.tar.bz2 px4-firmware-d88916d20e962ddd60915a80172e83535d526193.zip |
New mag cal changes
- Use new calibrate_from_orientation worker routine to detect
orientaions
- Calibrate all mags at once
- Change to 3-side calibration mechanism
Diffstat (limited to 'src/modules')
-rw-r--r-- | src/modules/commander/mag_calibration.cpp | 531 |
1 files changed, 325 insertions, 206 deletions
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index a0aadab39..5f0754f98 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -49,6 +49,7 @@ #include <math.h> #include <fcntl.h> #include <drivers/drv_hrt.h> +#include <drivers/drv_accel.h> #include <uORB/topics/sensor_combined.h> #include <drivers/drv_mag.h> #include <mavlink/mavlink_log.h> @@ -62,283 +63,401 @@ 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); + +/// Data passed to calibration worker routine +typedef struct { + int mavlink_fd; + unsigned done_count; + int sub_mag[max_mags]; + unsigned int calibration_points_perside; + unsigned int calibration_interval_perside_seconds; + uint64_t calibration_interval_perside_useconds; + unsigned int calibration_counter_total; + bool side_data_collected[detect_orientation_side_count]; + float* x[max_mags]; + float* y[max_mags]; + float* z[max_mags]; +} mag_worker_data_t; -int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id); int do_mag_calibration(int mavlink_fd) { - const unsigned max_mags = 3; - - int32_t device_id[max_mags]; mavlink_and_console_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name); - sleep(1); - struct mag_scale mscale_null[max_mags] = { - { + struct mag_scale mscale_null = { 0.0f, 1.0f, 0.0f, 1.0f, 0.0f, 1.0f, - } - } ; + }; - int res = ERROR; + int result = OK; + + // Determine which mags are available and reset each + int32_t device_ids[max_mags]; char str[30]; - unsigned calibrated_ok = 0; - - for (unsigned s = 0; s < max_mags; s++) { + for (size_t i=0; i<max_mags; i++) { + device_ids[i] = 0; // signals no mag + } + + for (unsigned cur_mag = 0; cur_mag < max_mags; cur_mag++) { + // Reset mag id to mag not available + (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); + break; + } - /* erase old calibration */ - (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); + // Attempt to open mag + (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, cur_mag); int fd = open(str, O_RDONLY); - if (fd < 0) { continue; } - mavlink_and_console_log_info(mavlink_fd, "Calibrating magnetometer #%u..", s); - sleep(3); - - device_id[s] = ioctl(fd, DEVIOCGDEVICEID, 0); + // Get device id for this mag + device_ids[cur_mag] = ioctl(fd, DEVIOCGDEVICEID, 0); - /* ensure all scale fields are initialized tha same as the first struct */ - (void)memcpy(&mscale_null[s], &mscale_null[0], sizeof(mscale_null[0])); + // Reset mag scale + result = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null); - res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale_null[s]); - - if (res != OK) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG); + if (result != OK) { + mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_RESET_CAL_MSG, cur_mag); } - if (res == OK) { + if (result == OK) { /* calibrate range */ - res = ioctl(fd, MAGIOCCALIBRATE, fd); + result = ioctl(fd, MAGIOCCALIBRATE, fd); - if (res != OK) { - mavlink_and_console_log_info(mavlink_fd, "Skipped scale calibration"); + if (result != OK) { + mavlink_and_console_log_info(mavlink_fd, "Skipped scale calibration, sensor %u", cur_mag); /* this is non-fatal - mark it accordingly */ - res = OK; + result = OK; } } close(fd); - - if (res == OK) { - res = calibrate_instance(mavlink_fd, s, device_id[s]); - - if (res == OK) { - calibrated_ok++; - } - } } - if (calibrated_ok) { - - mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 100); - usleep(100000); - mavlink_and_console_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name); - + 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 */ - res = param_save_default(); - - if (res != OK) { + result = param_save_default(); + if (result != OK) { mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG); } + } + + 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); } - return res; + return result; } -int calibrate_instance(int mavlink_fd, unsigned s, unsigned device_id) +int mag_calibration_worker(detect_orientation_return orientation, void* data) { - /* 45 seconds */ - uint64_t calibration_interval = 25 * 1000 * 1000; - - /* maximum 500 values */ - const unsigned int calibration_maxcount = 240; - unsigned int calibration_counter; - - float *x = new float[calibration_maxcount]; - float *y = new float[calibration_maxcount]; - float *z = new float[calibration_maxcount]; - - char str[30]; - int res = OK; + int result = OK; - /* allocate memory */ - mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20); + unsigned int calibration_counter_side; - if (x == nullptr || y == nullptr || z == nullptr) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: out of memory"); - - /* clean up */ - if (x != nullptr) { - delete x; + 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); + sleep(2); + + uint64_t calibration_deadline = hrt_absolute_time() + worker_data->calibration_interval_perside_useconds; + unsigned poll_errcount = 0; + + calibration_counter_side = 0; + + while (hrt_absolute_time() < calibration_deadline && + calibration_counter_side < worker_data->calibration_points_perside) { + + // Wait clocking for new data on all mags + struct pollfd fds[max_mags]; + size_t fd_count = 0; + for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { + if (worker_data->sub_mag[cur_mag] >= 0) { + fds[fd_count].fd = worker_data->sub_mag[cur_mag]; + fds[fd_count].events = POLLIN; + fd_count++; + } } - - if (y != nullptr) { - delete y; + int poll_ret = poll(fds, fd_count, 1000); + + if (poll_ret > 0) { + for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { + if (worker_data->sub_mag[cur_mag] >= 0) { + struct mag_report mag; + + orb_copy(ORB_ID(sensor_mag), worker_data->sub_mag[cur_mag], &mag); + + worker_data->x[cur_mag][worker_data->calibration_counter_total] = mag.x; + worker_data->y[cur_mag][worker_data->calibration_counter_total] = mag.y; + worker_data->z[cur_mag][worker_data->calibration_counter_total] = mag.z; + + } + } + + worker_data->calibration_counter_total++; + calibration_counter_side++; + + // Progress indicator for side + mavlink_and_console_log_info(worker_data->mavlink_fd, + "%s %s side calibration: progress <%u>", + sensor_name, + detect_orientation_str(orientation), + (unsigned)(100 * ((float)calibration_counter_side / (float)worker_data->calibration_points_perside))); + } else { + poll_errcount++; } - - if (z != nullptr) { - delete z; + + if (poll_errcount > worker_data->calibration_points_perside * 3) { + result = ERROR; + mavlink_and_console_log_info(worker_data->mavlink_fd, CAL_FAILED_SENSOR_MSG); + break; } - - res = ERROR; - return res; } + + // Mark the opposite side as collected as well. No need to collect opposite side since it + // would generate similar points. + switch (orientation) { + case DETECT_ORIENTATION_TAIL_DOWN: + worker_data->side_data_collected[DETECT_ORIENTATION_NOSE_DOWN] = true; + break; + case DETECT_ORIENTATION_NOSE_DOWN: + worker_data->side_data_collected[DETECT_ORIENTATION_TAIL_DOWN] = true; + break; + case DETECT_ORIENTATION_LEFT: + worker_data->side_data_collected[DETECT_ORIENTATION_RIGHT] = true; + break; + case DETECT_ORIENTATION_RIGHT: + worker_data->side_data_collected[DETECT_ORIENTATION_LEFT] = true; + break; + case DETECT_ORIENTATION_UPSIDE_DOWN: + worker_data->side_data_collected[DETECT_ORIENTATION_RIGHTSIDE_UP] = true; + break; + case DETECT_ORIENTATION_RIGHTSIDE_UP: + worker_data->side_data_collected[DETECT_ORIENTATION_UPSIDE_DOWN] = true; + break; + case DETECT_ORIENTATION_ERROR: + warnx("Invalid orientation in mag_calibration_worker"); + break; + } + + 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; +} - if (res == OK) { - int sub_mag = orb_subscribe_multi(ORB_ID(sensor_mag), s); - - if (sub_mag < 0) { - mavlink_and_console_log_critical(mavlink_fd, "No mag found, abort"); - res = ERROR; - } else { - struct mag_report mag; - - /* limit update rate to get equally spaced measurements over time (in ms) */ - orb_set_interval(sub_mag, (calibration_interval / 1000) / calibration_maxcount); - - /* calibrate offsets */ - uint64_t calibration_deadline = hrt_absolute_time() + calibration_interval; - unsigned poll_errcount = 0; - - mavlink_and_console_log_info(mavlink_fd, "Turn on all sides: front/back,left/right,up/down"); - - calibration_counter = 0U; - - while (hrt_absolute_time() < calibration_deadline && - calibration_counter < calibration_maxcount) { - - /* wait blocking for new data */ - struct pollfd fds[1]; - fds[0].fd = sub_mag; - fds[0].events = POLLIN; - - int poll_ret = poll(fds, 1, 1000); - - if (poll_ret > 0) { - orb_copy(ORB_ID(sensor_mag), sub_mag, &mag); - - x[calibration_counter] = mag.x; - y[calibration_counter] = mag.y; - z[calibration_counter] = mag.z; - - calibration_counter++; +int mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]) +{ + int result = OK; - if (calibration_counter % (calibration_maxcount / 20) == 0) { - mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20 + (calibration_counter * 50) / calibration_maxcount); - } + mag_worker_data_t worker_data; + + worker_data.mavlink_fd = mavlink_fd; + worker_data.done_count = 0; + worker_data.calibration_counter_total = 0; + worker_data.calibration_points_perside = 80; + 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; + } + + for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { + // Initialize to no subscription + worker_data.sub_mag[cur_mag] = -1; + + // Initialize to no memory allocated + worker_data.x[cur_mag] = NULL; + worker_data.y[cur_mag] = NULL; + worker_data.z[cur_mag] = NULL; + } - } else { - poll_errcount++; - } + const unsigned int calibration_sides = 3; + const unsigned int calibration_points_maxcount = calibration_sides * worker_data.calibration_points_perside; + + char str[30]; + + for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { + worker_data.x[cur_mag] = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount)); + 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; + } + } - if (poll_errcount > 1000) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SENSOR_MSG); - res = ERROR; + + // Setup subscriptions to mag sensors + if (result == 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; break; } } - - close(sub_mag); } } - - float sphere_x; - float sphere_y; - float sphere_z; - float sphere_radius; - - if (res == OK && calibration_counter > (calibration_maxcount / 2)) { - - /* sphere fit */ - mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70); - sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius); - mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 80); - - if (!isfinite(sphere_x) || !isfinite(sphere_y) || !isfinite(sphere_z)) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: NaN in sphere fit"); - res = ERROR; + + // Limit update rate to get equally spaced measurements over time (in ms) + if (result == OK) { + for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { + if (device_ids[cur_mag] != 0) { + // Mag in this slot is available + unsigned int orb_interval_msecs = (worker_data.calibration_interval_perside_useconds / 1000) / worker_data.calibration_points_perside; + + //mavlink_and_console_log_info(mavlink_fd, "Orb interval %u msecs", orb_interval_msecs); + orb_set_interval(worker_data.sub_mag[cur_mag], orb_interval_msecs); + } } + } - if (x != nullptr) { - delete x; + result = calibrate_from_orientation(mavlink_fd, worker_data.side_data_collected, mag_calibration_worker, &worker_data); + + // Close subscriptions + for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { + if (worker_data.sub_mag[cur_mag] >= 0) { + close(worker_data.sub_mag[cur_mag]); + } } - - if (y != nullptr) { - delete y; + + // Calculate calibration values for each mag + + + float sphere_x[max_mags]; + float sphere_y[max_mags]; + float sphere_z[max_mags]; + float sphere_radius[max_mags]; + + // Sphere fit the data to get calibration values + if (result == 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 + + sphere_fit_least_squares(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag], + worker_data.calibration_counter_total, + 100, 0.0f, + &sphere_x[cur_mag], &sphere_y[cur_mag], &sphere_z[cur_mag], + &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; + } + } + } } - - if (z != nullptr) { - delete z; + + // Data points are no longer needed + for (size_t cur_mag=0; cur_mag<max_mags; cur_mag++) { + free(worker_data.x[cur_mag]); + free(worker_data.y[cur_mag]); + free(worker_data.z[cur_mag]); } + + if (result == OK) { + for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) { + if (device_ids[cur_mag] != 0) { + int fd_mag = -1; + struct mag_scale mscale; + + // Set new scale + + (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; + } + + 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 (res == OK) { - /* apply calibration and set parameters */ - struct mag_scale mscale; - (void)sprintf(str, "%s%u", MAG_BASE_DEVICE_PATH, s); - int fd = open(str, 0); - res = ioctl(fd, MAGIOCGSCALE, (long unsigned int)&mscale); - - if (res != OK) { - mavlink_and_console_log_critical(mavlink_fd, "ERROR: failed to get current calibration"); - } - - if (res == OK) { - mscale.x_offset = sphere_x; - mscale.y_offset = sphere_y; - mscale.z_offset = sphere_z; - - res = ioctl(fd, MAGIOCSSCALE, (long unsigned int)&mscale); - - if (res != OK) { - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_APPLY_CAL_MSG); - } - } + if (result == OK) { + mscale.x_offset = sphere_x[cur_mag]; + mscale.y_offset = sphere_y[cur_mag]; + mscale.z_offset = sphere_z[cur_mag]; - close(fd); + 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; + } + } + + // Mag device no longer needed + if (fd_mag >= 0) { + close(fd_mag); + } - if (res == OK) { - - bool failed = false; - /* set parameters */ - (void)sprintf(str, "CAL_MAG%u_ID", s); - failed |= (OK != param_set(param_find(str), &(device_id))); - (void)sprintf(str, "CAL_MAG%u_XOFF", s); - failed |= (OK != param_set(param_find(str), &(mscale.x_offset))); - (void)sprintf(str, "CAL_MAG%u_YOFF", s); - failed |= (OK != param_set(param_find(str), &(mscale.y_offset))); - (void)sprintf(str, "CAL_MAG%u_ZOFF", s); - failed |= (OK != param_set(param_find(str), &(mscale.z_offset))); - (void)sprintf(str, "CAL_MAG%u_XSCALE", s); - failed |= (OK != param_set(param_find(str), &(mscale.x_scale))); - (void)sprintf(str, "CAL_MAG%u_YSCALE", s); - failed |= (OK != param_set(param_find(str), &(mscale.y_scale))); - (void)sprintf(str, "CAL_MAG%u_ZSCALE", s); - failed |= (OK != param_set(param_find(str), &(mscale.z_scale))); - - if (failed) { - res = ERROR; - mavlink_and_console_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG); + if (result == OK) { + bool failed = false; + + /* set parameters */ + (void)sprintf(str, "CAL_MAG%u_XOFF", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_offset))); + (void)sprintf(str, "CAL_MAG%u_YOFF", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_offset))); + (void)sprintf(str, "CAL_MAG%u_ZOFF", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.z_offset))); + (void)sprintf(str, "CAL_MAG%u_XSCALE", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.x_scale))); + (void)sprintf(str, "CAL_MAG%u_YSCALE", cur_mag); + failed |= (OK != param_set_no_notification(param_find(str), &(mscale.y_scale))); + (void)sprintf(str, "CAL_MAG%u_ZSCALE", cur_mag); + 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; + } else { + mavlink_and_console_log_info(mavlink_fd, "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", + cur_mag, + (double)mscale.x_scale, (double)mscale.y_scale, (double)mscale.z_scale); + } + } } - - mavlink_and_console_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 90); } - - mavlink_and_console_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); - mavlink_and_console_log_info(mavlink_fd, "mag scale: x:%.2f y:%.2f z:%.2f", (double)mscale.x_scale, - (double)mscale.y_scale, (double)mscale.z_scale); } - return res; + return result; } |