Add logging of mag calibration data (mag_worker_data)

Co-authored-by: Julian Kent <julian@auterion.com>

msg/CMakeLists.txt

@@ -79,6 +79,7 @@ set(msg_files
 	led_control.msg
 	log_message.msg
 	logger_status.msg
+	mag_worker_data.msg
 	manual_control_setpoint.msg
 	manual_control_switches.msg
 	mavlink_log.msg

msg/mag_worker_data.msg

@@ -0,0 +1,13 @@
+uint64 timestamp          # time since system start (microseconds)
+uint64 timestamp_sample
+
+uint8 MAX_MAGS = 4
+
+uint32 done_count
+uint32 calibration_points_perside
+uint64 calibration_interval_perside_us
+uint32[4] calibration_counter_total
+bool[4] side_data_collected
+float32[4] x
+float32[4] y
+float32[4] z

msg/tools/uorb_rtps_message_ids.yaml

@@ -308,6 +308,8 @@ rtps:
     id: 146
   - msg: control_allocator_status
     id: 147
+  - msg: mag_worker_data
+    id: 148
   ########## multi topics: begin ##########
   - msg: actuator_controls_0
     id: 170

src/modules/commander/mag_calibration.cpp

@@ -63,6 +63,7 @@
 #include <uORB/topics/sensor_gyro.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/mag_worker_data.h>
 
 using namespace matrix;
 using namespace time_literals;
@@ -78,6 +79,7 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 /// Data passed to calibration worker routine
 struct mag_worker_data_t {
 	orb_advert_t	*mavlink_log_pub;
+	orb_advert_t	mag_worker_data_pub;
 	bool		append_to_existing_calibration;
 	unsigned	last_mag_progress;
 	unsigned	done_count;
@@ -388,6 +390,38 @@ static calibrate_return mag_calibration_worker(detect_orientation_return orienta
 					}
 				}
 
+				hrt_abstime now = hrt_absolute_time();
+				mag_worker_data_s status;
+				status.timestamp = now;
+				status.timestamp_sample = now;
+				status.done_count = worker_data->done_count;
+				status.calibration_points_perside = worker_data->calibration_points_perside;
+				status.calibration_interval_perside_us = worker_data->calibration_interval_perside_us;
+
+				for (size_t cur_mag = 0; cur_mag < MAX_MAGS; cur_mag++) {
+					status.calibration_counter_total[cur_mag] = worker_data->calibration_counter_total[cur_mag];
+					status.side_data_collected[cur_mag] = worker_data->side_data_collected[cur_mag];
+
+					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];
+
+					} else {
+						status.x[cur_mag] = 0.f;
+						status.y[cur_mag] = 0.f;
+						status.z[cur_mag] = 0.f;
+					}
+				}
+
+				if (worker_data->mag_worker_data_pub == nullptr) {
+					worker_data->mag_worker_data_pub = orb_advertise(ORB_ID(mag_worker_data), &status);
+
+				} else {
+					orb_publish(ORB_ID(mag_worker_data), worker_data->mag_worker_data_pub, &status);
+				}
+
 				calibration_counter_side++;
 
 				unsigned new_progress = progress_percentage(worker_data) +
@@ -436,6 +470,8 @@ calibrate_return mag_calibrate_all(orb_advert_t *mavlink_log_pub, int32_t cal_ma
 
 	mag_worker_data_t worker_data{};
 
+	worker_data.mag_worker_data_pub = nullptr;
+
 	// keep and update the existing calibration when we are not doing a full 6-axis calibration
 	worker_data.append_to_existing_calibration = cal_mask < ((1 << 6) - 1);
 	worker_data.mavlink_log_pub = mavlink_log_pub;

src/modules/logger/logged_topics.cpp

@@ -65,6 +65,7 @@ void LoggedTopics::add_default_topics()
 	add_topic("home_position");
 	add_topic("hover_thrust_estimate", 100);
 	add_topic("input_rc", 500);
+	add_topic("mag_worker_data");
 	add_topic("manual_control_setpoint", 200);
 	add_topic("manual_control_switches");
 	add_topic("mission");