From 55f17c655acd3e9ef863f20367dbb30215c296a4 Mon Sep 17 00:00:00 2001 From: Jukka Laitinen Date: Tue, 19 Nov 2024 11:45:36 +0200 Subject: [PATCH] src/lib/drivers/magnetometer/PX4Magnetometer.cpp: Workaround for sensors 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 --- src/lib/drivers/magnetometer/PX4Magnetometer.cpp | 10 ++++++++++ 1 file changed, 10 insertions(+) diff --git a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp b/src/lib/drivers/magnetometer/PX4Magnetometer.cpp index 40907d870d6d..d07417013908 100644 --- a/src/lib/drivers/magnetometer/PX4Magnetometer.cpp +++ b/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()