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

@@ -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

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 <px4_platform_common/posix.h>
 #include <px4_platform_common/time.h>
 
+#include "lm_fit.hpp"
+
 #include <drivers/drv_hrt.h>
+#include <include/containers/Bitset.hpp>
 #include <lib/sensor_calibration/Accelerometer.hpp>
 #include <lib/sensor_calibration/Utilities.hpp>
 #include <lib/mathlib/mathlib.h>
 #include <lib/geo/geo.h>
 #include <matrix/math.hpp>
 #include <lib/conversion/rotation.h>
+#include <lib/mathlib/math/WelfordMean.hpp>
 #include <lib/parameters/param.h>
 #include <lib/systemlib/err.h>
 #include <lib/systemlib/mavlink_log.h>
@@ -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<sensor_accel_s, MAX_ACCEL_SENS> accel_subs{ORB_ID::sensor_accel};
+	math::WelfordMean<Vector3f> 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<sensor_accel_s> 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(&timestamp_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<enum detect_orientation_return>(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<float, 6, 3> 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<Vector3f> mean[MAX_ACCEL_SENS] {};
 	uORB::SubscriptionMultiArray<sensor_accel_s, MAX_ACCEL_SENS> accel_subs{ORB_ID::sensor_accel};
+	px4::Bitset<MAX_ACCEL_SENS> 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<MAX_ACCEL_SENS> calibrated;
+
+	static constexpr float g = CONSTANTS_ONE_G;
+	matrix::Matrix<float, 6, 3> 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<vehicle_attitude_s> 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;
 }

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++;

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);

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 &params, 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<float, 4> 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<float, 4> JTJ2 = JTJ;
+	matrix::SquareMatrix<float, 4> 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 &params, 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<float, 9> JTJ;
+	float fit1 = 0.f;
+	float fit2 = 0.f;
+
+	matrix::SquareMatrix<float, 9> 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<float, 9> 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<float, 9> 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 &params,
-	       bool full_ellipsoid)
+int lm_fit(const matrix::Vector3f data[], unsigned samples_collected, sphere_params &params, 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;
 		}

src/modules/commander/lm_fit.hpp

@@ -37,7 +37,9 @@
 #include <px4_platform_common/defines.h>
 
 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 &params,
-	       bool full_ellipsoid);
+int lm_fit(const matrix::Vector3f data[], unsigned samples_collected, sphere_params &params, bool full_ellipsoid);

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<float *>(malloc(sizeof(float) * calibration_points_maxcount));
-			worker_data.y[cur_mag] = static_cast<float *>(malloc(sizeof(float) * calibration_points_maxcount));
-			worker_data.z[cur_mag] = static_cast<float *>(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;

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<float>(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);

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);