From 9955cb4cc8b838872e7d99094dcb21f2ab358cfa Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Mon, 8 Mar 2021 11:23:44 -0500 Subject: [PATCH] update accelerometer calibration to use Levenberg Marquardt least squares fit - update accel cal to use Levenberg Marquardt least squares fit (same as magnetometer) - this is more robust to user positioning error as it only requires a nice distribution of points and doesn't actually depend on the user holding the vehicle in each orientation (although the calibration flow is still structured this way) - use online Welford mean to gather clean readings per orientation - reset if movement during per side data gathering - detect invalid rotations (and soon auto correct like mag) - add `CAL_ACC_SIDES` (analogous to CAL_MAG_SIDES) to limit accel calibration to a subset of orientations (when full set is difficult or impossible) - accel quick calibration (commander calibrate accel quick) assume vehicle is aligned with gravity and adjust offsets immediately --- src/lib/sensor_calibration/Accelerometer.hpp | 1 + .../commander/accelerometer_calibration.cpp | 707 ++++++++++-------- .../commander/calibration_routines.cpp | 11 +- src/modules/commander/calibration_routines.h | 11 +- src/modules/commander/lm_fit.cpp | 274 +++---- src/modules/commander/lm_fit.hpp | 11 +- src/modules/commander/mag_calibration.cpp | 96 +-- .../commander/mag_calibration_test.cpp | 79 +- src/modules/sensors/sensor_params_acc.c | 66 ++ 9 files changed, 712 insertions(+), 544 deletions(-) create mode 100644 src/modules/sensors/sensor_params_acc.c diff --git a/src/lib/sensor_calibration/Accelerometer.hpp b/src/lib/sensor_calibration/Accelerometer.hpp index 98d9c99e2e..19fe0d377b 100644 --- a/src/lib/sensor_calibration/Accelerometer.hpp +++ b/src/lib/sensor_calibration/Accelerometer.hpp @@ -76,6 +76,7 @@ public: const matrix::Dcmf &rotation() const { return _rotation; } const Rotation &rotation_enum() const { return _rotation_enum; } const matrix::Vector3f &scale() const { return _scale; } + const matrix::Vector3f &thermal_offset() const { return _thermal_offset; } // apply offsets and scale // rotate corrected measurements from sensor to body frame diff --git a/src/modules/commander/accelerometer_calibration.cpp b/src/modules/commander/accelerometer_calibration.cpp index 5907496d3f..92a4ab9b0c 100644 --- a/src/modules/commander/accelerometer_calibration.cpp +++ b/src/modules/commander/accelerometer_calibration.cpp @@ -1,6 +1,6 @@ /**************************************************************************** * - * Copyright (c) 2013-2020 PX4 Development Team. All rights reserved. + * Copyright (c) 2013-2021 PX4 Development Team. All rights reserved. * * Redistribution and use in source and binary forms, with or without * modification, are permitted provided that the following conditions @@ -130,13 +130,17 @@ #include #include +#include "lm_fit.hpp" + #include +#include #include #include #include #include #include #include +#include #include #include #include @@ -150,191 +154,174 @@ using namespace matrix; using namespace time_literals; static constexpr char sensor_name[] {"accel"}; -static constexpr unsigned MAX_ACCEL_SENS = 4; +static constexpr uint8_t MAX_ACCEL_SENS = 4; /// Data passed to calibration worker routine struct accel_worker_data_s { - orb_advert_t *mavlink_log_pub{nullptr}; - unsigned done_count{0}; - float accel_ref[MAX_ACCEL_SENS][detect_orientation_side_count][3] {}; - float accel_temperature_ref[MAX_ACCEL_SENS] {NAN, NAN, NAN, NAN}; + orb_advert_t *mavlink_log_pub{nullptr}; + unsigned done_count{0}; + matrix::Vector3f accel_ref[MAX_ACCEL_SENS][detect_orientation_side_count] {}; + float accel_temperature_ref[MAX_ACCEL_SENS] {NAN, NAN, NAN, NAN}; + + unsigned calibration_sides{0}; ///< The total number of sides + bool side_data_collected[detect_orientation_side_count] {}; }; // Read specified number of accelerometer samples, calculate average and dispersion. -static calibrate_return read_accelerometer_avg(float (&accel_avg)[MAX_ACCEL_SENS][detect_orientation_side_count][3], - float (&accel_temperature_avg)[MAX_ACCEL_SENS], unsigned orient, unsigned samples_num) +static calibrate_return read_accelerometer_avg(Vector3f(&accel_avg)[MAX_ACCEL_SENS][detect_orientation_side_count], + float (&accel_temperature_avg)[MAX_ACCEL_SENS], unsigned orient) { - Vector3f accel_sum[MAX_ACCEL_SENS] {}; + calibration::Accelerometer calibrations[MAX_ACCEL_SENS] {}; + uORB::SubscriptionMultiArray accel_subs{ORB_ID::sensor_accel}; + math::WelfordMean mean[MAX_ACCEL_SENS] {}; + + Vector3f accel_prev[MAX_ACCEL_SENS] {}; + float temperature_sum[MAX_ACCEL_SENS] {NAN, NAN, NAN, NAN}; - unsigned counts[MAX_ACCEL_SENS] {}; - - unsigned errcount = 0; - - // sensor thermal corrections - uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)}; - sensor_correction_s sensor_correction{}; - sensor_correction_sub.copy(&sensor_correction); - - uORB::SubscriptionBlocking accel_sub[MAX_ACCEL_SENS] { - {ORB_ID(sensor_accel), 0, 0}, - {ORB_ID(sensor_accel), 0, 1}, - {ORB_ID(sensor_accel), 0, 2}, - {ORB_ID(sensor_accel), 0, 3}, - }; + unsigned temperature_count[MAX_ACCEL_SENS] {}; /* use the first sensor to pace the readout, but do per-sensor counts */ - while (counts[0] < samples_num) { - if (accel_sub[0].updatedBlocking(100000)) { - for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) { - sensor_accel_s arp; + bool done = false; - while (accel_sub[accel_index].update(&arp)) { - // fetch optional thermal offset corrections in sensor/board frame - Vector3f offset{0, 0, 0}; - sensor_correction_sub.update(&sensor_correction); + const unsigned num_accels = math::min(accel_subs.advertised_count(), MAX_ACCEL_SENS); - if (sensor_correction.timestamp > 0 && arp.device_id != 0) { - for (uint8_t correction_index = 0; correction_index < MAX_ACCEL_SENS; correction_index++) { - if (sensor_correction.accel_device_ids[correction_index] == arp.device_id) { - switch (correction_index) { - case 0: - offset = Vector3f{sensor_correction.accel_offset_0}; - break; - case 1: - offset = Vector3f{sensor_correction.accel_offset_1}; - break; - case 2: - offset = Vector3f{sensor_correction.accel_offset_2}; - break; - case 3: - offset = Vector3f{sensor_correction.accel_offset_3}; - break; - } - } - } - } + const hrt_abstime timestamp_start = hrt_absolute_time(); - accel_sum[accel_index] += Vector3f{arp.x, arp.y, arp.z} - offset; + while (!done && (hrt_elapsed_time(×tamp_start) < 5_s)) { + unsigned good_count = 0; - counts[accel_index]++; + for (unsigned accel_index = 0; accel_index < num_accels; accel_index++) { + sensor_accel_s arp; - if (!PX4_ISFINITE(temperature_sum[accel_index])) { - // set first valid value - temperature_sum[accel_index] = (arp.temperature * counts[accel_index]); + while (accel_subs[accel_index].update(&arp)) { + calibrations[accel_index].set_device_id(arp.device_id); + calibrations[accel_index].SensorCorrectionsUpdate(); - } else { - temperature_sum[accel_index] += arp.temperature; - } + // fetch optional thermal offset corrections in sensor/board frame + const Vector3f accel{Vector3f{arp.x, arp.y, arp.z} - calibrations[accel_index].thermal_offset()}; + + if ((accel - accel_prev[accel_index]).longerThan(0.5f)) { + mean[accel_index].reset(); + accel_prev[accel_index] = accel; + + } else { + mean[accel_index].update(accel); + } + + if (!PX4_ISFINITE(temperature_sum[accel_index])) { + // set first valid value + temperature_sum[accel_index] = arp.temperature; + temperature_count[accel_index] = 1; + + } else { + temperature_sum[accel_index] += arp.temperature; + temperature_count[accel_index]++; } } + if ((mean[accel_index].count() > 1000) && !mean[accel_index].variance().longerThan(0.01)) { + good_count++; + } + } + + if ((good_count > 0) && (good_count == num_accels)) { + PX4_INFO("finished early: %d", mean[0].count()); + done = true; + } else { - errcount++; - continue; - } - - if (errcount > samples_num / 10) { - return calibrate_return_error; + px4_usleep(2000); } } - // rotate sensor measurements from sensor to body frame using board rotation matrix - const Dcmf board_rotation = calibration::GetBoardRotationMatrix(); + unsigned good_count = 0; - for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) { - accel_sum[s] = board_rotation * accel_sum[s]; + for (unsigned s = 0; s < num_accels; s++) { + if (mean[s].valid()) { + const Vector3f avg = mean[s].mean(); + const Vector3f var = mean[s].variance(); + + PX4_INFO("O: %d, Accel: %d, Mean: [%.6f, %.6f, %.6f], Variance: [%.6f, %.6f, %.6f]", orient, s, + (double)avg(0), (double)avg(1), (double)avg(2), + (double)var(0), (double)var(1), (double)var(2) + ); + + if (!var.longerThan(0.01f)) { + good_count++; + + } else { + break; + } + + accel_avg[s][orient] = mean[s].mean(); + + } else { + accel_avg[s][orient].zero(); + } } for (unsigned s = 0; s < MAX_ACCEL_SENS; s++) { - const Vector3f avg{accel_sum[s] / counts[s]}; - avg.copyTo(accel_avg[s][orient]); + if (temperature_count[s] > 0) { + accel_temperature_avg[s] = temperature_sum[s] / temperature_count[s]; - accel_temperature_avg[s] = temperature_sum[s] / counts[s]; + } else { + accel_temperature_avg[s] = NAN; + } } - return calibrate_return_ok; + return (good_count == num_accels) ? calibrate_return_ok : calibrate_return_error; } static calibrate_return accel_calibration_worker(detect_orientation_return orientation, void *data) { - static constexpr unsigned samples_num = 750; accel_worker_data_s *worker_data = (accel_worker_data_s *)(data); - calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side", - detect_orientation_str(orientation)); + int attempt = 0; - read_accelerometer_avg(worker_data->accel_ref, worker_data->accel_temperature_ref, orientation, samples_num); + while (attempt < 10) { + calibration_log_info(worker_data->mavlink_log_pub, "[cal] Hold still, measuring %s side", + detect_orientation_str(orientation)); - // check accel - for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) { - switch (orientation) { - case ORIENTATION_TAIL_DOWN: // [ g, 0, 0 ] - if (worker_data->accel_ref[accel_index][ORIENTATION_TAIL_DOWN][0] < 0.f) { - calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid X-axis, check rotation", accel_index); - return calibrate_return_error; - } + if (read_accelerometer_avg(worker_data->accel_ref, worker_data->accel_temperature_ref, + orientation) == calibrate_return_ok) { - break; + calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%.3f %.3f %.3f]", + detect_orientation_str(orientation), + (double)worker_data->accel_ref[0][orientation](0), + (double)worker_data->accel_ref[0][orientation](1), + (double)worker_data->accel_ref[0][orientation](2)); - case ORIENTATION_NOSE_DOWN: // [ -g, 0, 0 ] - if (worker_data->accel_ref[accel_index][ORIENTATION_NOSE_DOWN][0] > 0.f) { - calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid X-axis, check rotation", accel_index); - return calibrate_return_error; - } + worker_data->done_count++; + calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count); - break; - - case ORIENTATION_LEFT: // [ 0, g, 0 ] - if (worker_data->accel_ref[accel_index][ORIENTATION_LEFT][1] < 0.f) { - calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid Y-axis, check rotation", accel_index); - return calibrate_return_error; - } - - break; - - case ORIENTATION_RIGHT: // [ 0, -g, 0 ] - if (worker_data->accel_ref[accel_index][ORIENTATION_RIGHT][1] > 0.f) { - calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid Y-axis, check rotation", accel_index); - return calibrate_return_error; - } - - break; - - case ORIENTATION_UPSIDE_DOWN: // [ 0, 0, g ] - if (worker_data->accel_ref[accel_index][ORIENTATION_UPSIDE_DOWN][2] < 0.f) { - calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid Z-axis, check rotation", accel_index); - return calibrate_return_error; - } - - break; - - case ORIENTATION_RIGHTSIDE_UP: // [ 0, 0, -g ] - if (worker_data->accel_ref[accel_index][ORIENTATION_RIGHTSIDE_UP][2] > 0.f) { - calibration_log_emergency(worker_data->mavlink_log_pub, "[cal] accel %d invalid Z-axis, check rotation", accel_index); - return calibrate_return_error; - } - - break; - - default: - break; + return calibrate_return_ok; } + + attempt++; } - calibration_log_info(worker_data->mavlink_log_pub, "[cal] %s side result: [%.3f %.3f %.3f]", - detect_orientation_str(orientation), - (double)worker_data->accel_ref[0][orientation][0], - (double)worker_data->accel_ref[0][orientation][1], - (double)worker_data->accel_ref[0][orientation][2]); - - worker_data->done_count++; - calibration_log_info(worker_data->mavlink_log_pub, CAL_QGC_PROGRESS_MSG, 17 * worker_data->done_count); - - return calibrate_return_ok; + return calibrate_return_error; } int do_accel_calibration(orb_advert_t *mavlink_log_pub) { + // We should not try to subscribe if the topic doesn't actually exist and can be counted. + const unsigned orb_accel_count = orb_group_count(ORB_ID(sensor_accel)); + + // Warn that we will not calibrate more than MAX_GYROS gyroscopes + if (orb_accel_count > MAX_ACCEL_SENS) { + calibration_log_critical(mavlink_log_pub, "Detected %u accels, but will calibrate only %u", orb_accel_count, + MAX_ACCEL_SENS); + + } else if (orb_accel_count < 1) { + calibration_log_critical(mavlink_log_pub, "No accels found"); + return PX4_ERROR; + } + + // Collect: As defined by configuration + // start with a full mask, all six bits set + int32_t cal_mask = (1 << 6) - 1; + param_get(param_find("CAL_ACC_SIDES"), &cal_mask); + calibration_log_info(mavlink_log_pub, CAL_QGC_STARTED_MSG, sensor_name); calibration::Accelerometer calibrations[MAX_ACCEL_SENS] {}; @@ -370,83 +357,222 @@ int do_accel_calibration(orb_advert_t *mavlink_log_pub) /* measure and calculate offsets & scales */ accel_worker_data_s worker_data{}; worker_data.mavlink_log_pub = mavlink_log_pub; - bool data_collected[detect_orientation_side_count] {}; - if (calibrate_from_orientation(mavlink_log_pub, data_collected, accel_calibration_worker, &worker_data, - false) == calibrate_return_ok) { + for (unsigned i = 0; i < (sizeof(worker_data.side_data_collected) / sizeof(worker_data.side_data_collected[0])); i++) { + if ((cal_mask & (1 << i)) > 0) { + // mark as missing + worker_data.side_data_collected[i] = false; + worker_data.calibration_sides++; - const Dcmf board_rotation = calibration::GetBoardRotationMatrix(); - const Dcmf board_rotation_t = board_rotation.transpose(); + } else { + // mark as completed from the beginning + worker_data.side_data_collected[i] = true; + + calibration_log_info(mavlink_log_pub, + "[cal] %s side done, rotate to a different side", + detect_orientation_str(static_cast(i))); + px4_usleep(100000); + } + } + + if (calibrate_from_orientation(mavlink_log_pub, worker_data.side_data_collected, accel_calibration_worker, + &worker_data) == calibrate_return_ok) { + + const Rotation board_rotation = calibration::GetBoardRotation(); bool param_save = false; bool failed = true; - for (unsigned i = 0; i < MAX_ACCEL_SENS; i++) { - if (i < active_sensors) { - // calculate offsets - Vector3f offset{}; + int internal_rotation_count{0}; + Rotation internal_rotations[MAX_ACCEL_SENS] {}; - // X offset: average X from TAIL_DOWN + NOSE_DOWN - const Vector3f accel_tail_down{worker_data.accel_ref[i][ORIENTATION_TAIL_DOWN]}; - const Vector3f accel_nose_down{worker_data.accel_ref[i][ORIENTATION_NOSE_DOWN]}; - offset(0) = (accel_tail_down(0) + accel_nose_down(0)) * 0.5f; + for (uint8_t cur_accel = 0; cur_accel < active_sensors; cur_accel++) { + int accel_data_count = 0; - // Y offset: average Y from LEFT + RIGHT - const Vector3f accel_left{worker_data.accel_ref[i][ORIENTATION_LEFT]}; - const Vector3f accel_right{worker_data.accel_ref[i][ORIENTATION_RIGHT]}; - offset(1) = (accel_left(1) + accel_right(1)) * 0.5f; + matrix::Vector3f accel_data[6] {}; - // Z offset: average Z from UPSIDE_DOWN + RIGHTSIDE_UP - const Vector3f accel_upside_down{worker_data.accel_ref[i][ORIENTATION_UPSIDE_DOWN]}; - const Vector3f accel_rightside_up{worker_data.accel_ref[i][ORIENTATION_RIGHTSIDE_UP]}; - offset(2) = (accel_upside_down(2) + accel_rightside_up(2)) * 0.5f; + for (int orientation = 0; orientation < 6; orientation++) { + if ((cal_mask & (1 << orientation)) > 0) { + accel_data[accel_data_count] = worker_data.accel_ref[cur_accel][orientation]; + accel_data_count++; + } + } - // transform matrix - Matrix3f mat_A; - mat_A.row(0) = accel_tail_down - offset; - mat_A.row(1) = accel_left - offset; - mat_A.row(2) = accel_upside_down - offset; + bool sphere_fit_only = false; + bool sphere_fit_success = false; + bool ellipsoid_fit_success = false; - // calculate inverse matrix for A: simplify matrices mult because b has only one non-zero element == g at index i - const Matrix3f accel_T = mat_A.I() * CONSTANTS_ONE_G; + sphere_params sphere_data{}; + sphere_data.radius = CONSTANTS_ONE_G; + int ret = lm_fit(worker_data.accel_ref[cur_accel], 6, sphere_data, false); - // update calibration - const Vector3f accel_offs_rotated{board_rotation_t *offset}; - calibrations[i].set_offset(accel_offs_rotated); + if (ret == PX4_OK) { + sphere_fit_success = true; - const Matrix3f accel_T_rotated{board_rotation_t *accel_T * board_rotation}; - calibrations[i].set_scale(accel_T_rotated.diag()); + if (!sphere_fit_only) { + int ellipsoid_ret = lm_fit(accel_data, accel_data_count, sphere_data, true); - calibrations[i].set_temperature(worker_data.accel_temperature_ref[i]); + if (ellipsoid_ret == PX4_OK) { + ellipsoid_fit_success = true; + } + } + + PX4_INFO("Accel: %" PRIu8 " sphere radius: %.4f", cur_accel, (double)sphere_data.radius); + } + + static constexpr float scale_min = 0.9f; + static constexpr float scale_max = 1.1f; + + for (int axis = 0; axis < 3; axis++) { + if ((sphere_data.diag(axis) < scale_min) || (sphere_data.diag(axis) > scale_max)) { + PX4_WARN("bad scale factor"); // TODO, abort and report failure + } + } + + bool calibration_valid = sphere_fit_success || ellipsoid_fit_success; // TODO: scale range, offset range, etc #if defined(DEBUD_BUILD) - PX4_INFO("accel %d: offset", i); - offset.print(); - PX4_INFO("accel %d: bT * offset", i); - accel_offs_rotated.print(); + PX4_INFO("accel %d offset", cur_accel); + sphere_data.offset.print(); - PX4_INFO("accel %d: mat_A", i); - mat_A.print(); - PX4_INFO("accel %d: accel_T", i); - accel_T.print(); - PX4_INFO("accel %d: bT * accel_T * b", i); - accel_T_rotated.print(); + PX4_INFO("accel %d scale", cur_accel); + sphere_data.diag.print(); + + PX4_INFO("accel %d offdiagonal", cur_accel); + sphere_data.offdiag.print(); #endif // DEBUD_BUILD - calibrations[i].PrintStatus(); + + if (!calibration_valid) { + failed = true; + break; + } + + // determine best rotation + float calibration_metric[ROTATION_MAX] {}; + float min_error = FLT_MAX; + Rotation best_rotation = ROTATION_NONE; + + for (int r = ROTATION_NONE; r < ROTATION_MAX; r++) { + calibration_metric[r] = FLT_MAX; + + // try all rotations + switch (r) { + case ROTATION_ROLL_90_PITCH_68_YAW_293: // skip + case ROTATION_PITCH_180_YAW_90: // skip 26, same as 14 ROTATION_ROLL_180_YAW_270 + case ROTATION_PITCH_180_YAW_270: // skip 27, same as 10 ROTATION_ROLL_180_YAW_90 + break; + + default: { + matrix::Matrix Y; + static constexpr float g = CONSTANTS_ONE_G; + Y.row(0) = Vector3f{ g, 0, 0}; // ORIENTATION_TAIL_DOWN + Y.row(1) = Vector3f{-g, 0, 0}; // ORIENTATION_NOSE_DOWN, + Y.row(2) = Vector3f{ 0, g, 0}; // ORIENTATION_LEFT, + Y.row(3) = Vector3f{ 0, -g, 0}; // ORIENTATION_RIGHT, + Y.row(4) = Vector3f{ 0, 0, g}; // ORIENTATION_UPSIDE_DOWN, + Y.row(5) = Vector3f{ 0, 0, -g}; // ORIENTATION_RIGHTSIDE_UP + + // apply calibration and compare data with expected direction + for (int orientation = 0; orientation < 6; orientation++) { + + if ((cal_mask & (1 << orientation)) > 0) { + Vector3f calibrated_accel{sphere_data.diag.emult(worker_data.accel_ref[cur_accel][orientation] - sphere_data.offset)}; + + const Vector3f rotated_data{get_rot_matrix((Rotation)r) *calibrated_accel}; + + calibration_metric[r] += (rotated_data - Y.row(orientation)).norm(); + } + } + } + } - if (calibrations[i].ParametersSave()) { - param_save = true; - failed = false; + if (calibration_metric[r] < min_error) { + min_error = calibration_metric[r]; + best_rotation = (Rotation)r; + } - } else { - failed = true; - calibration_log_critical(mavlink_log_pub, "calibration save failed"); + } + + PX4_INFO("best rotation: %d", best_rotation); + + if (!calibrations[cur_accel].external()) { + internal_rotations[internal_rotation_count] = best_rotation; + internal_rotation_count++; + } + + bool print_all_errors = true; + + if (calibrations[cur_accel].external()) { + switch (calibrations[cur_accel].rotation_enum()) { + case ROTATION_ROLL_90_PITCH_68_YAW_293: + PX4_INFO("[cal] External Accel: %" PRIu8 " (%" PRIu32 "), keeping manually configured rotation %" PRIu8, + cur_accel, calibrations[cur_accel].device_id(), calibrations[cur_accel].rotation_enum()); + continue; + + default: { + if (best_rotation != calibrations[cur_accel].rotation_enum()) { + calibration_log_info(mavlink_log_pub, "[cal] External Accel: %" PRIu8 " (%" PRIu32 "), determined rotation: %" PRIu8, + cur_accel, calibrations[cur_accel].device_id(), best_rotation); + calibrations[cur_accel].set_rotation(best_rotation); + + } else { + PX4_INFO("[cal] External Accel: %" PRIu8 " (%" PRIu32 "), no rotation change: %" PRIu8, + cur_accel, calibrations[cur_accel].device_id(), best_rotation); + } + } break; } } + + if (print_all_errors) { + for (int r = ROTATION_NONE; r < ROTATION_MAX; r++) { + if (calibration_metric[r] < FLT_MAX) { + PX4_ERR("Accel: %" PRIu8 " (%" PRIu32 "), rotation: %" PRIu32 ", error: %.3f", + cur_accel, calibrations[cur_accel].device_id(), (uint32_t)r, (double)calibration_metric[r]); + } + } + } + + // update calibration + calibrations[cur_accel].set_offset(sphere_data.offset); + calibrations[cur_accel].set_scale(sphere_data.diag); + calibrations[cur_accel].set_temperature(worker_data.accel_temperature_ref[cur_accel]); + + calibrations[cur_accel].PrintStatus(); + + // save all calibrations including empty slots + if (calibrations[cur_accel].ParametersSave()) { + param_save = true; + failed = false; + + } else { + failed = true; + calibration_log_critical(mavlink_log_pub, "calibration save failed"); + break; + } } + // review SENS_BOARD_ROT, do all internal accels agree on SENS_BOARD_ROT? + if (!failed && (internal_rotation_count > 0)) { + Rotation best_rotation = internal_rotations[0]; + bool same = true; + + for (unsigned i = 0; i < MAX_ACCEL_SENS; i++) { + if (internal_rotations[0] != internal_rotations[i]) { + same = false; + break; + } + } + + if (same && (best_rotation != board_rotation)) { + PX4_ERR("Incorrect board rotation: %d", board_rotation); + int32_t sens_board_rot = best_rotation; + param_set_no_notification(param_find("SENS_BOARD_ROT"), &sens_board_rot); + } + } + + if (!failed && factory_storage.store() != PX4_OK) { failed = true; } @@ -472,7 +598,6 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub) #if !defined(CONSTRAINED_FLASH) PX4_INFO("Accelerometer quick calibration"); - bool param_save = false; bool failed = true; FactoryCalibrationStorage factory_storage; @@ -482,135 +607,113 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub) return PX4_ERROR; } - // sensor thermal corrections (optional) - uORB::Subscription sensor_correction_sub{ORB_ID(sensor_correction)}; - sensor_correction_s sensor_correction{}; - sensor_correction_sub.copy(&sensor_correction); - + calibration::Accelerometer calibrations[MAX_ACCEL_SENS] {}; + math::WelfordMean mean[MAX_ACCEL_SENS] {}; uORB::SubscriptionMultiArray accel_subs{ORB_ID::sensor_accel}; + px4::Bitset valid_cal; - /* use the first sensor to pace the readout, but do per-sensor counts */ - for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) { - sensor_accel_s arp{}; - Vector3f accel_sum{}; - float temperature_sum{NAN}; - unsigned count = 0; + const hrt_abstime start_time = hrt_absolute_time(); - while (accel_subs[accel_index].update(&arp)) { - // fetch optional thermal offset corrections in sensor/board frame - if ((arp.timestamp > 0) && (arp.device_id != 0)) { - Vector3f offset{0, 0, 0}; + Vector3f accel_prev[MAX_ACCEL_SENS] {}; - if (sensor_correction.timestamp > 0) { - for (uint8_t correction_index = 0; correction_index < MAX_ACCEL_SENS; correction_index++) { - if (sensor_correction.accel_device_ids[correction_index] == arp.device_id) { - switch (correction_index) { - case 0: - offset = Vector3f{sensor_correction.accel_offset_0}; - break; - case 1: - offset = Vector3f{sensor_correction.accel_offset_1}; - break; - case 2: - offset = Vector3f{sensor_correction.accel_offset_2}; - break; - case 3: - offset = Vector3f{sensor_correction.accel_offset_3}; - break; - } - } - } - } + while ((hrt_elapsed_time(&start_time) < 3_s) && (valid_cal.count() < accel_subs.advertised_count())) { + for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) { + sensor_accel_s sensor_accel; - const Vector3f accel{Vector3f{arp.x, arp.y, arp.z} - offset}; + while (accel_subs[accel_index].update(&sensor_accel)) { + if ((sensor_accel.timestamp > 0) && (sensor_accel.device_id != 0)) { + calibrations[accel_index].set_device_id(sensor_accel.device_id); + calibrations[accel_index].SensorCorrectionsUpdate(); - if (count > 0) { - const Vector3f diff{accel - (accel_sum / count)}; + const Vector3f accel{Vector3f{sensor_accel.x, sensor_accel.y, sensor_accel.z} - calibrations[accel_index].thermal_offset()}; - if (diff.norm() < 1.f) { - accel_sum += Vector3f{arp.x, arp.y, arp.z} - offset; + if ((accel - accel_prev[accel_index]).longerThan(0.5f)) { + mean[accel_index].reset(); + accel_prev[accel_index] = accel; - count++; - - if (!PX4_ISFINITE(temperature_sum)) { - // set first valid value - temperature_sum = (arp.temperature * count); - - } else { - temperature_sum += arp.temperature; - } + } else { + mean[accel_index].update(accel); } - } else { - accel_sum = accel; - temperature_sum = arp.temperature; - count = 1; + if (mean[accel_index].count() > 300 && !mean[accel_index].variance().longerThan(0.01f)) { + valid_cal.set(accel_index, true); + } } } } - if ((count > 0) && (arp.device_id != 0)) { + px4_usleep(1000); - bool calibrated = false; - const Vector3f accel_avg = accel_sum / count; - const float temperature_avg = temperature_sum / count; + for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) { + if (mean[accel_index].count() > 0) { + PX4_DEBUG("accel %d/%d valid %d var: %.6f", accel_index, accel_subs.advertised_count(), mean[accel_index].count(), + (double)mean[accel_index].variance().length()); + } + } + } + + bool calibration_updated = false; + + px4::Bitset calibrated; + + static constexpr float g = CONSTANTS_ONE_G; + matrix::Matrix Y; + Y.row(0) = Vector3f{ g, 0, 0}; // ORIENTATION_TAIL_DOWN + Y.row(1) = Vector3f{-g, 0, 0}; // ORIENTATION_NOSE_DOWN, + Y.row(2) = Vector3f{ 0, g, 0}; // ORIENTATION_LEFT, + Y.row(3) = Vector3f{ 0, -g, 0}; // ORIENTATION_RIGHT, + Y.row(4) = Vector3f{ 0, 0, g}; // ORIENTATION_UPSIDE_DOWN, + Y.row(5) = Vector3f{ 0, 0, -g}; // ORIENTATION_RIGHTSIDE_UP + + for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) { + if (valid_cal[accel_index]) { + + const Vector3f accel_avg = mean[accel_index].mean(); + + int closest_orientation = -1; + float smallest_orientation_error = INFINITY; + + for (int i = 0; i < 6; i++) { + // assume sensor/board is in one of these orientations + float accel_error = (Vector3f{Y.row(i)} - accel_avg).length(); + + if (accel_error < smallest_orientation_error) { + closest_orientation = i; + smallest_orientation_error = accel_error; + } + } Vector3f offset{0.f, 0.f, 0.f}; - uORB::SubscriptionData attitude_sub{ORB_ID(vehicle_attitude)}; - attitude_sub.update(); + if (closest_orientation != -1) { + PX4_INFO("assuming accel %d is orientation %s (%d)", accel_index, + detect_orientation_str((enum detect_orientation_return)closest_orientation), + closest_orientation); - if (attitude_sub.advertised() && attitude_sub.get().timestamp != 0) { - // use vehicle_attitude if available - const vehicle_attitude_s &att = attitude_sub.get(); - const matrix::Quatf q{att.q}; - const Vector3f accel_ref = q.conjugate_inversed(Vector3f{0.f, 0.f, -CONSTANTS_ONE_G}); - - // sanity check angle between acceleration vectors - const float angle = AxisAnglef(Quatf(accel_avg, accel_ref)).angle(); - - if (angle <= math::radians(10.f)) { - offset = accel_avg - accel_ref; - calibrated = true; - } + offset = accel_avg - (calibrations[accel_index].scale().emult(Vector3f{Y.row(closest_orientation)})); + calibrated.set(accel_index, true); } - if (!calibrated) { - // otherwise simply normalize to gravity and remove offset - Vector3f accel{accel_avg}; - accel.normalize(); - accel = accel * CONSTANTS_ONE_G; + const Vector3f accel_avg_calibrated = calibrations[accel_index].scale().emult(accel_avg - offset); - offset = accel_avg - accel; - calibrated = true; - } - - calibration::Accelerometer calibration{arp.device_id}; - - // reset cal index to uORB - calibration.set_calibration_index(accel_index); - - if (!calibrated || (offset.norm() > CONSTANTS_ONE_G) - || !PX4_ISFINITE(offset(0)) - || !PX4_ISFINITE(offset(1)) - || !PX4_ISFINITE(offset(2))) { + if (!calibrated[accel_index] || !accel_avg_calibrated.longerThan(CONSTANTS_ONE_G * 0.9f) + || accel_avg_calibrated.longerThan(CONSTANTS_ONE_G * 1.1f) + || !PX4_ISFINITE(offset(0)) || !PX4_ISFINITE(offset(1)) || !PX4_ISFINITE(offset(2))) { PX4_ERR("accel %d quick calibrate failed", accel_index); + failed = true; + break; } else { - calibration.set_offset(offset); - calibration.set_temperature(temperature_avg); + // reset cal index to uORB + calibrations[accel_index].set_calibration_index(accel_index); - if (calibration.ParametersSave()) { - calibration.PrintStatus(); - param_save = true; - failed = false; - - } else { - failed = true; - calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, "calibration save failed"); - break; + if (calibrations[accel_index].set_offset(offset)) { + calibrations[accel_index].PrintStatus(); + calibration_updated = true; } + + failed = false; } } } @@ -619,15 +722,29 @@ int do_accel_calibration_quick(orb_advert_t *mavlink_log_pub) failed = true; } - if (param_save) { - param_notify_changes(); - } - if (!failed) { + if (calibration_updated) { + bool param_save = false; + + for (unsigned accel_index = 0; accel_index < MAX_ACCEL_SENS; accel_index++) { + if (calibrated[accel_index]) { + if (calibrations[accel_index].ParametersSave()) { + param_save = true; + } + } + } + + if (param_save) { + param_notify_changes(); + } + } + + calibration_log_info(mavlink_log_pub, CAL_QGC_DONE_MSG, sensor_name); return PX4_OK; } #endif // !CONSTRAINED_FLASH + calibration_log_critical(mavlink_log_pub, CAL_QGC_FAILED_MSG, sensor_name); return PX4_ERROR; } diff --git a/src/modules/commander/calibration_routines.cpp b/src/modules/commander/calibration_routines.cpp index 76d373a664..0da1043464 100644 --- a/src/modules/commander/calibration_routines.cpp +++ b/src/modules/commander/calibration_routines.cpp @@ -63,17 +63,16 @@ using namespace time_literals; -enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, bool lenient_still_position) +enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub) { static constexpr unsigned ndim = 3; float accel_ema[ndim] {}; // exponential moving average of accel float accel_disp[3] {}; // max-hold dispersion of accel static constexpr float ema_len = 0.5f; // EMA time constant in seconds - static constexpr float normal_still_thr = 0.25; // normal still threshold - float still_thr2 = powf(lenient_still_position ? (normal_still_thr * 3) : normal_still_thr, 2); + float still_thr2 = powf(0.25f * 3, 2); static constexpr float accel_err_thr = 5.0f; // set accel error threshold to 5m/s^2 - const hrt_abstime still_time = lenient_still_position ? 500000 : 1300000; // still time required in us + static constexpr hrt_abstime still_time = 500000; /* set timeout to 90s */ static constexpr hrt_abstime timeout = 90_s; @@ -219,7 +218,7 @@ const char *detect_orientation_str(enum detect_orientation_return orientation) calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, bool side_data_collected[detect_orientation_side_count], calibration_from_orientation_worker_t calibration_worker, - void *worker_data, bool lenient_still_position) + void *worker_data) { const hrt_abstime calibration_started = hrt_absolute_time(); calibrate_return result = calibrate_return_ok; @@ -268,7 +267,7 @@ calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, px4_usleep(20000); calibration_log_info(mavlink_log_pub, "[cal] hold vehicle still on a pending side"); px4_usleep(20000); - enum detect_orientation_return orient = detect_orientation(mavlink_log_pub, lenient_still_position); + enum detect_orientation_return orient = detect_orientation(mavlink_log_pub); if (orient == ORIENTATION_ERROR) { orientation_failures++; diff --git a/src/modules/commander/calibration_routines.h b/src/modules/commander/calibration_routines.h index a18fa1d4d6..bcee1d213a 100644 --- a/src/modules/commander/calibration_routines.h +++ b/src/modules/commander/calibration_routines.h @@ -54,8 +54,7 @@ static constexpr unsigned detect_orientation_side_count = 6; /// Wait for vehicle to become still and detect it's orientation /// @return Returns detect_orientation_return according to orientation when vehicle /// and ready for measurements -enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub, ///< uORB handle to write output to - bool lenient_still_detection); ///< true: Use more lenient still position detection +enum detect_orientation_return detect_orientation(orb_advert_t *mavlink_log_pub); /// Returns the human readable string representation of the orientation /// @param orientation Orientation to return string for, "error" if buffer is too small @@ -67,17 +66,15 @@ enum calibrate_return { calibrate_return_cancelled }; -typedef calibrate_return(*calibration_from_orientation_worker_t)(detect_orientation_return - orientation, ///< Orientation which was detected - void *worker_data); ///< Opaque worker data +typedef calibrate_return(*calibration_from_orientation_worker_t)(detect_orientation_return orientation, + void *worker_data); /// Perform calibration sequence which require a rest orientation detection prior to calibration. /// @return OK: Calibration succeeded, ERROR: Calibration failed calibrate_return calibrate_from_orientation(orb_advert_t *mavlink_log_pub, ///< uORB handle to write output to bool side_data_collected[detect_orientation_side_count], ///< Sides for which data still needs calibration calibration_from_orientation_worker_t calibration_worker, ///< Worker routine which performs the actual calibration - void *worker_data, ///< Opaque data passed to worker routine - bool lenient_still_detection); ///< true: Use more lenient still position detection + void *worker_data); ///< Opaque data passed to worker routine /// Used to periodically check for a cancel command bool calibrate_cancel_check(orb_advert_t *mavlink_log_pub, const hrt_abstime &calibration_started); diff --git a/src/modules/commander/lm_fit.cpp b/src/modules/commander/lm_fit.cpp index 39eb571a79..f8a4ca3a13 100644 --- a/src/modules/commander/lm_fit.cpp +++ b/src/modules/commander/lm_fit.cpp @@ -41,43 +41,43 @@ struct iteration_result { } result = STATUS::SUCCESS; }; -void lm_sphere_fit_iteration(const float x[], const float y[], const float z[], - unsigned int samples_collected, sphere_params ¶ms, iteration_result &result) +static void lm_sphere_fit_iteration(const matrix::Vector3f data[], unsigned samples_collected, sphere_params &p, + iteration_result &result) { // Run Sphere Fit using Levenberg Marquardt LSq Fit const float lma_damping = 10.f; - float fitness = result.cost; float fit1 = 0.f; float fit2 = 0.f; matrix::SquareMatrix JTJ; float JTFI[4] {}; - float residual = 0.0f; // Gauss Newton Part common for all kind of extensions including LM - for (uint16_t k = 0; k < samples_collected; k++) { + for (unsigned k = 0; k < samples_collected; k++) { + float x = data[k](0); + float y = data[k](1); + float z = data[k](2); + + // Calculate Jacobian + float A = (p.diag(0) * (x - p.offset(0))) + (p.offdiag(0) * (y - p.offset(1))) + (p.offdiag(1) * (z - p.offset(2))); + float B = (p.offdiag(0) * (x - p.offset(0))) + (p.diag(1) * (y - p.offset(1))) + (p.offdiag(2) * (z - p.offset(2))); + float C = (p.offdiag(1) * (x - p.offset(0))) + (p.offdiag(2) * (y - p.offset(1))) + (p.diag(2) * (z - p.offset(2))); - float sphere_jacob[4]; - //Calculate Jacobian - float A = (params.diag(0) * (x[k] - params.offset(0))) + (params.offdiag(0) * (y[k] - params.offset(1))) + - (params.offdiag(1) * (z[k] - params.offset(2))); - float B = (params.offdiag(0) * (x[k] - params.offset(0))) + (params.diag(1) * (y[k] - params.offset(1))) + - (params.offdiag(2) * (z[k] - params.offset(2))); - float C = (params.offdiag(1) * (x[k] - params.offset(0))) + (params.offdiag(2) * (y[k] - params.offset(1))) + - (params.diag(2) * (z[k] - params.offset(2))); float length = sqrtf(A * A + B * B + C * C); + float sphere_jacob[4]; // 0: partial derivative (radius wrt fitness fn) fn operated on sample - sphere_jacob[0] = 1.0f; + sphere_jacob[0] = 1.f; // 1-3: partial derivative (offsets wrt fitness fn) fn operated on sample - sphere_jacob[1] = 1.0f * (((params.diag(0) * A) + (params.offdiag(0) * B) + (params.offdiag(1) * C)) / length); - sphere_jacob[2] = 1.0f * (((params.offdiag(0) * A) + (params.diag(1) * B) + (params.offdiag(2) * C)) / length); - sphere_jacob[3] = 1.0f * (((params.offdiag(1) * A) + (params.offdiag(2) * B) + (params.diag(2) * C)) / length); - residual = params.radius - length; + sphere_jacob[1] = 1.f * (((p.diag(0) * A) + (p.offdiag(0) * B) + (p.offdiag(1) * C)) / length); + sphere_jacob[2] = 1.f * (((p.offdiag(0) * A) + (p.diag(1) * B) + (p.offdiag(2) * C)) / length); + sphere_jacob[3] = 1.f * (((p.offdiag(1) * A) + (p.offdiag(2) * B) + (p.diag(2) * C)) / length); - for (uint8_t i = 0; i < 4; i++) { + float residual = p.radius - length; + + for (int i = 0; i < 4; i++) { // compute JTJ - for (uint8_t j = 0; j < 4; j++) { + for (int j = 0; j < 4; j++) { JTJ(i, j) += sphere_jacob[i] * sphere_jacob[j]; } @@ -87,12 +87,12 @@ void lm_sphere_fit_iteration(const float x[], const float y[], const float z[], //------------------------Levenberg-Marquardt-part-starts-here---------------------------------// - // refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter - float fit1_params[4] = {params.radius, params.offset(0), params.offset(1), params.offset(2)}; - float fit2_params[4]; - memcpy(fit2_params, fit1_params, sizeof(fit1_params)); + // refer: https://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter + float fit1_p[4] {p.radius, p.offset(0), p.offset(1), p.offset(2)}; + float fit2_p[4] {p.radius, p.offset(0), p.offset(1), p.offset(2)}; + JTJ = (JTJ + JTJ.transpose()) * 0.5f; // fix numerical issues - matrix::SquareMatrix JTJ2 = JTJ; + matrix::SquareMatrix JTJ2{JTJ}; for (uint8_t i = 0; i < 4; i++) { JTJ(i, i) += result.gradient_damping; @@ -107,42 +107,39 @@ void lm_sphere_fit_iteration(const float x[], const float y[], const float z[], if (!JTJ2.I(JTJ2)) { result.result = iteration_result::STATUS::FAILURE; return; - } - for (uint8_t row = 0; row < 4; row++) { - for (uint8_t col = 0; col < 4; col++) { - fit1_params[row] -= JTFI[col] * JTJ(row, col); - fit2_params[row] -= JTFI[col] * JTJ2(row, col); + for (int row = 0; row < 4; row++) { + for (int col = 0; col < 4; col++) { + fit1_p[row] -= JTFI[col] * JTJ(row, col); + fit2_p[row] -= JTFI[col] * JTJ2(row, col); } } // Calculate mean squared residuals - for (uint16_t k = 0; k < samples_collected; k++) { - float A = (params.diag(0) * (x[k] - fit1_params[1])) + (params.offdiag(0) * (y[k] - fit1_params[2])) + - (params.offdiag(1) * - (z[k] + fit1_params[3])); - float B = (params.offdiag(0) * (x[k] - fit1_params[1])) + (params.diag(1) * (y[k] - fit1_params[2])) + - (params.offdiag(2) * - (z[k] + fit1_params[3])); - float C = (params.offdiag(1) * (x[k] - fit1_params[1])) + (params.offdiag(2) * (y[k] - fit1_params[2])) + (params.diag( - 2) * - (z[k] - fit1_params[3])); + for (unsigned k = 0; k < samples_collected; k++) { + float x = data[k](0); + float y = data[k](1); + float z = data[k](2); + + float A = (p.diag(0) * (x - fit1_p[1])) + (p.offdiag(0) * (y - fit1_p[2])) + (p.offdiag(1) * (z + fit1_p[3])); + float B = (p.offdiag(0) * (x - fit1_p[1])) + (p.diag(1) * (y - fit1_p[2])) + (p.offdiag(2) * (z + fit1_p[3])); + float C = (p.offdiag(1) * (x - fit1_p[1])) + (p.offdiag(2) * (y - fit1_p[2])) + (p.diag(2) * (z - fit1_p[3])); + float length = sqrtf(A * A + B * B + C * C); - residual = fit1_params[0] - length; + float residual = fit1_p[0] - length; fit1 += residual * residual; - A = (params.diag(0) * (x[k] - fit2_params[1])) + (params.offdiag(0) * (y[k] - fit2_params[2])) + (params.offdiag(1) * - (z[k] - fit2_params[3])); - B = (params.offdiag(0) * (x[k] - fit2_params[1])) + (params.diag(1) * (y[k] - fit2_params[2])) + (params.offdiag(2) * - (z[k] - fit2_params[3])); - C = (params.offdiag(1) * (x[k] - fit2_params[1])) + (params.offdiag(2) * (y[k] - fit2_params[2])) + (params.diag(2) * - (z[k] - fit2_params[3])); + A = (p.diag(0) * (x - fit2_p[1])) + (p.offdiag(0) * (y - fit2_p[2])) + (p.offdiag(1) * (z - fit2_p[3])); + B = (p.offdiag(0) * (x - fit2_p[1])) + (p.diag(1) * (y - fit2_p[2])) + (p.offdiag(2) * (z - fit2_p[3])); + C = (p.offdiag(1) * (x - fit2_p[1])) + (p.offdiag(2) * (y - fit2_p[2])) + (p.diag(2) * (z - fit2_p[3])); length = sqrtf(A * A + B * B + C * C); - residual = fit2_params[0] - length; + residual = fit2_p[0] - length; fit2 += residual * residual; } + float fitness = result.cost; + fit1 = sqrtf(fit1) / samples_collected; fit2 = sqrtf(fit2) / samples_collected; @@ -151,7 +148,7 @@ void lm_sphere_fit_iteration(const float x[], const float y[], const float z[], } else if (fit2 < result.cost && fit2 < fit1) { result.gradient_damping /= lma_damping; - memcpy(fit1_params, fit2_params, sizeof(fit1_params)); + memcpy(fit1_p, fit2_p, sizeof(fit1_p)); fitness = fit2; } else if (fit1 < result.cost) { @@ -159,13 +156,15 @@ void lm_sphere_fit_iteration(const float x[], const float y[], const float z[], } //--------------------Levenberg-Marquardt-part-ends-here--------------------------------// - if (PX4_ISFINITE(fitness) && fitness <= result.cost) { result.cost = fitness; - params.radius = fit1_params[0]; - params.offset(0) = fit1_params[1]; - params.offset(1) = fit1_params[2]; - params.offset(2) = fit1_params[3]; + + p.radius = fit1_p[0]; + + p.offset(0) = fit1_p[1]; + p.offset(1) = fit1_p[2]; + p.offset(2) = fit1_p[3]; + result.result = iteration_result::STATUS::SUCCESS; } else { @@ -173,49 +172,49 @@ void lm_sphere_fit_iteration(const float x[], const float y[], const float z[], } } -void lm_ellipsoid_fit_iteration(const float x[], const float y[], const float z[], - unsigned int samples_collected, sphere_params ¶ms, iteration_result &result) +static void lm_ellipsoid_fit_iteration(const matrix::Vector3f data[], unsigned samples_collected, sphere_params &p, + iteration_result &result) { // Run Sphere Fit using Levenberg Marquardt LSq Fit - const float lma_damping = 10.0f; - float fitness = result.cost; - float fit1 = 0.0f; - float fit2 = 0.0f; + const float lma_damping = 10.f; - matrix::SquareMatrix JTJ; + float fit1 = 0.f; + float fit2 = 0.f; + + matrix::SquareMatrix JTJ{}; float JTFI[9] {}; - float residual = 0.0f; - float ellipsoid_jacob[9]; // Gauss Newton Part common for all kind of extensions including LM - for (uint16_t k = 0; k < samples_collected; k++) { + for (unsigned k = 0; k < samples_collected; k++) { + float x = data[k](0); + float y = data[k](1); + float z = data[k](2); // Calculate Jacobian - float A = (params.diag(0) * (x[k] - params.offset(0))) + (params.offdiag(0) * (y[k] - params.offset(1))) + - (params.offdiag(1) * (z[k] - params.offset(2))); - float B = (params.offdiag(0) * (x[k] - params.offset(0))) + (params.diag(1) * (y[k] - params.offset(1))) + - (params.offdiag(2) * (z[k] - params.offset(2))); - float C = (params.offdiag(1) * (x[k] - params.offset(0))) + (params.offdiag(2) * (y[k] - params.offset(1))) + - (params.diag(2) * (z[k] - params.offset(2))); + float A = (p.diag(0) * (x - p.offset(0))) + (p.offdiag(0) * (y - p.offset(1))) + (p.offdiag(1) * (z - p.offset(2))); + float B = (p.offdiag(0) * (x - p.offset(0))) + (p.diag(1) * (y - p.offset(1))) + (p.offdiag(2) * (z - p.offset(2))); + float C = (p.offdiag(1) * (x - p.offset(0))) + (p.offdiag(2) * (y - p.offset(1))) + (p.diag(2) * (z - p.offset(2))); float length = sqrtf(A * A + B * B + C * C); - residual = params.radius - length; + float residual = p.radius - length; fit1 += residual * residual; - // 0-2: partial derivative (offset wrt fitness fn) fn operated on sample - ellipsoid_jacob[0] = 1.0f * (((params.diag(0) * A) + (params.offdiag(0) * B) + (params.offdiag(1) * C)) / length); - ellipsoid_jacob[1] = 1.0f * (((params.offdiag(0) * A) + (params.diag(1) * B) + (params.offdiag(2) * C)) / length); - ellipsoid_jacob[2] = 1.0f * (((params.offdiag(1) * A) + (params.offdiag(2) * B) + (params.diag(2) * C)) / length); - // 3-5: partial derivative (diag offset wrt fitness fn) fn operated on sample - ellipsoid_jacob[3] = -1.0f * ((x[k] - params.offset(0)) * A) / length; - ellipsoid_jacob[4] = -1.0f * ((y[k] - params.offset(1)) * B) / length; - ellipsoid_jacob[5] = -1.0f * ((z[k] - params.offset(2)) * C) / length; - // 6-8: partial derivative (off-diag offset wrt fitness fn) fn operated on sample - ellipsoid_jacob[6] = -1.0f * (((y[k] - params.offset(1)) * A) + ((x[k] - params.offset(0)) * B)) / length; - ellipsoid_jacob[7] = -1.0f * (((z[k] - params.offset(2)) * A) + ((x[k] - params.offset(0)) * C)) / length; - ellipsoid_jacob[8] = -1.0f * (((z[k] - params.offset(2)) * B) + ((y[k] - params.offset(1)) * C)) / length; - for (uint8_t i = 0; i < 9; i++) { + float ellipsoid_jacob[9]; + // 0-2: partial derivative (offset wrt fitness fn) fn operated on sample + ellipsoid_jacob[0] = 1.f * (((p.diag(0) * A) + (p.offdiag(0) * B) + (p.offdiag(1) * C)) / length); + ellipsoid_jacob[1] = 1.f * (((p.offdiag(0) * A) + (p.diag(1) * B) + (p.offdiag(2) * C)) / length); + ellipsoid_jacob[2] = 1.f * (((p.offdiag(1) * A) + (p.offdiag(2) * B) + (p.diag(2) * C)) / length); + // 3-5: partial derivative (diag offset wrt fitness fn) fn operated on sample + ellipsoid_jacob[3] = -1.f * ((x - p.offset(0)) * A) / length; + ellipsoid_jacob[4] = -1.f * ((y - p.offset(1)) * B) / length; + ellipsoid_jacob[5] = -1.f * ((z - p.offset(2)) * C) / length; + // 6-8: partial derivative (off-diag offset wrt fitness fn) fn operated on sample + ellipsoid_jacob[6] = -1.f * (((y - p.offset(1)) * A) + ((x - p.offset(0)) * B)) / length; + ellipsoid_jacob[7] = -1.f * (((z - p.offset(2)) * A) + ((x - p.offset(0)) * C)) / length; + ellipsoid_jacob[8] = -1.f * (((z - p.offset(2)) * B) + ((y - p.offset(1)) * C)) / length; + + for (int i = 0; i < 9; i++) { // compute JTJ - for (uint8_t j = 0; j < 9; j++) { + for (int j = 0; j < 9; j++) { JTJ(i, j) += ellipsoid_jacob[i] * ellipsoid_jacob[j]; } @@ -226,12 +225,13 @@ void lm_ellipsoid_fit_iteration(const float x[], const float y[], const float z[ //------------------------Levenberg-Marquardt-part-starts-here---------------------------------// // refer: http://en.wikipedia.org/wiki/Levenberg%E2%80%93Marquardt_algorithm#Choice_of_damping_parameter - float fit1_params[9] = {params.offset(0), params.offset(1), params.offset(2), params.diag(0), params.diag(1), params.diag(2), params.offdiag(0), params.offdiag(1), params.offdiag(2)}; - float fit2_params[9]; - memcpy(fit2_params, fit1_params, sizeof(fit1_params)); - matrix::SquareMatrix JTJ2 = JTJ; + float fit1_p[9] {p.offset(0), p.offset(1), p.offset(2), p.diag(0), p.diag(1), p.diag(2), p.offdiag(0), p.offdiag(1), p.offdiag(2)}; + float fit2_p[9]; + memcpy(fit2_p, fit1_p, sizeof(fit1_p)); - for (uint8_t i = 0; i < 9; i++) { + matrix::SquareMatrix JTJ2{JTJ}; + + for (int i = 0; i < 9; i++) { JTJ(i, i) += result.gradient_damping; JTJ2(i, i) += result.gradient_damping / lma_damping; } @@ -247,36 +247,36 @@ void lm_ellipsoid_fit_iteration(const float x[], const float y[], const float z[ return; } - for (uint8_t row = 0; row < 9; row++) { - for (uint8_t col = 0; col < 9; col++) { - fit1_params[row] -= JTFI[col] * JTJ(row, col); - fit2_params[row] -= JTFI[col] * JTJ2(row, col); + for (int row = 0; row < 9; row++) { + for (int col = 0; col < 9; col++) { + fit1_p[row] -= JTFI[col] * JTJ(row, col); + fit2_p[row] -= JTFI[col] * JTJ2(row, col); } } // Calculate mean squared residuals - for (uint16_t k = 0; k < samples_collected; k++) { - float A = (fit1_params[3] * (x[k] - fit1_params[0])) + (fit1_params[6] * (y[k] - fit1_params[1])) + (fit1_params[7] * - (z[k] - fit1_params[2])); - float B = (fit1_params[6] * (x[k] - fit1_params[0])) + (fit1_params[4] * (y[k] - fit1_params[1])) + (fit1_params[8] * - (z[k] - fit1_params[2])); - float C = (fit1_params[7] * (x[k] - fit1_params[0])) + (fit1_params[8] * (y[k] - fit1_params[1])) + (fit1_params[5] * - (z[k] - fit1_params[2])); + for (unsigned k = 0; k < samples_collected; k++) { + float x = data[k](0); + float y = data[k](1); + float z = data[k](2); + + float A = (fit1_p[3] * (x - fit1_p[0])) + (fit1_p[6] * (y - fit1_p[1])) + (fit1_p[7] * (z - fit1_p[2])); + float B = (fit1_p[6] * (x - fit1_p[0])) + (fit1_p[4] * (y - fit1_p[1])) + (fit1_p[8] * (z - fit1_p[2])); + float C = (fit1_p[7] * (x - fit1_p[0])) + (fit1_p[8] * (y - fit1_p[1])) + (fit1_p[5] * (z - fit1_p[2])); float length = sqrtf(A * A + B * B + C * C); - residual = params.radius - length; + float residual = p.radius - length; fit1 += residual * residual; - A = (fit2_params[3] * (x[k] - fit2_params[0])) + (fit2_params[6] * (y[k] - fit2_params[1])) + (fit2_params[7] * - (z[k] - fit2_params[2])); - B = (fit2_params[6] * (x[k] - fit2_params[0])) + (fit2_params[4] * (y[k] - fit2_params[1])) + (fit2_params[8] * - (z[k] - fit2_params[2])); - C = (fit2_params[7] * (x[k] - fit2_params[0])) + (fit2_params[8] * (y[k] - fit2_params[1])) + (fit2_params[5] * - (z[k] - fit2_params[2])); + A = (fit2_p[3] * (x - fit2_p[0])) + (fit2_p[6] * (y - fit2_p[1])) + (fit2_p[7] * (z - fit2_p[2])); + B = (fit2_p[6] * (x - fit2_p[0])) + (fit2_p[4] * (y - fit2_p[1])) + (fit2_p[8] * (z - fit2_p[2])); + C = (fit2_p[7] * (x - fit2_p[0])) + (fit2_p[8] * (y - fit2_p[1])) + (fit2_p[5] * (z - fit2_p[2])); length = sqrtf(A * A + B * B + C * C); - residual = params.radius - length; + residual = p.radius - length; fit2 += residual * residual; } + float fitness = result.cost; + fit1 = sqrtf(fit1) / samples_collected; fit2 = sqrtf(fit2) / samples_collected; @@ -285,7 +285,7 @@ void lm_ellipsoid_fit_iteration(const float x[], const float y[], const float z[ } else if (fit2 < result.cost && fit2 < fit1) { result.gradient_damping /= lma_damping; - memcpy(fit1_params, fit2_params, sizeof(fit1_params)); + memcpy(fit1_p, fit2_p, sizeof(fit1_p)); fitness = fit2; } else if (fit1 < result.cost) { @@ -295,15 +295,17 @@ void lm_ellipsoid_fit_iteration(const float x[], const float y[], const float z[ //--------------------Levenberg-Marquardt-part-ends-here--------------------------------// if (PX4_ISFINITE(fitness) && fitness <= result.cost) { result.cost = fitness; - params.offset(0) = fit1_params[0]; - params.offset(1) = fit1_params[1]; - params.offset(2) = fit1_params[2]; - params.diag(0) = fit1_params[3]; - params.diag(1) = fit1_params[4]; - params.diag(2) = fit1_params[5]; - params.offdiag(0) = fit1_params[6]; - params.offdiag(1) = fit1_params[7]; - params.offdiag(2) = fit1_params[8]; + + p.offset(0) = fit1_p[0]; + p.offset(1) = fit1_p[1]; + p.offset(2) = fit1_p[2]; + p.diag(0) = fit1_p[3]; + p.diag(1) = fit1_p[4]; + p.diag(2) = fit1_p[5]; + p.offdiag(0) = fit1_p[6]; + p.offdiag(1) = fit1_p[7]; + p.offdiag(2) = fit1_p[8]; + result.result = iteration_result::STATUS::SUCCESS; } else { @@ -311,19 +313,13 @@ void lm_ellipsoid_fit_iteration(const float x[], const float y[], const float z[ } } - - -int lm_mag_fit(const float x[], const float y[], const float z[], unsigned int samples_collected, sphere_params ¶ms, - bool full_ellipsoid) +int lm_fit(const matrix::Vector3f data[], unsigned samples_collected, sphere_params ¶ms, bool full_ellipsoid) { + static constexpr int min_iterations = 10; + static constexpr int max_iterations = 100; - const int max_iterations = 100; - const int min_iterations = 10; - const float cost_threshold = 0.01; - const float step_threshold = 0.001; - - const float min_radius = 0.2; - const float max_radius = 0.7; + static constexpr float cost_threshold = 0.02f; + static constexpr float step_threshold = 0.001f; iteration_result iter; iter.cost = 1e30f; @@ -333,15 +329,27 @@ int lm_mag_fit(const float x[], const float y[], const float z[], unsigned int s for (int i = 0; i < max_iterations; i++) { if (full_ellipsoid) { - lm_ellipsoid_fit_iteration(x, y, z, samples_collected, params, iter); + lm_ellipsoid_fit_iteration(data, samples_collected, params, iter); + + PX4_INFO("[%d] O:[%.5f, %.5f, %.5f], S:[%.3f, %.3f, %.3f], Result: %d, Cost: %.6f, Damping: %.6f\n", + i, + (double)params.offset(0), (double)params.offset(1), (double)params.offset(2), + (double)params.diag(0), (double)params.diag(1), (double)params.diag(2), + iter.result == iteration_result::STATUS::SUCCESS, (double)iter.cost, (double)iter.gradient_damping); } else { - lm_sphere_fit_iteration(x, y, z, samples_collected, params, iter); + lm_sphere_fit_iteration(data, samples_collected, params, iter); + + PX4_INFO("[%d] O:[%.5f, %.5f, %.5f], Result: %d, Cost: %.6f, Damping: %.6f, Radius: %.4f\n", + i, + (double)params.offset(0), (double)params.offset(1), (double)params.offset(2), + iter.result == iteration_result::STATUS::SUCCESS, (double)iter.cost, (double)iter.gradient_damping, + (double)params.radius); } if (iter.result == iteration_result::STATUS::SUCCESS - && min_radius < params.radius && params.radius < max_radius && i > min_iterations && (iter.cost < cost_threshold || iter.gradient_damping < step_threshold)) { + success = true; break; } diff --git a/src/modules/commander/lm_fit.hpp b/src/modules/commander/lm_fit.hpp index 52fa674d02..c3059e24f5 100644 --- a/src/modules/commander/lm_fit.hpp +++ b/src/modules/commander/lm_fit.hpp @@ -37,7 +37,9 @@ #include struct sphere_params { - matrix::Vector3f offset, diag{1.f, 1.f, 1.f}, offdiag; + matrix::Vector3f offset{0.f, 0.f, 0.f}; + matrix::Vector3f diag{1.f, 1.f, 1.f}; + matrix::Vector3f offdiag{0.f, 0.f, 0.f}; float radius{0.2f}; }; @@ -47,9 +49,7 @@ struct sphere_params { * * Fits a sphere to a set of points on the sphere surface. * - * @param x point coordinates on the X axis - * @param y point coordinates on the Y axis - * @param z point coordinates on the Z axis + * @param data x,y,z point coordinates * @param samples_collected number of points * @param max_iterations abort if maximum number of iterations have been reached. If unsure, set to 100. * @param params the values to be optimized @@ -59,5 +59,4 @@ struct sphere_params { * * @return 0 on success, 1 on failure */ -int lm_mag_fit(const float x[], const float y[], const float z[], unsigned int samples_collected, sphere_params ¶ms, - bool full_ellipsoid); +int lm_fit(const matrix::Vector3f data[], unsigned samples_collected, sphere_params ¶ms, bool full_ellipsoid); diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp index 6900be8c04..3231f25f0f 100644 --- a/src/modules/commander/mag_calibration.cpp +++ b/src/modules/commander/mag_calibration.cpp @@ -90,9 +90,7 @@ struct mag_worker_data_t { uint64_t calibration_interval_perside_us; unsigned int calibration_counter_total[MAX_MAGS]; - float *x[MAX_MAGS]; - float *y[MAX_MAGS]; - float *z[MAX_MAGS]; + matrix::Vector3f *data[MAX_MAGS] {nullptr, nullptr, nullptr, nullptr}; float temperature[MAX_MAGS] {NAN, NAN, NAN, NAN}; @@ -141,19 +139,20 @@ int do_mag_calibration(orb_advert_t *mavlink_log_pub) return result; } -static bool reject_sample(float sx, float sy, float sz, float x[], float y[], float z[], unsigned count, +static bool reject_sample(Vector3f s, Vector3f data[], unsigned count, unsigned max_count, float mag_sphere_radius) { float min_sample_dist = fabsf(5.4f * mag_sphere_radius / sqrtf(max_count)) / 3.0f; for (size_t i = 0; i < count; i++) { - float dx = sx - x[i]; - float dy = sy - y[i]; - float dz = sz - z[i]; + float dx = s(0) - data[i](0); + float dy = s(1) - data[i](1); + float dz = s(2) - data[i](2); float dist = sqrtf(dx * dx + dy * dy + dz * dz); if (dist < min_sample_dist) { - PX4_DEBUG("rejected X: %.3f Y: %.3f Z: %.3f (%.3f < %.3f) (%u/%u) ", (double)sx, (double)sy, (double)sz, (double)dist, + PX4_DEBUG("rejected X: %.3f Y: %.3f Z: %.3f (%.3f < %.3f) (%u/%u) ", (double)s(0), (double)s(1), (double)s(2), + (double)dist, (double)min_sample_dist, count, max_count); return true; @@ -363,8 +362,7 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta } // Check if this measurement is good to go in - bool reject = reject_sample(mag.x, mag.y, mag.z, - worker_data->x[cur_mag], worker_data->y[cur_mag], worker_data->z[cur_mag], + bool reject = reject_sample(Vector3f{mag.x, mag.y, mag.z}, worker_data->data[cur_mag], worker_data->calibration_counter_total[cur_mag], worker_data->calibration_sides * worker_data->calibration_points_perside, mag_sphere_radius); @@ -388,9 +386,9 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta if (!rejected) { for (uint8_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { if (worker_data->calibration[cur_mag].device_id() != 0) { - worker_data->x[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](0); - worker_data->y[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](1); - worker_data->z[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag](2); + + worker_data->data[cur_mag][worker_data->calibration_counter_total[cur_mag]] = new_samples[cur_mag]; + worker_data->calibration_counter_total[cur_mag]++; if (!PX4_ISFINITE(worker_data->temperature[cur_mag])) { // set first valid value @@ -399,13 +397,11 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta } else { worker_data->temperature[cur_mag] = 0.5f * (worker_data->temperature[cur_mag] + new_temperature[cur_mag]); } - - worker_data->calibration_counter_total[cur_mag]++; } } hrt_abstime now = hrt_absolute_time(); - mag_worker_data_s status; + mag_worker_data_s status{}; status.timestamp = now; status.timestamp_sample = now; status.done_count = worker_data->done_count; @@ -418,9 +414,9 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta if (worker_data->calibration[cur_mag].device_id() != 0) { const unsigned int sample = worker_data->calibration_counter_total[cur_mag] - 1; - status.x[cur_mag] = worker_data->x[cur_mag][sample]; - status.y[cur_mag] = worker_data->y[cur_mag][sample]; - status.z[cur_mag] = worker_data->z[cur_mag][sample]; + status.x[cur_mag] = worker_data->data[cur_mag][sample](0); + status.y[cur_mag] = worker_data->data[cur_mag][sample](1); + status.z[cur_mag] = worker_data->data[cur_mag][sample](2); } else { status.x[cur_mag] = 0.f; @@ -527,9 +523,6 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) { // Initialize to no memory allocated - worker_data.x[cur_mag] = nullptr; - worker_data.y[cur_mag] = nullptr; - worker_data.z[cur_mag] = nullptr; worker_data.calibration_counter_total[cur_mag] = 0; } @@ -547,11 +540,10 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma worker_data.calibration[cur_mag].set_calibration_index(cur_mag); if (worker_data.calibration[cur_mag].device_id() != 0) { - worker_data.x[cur_mag] = static_cast(malloc(sizeof(float) * calibration_points_maxcount)); - worker_data.y[cur_mag] = static_cast(malloc(sizeof(float) * calibration_points_maxcount)); - worker_data.z[cur_mag] = static_cast(malloc(sizeof(float) * calibration_points_maxcount)); - if (worker_data.x[cur_mag] == nullptr || worker_data.y[cur_mag] == nullptr || worker_data.z[cur_mag] == nullptr) { + worker_data.data[cur_mag] = new matrix::Vector3f[calibration_points_maxcount]; + + if (worker_data.data[cur_mag] == nullptr) { calibration_log_critical(mavlink_log_pub, "ERROR: out of memory"); result = calibrate_return_error; break; @@ -566,8 +558,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma result = calibrate_from_orientation(mavlink_log_pub, // uORB handle to write output 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 + &worker_data); // Opaque data for calibration worked } // calibration values for each mag @@ -594,26 +585,26 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma // Estimate only the offsets if two-sided calibration is selected, as the problem is not constrained // enough to reliably estimate both scales and offsets with 2 sides only (even if the existing calibration // is already close) - bool sphere_fit_only = worker_data.calibration_sides <= 2; + const bool sphere_fit_only = worker_data.calibration_sides <= 2; - sphere_params sphere_data; + sphere_params sphere_data{}; sphere_data.radius = sphere_radius[cur_mag]; - sphere_data.offset = matrix::Vector3f(sphere[cur_mag](0), sphere[cur_mag](1), sphere[cur_mag](2)); - sphere_data.diag = matrix::Vector3f(diag[cur_mag](0), diag[cur_mag](1), diag[cur_mag](2)); - sphere_data.offdiag = matrix::Vector3f(offdiag[cur_mag](0), offdiag[cur_mag](1), offdiag[cur_mag](2)); + sphere_data.offset = sphere[cur_mag]; + sphere_data.diag = diag[cur_mag]; + sphere_data.offdiag = offdiag[cur_mag]; bool sphere_fit_success = false; bool ellipsoid_fit_success = false; - int ret = lm_mag_fit(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag], - worker_data.calibration_counter_total[cur_mag], sphere_data, false); + + int ret = lm_fit(worker_data.data[cur_mag], worker_data.calibration_counter_total[cur_mag], sphere_data, false); if (ret == PX4_OK) { sphere_fit_success = true; PX4_INFO("Mag: %" PRIu8 " sphere radius: %.4f", cur_mag, (double)sphere_data.radius); if (!sphere_fit_only) { - int ellipsoid_ret = lm_mag_fit(worker_data.x[cur_mag], worker_data.y[cur_mag], worker_data.z[cur_mag], - worker_data.calibration_counter_total[cur_mag], sphere_data, true); + int ellipsoid_ret = lm_fit(worker_data.data[cur_mag], worker_data.calibration_counter_total[cur_mag], sphere_data, + true); if (ellipsoid_ret == PX4_OK) { ellipsoid_fit_success = true; @@ -621,13 +612,14 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma } } + + // TODO: verify sphere radius + sphere_radius[cur_mag] = sphere_data.radius; - for (int i = 0; i < 3; i++) { - sphere[cur_mag](i) = sphere_data.offset(i); - diag[cur_mag](i) = sphere_data.diag(i); - offdiag[cur_mag](i) = sphere_data.offdiag(i); - } + sphere[cur_mag] = sphere_data.offset; + diag[cur_mag] = sphere_data.diag; + offdiag[cur_mag] = sphere_data.offdiag; if (!sphere_fit_success && !ellipsoid_fit_success) { calibration_log_emergency(mavlink_log_pub, "Retry calibration (unable to fit mag %" PRIu8 ")", cur_mag); @@ -728,7 +720,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma for (unsigned i = 0; i < worker_data.calibration_counter_total[cur_mag]; i++) { - const Vector3f sample{worker_data.x[cur_mag][i], worker_data.y[cur_mag][i], worker_data.z[cur_mag][i]}; + const Vector3f sample{worker_data.data[cur_mag][i]}; // apply calibration Vector3f m{scale *(sample - offset)}; @@ -739,9 +731,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma } // store back in worker_data - worker_data.x[cur_mag][i] = m(0); - worker_data.y[cur_mag][i] = m(1); - worker_data.z[cur_mag][i] = m(2); + worker_data.data[cur_mag][i] = m; } } } @@ -775,14 +765,8 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma float diff_sum = 0.f; for (int i = 0; i < last_sample_index; i++) { - float x = worker_data.x[cur_mag][i]; - float y = worker_data.y[cur_mag][i]; - float z = worker_data.z[cur_mag][i]; - rotate_3f((enum Rotation)r, x, y, z); - - Vector3f diff = Vector3f{x, y, z} - Vector3f{worker_data.x[internal_index][i], worker_data.y[internal_index][i], worker_data.z[internal_index][i]}; - - diff_sum += diff.norm_squared(); + const Vector3f &m{worker_data.data[cur_mag][i]}; + diff_sum += Vector3f((get_rot_matrix((enum Rotation)r) * m) - m).norm_squared(); } // compute mean squared error @@ -878,9 +862,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma // 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]); + delete[] worker_data.data[cur_mag]; } FactoryCalibrationStorage factory_storage; diff --git a/src/modules/commander/mag_calibration_test.cpp b/src/modules/commander/mag_calibration_test.cpp index 3343a28cd3..92d9d37647 100644 --- a/src/modules/commander/mag_calibration_test.cpp +++ b/src/modules/commander/mag_calibration_test.cpp @@ -50,38 +50,38 @@ using matrix::Vector3f; class MagCalTest : public ::testing::Test { public: - void generate2SidesMagData(float *x, float *y, float *z, unsigned int n_samples, float mag_str); + void generate2SidesMagData(matrix::Vector3f mag_data[], unsigned int n_samples, float mag_str); /* Generate regularly spaced data on a sphere * Ref.: How to generate equidistributed points on the surface of a sphere, Markus Deserno, 2004 */ - void generateRegularData(float *x, float *y, float *z, unsigned int n_samples, float mag_str); + void generateRegularData(matrix::Vector3f mag_data[], unsigned int n_samples, float mag_str); - void modifyOffsetScale(float *x, float *y, float *z, unsigned int n_samples, Vector3f offsets, Vector3f scale_factors); + void modifyOffsetScale(matrix::Vector3f mag_data[], unsigned int n_samples, Vector3f offsets, Vector3f scale_factors); }; -void MagCalTest::generate2SidesMagData(float *x, float *y, float *z, unsigned int n_samples, float mag_str) +void MagCalTest::generate2SidesMagData(matrix::Vector3f data[], unsigned int n_samples, float mag_str) { float psi = 0.f; float theta = 0.f; const float d_angle = 2.f * M_PI_F / float(n_samples / 2); for (int i = 0; i < int(n_samples / 2); i++) { - x[i] = mag_str * sinf(psi); - y[i] = mag_str * cosf(psi); - z[i] = 0.f; + data[i](0) = mag_str * sinf(psi); + data[i](1) = mag_str * cosf(psi); + data[i](2) = 0.f; psi += d_angle; } for (int i = int(n_samples / 2); i < int(n_samples); i++) { - x[i] = mag_str * sinf(theta); - y[i] = 0.f; - z[i] = mag_str * cosf(theta); + data[i](0) = mag_str * sinf(theta); + data[i](1) = 0.f; + data[i](2) = mag_str * cosf(theta); theta += d_angle; } } -void MagCalTest::generateRegularData(float *x, float *y, float *z, unsigned int n_samples, float mag_str) +void MagCalTest::generateRegularData(matrix::Vector3f mag_data[], unsigned int n_samples, float mag_str) { const float a = 4.f * M_PI_F * mag_str * mag_str / n_samples; const float d = sqrtf(a); @@ -97,9 +97,9 @@ void MagCalTest::generateRegularData(float *x, float *y, float *z, unsigned int for (int n = 0; n < m_phi; n++) { const float phi = 2.f * M_PI_F * n / static_cast(m_phi); - x[n_count] = mag_str * sinf(theta) * cosf(phi); - y[n_count] = mag_str * sinf(theta) * sinf(phi); - z[n_count] = mag_str * cosf(theta); + mag_data[n_count](0) = mag_str * sinf(theta) * cosf(phi); + mag_data[n_count](1) = mag_str * sinf(theta) * sinf(phi); + mag_data[n_count](2) = mag_str * cosf(theta); n_count++; } } @@ -111,20 +111,16 @@ void MagCalTest::generateRegularData(float *x, float *y, float *z, unsigned int // Padd with constant data while (n_count < n_samples) { - x[n_count] = x[n_count - 1]; - y[n_count] = y[n_count - 1]; - z[n_count] = z[n_count - 1]; + mag_data[n_count] = mag_data[n_count - 1]; n_count++; } } -void MagCalTest::modifyOffsetScale(float *x, float *y, float *z, unsigned int n_samples, Vector3f offsets, +void MagCalTest::modifyOffsetScale(matrix::Vector3f mag_data[], unsigned int n_samples, Vector3f offsets, Vector3f scale_factors) { for (unsigned int k = 0; k < n_samples; k++) { - x[k] = x[k] * scale_factors(0) + offsets(0); - y[k] = y[k] * scale_factors(1) + offsets(1); - z[k] = z[k] * scale_factors(2) + offsets(2); + mag_data[k] = mag_data[k].emult(scale_factors) + offsets; } } @@ -138,17 +134,13 @@ TEST_F(MagCalTest, sphere2Sides) const Vector3f offset_true; const Vector3f scale_true = {1.f, 1.f, 1.f}; - float x[N_SAMPLES]; - float y[N_SAMPLES]; - float z[N_SAMPLES]; - - generate2SidesMagData(x, y, z, N_SAMPLES, mag_str_true); + Vector3f mag_data[N_SAMPLES]; + generate2SidesMagData(mag_data, N_SAMPLES, mag_str_true); // WHEN: fitting a sphere with the data and given a wrong initial radius - sphere_params sphere; - sphere.diag = {1.f, 1.f, 1.f}; + sphere_params sphere{}; sphere.radius = 0.2; - int success = lm_mag_fit(x, y, z, N_SAMPLES, sphere, false); + int success = lm_fit(mag_data, N_SAMPLES, sphere, false); // THEN: the algorithm should converge in a single step EXPECT_EQ(success, PX4_OK); @@ -171,17 +163,15 @@ TEST_F(MagCalTest, sphereRegularlySpaced) const Vector3f offset_true = {-1.07f, 0.35f, -0.78f}; const Vector3f scale_true = {1.f, 1.f, 1.f}; - float x[N_SAMPLES]; - float y[N_SAMPLES]; - float z[N_SAMPLES]; - generateRegularData(x, y, z, N_SAMPLES, mag_str_true); - modifyOffsetScale(x, y, z, N_SAMPLES, offset_true, scale_true); + Vector3f mag_data[N_SAMPLES]; + generateRegularData(mag_data, N_SAMPLES, mag_str_true); + modifyOffsetScale(mag_data, N_SAMPLES, offset_true, scale_true); // WHEN: fitting a sphere to the data - sphere_params sphere; + sphere_params sphere{}; sphere.diag = {1.f, 1.f, 1.f}; sphere.radius = 0.2; - int success = lm_mag_fit(x, y, z, N_SAMPLES, sphere, false); + int success = lm_fit(mag_data, N_SAMPLES, sphere, false); // THEN: the algorithm should converge in a few iterations and // find the correct parameters @@ -200,7 +190,7 @@ TEST_F(MagCalTest, replayTestData) // GIVEN: a real test dataset with large offsets // and where the two first iterations of the LM algorithm // produces a negative radius and a constant fitness value - constexpr unsigned int N_SAMPLES = 231; + constexpr unsigned N_SAMPLES = 231; const float mag_str_true = 0.4f; const Vector3f offset_true = {-0.18f, 0.05f, -0.58f}; @@ -209,7 +199,16 @@ TEST_F(MagCalTest, replayTestData) sphere_params sphere; sphere.diag = {1.f, 1.f, 1.f}; sphere.radius = 0.2; - int sphere_success = lm_mag_fit(mag_data1_x, mag_data1_y, mag_data1_z, N_SAMPLES, sphere, false); + + matrix::Vector3f mag_data1[231]; + + for (unsigned i = 0; i < N_SAMPLES; i++) { + mag_data1[i](0) = mag_data1_x[i]; + mag_data1[i](1) = mag_data1_y[i]; + mag_data1[i](2) = mag_data1_z[i]; + } + + int sphere_success = lm_fit(mag_data1, N_SAMPLES, sphere, false); // THEN: the algorithm should converge and find the correct parameters EXPECT_EQ(sphere_success, PX4_OK); @@ -222,8 +221,8 @@ TEST_F(MagCalTest, replayTestData) sphere_params ellipsoid; ellipsoid.diag = {1.f, 1.f, 1.f}; ellipsoid.radius = 0.2; - int ellipsoid_step_1_success = lm_mag_fit(mag_data1_x, mag_data1_y, mag_data1_z, N_SAMPLES, ellipsoid, false); - int ellipsoid_success = lm_mag_fit(mag_data1_x, mag_data1_y, mag_data1_z, N_SAMPLES, ellipsoid, true); + int ellipsoid_step_1_success = lm_fit(mag_data1, N_SAMPLES, ellipsoid, false); + int ellipsoid_success = lm_fit(mag_data1, N_SAMPLES, ellipsoid, true); const Vector3f scale_true = {1.f, 1.06f, 0.94f}; EXPECT_EQ(ellipsoid_step_1_success, PX4_OK); diff --git a/src/modules/sensors/sensor_params_acc.c b/src/modules/sensors/sensor_params_acc.c new file mode 100644 index 0000000000..f8c82267d8 --- /dev/null +++ b/src/modules/sensors/sensor_params_acc.c @@ -0,0 +1,66 @@ +/**************************************************************************** + * + * Copyright (c) 2021 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * Bitfield selecting mag sides for calibration + * + * If set to two side calibration, only the offsets are estimated, the scale + * calibration is left unchanged. Thus an initial six side calibration is + * recommended. + * + * Bits: + * ORIENTATION_TAIL_DOWN = 1 + * ORIENTATION_NOSE_DOWN = 2 + * ORIENTATION_LEFT = 4 + * ORIENTATION_RIGHT = 8 + * ORIENTATION_UPSIDE_DOWN = 16 + * ORIENTATION_RIGHTSIDE_UP = 32 + * + * @min 34 + * @max 63 + * @value 34 Two side calibration + * @value 38 Three side calibration + * @value 63 Six side calibration + * @group Sensors + */ +PARAM_DEFINE_INT32(CAL_ACC_SIDES, 63); + +/** + * Automatically set external rotations. + * + * During calibration attempt to automatically determine the rotation of external magnetometers. + * + * @boolean + * @group Sensors + */ +PARAM_DEFINE_INT32(CAL_CAL_ROT_AUTO, 1);