sbgecom: Implement sbgECom INS driver (#24137)

* Add new INS driver sbgECom

Implement sbgECom messages handling to provide IMU sensors, GNSS and EKF data to the autopilot
Be able to parametrize the serial port, baudrate and the communicating mode
Clone sbgECom library only if sbgecom support is enabled and apply a patch
Be able to send SBG Systems INS settings in several ways when starting sbgecom driver

* Fix sensor airspeed simulator units

* Fix HIGHRES_IMU pressure unit

* Allow HIGHRES_IMU to support 4 IMUs

* sbgECom: Add documentation

* Use submodule instead of fetching sbgECom using CMake

* Remove patch strategy

* Fix dates

* Remove patch file

* Update SBG dev type ID

Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>

---------

Co-authored-by: Samuel Toledano <samuel.toledano@sbg-systems.com>
Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>

.gitmodules

@@ -100,3 +100,6 @@
 [submodule "src/drivers/ins/microstrain/mip_sdk"]
 	path = src/drivers/ins/microstrain/mip_sdk
 	url = https://github.com/PX4/LORD-MicroStrain_mip_sdk.git
+[submodule "src/drivers/ins/sbgecom/sbgECom"]
+	path = src/drivers/ins/sbgecom/sbgECom
+	url = https://github.com/PX4/sbgECom.git

docs/en/SUMMARY.md

@@ -282,6 +282,7 @@
       - [CubePilot Here+ (Discontined)](gps_compass/rtk_gps_hex_hereplus.md)
     - [INS (Inertial Navigation/GNSS)](sensor/inertial_navigation_systems.md)
       - [InertialLabs](sensor/inertiallabs.md)
+      - [sbgECom](sensor/sbgecom.md)
       - [VectorNav](sensor/vectornav.md)
     - [Optical Flow](sensor/optical_flow.md)
       - [ARK Flow](dronecan/ark_flow.md)

docs/en/releases/main.md

@@ -52,7 +52,7 @@ Please continue reading for [upgrade instructions](#upgrade-guide).
 
 ### Sensors
 
-- TBD
+- Add [sbgECom INS driver](../sensor/sbgecom.md) ([PX4-Autopilot#24137](https://github.com/PX4/PX4-Autopilot/pull/24137))
 
 ### Simulation
 

docs/en/sensor/inertial_navigation_systems.md

@@ -9,6 +9,7 @@ However PX4 can also use some INS devices as either sources of raw data, or as a
 INS systems that can be used as a replacement for EKF2 in PX4:
 
 - [InertialLabs](../sensor/inertiallabs.md)
+- [SBG Systems](../sensor/sbgecom.md): IMU/AHRS, GNSS/INS, Dual GNSS/INS systems that can be used as an external INS or as a source of raw sensor data.
 - [VectorNav](../sensor/vectornav.md): IMU/AHRS, GNSS/INS, Dual GNSS/INS systems that can be used as an external INS or as a source of raw sensor data.
 
 ## PX4 Firmware

docs/en/sensor/sbgecom.md

@@ -0,0 +1,150 @@
+# SBG Systems INS/AHRS (Pulse, Ellipse, etc.)
+
+[SBG-Systems](https://www.sbg-systems.com/) designs, manufactures, and support an extensive range of state-of-the-art inertial sensors such as Inertial Measurement Units (IMU), Attitude and Heading Reference Systems (AHRS), Inertial Navigation Systems with embedded GNSS (INS/GNSS), and so on.
+
+PX4 supports [all SBG Systems products](https://www.sbg-systems.com/products/) and can use these as an [external INS](../sensor/inertial_navigation_systems.md) (bypassing/replacing the EKF2 estimator), or as a source of raw sensor data provided to the navigation estimator.
+
+![Ellipse](../../assets/hardware/sensors/inertial/ellipse-inertial-navigation-system.png)
+
+## Overview
+
+SBG Systems products provide a range of benefits to PX4 users and can be integrated for:
+
+- Higher accuracy heading, pitch, and roll estimates
+- More robust and reliable GNSS positioning
+- Improved positioning and attitude performance in GNSS-contested environments
+- Performance under challenging dynamic conditions (e.g. catapult launches, VTOL operations, high-g or high angular rate operations)
+
+The sbgECom PX4 driver is streamlined to provide a simple plug-and-play architecture, removing engineering obstacles and allowing the acceleration of the design, development, and launch of platforms to keep pace with the rapid rate of innovation.
+
+The driver supports [all SBG Systems products](https://www.sbg-systems.com/products/).
+In particular the following systems are recommended:
+
+- **Pulse:** Recommended for fixed-wing systems without hovering, where static heading is not necessary.
+- **Ellipse:** Recommended for multicopter systems where hovering and low dynamics requires the use of static heading.
+
+## Where to Buy
+
+SBG Systems solutions are available directly from [MySBG](https://my.sbg-systems.com) (FR) or through their Global Sales Representatives. For more information on their solutions or for international orders, please contact contact@sbg-systems.com.
+
+## Hardware Setup
+
+### Wiring
+
+Connect any unused flight controller serial interface, such as a spare `GPS` or `TELEM` port, to the SBG Systems product MAIN port (required by PX4).
+
+### Mounting
+
+The SBG Systems product sensor can be mounted in any orientation, in any position on the vehicle, without regard to center of gravity.
+All SBG Systems product sensors default to a coordinate system of x-forward, y-right, and z-down, making the default mounting as connector-back, base down.
+This can be changed to any rigid rotation using the sbgECom Reference Frame Rotation register.
+
+If using a GNSS-enabled product, the GNSS antenna must be mounted rigidly with respect to the inertial sensor and with an unobstructed sky view. If using a dual-GNSS-enabled product (Ellipse-D), the secondary antenna must be mounted rigidly with respect to the primary antenna and the inertial sensor with an unobstructed sky view.
+
+For more mounting and configuration requirements and recommendations, see the relevant [SBG SUPPORT CENTER](https://support.sbg-systems.com/sc).
+
+## Firmware Configuration
+
+### PX4 Configuration
+
+To use the sbgECom driver:
+
+1. Include the module in firmware in the [kconfig board configuration](../hardware/porting_guide_config.md#px4-board-configuration-kconfig) by setting the kconfig variables: `CONFIG_DRIVERS_INS_SBGECOM` or `CONFIG_COMMON_INS`.
+2. [Set the parameter](../advanced_config/parameters.md) [SENS_SBG_CFG](../advanced_config/parameter_reference.md#SENS_SBG_CFG) to the hardware port connected to the SBG Systems product (for more information see [Serial Port Configuration](../peripherals/serial_configuration.md)).
+
+   ::: warning
+   Disable or change port of other sensors that are using the same one, for example [GPS_1_CONFIG](../advanced_config/parameter_reference.md#GPS_1_CONFIG) if using GPS1 port.
+   :::
+
+3. Set [SBG_BAUDRATE](../advanced_config/parameter_reference.md#SBG_BAUDRATE) to the desired default baudrate value.
+4. Allow the sbgECom driver to initialize by restarting PX4.
+5. Configure driver to provide IMU data, GNSS data and INS :
+
+   1. Set [SBG_MODE](../advanced_config/parameter_reference.md#SBG_MODE) to the desired mode.
+   2. Make sensor module select sensors by enabling [SENS_IMU_MODE](../advanced_config/parameter_reference.md#SENS_IMU_MODE).
+   3. Prioritize SBG Systems sensors using [CAL_GYROn_PRIO](../advanced_config/parameter_reference.md#CAL_GYRO0_PRIO), [CAL_ACCn_PRIO](../advanced_config/parameter_reference.md#CAL_ACC0_PRIO), [CAL_BAROn_PRIO](../advanced_config/parameter_reference.md#CAL_BARO0_PRIO), [CAL_MAGn_PRIO](../advanced_config/parameter_reference.md#CAL_MAG0_PRIO), where _n_ is the instance number of the IMU component (0, 1, etc.).
+
+   ::: tip
+   In most cases the external IMU (SBG) is the highest-numbered.
+   You can get a list of the IMU components available using [`uorb top -1`](../middleware/uorb.md#uorb-top-command), you can differentiate between them using the [`listener`](../modules/modules_command.md#listener) command and looking through the data, or just the rates.
+
+   Alternatively, you can check [CAL_GYROn_ID](../advanced_config/parameter_reference.md#CAL_GYRO0_ID) to see the device id.
+   The priority is 0-255, where 0 is entirely disabled and 255 is highest priority.
+   :::
+
+   ::: warning
+   When configuring both SBG Systems and Pixhawk sensors to have non-zero priority, if the selected sensor is errored (timeout), it can change during operation without being notified.
+   In this case, MAVLink messages will be updated with the newly selected sensor.
+
+   If you don't want to have this fallback mechanism, you must disable unwanted sensors.
+   :::
+
+   4. If using the sbgECom as an INS, disable EKF2 using [EKF2_EN](../advanced_config/parameter_reference.md#EKF2_EN).
+
+6. Restart PX4.
+
+Once enabled, the module will be detected on boot.
+IMU data should be published at 200Hz.
+
+## SBG Systems Configuration
+
+All High Performance and Ellipse 3.0 and higher SBG Systems INS can be configured directly from PX4 firmware:
+
+1. Enable [SBG_CONFIGURATION_EN](../advanced_config/parameter_reference.md#SBG_CONFIGURATION_EN)
+2. Provide a JSON file `sbg_settings.json` containing SBG Systems INS settings to be applied in your PX4 board `extras` directory (ex: `boards/px4/fmu-v5/extras`). The settings JSON file will be installed in `/etc/extras/sbg_settings.json` on the board.
+
+   ::: tip
+   The settings can be retrieved using [sbgEComAPI](https://github.com/SBG-Systems/sbgECom/tree/main/tools/sbgEComApi) or [sbgInsRestApi](https://developer.sbg-systems.com/sbgInsRestApi/1.3/#tag/Settings) and then modified as a JSON file.
+   :::
+
+   ::: tip
+   The settings file can be provided in the SD card in q`/fs/microsd/etc/extras/sbg_settings.json` to avoid rebuilding a new firmware to change JSON settings file.
+   :::
+
+3. For testing purpose, it's also possible to modify SBG Systems INS settings on the fly:
+
+   - By passing a JSON file path as argument when starting sbgecom driver (ex: `sbgecom start -f /fs/microsd/new_sbg_settings.json`)
+   - By passing a JSON string as argument when starting sbgecom driver: (ex: `sbgecom start -s {"output":{"comA":{"messages":{"airData":"onChange"}}}}`)
+
+For older Ellipse SBG Systems INS or to configure any SBG Systems INS directly, all commands and registers can be found in the [SBG SUPPORT CENTER](https://support.sbg-systems.com/sc).
+
+::: warning
+If the baudrate of the serial port on the INS product (used to communicate with PX4) is changed, the parameter [SBG_BAUDRATE](../advanced_config/parameter_reference.md#SBG_BAUDRATE) must be changed to match.
+:::
+
+## Published Data
+
+Upon initialization, the driver should print the following information to console (printed using `PX4_INFO`)
+
+- Unit model number
+- Unit hardware version
+- Unit serial number
+- Unit firmware number
+
+This should be accessible using the [`dmesg`](../modules/modules_system.md#dmesg) command.
+
+The sbgECom driver always publishes the unit's data to the following uORB topics:
+
+- [sensor_accel](../msg_docs/SensorAccel.md)
+- [sensor_gyro](../msg_docs/SensorGyro.md)
+- [sensor_mag](../msg_docs/SensorMag.md)
+
+if configured as a GNSS, publishes:
+
+- [sensor_gps](../msg_docs/SensorGps.md)
+
+and, if configured as an INS, publishes:
+
+- [estimator_status](../msg_docs/EstimatorStatus.md)
+- [vehicle_local_position](../msg_docs/VehicleLocalPosition.md)
+- [vehicle_global_positon](../msg_docs/VehicleGlobalPosition.md)
+- [vehicle_attitude](../msg_docs/VehicleAttitude.md)
+
+::: tip
+Published topics can be viewed using the `listener` command.
+:::
+
+## Hardware Specifications
+
+- [Product Briefs](https://www.sbg-systems.com/products/)
+- [Datasheets](https://www.sbg-systems.com/contact/#products)

src/drivers/drv_sensor.h

@@ -256,6 +256,8 @@
 #define DRV_INS_DEVTYPE_MICROSTRAIN	0xEA
 #define DRV_INS_DEVTYPE_BAHRS           0xEB
 
+#define DRV_INS_DEVTYPE_SBG		0xEC
+
 #define DRV_DEVTYPE_UNUSED		0xff
 
 #endif /* _DRV_SENSOR_H */

src/drivers/ins/Kconfig

@@ -5,7 +5,8 @@ menu "Inertial Navigation Systems (INS)"
         select DRIVERS_INS_VECTORNAV
         select DRIVERS_INS_ILABS
         select DRIVERS_INS_MICROSTRAIN
-	select DRIVERS_INS_EULERNAV_BAHRS
+        select DRIVERS_INS_EULERNAV_BAHRS
+        select DRIVERS_INS_SBGECOM
         ---help---
             Enable default set of INS sensors
     rsource "*/Kconfig"

src/drivers/ins/sbgecom/CMakeLists.txt

@@ -0,0 +1,73 @@
+############################################################################
+#
+#   Copyright (c) 2025 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.
+#
+############################################################################
+
+add_compile_definitions(SBG_CONFIG_LOG_MAX_SIZE=128)
+
+set(SBGECOM_DIR ${CMAKE_CURRENT_SOURCE_DIR}/sbgECom)
+px4_add_git_submodule(TARGET git_sbgECom PATH ${SBGECOM_DIR})
+
+add_subdirectory(sbgECom)
+
+add_dependencies(sbgECom prebuild_targets)
+
+target_compile_options(sbgECom
+        PRIVATE
+        -Wno-format
+        -Wno-format-security
+        -Wno-bad-function-cast
+        -Wno-double-promotion
+        -Wno-type-limits
+        -Wno-maybe-uninitialized
+        -Wno-float-equal
+)
+
+if("${PX4_PLATFORM}" MATCHES "nuttx")
+    target_compile_definitions(sbgECom PUBLIC __NUTTX__)
+endif()
+
+
+px4_add_module(
+	MODULE drivers__ins__sbgecom
+	MAIN sbgecom
+	INCLUDES
+		sbgECom/common
+		sbgECom/src
+	COMPILE_FLAGS
+	SRCS
+		sbgecom.cpp
+		sbgecom.hpp
+	MODULE_CONFIG
+		module.yaml
+	DEPENDS
+		sbgECom
+	)

src/drivers/ins/sbgecom/Kconfig

@@ -0,0 +1,5 @@
+menuconfig DRIVERS_INS_SBGECOM
+	bool "sbgecom"
+	default n
+	---help---
+		Enable support for sbgecom

src/drivers/ins/sbgecom/module.yaml

@@ -0,0 +1,50 @@
+module_name: sbgECom
+
+serial_config:
+    - command: sbgecom start -d ${SERIAL_DEV}
+      port_config_param:
+        name: SENS_SBG_CFG
+        group: Sensors
+
+parameters:
+    - group: Sensors
+      definitions:
+        SBG_MODE:
+            description:
+                short: sbgECom driver mode
+                long: |
+                    Modes available for sbgECom driver.
+                    In Sensors Only mode, use external IMU and magnetometer.
+                    In GNSS mode, use external GNSS in addition to sensors only mode.
+                    In INS mode, use external Kalman Filter in addition to GNSS mode.
+
+                    In INS mode, requires EKF2_EN 0. Keeping both enabled
+                    can lead to an unexpected behavior and vehicle instability.
+            category: System
+            type: enum
+            values:
+                0: Sensors Only
+                1: GNSS
+                2: INS (default)
+            default: 2
+        SBG_BAUDRATE:
+            description:
+                short: sbgECom driver baudrate
+                long: |
+                    Baudrate used by default for serial communication between PX4
+                    and SBG Systems INS through sbgECom driver.
+            category: System
+            type: int32
+            min: 9600
+            max: 921600
+            default: 921600
+            reboot_required: true
+        SBG_CONFIGURE_EN:
+            description:
+                short: sbgECom driver INS configuration enable
+                long: |
+                    Enable SBG Systems INS configuration through sbgECom driver
+                    on start.
+            category: System
+            type: boolean
+            default: 0

src/drivers/ins/sbgecom/sbgecom.hpp

@@ -0,0 +1,294 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2012-2025 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.
+ *
+ ****************************************************************************/
+
+/**
+ * @file sbgecom.hpp
+ * Driver for the SBG Systems products
+ *
+ * @author SBG Systems <contact@sbg-systems.com>
+ */
+
+#pragma once
+
+#include <sbgEComLib.h>
+#include <version/sbgVersion.h>
+
+#include <px4_platform_common/module.h>
+#include <px4_platform_common/module_params.h>
+#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
+#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
+#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
+#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
+#include <uORB/Subscription.hpp>
+#include <uORB/topics/airspeed.h>
+#include <uORB/topics/differential_pressure.h>
+#include <uORB/topics/estimator_status.h>
+#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/sensor_baro.h>
+#include <uORB/topics/sensor_gps.h>
+#include <uORB/topics/sensor_selection.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_air_data.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_local_position.h>
+#include <uORB/topics/vehicle_magnetometer.h>
+
+class SbgEcom : public ModuleBase<SbgEcom>, public ModuleParams, public px4::ScheduledWorkItem
+{
+public:
+
+	SbgEcom(const char *port, uint32_t baudrate, const char *config_file, const char *config_string);
+	~SbgEcom() override;
+
+	/** @see ModuleBase */
+	static int task_spawn(int argc, char **argv);
+
+	/** @see ModuleBase */
+	static int custom_command(int argc, char **argv);
+
+	/** @see ModuleBase */
+	static int print_usage(const char *reason = nullptr);
+
+	/** @see ModuleBase */
+	int print_status() override;
+
+	/** @see ModuleBase::run() */
+	void Run() override;
+
+	int init();
+
+private:
+
+	/**
+	* @brief Type for logging functions.
+	*
+	* @param file_name File name where the error occurred.
+	* @param function_name Function name where the error occurred.
+	* @param line Line number where the error occurred.
+	* @param category Category for this log or "None" if no category has been specified.
+	* @param log_type Define if we have an error, a warning, an info or a debug log.
+	* @param error_code The error code associated with the message.
+	* @param message The message to log.
+	*/
+	static void printLogCallBack(const char *file_name, const char *function_name, uint32_t line, const char *category,
+				     SbgDebugLogType log_type, SbgErrorCode error_code, const char *message);
+
+	/**
+	* @brief Parse IMU (Inertial Measurement Unit) measurement logs.
+	*
+	* @param ref_sbg_data Contains the received log data as an union.
+	* @param user_arg Optional user supplied argument.
+	*/
+	static void handleLogImuShort(const SbgEComLogUnion *ref_sbg_data, void *user_arg);
+
+	/**
+	* @brief Parse magnetic field measurements logs.
+	*
+	* @param ref_sbg_data Contains the received log data as an union.
+	* @param user_arg Optional user supplied argument.
+	*/
+	static void handleLogMag(const SbgEComLogUnion *ref_sbg_data, void *user_arg);
+
+	/**
+	* @brief Parse EKF quaternion measurement logs.
+	*
+	* @param ref_sbg_data Contains the received log data as an union.
+	* @param user_arg Optional user supplied argument.
+	*/
+	static void handleLogEkfQuat(const SbgEComLogUnion *ref_sbg_data, void *user_arg);
+
+	/**
+	* @brief Parse EKF navigation measurement logs.
+	*
+	* @param ref_sbg_data Contains the received log data as an union.
+	* @param user_arg Optional user supplied argument.
+	*/
+	static void handleLogEkfNav(const SbgEComLogUnion *ref_sbg_data, void *user_arg);
+
+	/**
+	* @brief GNSS position, velocity and heading related logs.
+	*
+	* @param msg Message ID of the log received.
+	* @param ref_sbg_data Contains the received log data as an union.
+	* @param user_arg Optional user supplied argument.
+	*/
+	static void handleLogGnssPosVelHdt(SbgEComMsgId msg, const SbgEComLogUnion *ref_sbg_data, void *user_arg);
+
+	/**
+	* @brief Update estimator status message from EKF status flags.
+	*
+	* @param ekf_status EKF status flags.
+	* @param estimator_status Estimator status message.
+	*/
+	static void updateEstimatorStatus(uint32_t ekf_status, estimator_status_s *estimator_status);
+
+	/**
+	* @brief Callback definition called each time a new log is received.
+	*
+	* @param handle Valid handle on the sbgECom instance that has called this callback.
+	* @param msg_class Class of the message we have received
+	* @param msg Message ID of the log received.
+	* @param ref_sbg_data Contains the received log data as an union.
+	* @param user_arg Optional user supplied argument.
+	* @return SBG_NO_ERROR if the received log has been used successfully.
+	*/
+	static SbgErrorCode onLogReceived(SbgEComHandle *handle, SbgEComClass msg_class, SbgEComMsgId msg,
+					  const SbgEComLogUnion *ref_sbg_data, void *user_arg);
+
+	/**
+	* @brief Send a config to the INS
+	*
+	* @param pHandle SbgECom instance.
+	* @param config Config json string.
+	*/
+	static void send_config(SbgEComHandle *pHandle, const char *config);
+
+	/**
+	* @brief Send a config file to the INS
+	*
+	* @param pHandle SbgECom instance.
+	* @param file_path Config file path.
+	*/
+	static void send_config_file(SbgEComHandle *pHandle, const char *file_path);
+
+	/**
+	* @brief Get and print product info.
+	*
+	* @param handle SbgECom instance.
+	* @return SBG_NO_ERROR if successful.
+	*/
+	SbgErrorCode getAndPrintProductInfo(SbgEComHandle *handle);
+
+	/**
+	* @brief Try to parse one log from the input interface and then return.
+	*
+	* @param handle A valid sbgECom handle.
+	* @return SBG_NO_ERROR if no error occurs during incoming log parsing.
+	*/
+	SbgErrorCode handleOneLog(SbgEComHandle *handle);
+
+	/**
+	* @brief Get air data and send it.
+	*
+	* @param handle A valid sbgECom handle.
+	* @param instance An SbgEcom object.
+	* @return SBG_NO_ERROR if no error occurs during incoming log parsing.
+	*/
+	SbgErrorCode sendAirDataLog(SbgEComHandle *handle, SbgEcom *instance);
+
+	/**
+	* @brief Get magnetometer data and send it.
+	*
+	* @param handle A valid sbgECom handle.
+	* @param instance An SbgEcom object.
+	* @return SBG_NO_ERROR if no error occurs during incoming log parsing.
+	*/
+	SbgErrorCode sendMagLog(SbgEComHandle *handle, SbgEcom *instance);
+
+	void set_device_id(uint32_t device_id);
+	uint32_t get_device_id(void);
+
+	// SBG interface and state variables
+	SbgInterface _sbg_interface;
+	SbgEComHandle _com_handle;
+	SbgEComLogUnion _log_data;
+
+	uint32_t _baudrate;
+	const char *_config_file;
+	const char *_config_string;
+	char _device_name[25];
+	uint32_t _device_id{0};
+
+	const int log_interval = 10;
+	int iteration_count = log_interval;
+
+	bool failure = false;
+	bool _ekf_failure = false;
+
+	bool _initialized = false;
+	int init_result;
+
+	MapProjection _pos_ref{};
+	double _gps_alt_ref{NAN};
+
+	struct GnssData {
+		bool pos_received = false;
+		bool vel_received = false;
+		bool hdt_received = false;
+
+		SbgEComLogGnssPos gps_pos;
+		SbgEComLogGnssVel gps_vel;
+		SbgEComLogGnssHdt gps_hdt;
+
+		hrt_abstime pos_timestamp = 0;
+		hrt_abstime vel_timestamp = 0;
+		hrt_abstime hdt_timestamp = 0;
+	};
+
+	GnssData gnss_data;
+	float _heading;
+
+	px4::atomic<hrt_abstime> _time_last_valid_imu_us{false};
+
+	// Sensors topics
+	PX4Accelerometer _px4_accel{0};
+	PX4Gyroscope	 _px4_gyro{0};
+	PX4Magnetometer  _px4_mag{0};
+
+	// Publications with topic dependent on multi-mode
+	uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub{ORB_ID(sensor_gps)};
+	uORB::PublicationMulti<vehicle_attitude_s> _attitude_pub{ORB_ID(vehicle_attitude)};
+	uORB::PublicationMulti<vehicle_local_position_s> _local_position_pub{ORB_ID(vehicle_local_position)};
+	uORB::PublicationMulti<vehicle_global_position_s> _global_position_pub{ORB_ID(vehicle_global_position)};
+	uORB::Publication<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
+
+	// Subscription for INS EKF aiding
+	uORB::Subscription _air_data_sub{ORB_ID(vehicle_air_data)};
+	uORB::Subscription _airspeed_sub{ORB_ID(airspeed)};
+	uORB::Subscription _diff_pressure_sub{ORB_ID(differential_pressure)};
+	uORB::Subscription _mag_sub{ORB_ID(vehicle_magnetometer)};
+
+	// Performance mounters for monitoring and debugging
+	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": errors")};
+	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": sample")};
+	perf_counter_t _write_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": write")};
+
+	perf_counter_t _accel_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": accel publish interval")};
+	perf_counter_t _gyro_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": gyro publish interval")};
+	perf_counter_t _mag_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": mag publish interval")};
+	perf_counter_t _gnss_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": GNSS publish interval")};
+
+	perf_counter_t _attitude_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": attitude publish interval")};
+	perf_counter_t _local_position_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": local position publish interval")};
+	perf_counter_t _global_position_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": global position publish interval")};
+};

src/modules/mavlink/streams/HIGHRES_IMU.hpp

@@ -63,7 +63,7 @@ public:
 private:
 	explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink) {}
 
-	uORB::SubscriptionMultiArray<vehicle_imu_s, 3> _vehicle_imu_subs{ORB_ID::vehicle_imu};
+	uORB::SubscriptionMultiArray<vehicle_imu_s, 4> _vehicle_imu_subs{ORB_ID::vehicle_imu};
 	uORB::Subscription _estimator_sensor_bias_sub{ORB_ID(estimator_sensor_bias)};
 	uORB::Subscription _estimator_selector_status_sub{ORB_ID(estimator_selector_status)};
 	uORB::Subscription _sensor_selection_sub{ORB_ID(sensor_selection)};
@@ -190,8 +190,8 @@ private:
 			msg.xmag = mag(0);
 			msg.ymag = mag(1);
 			msg.zmag = mag(2);
-			msg.abs_pressure = air_data.baro_pressure_pa;
-			msg.diff_pressure = differential_pressure.differential_pressure_pa;
+			msg.abs_pressure = air_data.baro_pressure_pa * 0.01f; // Pa to hPa
+			msg.diff_pressure = differential_pressure.differential_pressure_pa * 0.01f; // Pa to hPa
 			msg.pressure_alt = air_data.baro_alt_meter;
 			msg.temperature = air_data.ambient_temperature;
 			msg.fields_updated = fields_updated;

src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp

@@ -144,7 +144,7 @@ void SensorAirspeedSim::Run()
 			// report.timestamp_sample = time;
 			differential_pressure.device_id = 1377548; // 1377548: DRV_DIFF_PRESS_DEVTYPE_SIM, BUS: 1, ADDR: 5, TYPE: SIMULATION
 			differential_pressure.differential_pressure_pa = (double)diff_pressure * 100.0; // hPa to Pa;
-			differential_pressure.temperature = temperature_local;
+			differential_pressure.temperature = temperature_local + ABSOLUTE_ZERO_C; // K to C
 			differential_pressure.timestamp = hrt_absolute_time();
 			_differential_pressure_pub.publish(differential_pressure);
 

src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp

@@ -49,6 +49,7 @@
 
 using namespace time_literals;
 
+static constexpr float ABSOLUTE_ZERO_C = -273.15; // absolute 0 temperature [C]
 static constexpr float TEMPERATURE_MSL = 288.15; // temperature at MSL [K] (15 [C])
 static constexpr float PRESSURE_MSL = 101325.0; // pressure at MSL [Pa]
 static constexpr float LAPSE_RATE = 0.0065; // reduction in temperature with altitude for troposphere [K/m]