src/lib/drivers/magnetometer/PX4Magnetometer.cpp: Workaround for sens…

…ors not initializing with all the magnetometers

There is some issue with "sensors" module; it only takes into account the magnetometers which have published data
before the module start.

Workaround this issue by publishing some initial "0" data right at the time of constructing the magnetometer driver.
If the probing of the sensor fails, the orb gets anyhow unadvertised, so there is no extra sensor data lying
around in the case the magnetometer is not present.

This also forces the magnetometer numbering to follow the startup order; without advertising the data
the orb numbering depends on magnetometer initialization sequence - a magnetometer started later in the
init scripts may publish it's first data before some other mag, which was started earlier. This is especially
problematic when using the same scripts on different HW platforms which run on different speeds.

Signed-off-by: Jukka Laitinen <jukkax@ssrc.tii.ae>

src/lib/drivers/magnetometer/PX4Magnetometer.cpp

@@ -40,6 +40,16 @@ PX4Magnetometer::PX4Magnetometer(uint32_t device_id, enum Rotation rotation) :
 	_device_id{device_id},
 	_rotation{rotation}
 {
+	// Publish initial 0 data just for the sensors module to find
+	// this device, in case "sensors" module start up before initialization
+	// of the magnetometer is not fully complete
+
+	// TODO: This is a workaround. The "sensors" module should find the mags
+	// which start publishing data later
+
+	sensor_mag_s report = {};
+	report.device_id = _device_id;
+	_sensor_pub.publish(report);
 }
 
 PX4Magnetometer::~PX4Magnetometer()