sensors: add SENS_MAG_MODE to optionally publish all magnetometers

src/modules/sensors/sensor_params_mag.c

@@ -93,3 +93,15 @@ PARAM_DEFINE_INT32(CAL_MAG_ROT_AUTO, 1);
  *
  */
 PARAM_DEFINE_FLOAT(SENS_MAG_RATE, 50.0f);
+
+/**
+ * Sensors hub mag mode
+ *
+ * @value 0 Publish all magnetometers
+ * @value 1 Publish primary magnetometer
+ *
+ * @category system
+ * @reboot_required true
+ * @group Sensors
+ */
+PARAM_DEFINE_INT32(SENS_MAG_MODE, 1);

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.cpp

@@ -212,6 +212,13 @@ void VehicleMagnetometer::Run()
 
 					_advertised[uorb_index] = true;
 
+					// advertise outputs in order if publishing all
+					if (!_param_sens_mag_mode.get()) {
+						for (int instance = 0; instance < uorb_index; instance++) {
+							_vehicle_magnetometer_multi_pub[instance].advertise();
+						}
+					}
+
 				} else {
 					_last_data[uorb_index].timestamp = hrt_absolute_time();
 				}
@@ -229,16 +236,20 @@ void VehicleMagnetometer::Run()
 				}
 
 				if (_calibration[uorb_index].enabled()) {
-					Vector3f vect = _calibration[uorb_index].Correct(Vector3f{report.x, report.y, report.z});
+					const Vector3f vect = _calibration[uorb_index].Correct(Vector3f{report.x, report.y, report.z});
+
+					float mag_array[3] {vect(0), vect(1), vect(2)};
+					_voter.put(uorb_index, report.timestamp, mag_array, report.error_count, _priority[uorb_index]);
+
+					_timestamp_sample_sum[uorb_index] += report.timestamp_sample;
+					_mag_sum[uorb_index] += vect;
+					_mag_sum_count[uorb_index]++;
 
 					_last_data[uorb_index].timestamp_sample = report.timestamp_sample;
 					_last_data[uorb_index].device_id = report.device_id;
 					_last_data[uorb_index].x = vect(0);
 					_last_data[uorb_index].y = vect(1);
 					_last_data[uorb_index].z = vect(2);
-
-					float mag_array[3] {vect(0), vect(1), vect(2)};
-					_voter.put(uorb_index, report.timestamp, mag_array, report.error_count, _priority[uorb_index]);
 				}
 			}
 		}
@@ -255,8 +266,10 @@ void VehicleMagnetometer::Run()
 				sub.unregisterCallback();
 			}
 
-			if (_selected_sensor_sub_index >= 0) {
+			if (_param_sens_mag_mode.get()) {
-				PX4_INFO("%s switch from #%u -> #%d", "MAG", _selected_sensor_sub_index, best_index);
+				if (_selected_sensor_sub_index >= 0) {
+					PX4_INFO("%s switch from #%u -> #%d", "MAG", _selected_sensor_sub_index, best_index);
+				}
 			}
 
 			_selected_sensor_sub_index = best_index;
@@ -264,67 +277,56 @@ void VehicleMagnetometer::Run()
 		}
 	}
 
-	if ((_selected_sensor_sub_index >= 0)
+	// Publish
-	    && (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR)
+	if (_param_sens_mag_mode.get()) {
-	    && updated[_selected_sensor_sub_index]) {
+		// publish only best mag
+		if ((_selected_sensor_sub_index >= 0)
+		    && (_voter.get_sensor_state(_selected_sensor_sub_index) == DataValidator::ERROR_FLAG_NO_ERROR)
+		    && updated[_selected_sensor_sub_index]) {
 
-		const sensor_mag_s &mag = _last_data[_selected_sensor_sub_index];
+			Publish(_selected_sensor_sub_index);
+		}
 
-		_mag_timestamp_sum += mag.timestamp_sample;
+	} else {
-		_mag_sum += Vector3f{mag.x, mag.y, mag.z};
+		// publish all
-		_mag_sum_count++;
+		for (int uorb_index = 0; uorb_index < MAX_SENSOR_COUNT; uorb_index++) {
-
+			// publish all magnetometers as separate instances
-		if ((_param_sens_mag_rate.get() > 0)
+			if (updated[uorb_index] && (_calibration[uorb_index].device_id() != 0)) {
-		    && hrt_elapsed_time(&_last_publication_timestamp) >= (1e6f / _param_sens_mag_rate.get())) {
+				Publish(uorb_index, true);
-
+			}
-			const Vector3f magnetometer_data = _mag_sum / _mag_sum_count;
-			const hrt_abstime timestamp_sample = _mag_timestamp_sum / _mag_sum_count;
-
-			// reset
-			_mag_timestamp_sum = 0;
-			_mag_sum.zero();
-			_mag_sum_count = 0;
-
-			// populate vehicle_magnetometer with primary mag and publish
-			vehicle_magnetometer_s out{};
-			out.timestamp_sample = timestamp_sample;
-			out.device_id = mag.device_id;
-			magnetometer_data.copyTo(out.magnetometer_ga);
-
-			out.timestamp = hrt_absolute_time();
-			_vehicle_magnetometer_pub.publish(out);
-
-			_last_publication_timestamp = out.timestamp;
 		}
 	}
 
+
 	// check failover and report
-	if (_last_failover_count != _voter.failover_count()) {
+	if (_param_sens_mag_mode.get()) {
-		uint32_t flags = _voter.failover_state();
+		if (_last_failover_count != _voter.failover_count()) {
-		int failover_index = _voter.failover_index();
+			uint32_t flags = _voter.failover_state();
+			int failover_index = _voter.failover_index();
 
-		if (flags != DataValidator::ERROR_FLAG_NO_ERROR) {
+			if (flags != DataValidator::ERROR_FLAG_NO_ERROR) {
-			if (failover_index != -1) {
+				if (failover_index != -1) {
-				const hrt_abstime now = hrt_absolute_time();
+					const hrt_abstime now = hrt_absolute_time();
 
-				if (now - _last_error_message > 3_s) {
+					if (now - _last_error_message > 3_s) {
-					mavlink_log_emergency(&_mavlink_log_pub, "%s #%i failed: %s%s%s%s%s!",
+						mavlink_log_emergency(&_mavlink_log_pub, "%s #%i failed: %s%s%s%s%s!",
-							      "MAG",
+								      "MAG",
-							      failover_index,
+								      failover_index,
-							      ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""),
+								      ((flags & DataValidator::ERROR_FLAG_NO_DATA) ? " OFF" : ""),
-							      ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""),
+								      ((flags & DataValidator::ERROR_FLAG_STALE_DATA) ? " STALE" : ""),
-							      ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""),
+								      ((flags & DataValidator::ERROR_FLAG_TIMEOUT) ? " TIMEOUT" : ""),
-							      ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""),
+								      ((flags & DataValidator::ERROR_FLAG_HIGH_ERRCOUNT) ? " ERR CNT" : ""),
-							      ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : ""));
+								      ((flags & DataValidator::ERROR_FLAG_HIGH_ERRDENSITY) ? " ERR DNST" : ""));
-					_last_error_message = now;
+						_last_error_message = now;
+					}
+
+					// reduce priority of failed sensor to the minimum
+					_priority[failover_index] = 1;
 				}
-
-				// reduce priority of failed sensor to the minimum
-				_priority[failover_index] = 1;
 			}
-		}
 
-		_last_failover_count = _voter.failover_count();
+			_last_failover_count = _voter.failover_count();
+		}
 	}
 
 	if (!_armed) {
@@ -332,11 +334,44 @@ void VehicleMagnetometer::Run()
 	}
 
 	// reschedule timeout
-	ScheduleDelayed(100_ms);
+	ScheduleDelayed(20_ms);
 
 	perf_end(_cycle_perf);
 }
 
+void VehicleMagnetometer::Publish(uint8_t instance, bool multi)
+{
+	if ((_param_sens_mag_rate.get() > 0)
+	    && hrt_elapsed_time(&_last_publication_timestamp[instance]) >= (1e6f / _param_sens_mag_rate.get())) {
+
+		const Vector3f magnetometer_data = _mag_sum[instance] / _mag_sum_count[instance];
+		const hrt_abstime timestamp_sample = _timestamp_sample_sum[instance] / _mag_sum_count[instance];
+
+		// reset
+		_timestamp_sample_sum[instance] = 0;
+		_mag_sum[instance].zero();
+		_mag_sum_count[instance] = 0;
+
+		// populate vehicle_magnetometer with primary mag and publish
+		vehicle_magnetometer_s out{};
+		out.timestamp_sample = timestamp_sample;
+		out.device_id = _calibration[instance].device_id();
+		magnetometer_data.copyTo(out.magnetometer_ga);
+
+		out.timestamp = hrt_absolute_time();
+
+		if (multi) {
+			_vehicle_magnetometer_multi_pub[instance].publish(out);
+
+		} else {
+			// otherwise only ever publish the first instance
+			_vehicle_magnetometer_pub.publish(out);
+		}
+
+		_last_publication_timestamp[instance] = out.timestamp;
+	}
+}
+
 void VehicleMagnetometer::calcMagInconsistency()
 {
 	sensor_preflight_mag_s preflt{};

src/modules/sensors/vehicle_magnetometer/VehicleMagnetometer.hpp

@@ -46,6 +46,7 @@
 #include <px4_platform_common/px4_config.h>
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/Publication.hpp>
+#include <uORB/PublicationMulti.hpp>
 #include <uORB/Subscription.hpp>
 #include <uORB/SubscriptionCallback.hpp>
 #include <uORB/topics/actuator_controls.h>
@@ -75,6 +76,8 @@ private:
 
 	void ParametersUpdate(bool force = false);
 
+	void Publish(uint8_t instance, bool multi = false);
+
 	/**
 	 * Calculates the magnitude in Gauss of the largest difference between the primary and any other magnetometers
 	 */
@@ -83,7 +86,14 @@ private:
 	static constexpr int MAX_SENSOR_COUNT = 4;
 
 	uORB::Publication<sensor_preflight_mag_s> _sensor_preflight_mag_pub{ORB_ID(sensor_preflight_mag)};
+
 	uORB::Publication<vehicle_magnetometer_s> _vehicle_magnetometer_pub{ORB_ID(vehicle_magnetometer)};
+	uORB::PublicationMulti<vehicle_magnetometer_s> _vehicle_magnetometer_multi_pub[MAX_SENSOR_COUNT] {
+		{ORB_ID(vehicle_magnetometer)},
+		{ORB_ID(vehicle_magnetometer)},
+		{ORB_ID(vehicle_magnetometer)},
+		{ORB_ID(vehicle_magnetometer)},
+	};
 
 	uORB::Subscription _actuator_controls_0_sub{ORB_ID(actuator_controls_0)};
 	uORB::Subscription _battery_status_sub{ORB_ID(battery_status), 0};
@@ -110,16 +120,16 @@ private:
 
 	perf_counter_t _cycle_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")};
 
-	hrt_abstime _last_publication_timestamp{0};
 	hrt_abstime _last_error_message{0};
 	orb_advert_t _mavlink_log_pub{nullptr};
 
 	DataValidatorGroup _voter{1};
 	unsigned _last_failover_count{0};
 
-	uint64_t _mag_timestamp_sum{0};
+	uint64_t _timestamp_sample_sum[MAX_SENSOR_COUNT] {0};
-	matrix::Vector3f _mag_sum{};
+	matrix::Vector3f _mag_sum[MAX_SENSOR_COUNT] {};
-	int _mag_sum_count{0};
+	int _mag_sum_count[MAX_SENSOR_COUNT] {};
+	hrt_abstime _last_publication_timestamp[MAX_SENSOR_COUNT] {};
 
 	sensor_mag_s _last_data[MAX_SENSOR_COUNT] {};
 	bool _advertised[MAX_SENSOR_COUNT] {};
@@ -134,6 +144,7 @@ private:
 
 	DEFINE_PARAMETERS(
 		(ParamInt<px4::params::CAL_MAG_COMP_TYP>) _param_mag_comp_typ,
+		(ParamBool<px4::params::SENS_MAG_MODE>) _param_sens_mag_mode,
 		(ParamFloat<px4::params::SENS_MAG_RATE>) _param_sens_mag_rate
 	)
 };