From 2b29b772becbc0fbf01583ae196db713527dff6a Mon Sep 17 00:00:00 2001 From: Samuel Toledano Date: Thu, 12 Sep 2024 18:19:02 +0200 Subject: [PATCH 1/5] 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 --- src/drivers/drv_sensor.h | 2 + src/drivers/ins/Kconfig | 1 + src/drivers/ins/sbgecom/CMakeLists.txt | 60 ++ src/drivers/ins/sbgecom/Kconfig | 5 + src/drivers/ins/sbgecom/module.yaml | 50 ++ src/drivers/ins/sbgecom/sbgecom.cpp | 1094 ++++++++++++++++++++++++ src/drivers/ins/sbgecom/sbgecom.hpp | 293 +++++++ src/drivers/ins/sbgecom/sbgecom.patch | 124 +++ 8 files changed, 1629 insertions(+) create mode 100644 src/drivers/ins/sbgecom/CMakeLists.txt create mode 100644 src/drivers/ins/sbgecom/Kconfig create mode 100644 src/drivers/ins/sbgecom/module.yaml create mode 100644 src/drivers/ins/sbgecom/sbgecom.cpp create mode 100644 src/drivers/ins/sbgecom/sbgecom.hpp create mode 100644 src/drivers/ins/sbgecom/sbgecom.patch diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 8b33acb0f1ad..060c512833b9 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -249,6 +249,8 @@ #define DRV_BARO_DEVTYPE_AUAV 0xE7 #define DRV_BARO_DEVTYPE_SPA06 0xE8 +#define DRV_INS_DEVTYPE_SBG 0xE9 + #define DRV_DEVTYPE_UNUSED 0xff #endif /* _DRV_SENSOR_H */ diff --git a/src/drivers/ins/Kconfig b/src/drivers/ins/Kconfig index 5c215e3cca15..4eb084e29c4a 100644 --- a/src/drivers/ins/Kconfig +++ b/src/drivers/ins/Kconfig @@ -3,6 +3,7 @@ menu "Inertial Navigation Systems (INS)" bool "All INS sensors" default n select DRIVERS_INS_VECTORNAV + select DRIVERS_INS_SBGECOM ---help--- Enable default set of INS sensors rsource "*/Kconfig" diff --git a/src/drivers/ins/sbgecom/CMakeLists.txt b/src/drivers/ins/sbgecom/CMakeLists.txt new file mode 100644 index 000000000000..c90730b3df97 --- /dev/null +++ b/src/drivers/ins/sbgecom/CMakeLists.txt @@ -0,0 +1,60 @@ +############################################################################ +# +# Copyright (c) 2022 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. +# +############################################################################ + +include(FetchContent) + +FetchContent_Declare( + sbgECom + GIT_REPOSITORY https://github.com/SBG-Systems/sbgECom.git + GIT_TAG 5.1.708-stable + GIT_SHALLOW TRUE + PATCH_COMMAND git apply --reverse --check "${CMAKE_CURRENT_LIST_DIR}/sbgecom.patch" || git apply --ignore-whitespace "${CMAKE_CURRENT_LIST_DIR}/sbgecom.patch" +) + +FetchContent_MakeAvailable(sbgECom) + +px4_add_module( + MODULE drivers__ins__sbgecom + MAIN sbgecom + INCLUDES + ${sbgECom_SOURCE_DIR}/common + ${sbgECom_SOURCE_DIR}/src + COMPILE_FLAGS + SRCS + sbgecom.cpp + sbgecom.hpp + MODULE_CONFIG + module.yaml + DEPENDS + sbgECom + ) diff --git a/src/drivers/ins/sbgecom/Kconfig b/src/drivers/ins/sbgecom/Kconfig new file mode 100644 index 000000000000..2569367c7a48 --- /dev/null +++ b/src/drivers/ins/sbgecom/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_INS_SBGECOM + bool "sbgecom" + default n + ---help--- + Enable support for sbgecom diff --git a/src/drivers/ins/sbgecom/module.yaml b/src/drivers/ins/sbgecom/module.yaml new file mode 100644 index 000000000000..66c247f9cac4 --- /dev/null +++ b/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 (default) + 1: GNSS + 2: INS + default: 0 + 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 diff --git a/src/drivers/ins/sbgecom/sbgecom.cpp b/src/drivers/ins/sbgecom/sbgecom.cpp new file mode 100644 index 000000000000..60f8c05a34a3 --- /dev/null +++ b/src/drivers/ins/sbgecom/sbgecom.cpp @@ -0,0 +1,1094 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2022 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.cpp + * Driver for the SBG Systems products + * + * @author SBG Systems + */ + +#include "sbgecom.hpp" + +#include +#include + +#include +#include + +#define DEFAULT_DEVNAME "/dev/ttyS0" + +#define LSB_DELTA_VELOCITY 1048576.0f +#define LSB_DELTA_ANGLE 67108864.0f +#define LSB_TEMPERATURE 256.0f + +#define SBG_MODE_SENSOR 0 +#define SBG_MODE_GNSS 1 +#define SBG_MODE_INS 2 + +#define SBG_NAV_SOLUTION_MODE_MASK (0xF) + +#define SBG_ESTIMATOR_ATTITUDE (1 << 0) ///< 0 - attitude estimate is good +#define SBG_ESTIMATOR_VELOCITY_HORIZ (1 << 1) ///< 1 - horizontal velocity estimate is good +#define SBG_ESTIMATOR_VELOCITY_VERT (1 << 2) ///< 2 - vertical velocity estimate is good +#define SBG_ESTIMATOR_POS_HORIZ_REL (1 << 3) ///< 3 - horizontal position (relative) estimate is good +#define SBG_ESTIMATOR_POS_HORIZ_ABS (1 << 4) ///< 4 - horizontal position (absolute) estimate is good +#define SBG_ESTIMATOR_POS_VERT_ABS (1 << 5) ///< 5 - vertical position (absolute) estimate is good +#define SBG_ESTIMATOR_POS_VERT_AGL (1 << 6) ///< 6 - vertical position (above ground) estimate is good +#define SBG_ESTIMATOR_CONST_POS_MODE (1 << 7) ///< 7 - EKF is in a constant position mode and is not using external measurements (eg GPS or optical flow) +#define SBG_ESTIMATOR_PRED_POS_HORIZ_REL (1 << 8) ///< 8 - EKF has sufficient data to enter a mode that will provide a (relative) position estimate +#define SBG_ESTIMATOR_PRED_POS_HORIZ_ABS (1 << 9) ///< 9 - EKF has sufficient data to enter a mode that will provide a (absolute) position estimate +#define SBG_ESTIMATOR_GPS_GLITCH (1 << 10) ///< 10 - EKF has detected a GPS glitch +#define SBG_ESTIMATOR_ACCEL_ERROR (1 << 11) ///< 11 - EKF has detected bad accelerometer data + +#define DEFAULT_CONFIG_PATH "/etc/extras/sbg_settings.json" +#define OVERRIDE_CONFIG_PATH CONFIG_BOARD_ROOT_PATH DEFAULT_CONFIG_PATH +#define NEED_REBOOT_STR "\"needReboot\":true" + +using matrix::Vector2f; + +SbgEcom::SbgEcom(const char *device_name, uint32_t baudrate, const char *config_file, const char *config_string): + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(device_name)), + _baudrate(baudrate), + _config_file(config_file), + _config_string(config_string) +{ + if (device_name) { + strncpy(_device_name, device_name, sizeof(_device_name) - 1); + _device_name[sizeof(_device_name) - 1] = '\0'; + } + + device::Device::DeviceId device_id{}; + device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL; + device_id.devid_s.devtype = DRV_INS_DEVTYPE_SBG; + + set_device_id(device_id.devid); + _px4_accel.set_device_id(device_id.devid); + _px4_gyro.set_device_id(device_id.devid); + _px4_mag.set_device_id(device_id.devid); +} + +SbgEcom::~SbgEcom() +{ + sbgEComClose(&_com_handle); + sbgInterfaceDestroy(&_sbg_interface); + + perf_free(_accel_pub_interval_perf); + perf_free(_gyro_pub_interval_perf); + perf_free(_mag_pub_interval_perf); + perf_free(_gnss_pub_interval_perf); + + perf_free(_attitude_pub_interval_perf); + perf_free(_local_position_pub_interval_perf); + perf_free(_global_position_pub_interval_perf); +} + +void SbgEcom::set_device_id(uint32_t device_id) +{ + _device_id = device_id; +} + +uint32_t SbgEcom::get_device_id() +{ + return _device_id; +} + +SbgErrorCode SbgEcom::getAndPrintProductInfo(SbgEComHandle *handle) +{ + SbgErrorCode error_code; + SbgEComDeviceInfo device_info; + + assert(handle); + + error_code = sbgEComCmdGetInfo(handle, &device_info); + + if (error_code == SBG_NO_ERROR) { + char calib_version_str[32]; + char hw_revision_str[32]; + char fmw_version_str[32]; + + sbgVersionToStringEncoded(device_info.calibationRev, calib_version_str, sizeof(calib_version_str)); + sbgVersionToStringEncoded(device_info.hardwareRev, hw_revision_str, sizeof(hw_revision_str)); + sbgVersionToStringEncoded(device_info.firmwareRev, fmw_version_str, sizeof(fmw_version_str)); + + PX4_INFO(" Serial Number: %09" PRIu32, device_info.serialNumber); + PX4_INFO(" Product Code: %s", device_info.productCode); + PX4_INFO(" Hardware Revision: %s", hw_revision_str); + PX4_INFO(" Firmware Version: %s", fmw_version_str); + PX4_INFO(" Calib. Version: %s", calib_version_str); + + } else { + SBG_LOG_WARNING(error_code, "Unable to retrieve device information"); + } + + return error_code; +} + +void SbgEcom::printLogCallBack(const char *file_name, const char *function_name, uint32_t line, const char *category, + SbgDebugLogType log_type, SbgErrorCode error_code, const char *message) +{ + const char *base_name; + + assert(file_name); + assert(function_name); + assert(category); + assert(message); + + base_name = strrchr(file_name, '/'); + + if (!base_name) { + base_name = file_name; + + } else { + base_name++; + } + + switch (log_type) { + case SBG_DEBUG_LOG_TYPE_DEBUG: + PX4_DEBUG("%s:%" PRIu32 ": %s: %s", base_name, line, function_name, message); + break; + + case SBG_DEBUG_LOG_TYPE_INFO: + PX4_INFO("%s:%" PRIu32 ": %s: %s", base_name, line, function_name, message); + break; + + case SBG_DEBUG_LOG_TYPE_WARNING: + PX4_WARN("%s:%" PRIu32 ": %s: err:%s: %s", base_name, line, function_name, sbgErrorCodeToString(error_code), message); + break; + + case SBG_DEBUG_LOG_TYPE_ERROR: + PX4_ERR("%s:%" PRIu32 ": %s: err:%s: %s", base_name, line, function_name, sbgErrorCodeToString(error_code), message); + break; + } +} + +void SbgEcom::handleLogImuShort(const SbgEComLogUnion *ref_sbg_data, void *user_arg) +{ + assert(ref_sbg_data); + assert(user_arg); + + SbgEcom *instance = static_cast(user_arg); + + const float temperature = ref_sbg_data->imuShort.temperature / LSB_TEMPERATURE; + + // publish sensor_accel + instance->_px4_accel.update(ref_sbg_data->imuShort.timeStamp, + (ref_sbg_data->imuShort.deltaVelocity[0] / LSB_DELTA_VELOCITY), + (ref_sbg_data->imuShort.deltaVelocity[1] / LSB_DELTA_VELOCITY), + (ref_sbg_data->imuShort.deltaVelocity[2] / LSB_DELTA_VELOCITY)); + instance->_px4_accel.set_error_count(perf_event_count(instance->_comms_errors)); + instance->_px4_accel.set_temperature(temperature); + perf_count(instance->_accel_pub_interval_perf); + + // publish sensor_gyro + instance->_px4_gyro.update(ref_sbg_data->imuShort.timeStamp, + (ref_sbg_data->imuShort.deltaAngle[0] / LSB_DELTA_ANGLE), + (ref_sbg_data->imuShort.deltaAngle[1] / LSB_DELTA_ANGLE), + (ref_sbg_data->imuShort.deltaAngle[2] / LSB_DELTA_ANGLE)); + instance->_px4_gyro.set_error_count(perf_event_count(instance->_comms_errors)); + instance->_px4_gyro.set_temperature(temperature); + perf_count(instance->_gyro_pub_interval_perf); +} + +void SbgEcom::handleLogMag(const SbgEComLogUnion *ref_sbg_data, void *user_arg) +{ + assert(ref_sbg_data); + assert(user_arg); + + SbgEcom *instance = static_cast(user_arg); + + // publish sensor_mag + instance->_px4_mag.update(ref_sbg_data->magData.timeStamp, + (ref_sbg_data->magData.magnetometers[0]), + (ref_sbg_data->magData.magnetometers[1]), + (ref_sbg_data->magData.magnetometers[2])); + instance->_px4_mag.set_error_count(perf_event_count(instance->_comms_errors)); + perf_count(instance->_mag_pub_interval_perf); +} + +void SbgEcom::updateEstimatorStatus(uint32_t ekf_status, estimator_status_s *estimator_status) +{ + SbgEComSolutionMode ekf_nav_status = (SbgEComSolutionMode)(ekf_status & SBG_NAV_SOLUTION_MODE_MASK); + + estimator_status->solution_status_flags |= ((ekf_status & SBG_ECOM_SOL_ATTITUDE_VALID) + && (ekf_status & SBG_ECOM_SOL_HEADING_VALID)) ? SBG_ESTIMATOR_ATTITUDE : 0; + estimator_status->solution_status_flags |= (ekf_status & SBG_ECOM_SOL_VELOCITY_VALID) ? SBG_ESTIMATOR_VELOCITY_HORIZ : + 0; + estimator_status->solution_status_flags |= (ekf_status & SBG_ECOM_SOL_VELOCITY_VALID) ? SBG_ESTIMATOR_VELOCITY_VERT : 0; + estimator_status->solution_status_flags |= (ekf_status & SBG_ECOM_SOL_POSITION_VALID) ? SBG_ESTIMATOR_POS_HORIZ_REL : 0; + estimator_status->solution_status_flags |= (ekf_status & SBG_ECOM_SOL_POSITION_VALID) ? SBG_ESTIMATOR_POS_HORIZ_ABS : 0; + estimator_status->solution_status_flags |= (ekf_status & SBG_ECOM_SOL_POSITION_VALID) ? SBG_ESTIMATOR_POS_VERT_ABS : 0; + estimator_status->solution_status_flags |= (ekf_status & SBG_ECOM_SOL_POSITION_VALID) ? SBG_ESTIMATOR_POS_VERT_AGL : 0; + estimator_status->solution_status_flags |= (ekf_status & SBG_ECOM_SOL_ZUPT_USED) ? SBG_ESTIMATOR_CONST_POS_MODE : 0; + + estimator_status->solution_status_flags |= (ekf_nav_status == SBG_ECOM_SOL_MODE_NAV_POSITION) ? + SBG_ESTIMATOR_PRED_POS_HORIZ_REL : 0; + estimator_status->solution_status_flags |= (ekf_nav_status == SBG_ECOM_SOL_MODE_NAV_POSITION) ? + SBG_ESTIMATOR_PRED_POS_HORIZ_ABS : 0; +} + +void SbgEcom::handleLogEkfQuat(const SbgEComLogUnion *ref_sbg_data, void *user_arg) +{ + const hrt_abstime time_now_us = hrt_absolute_time(); + + assert(ref_sbg_data); + assert(user_arg); + + SbgEcom *instance = static_cast(user_arg); + + // publish estimator_status + estimator_status_s estimator_status{}; + estimator_status.timestamp = time_now_us; + estimator_status.timestamp_sample = ref_sbg_data->ekfQuatData.timeStamp; + + instance->updateEstimatorStatus(ref_sbg_data->ekfQuatData.status, &estimator_status); + + instance->_estimator_status_pub.publish(estimator_status); + + // publish attitude + vehicle_attitude_s attitude{}; + + attitude.timestamp = time_now_us; + attitude.timestamp_sample = ref_sbg_data->ekfQuatData.timeStamp; + + attitude.q[0] = ref_sbg_data->ekfQuatData.quaternion[0]; + attitude.q[1] = ref_sbg_data->ekfQuatData.quaternion[1]; + attitude.q[2] = ref_sbg_data->ekfQuatData.quaternion[2]; + attitude.q[3] = ref_sbg_data->ekfQuatData.quaternion[3]; + + instance->_attitude_pub.publish(attitude); + perf_count(instance->_attitude_pub_interval_perf); +} + +void SbgEcom::handleLogEkfNav(const SbgEComLogUnion *ref_sbg_data, void *user_arg) +{ + const hrt_abstime time_now_us = hrt_absolute_time(); + + assert(ref_sbg_data); + assert(user_arg); + + SbgEcom *instance = static_cast(user_arg); + + // publish estimator_status + estimator_status_s estimator_status{}; + estimator_status.timestamp = time_now_us; + estimator_status.timestamp_sample = ref_sbg_data->ekfNavData.timeStamp; + + instance->updateEstimatorStatus(ref_sbg_data->ekfNavData.status, &estimator_status); + + instance->_estimator_status_pub.publish(estimator_status); + + const double latitude = ref_sbg_data->ekfNavData.position[0]; + const double longitude = ref_sbg_data->ekfNavData.position[1]; + const double altitude = ref_sbg_data->ekfNavData.position[2]; + + const double north_velocity = ref_sbg_data->ekfNavData.velocity[0]; + const double east_velocity = ref_sbg_data->ekfNavData.velocity[1]; + const double down_velocity = ref_sbg_data->ekfNavData.velocity[2]; + + if (!instance->_pos_ref.isInitialized()) { + instance->_pos_ref.initReference(latitude, longitude, time_now_us); + instance->_gps_alt_ref = altitude; + } + + const Vector2f pos_ned = instance->_pos_ref.project(latitude, longitude); + + // publish local_position + vehicle_local_position_s local_position{}; + + local_position.timestamp = time_now_us; + local_position.timestamp_sample = ref_sbg_data->ekfNavData.timeStamp; + + local_position.xy_valid = math::isFinite(latitude) && math::isFinite(longitude); + local_position.z_valid = math::isFinite(altitude); + local_position.v_xy_valid = math::isFinite(north_velocity) && math::isFinite(east_velocity); + local_position.v_z_valid = math::isFinite(down_velocity); + + local_position.x = pos_ned(0); + local_position.y = pos_ned(1); + local_position.z = -(altitude - instance->_gps_alt_ref); + + local_position.vx = north_velocity; + local_position.vy = east_velocity; + local_position.vz = down_velocity; + + local_position.unaided_heading = NAN; + + if (instance->_pos_ref.isInitialized()) { + local_position.xy_global = true; + local_position.z_global = true; + local_position.ref_timestamp = instance->_pos_ref.getProjectionReferenceTimestamp(); + local_position.ref_lat = instance->_pos_ref.getProjectionReferenceLat(); + local_position.ref_lon = instance->_pos_ref.getProjectionReferenceLon(); + local_position.ref_alt = instance->_gps_alt_ref; + } + + local_position.dist_bottom_valid = false; + + local_position.eph = sqrt(pow(ref_sbg_data->ekfNavData.positionStdDev[0], 2) + + pow(ref_sbg_data->ekfNavData.positionStdDev[1], 2)); + local_position.epv = ref_sbg_data->ekfNavData.positionStdDev[2]; + + local_position.dead_reckoning = false; + + local_position.vxy_max = INFINITY; + local_position.vz_max = INFINITY; + local_position.hagl_min = INFINITY; + local_position.hagl_max = INFINITY; + + instance->_local_position_pub.publish(local_position); + perf_count(instance->_local_position_pub_interval_perf); + + // publish global_position + vehicle_global_position_s global_position{}; + + global_position.timestamp = time_now_us; + global_position.timestamp_sample = ref_sbg_data->ekfNavData.timeStamp; + + global_position.lat = latitude; + global_position.lon = longitude; + global_position.alt = altitude; + global_position.alt_ellipsoid = ref_sbg_data->ekfNavData.undulation; + + global_position.eph = sqrt(pow(ref_sbg_data->ekfNavData.positionStdDev[0], 2) + + pow(ref_sbg_data->ekfNavData.positionStdDev[1], 2)); + global_position.epv = ref_sbg_data->ekfNavData.positionStdDev[2]; + + global_position.dead_reckoning = false; + + instance->_global_position_pub.publish(global_position); + perf_count(instance->_global_position_pub_interval_perf); +} + +void SbgEcom::handleLogGnssPosVelHdt(SbgEComMsgId msg, const SbgEComLogUnion *ref_sbg_data, void *user_arg) +{ + const hrt_abstime time_now_us = hrt_absolute_time(); + uint8_t type; + uint8_t state; + uint8_t spoofing; + + assert(msg); + assert(ref_sbg_data); + assert(user_arg); + + SbgEcom *instance = static_cast(user_arg); + GnssData *gnss_data = &instance->gnss_data; + + // Store the data based on its type + switch (msg) { + case SBG_ECOM_LOG_GPS1_POS: + gnss_data->gps_pos = ref_sbg_data->gpsPosData; + gnss_data->pos_received = true; + gnss_data->pos_timestamp = time_now_us; + break; + + case SBG_ECOM_LOG_GPS1_VEL: + gnss_data->gps_vel = ref_sbg_data->gpsVelData; + gnss_data->vel_received = true; + gnss_data->vel_timestamp = time_now_us; + break; + + case SBG_ECOM_LOG_GPS1_HDT: + gnss_data->gps_hdt = ref_sbg_data->gpsHdtData; + gnss_data->hdt_received = true; + gnss_data->hdt_timestamp = time_now_us; + break; + } + + if (gnss_data->pos_received && gnss_data->vel_received && gnss_data->hdt_received) { + // publish sensor_gps + sensor_gps_s sensor_gps{}; + + sensor_gps.timestamp = time_now_us; + sensor_gps.timestamp_sample = gnss_data->gps_pos.timeStamp; + + sensor_gps.device_id = instance->get_device_id(); + + sensor_gps.latitude_deg = gnss_data->gps_pos.latitude; + sensor_gps.longitude_deg = gnss_data->gps_pos.longitude; + sensor_gps.altitude_msl_m = gnss_data->gps_pos.altitude; + sensor_gps.altitude_ellipsoid_m = gnss_data->gps_pos.undulation; + + sensor_gps.s_variance_m_s = sqrt(pow(gnss_data->gps_vel.velocityAcc[0], 2) + + pow(gnss_data->gps_vel.velocityAcc[1], 2) + + pow(gnss_data->gps_vel.velocityAcc[2], 2)); + sensor_gps.c_variance_rad = math::radians(gnss_data->gps_vel.courseAcc); + + type = sbgEComLogGnssPosGetType(&gnss_data->gps_pos); + + switch (type) { + case SBG_ECOM_GNSS_POS_TYPE_NO_SOLUTION: + sensor_gps.fix_type = 0; + break; + + case SBG_ECOM_GNSS_POS_TYPE_PSRDIFF: + case SBG_ECOM_GNSS_POS_TYPE_SBAS: + sensor_gps.fix_type = 4; + break; + + case SBG_ECOM_GNSS_POS_TYPE_RTK_FLOAT: + sensor_gps.fix_type = 5; + break; + + case SBG_ECOM_GNSS_POS_TYPE_RTK_INT: + sensor_gps.fix_type = 6; + break; + + case SBG_ECOM_GNSS_POS_TYPE_FIXED: + sensor_gps.fix_type = 7; + break; + + case SBG_ECOM_GNSS_POS_TYPE_PPP_FLOAT: + case SBG_ECOM_GNSS_POS_TYPE_PPP_INT: + sensor_gps.fix_type = 8; + break; + + default: + sensor_gps.fix_type = 3; + break; + } + + sensor_gps.eph = sqrt(pow(gnss_data->gps_pos.longitudeAccuracy, 2) + + pow(gnss_data->gps_pos.latitudeAccuracy, 2)); + sensor_gps.epv = gnss_data->gps_pos.altitudeAccuracy; + + state = sbgEComLogGnssPosGetIfmStatus(&gnss_data->gps_pos); + + switch (state) { + case SBG_ECOM_GNSS_IFM_STATUS_UNKNOWN: + sensor_gps.jamming_state = 0; + break; + + case SBG_ECOM_GNSS_IFM_STATUS_CLEAN: + sensor_gps.jamming_state = 1; + break; + + case SBG_ECOM_GNSS_IFM_STATUS_MITIGATED: + sensor_gps.jamming_state = 2; + break; + + case SBG_ECOM_GNSS_IFM_STATUS_CRITICAL: + sensor_gps.jamming_state = 3; + break; + } + + spoofing = sbgEComLogGnssPosGetSpoofingStatus(&gnss_data->gps_pos); + + switch (spoofing) { + case SBG_ECOM_GNSS_SPOOFING_STATUS_UNKNOWN: + sensor_gps.spoofing_state = 0; + break; + + case SBG_ECOM_GNSS_SPOOFING_STATUS_CLEAN: + sensor_gps.spoofing_state = 1; + break; + + case SBG_ECOM_GNSS_SPOOFING_STATUS_SINGLE: + sensor_gps.spoofing_state = 2; + break; + + case SBG_ECOM_GNSS_SPOOFING_STATUS_MULTIPLE: + sensor_gps.spoofing_state = 3; + break; + } + + sensor_gps.vel_m_s = sqrt(pow(gnss_data->gps_vel.velocity[0], 2) + + pow(gnss_data->gps_vel.velocity[1], 2) + + pow(gnss_data->gps_vel.velocity[2], 2)); + sensor_gps.vel_n_m_s = gnss_data->gps_vel.velocity[0]; + sensor_gps.vel_e_m_s = gnss_data->gps_vel.velocity[1]; + sensor_gps.vel_d_m_s = gnss_data->gps_vel.velocity[2]; + sensor_gps.vel_ned_valid = true; + + sensor_gps.cog_rad = math::radians(gnss_data->gps_vel.course); + + sensor_gps.timestamp_time_relative = sensor_gps.timestamp_sample - time_now_us; + sensor_gps.time_utc_usec = 0; + + sensor_gps.satellites_used = gnss_data->gps_pos.numSvUsed; + + sensor_gps.heading = math::radians(gnss_data->gps_hdt.heading); + sensor_gps.heading_offset = math::radians(gnss_data->gps_hdt.pitch); + sensor_gps.heading_accuracy = math::radians(gnss_data->gps_hdt.headingAccuracy); + + // Check timestamp synchronization + const hrt_abstime max_time_diff = 1000000; // Maximum allowed time difference in microseconds (e.g., 1 second) + hrt_abstime pos_time = gnss_data->pos_timestamp; + hrt_abstime vel_time = gnss_data->vel_timestamp; + hrt_abstime hdt_time = gnss_data->hdt_timestamp; + + if ((hrt_absolute_time() - pos_time < max_time_diff) && + (hrt_absolute_time() - vel_time < max_time_diff) && + (hrt_absolute_time() - hdt_time < max_time_diff) && + (std::abs(pos_time - vel_time) < max_time_diff) && + (std::abs(pos_time - hdt_time) < max_time_diff) && + (std::abs(vel_time - hdt_time) < max_time_diff)) { + instance->_sensor_gps_pub.publish(sensor_gps); + perf_count(instance->_gnss_pub_interval_perf); + } + + // Reset the flags and timestamps + gnss_data->pos_received = false; + gnss_data->vel_received = false; + gnss_data->hdt_received = false; + + gnss_data->pos_timestamp = 0; + gnss_data->vel_timestamp = 0; + gnss_data->hdt_timestamp = 0; + } +} + +SbgErrorCode SbgEcom::onLogReceived(SbgEComHandle *handle, SbgEComClass msg_class, SbgEComMsgId msg, + const SbgEComLogUnion *ref_sbg_data, void *user_arg) +{ + SBG_UNUSED_PARAMETER(handle); + + assert(msg_class); + assert(msg); + assert(ref_sbg_data); + assert(user_arg); + + SbgEcom *instance = static_cast(user_arg); + + if (msg_class == SBG_ECOM_CLASS_LOG_ECOM_0) { + int32_t mode; + int32_t ekf2_en; + param_get(param_find("SBG_MODE"), &mode); + param_get(param_find("EKF2_EN"), &ekf2_en); + + bool ekf_failure = (ekf2_en && mode == SBG_MODE_INS); + + if (!instance->_ekf_failure && ekf_failure) { + PX4_WARN("Both SBG EKF and EKF2 are configured, this can lead to an unexpected behaviour"); + instance->_ekf_failure = true; + + } else if (instance->_ekf_failure && !ekf_failure) { + PX4_INFO("EKF management is back to good"); + instance->_ekf_failure = false; + } + + switch (msg) { + case SBG_ECOM_LOG_IMU_SHORT: + instance->handleLogImuShort(ref_sbg_data, user_arg); + break; + + case SBG_ECOM_LOG_MAG: + instance->handleLogMag(ref_sbg_data, user_arg); + break; + + case SBG_ECOM_LOG_GPS1_POS: + case SBG_ECOM_LOG_GPS1_VEL: + case SBG_ECOM_LOG_GPS1_HDT: + if (mode == SBG_MODE_GNSS || mode == SBG_MODE_INS) { + instance->handleLogGnssPosVelHdt(msg, ref_sbg_data, user_arg); + } + + break; + + case SBG_ECOM_LOG_EKF_QUAT: + if (mode == SBG_MODE_INS) { + instance->handleLogEkfQuat(ref_sbg_data, user_arg); + } + + break; + + case SBG_ECOM_LOG_EKF_NAV: + if (mode == SBG_MODE_INS) { + instance->handleLogEkfNav(ref_sbg_data, user_arg); + } + + break; + + default: + PX4_DEBUG("Received unknown SBG message (class %u, id %u)", msg_class, msg); + break; + } + + } else { + PX4_INFO("Received message from unsupported SBGEcom class %u", msg_class); + } + + return SBG_NO_ERROR; +} + +SbgErrorCode SbgEcom::handleOneLog(SbgEComHandle *handle) +{ + SbgErrorCode error_code; + SbgEComProtocolPayload payload; + uint8_t received_msg; + uint8_t received_msg_class; + + assert(handle); + + sbgEComProtocolPayloadConstruct(&payload); + + perf_begin(_sample_perf); + + error_code = sbgEComProtocolReceive2(&handle->protocolHandle, &received_msg_class, &received_msg, &payload); + + if (error_code == SBG_NO_ERROR) { + if (sbgEComMsgClassIsALog((SbgEComClass)received_msg_class)) { + error_code = sbgEComLogParse((SbgEComClass)received_msg_class, (SbgEComMsgId)received_msg, + sbgEComProtocolPayloadGetBuffer(&payload), sbgEComProtocolPayloadGetSize(&payload), &_log_data); + + if (error_code == SBG_NO_ERROR) { + if (handle->pReceiveLogCallback) { + error_code = handle->pReceiveLogCallback(handle, (SbgEComClass)received_msg_class, (SbgEComMsgId)received_msg, + &_log_data, handle->pUserArg); + } + + sbgEComLogCleanup(&_log_data, (SbgEComClass)received_msg_class, (SbgEComMsgId)received_msg); + + perf_end(_sample_perf); + + } else { + perf_count(_comms_errors); + } + + } else { + PX4_ERR("command received %d", error_code); + } + + } else if (error_code != SBG_NOT_READY) { + PX4_WARN("Invalid frame received %d", error_code); + perf_count(_comms_errors); + } + + sbgEComProtocolPayloadDestroy(&payload); + + return error_code; +} + +SbgErrorCode SbgEcom::sendAirDataLog(SbgEComHandle *handle, SbgEcom *instance) +{ + SbgErrorCode error_code = SBG_NO_ERROR; + SbgEComLogAirData air_data_log; + uint8_t output_buffer[64]; + SbgStreamBuffer output_stream; + + assert(handle); + assert(instance); + + vehicle_air_data_s air_data{}; + + if (instance->_air_data_sub.update(&air_data)) { + memset(&air_data_log, 0x00, sizeof(air_data_log)); + + air_data_log.timeStamp = air_data.timestamp_sample; + air_data_log.status |= SBG_ECOM_AIR_DATA_TIME_IS_DELAY; + + air_data_log.pressureAbs = air_data.baro_pressure_pa; + air_data_log.status |= SBG_ECOM_AIR_DATA_PRESSURE_ABS_VALID; + + air_data_log.altitude = air_data.baro_alt_meter; + air_data_log.status |= SBG_ECOM_AIR_DATA_ALTITUDE_VALID; + + air_data_log.airTemperature = air_data.baro_temp_celcius; + air_data_log.status |= SBG_ECOM_AIR_DATA_TEMPERATURE_VALID; + + differential_pressure_s differential_pressure{}; + + if (instance->_diff_pressure_sub.update(&differential_pressure)) { + air_data_log.pressureDiff = differential_pressure.differential_pressure_pa; + air_data_log.status |= SBG_ECOM_AIR_DATA_PRESSURE_DIFF_VALID; + } + + airspeed_s airspeed{}; + + if (instance->_airspeed_sub.update(&airspeed)) { + air_data_log.trueAirspeed = airspeed.true_airspeed_m_s; + air_data_log.status |= SBG_ECOM_AIR_DATA_AIRPSEED_VALID; + } + + sbgStreamBufferInitForWrite(&output_stream, output_buffer, sizeof(output_buffer)); + + perf_begin(_write_perf); + + error_code = sbgEComLogAirDataWriteToStream(&air_data_log, &output_stream); + + if (error_code == SBG_NO_ERROR) { + error_code = sbgEComProtocolSend(&handle->protocolHandle, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_AIR_DATA, + sbgStreamBufferGetLinkedBuffer(&output_stream), sbgStreamBufferGetLength(&output_stream)); + + if (error_code != SBG_NO_ERROR) { + PX4_ERR("Unable to send the AirData log %d", error_code); + + } else { + perf_end(_write_perf); + } + + } else { + PX4_ERR("Unable to write the AirData payload. %d", error_code); + } + } + + return error_code; +} + +SbgErrorCode SbgEcom::sendMagLog(SbgEComHandle *handle, SbgEcom *instance) +{ + SbgErrorCode error_code = SBG_NO_ERROR; + SbgEComLogMag mag_log; + uint8_t output_buffer[64]; + SbgStreamBuffer output_stream; + + assert(handle); + assert(instance); + + vehicle_magnetometer_s mag{}; + + if (instance->_mag_sub.update(&mag)) { + memset(&mag_log, 0x00, sizeof(mag_log)); + + mag_log.timeStamp = mag.timestamp_sample; + // mag_log.status = 0; // STO: don't know how to set it + + mag_log.magnetometers[0] = mag.magnetometer_ga[0]; + mag_log.magnetometers[1] = mag.magnetometer_ga[1]; + mag_log.magnetometers[2] = mag.magnetometer_ga[2]; + + sbgStreamBufferInitForWrite(&output_stream, output_buffer, sizeof(output_buffer)); + + perf_begin(_write_perf); + + error_code = sbgEComLogMagWriteToStream(&mag_log, &output_stream); + + if (error_code == SBG_NO_ERROR) { + error_code = sbgEComProtocolSend(&handle->protocolHandle, SBG_ECOM_CLASS_LOG_ECOM_0, SBG_ECOM_LOG_MAG, + sbgStreamBufferGetLinkedBuffer(&output_stream), sbgStreamBufferGetLength(&output_stream)); + + if (error_code != SBG_NO_ERROR) { + PX4_ERR("Unable to send the Mag log %d", error_code); + + } else { + perf_end(_write_perf); + } + + } else { + PX4_ERR("Unable to write the Mag payload. %d", error_code); + } + } + + return error_code; +} + +void SbgEcom::send_config(SbgEComHandle *pHandle, const char *config) +{ + SbgEComCmdApiReply reply; + + assert(pHandle); + assert(config); + + sbgEComCmdApiReplyConstruct(&reply); + + sbgEComCmdApiPost(pHandle, "/api/v1/settings", NULL, config, &reply); + + if (!sbgEComCmdApiReplySuccessful(&reply)) { + PX4_ERR("Fail to apply SBG configuration: %s", reply.pContent); + + } else { + bool need_reboot = (strstr(reply.pContent, NEED_REBOOT_STR) != NULL); + sbgEComCmdApiPost(pHandle, "/api/v1/settings/save", NULL, NULL, &reply); + + if (need_reboot) { + PX4_INFO("Reboot SBG device"); + sbgEComCmdApiPost(pHandle, "/api/v1/system/reboot", NULL, NULL, &reply); + } + } + + sbgEComCmdApiReplyDestroy(&reply); +} + +void SbgEcom::send_config_file(SbgEComHandle *pHandle, const char *file_path) +{ + int fd; + char *body = NULL; + struct stat s; + + assert(pHandle); + assert(file_path); + + fd = open(file_path, O_RDONLY); + + if (fd == -1) { + PX4_ERR("Failed to open config"); + return; + } + + fstat(fd, &s); + body = (char *)malloc(s.st_size + 1); + + if (!body) { + PX4_ERR("Failed to allocate memory (%ld) - %s", s.st_size + 1, strerror(get_errno())); + close(fd); + return; + } + + read(fd, body, s.st_size); + body[s.st_size] = '\0'; + + send_config(pHandle, body); + + free(body); + + if (close(fd) == -1) { + perror("Error closing the file"); + return; + } +} + +int SbgEcom::init() +{ + SbgErrorCode error_code; + + error_code = sbgInterfaceSerialCreate(&_sbg_interface, _device_name, _baudrate); + + if (error_code == SBG_NO_ERROR) { + PX4_INFO("Serial interface created successfully on port: %s, baudrate: %ld", _device_name, _baudrate); + } + + error_code = sbgEComInit(&_com_handle, &_sbg_interface); + // Increase sbgECom timeout for the initialization + sbgEComSetCmdTrialsAndTimeOut(&_com_handle, 3, 5000); + + if (error_code == SBG_NO_ERROR) { + int32_t ins_config_enable; + param_get(param_find("SBG_CONFIGURE_EN"), &ins_config_enable); + + getAndPrintProductInfo(&_com_handle); + + if (ins_config_enable) { + if (_config_string != nullptr) { + PX4_INFO("Apply config string instead of config file"); + send_config(&_com_handle, _config_string); + + } else { + send_config_file(&_com_handle, _config_file); + } + } + + // Reset sbgECom timeout to its defaut value + sbgEComSetCmdTrialsAndTimeOut(&_com_handle, 3, SBG_ECOM_DEFAULT_CMD_TIME_OUT); + sbgEComSetReceiveLogCallback(&_com_handle, onLogReceived, this); + return PX4_OK; + + } else { + PX4_ERR("sbgECom initialization failed (%d)", error_code); + return PX4_ERROR; + } +} + +void SbgEcom::Run() +{ + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + if (!_initialized) { + init_result = init(); + _initialized = true; + } + + if (init_result == PX4_OK) { + SbgErrorCode error_code; + + error_code = handleOneLog(&_com_handle); + + if (error_code == SBG_NO_ERROR) { + ScheduleDelayed(time_literals::operator ""_ms(0)); + + if (failure) { + assert(iteration_count >= 0); + iteration_count--; + failure = false; + } + + error_code = sendAirDataLog(&_com_handle, this); + + if (error_code == SBG_NO_ERROR) { + PX4_DEBUG("Airdata log sent!"); + + } else { + PX4_WARN("Unable to send AirData log %d", error_code); + error_code = SBG_NO_ERROR; + } + + error_code = sendMagLog(&_com_handle, this); + + if (error_code == SBG_NO_ERROR) { + PX4_DEBUG("Mag log sent!"); + + } else { + PX4_WARN("Unable to send Mag log %d", error_code); + error_code = SBG_NO_ERROR; + } + + } else if (error_code != SBG_NOT_READY) { + PX4_ERR("Unable to process incoming sbgECom logs %d", error_code); + } + + if (error_code != SBG_NO_ERROR) { + ScheduleDelayed(time_literals::operator ""_ms(1)); + failure = true; + } + } +} + +int SbgEcom::task_spawn(int argc, char **argv) +{ + sbgCommonLibSetLogCallback(printLogCallBack); + + bool error_flag = false; + + const char *myoptarg = nullptr; + int myoptind = 1; + int ch; + + int32_t baudrate; + param_get(param_find("SBG_BAUDRATE"), &baudrate); + const char *dev_name = DEFAULT_DEVNAME; + const char *config_file = DEFAULT_CONFIG_PATH; + + /* INS settings can be overwritten from the SD card */ + if (access(OVERRIDE_CONFIG_PATH, F_OK) == 0) { + config_file = OVERRIDE_CONFIG_PATH; + + } else { + config_file = DEFAULT_CONFIG_PATH; + } + + const char *config_string = nullptr; + + while ((ch = px4_getopt(argc, argv, "b:d:f:s:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'b': + baudrate = atoi(myoptarg); + break; + + case 'd': + dev_name = myoptarg; + break; + + case 'f': + config_file = myoptarg; + break; + + case 's': + config_string = myoptarg; + break; + + case '?': + PX4_WARN("unrecognized flag ?"); + error_flag = true; + break; + + default: + PX4_WARN("unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return -1; + } + + if (dev_name && (access(dev_name, R_OK | W_OK) == 0)) { + SbgEcom *instance = new SbgEcom(dev_name, baudrate, config_file, config_string); + + if (instance == nullptr) { + PX4_ERR("alloc failed"); + return PX4_ERROR; + } + + _task_id = task_id_is_work_queue; + _object.store(instance); + instance->ScheduleNow(); + return PX4_OK; + + } else { + if (dev_name) { + PX4_ERR("invalid device (-d) %s", dev_name); + + } else { + PX4_ERR("valid device required"); + } + } + + return PX4_ERROR; +} + +int SbgEcom::custom_command(int argc, char **argv) +{ + return print_usage("unrecognized command"); +} + +int SbgEcom::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION("Description du module"); + + PRINT_MODULE_USAGE_NAME("sbgecom", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("ins"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver"); + PRINT_MODULE_USAGE_PARAM_STRING('d', DEFAULT_DEVNAME, nullptr, "Serial device", true); + PRINT_MODULE_USAGE_PARAM_INT('b', 921600, 9600, 921600, "Baudrate device", true); + PRINT_MODULE_USAGE_PARAM_STRING('f', DEFAULT_CONFIG_PATH, nullptr, "Config JSON file path", true); + PRINT_MODULE_USAGE_PARAM_STRING('s', nullptr, nullptr, "Config JSON string", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Driver status"); + PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver"); + + return PX4_OK; +} + +int SbgEcom::print_status() +{ + printf("Using port '%s'\n", _device_name); + + perf_print_counter(_sample_perf); + perf_print_counter(_write_perf); + perf_print_counter(_comms_errors); + + return 0; +} + +extern "C" __EXPORT int sbgecom_main(int argc, char **argv) +{ + return SbgEcom::main(argc, argv); +} diff --git a/src/drivers/ins/sbgecom/sbgecom.hpp b/src/drivers/ins/sbgecom/sbgecom.hpp new file mode 100644 index 000000000000..ef19674e5d7a --- /dev/null +++ b/src/drivers/ins/sbgecom/sbgecom.hpp @@ -0,0 +1,293 @@ +/**************************************************************************** + * + * Copyright (c) 2012-2022 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 + */ + +#pragma once + +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class SbgEcom : public ModuleBase, 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; + + px4::atomic _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_pub{ORB_ID(sensor_gps)}; + uORB::PublicationMulti _attitude_pub{ORB_ID(vehicle_attitude)}; + uORB::PublicationMulti _local_position_pub{ORB_ID(vehicle_local_position)}; + uORB::PublicationMulti _global_position_pub{ORB_ID(vehicle_global_position)}; + uORB::Publication _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")}; +}; diff --git a/src/drivers/ins/sbgecom/sbgecom.patch b/src/drivers/ins/sbgecom/sbgecom.patch new file mode 100644 index 000000000000..67903e27f401 --- /dev/null +++ b/src/drivers/ins/sbgecom/sbgecom.patch @@ -0,0 +1,124 @@ +diff --git a/CMakeLists.txt b/CMakeLists.txt +index 42d1404..3355005 100644 +--- a/CMakeLists.txt ++++ b/CMakeLists.txt +@@ -36,6 +36,7 @@ if (USE_DEPRECATED_MACROS) + endif() + + add_library(${PROJECT_NAME} STATIC) ++add_dependencies(${PROJECT_NAME} prebuild_targets) + + file(GLOB_RECURSE COMMON_SRC ${PROJECT_SOURCE_DIR}/common/*.c) + file(GLOB_RECURSE ECOM_SRC ${PROJECT_SOURCE_DIR}/src/*.c) +@@ -194,3 +195,18 @@ install(DIRECTORY common/ + install(DIRECTORY src/ + DESTINATION include + FILES_MATCHING PATTERN "*.h") ++ ++target_compile_options(${PROJECT_NAME} ++ 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(${PROJECT_NAME} PUBLIC __NUTTX__) ++endif() +diff --git a/common/interfaces/sbgInterfaceSerialUnix.c b/common/interfaces/sbgInterfaceSerialUnix.c +index eb7796c..99c52e0 100644 +--- a/common/interfaces/sbgInterfaceSerialUnix.c ++++ b/common/interfaces/sbgInterfaceSerialUnix.c +@@ -6,10 +6,17 @@ + #include + #include + #include ++#include + + // sbgCommonLib headers + #include + #include ++#include ++ ++#ifdef __PX4_NUTTX ++#include ++#include ++#endif + + //----------------------------------------------------------------------// + //- Definitions -// +@@ -447,19 +454,21 @@ SbgErrorCode sbgInterfaceSerialCreate(SbgInterface *pInterface, const char *devi + // Define com port options + // + options.c_cflag |= (CLOCAL | CREAD); // Enable the receiver and set local mode... +- options.c_cflag &= ~(PARENB|CSTOPB|CSIZE); // No parity, 1 stop bit, mask character size bits ++ options.c_cflag &= ~(PARENB | CSTOPB); // No parity, 1 stop bit, mask character size bits ++ options.c_cflag &= CSIZE; + options.c_cflag |= CS8; // Select 8 data bits + options.c_cflag &= ~CRTSCTS; // Disable Hardware flow control + + // + // Disable software flow control + // +- options.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|IGNCR|ICRNL|IXON); ++ options.c_iflag &= ~(IXON | IXOFF | IXANY); // Disable software flow control ++ options.c_iflag &= ~(IGNBRK|BRKINT|PARMRK|ISTRIP|INLCR|ICRNL|INPCK); // Disable any special handling of received bytes + + // + // We would like raw input + // +- options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG /*| IEXTEN | ECHONL*/); ++ options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG | ECHONL | IEXTEN); + options.c_oflag &= ~OPOST; + + // +diff --git a/common/platform/sbgPlatform.c b/common/platform/sbgPlatform.c +index 95a50ac..0c9f358 100644 +--- a/common/platform/sbgPlatform.c ++++ b/common/platform/sbgPlatform.c +@@ -119,28 +119,4 @@ SBG_COMMON_LIB_API void sbgPlatformDebugLogMsg(const char *pFileName, const char + { + gLogCallback(pFileName, pFunctionName, line, pCategory, logType, errorCode, errorMsg); + } +- else +- { +- // +- // Log the correct message according to the log type +- // +- switch (logType) +- { +- case SBG_DEBUG_LOG_TYPE_ERROR: +- fprintf(stderr, "*ERR * %s(%"PRIu32"): %s - %s\n\r", pFunctionName, line, sbgErrorCodeToString(errorCode), errorMsg); +- break; +- case SBG_DEBUG_LOG_TYPE_WARNING: +- fprintf(stderr, "*WARN* %s(%"PRIu32"): %s - %s\n\r", pFunctionName, line, sbgErrorCodeToString(errorCode), errorMsg); +- break; +- case SBG_DEBUG_LOG_TYPE_INFO: +- fprintf(stderr, "*INFO* %s(%"PRIu32"): %s\n\r", pFunctionName, line, errorMsg); +- break; +- case SBG_DEBUG_LOG_TYPE_DEBUG: +- fprintf(stderr, "*DBG * %s(%"PRIu32"): %s\n\r", pFunctionName, line, errorMsg); +- break; +- default: +- fprintf(stderr, "*UKNW* %s(%"PRIu32"): %s\n\r", pFunctionName, line, errorMsg); +- break; +- } +- } + } +diff --git a/common/sbgCommon.h b/common/sbgCommon.h +index 4bef011..fd858d4 100644 +--- a/common/sbgCommon.h ++++ b/common/sbgCommon.h +@@ -103,7 +103,7 @@ extern "C" { + * Default: 1024 + */ + #ifndef SBG_CONFIG_LOG_MAX_SIZE +-#define SBG_CONFIG_LOG_MAX_SIZE ((size_t)(1024)) ++#define SBG_CONFIG_LOG_MAX_SIZE ((size_t)(128)) + #endif + + /*! From 07ce7d555f9d7f2cf40cf528b339d25628bc46f6 Mon Sep 17 00:00:00 2001 From: Samuel Toledano Date: Thu, 12 Sep 2024 18:21:49 +0200 Subject: [PATCH 2/5] Fix sensor airspeed simulator units --- .../simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp | 4 ++-- .../simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp | 1 + 2 files changed, 3 insertions(+), 2 deletions(-) diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp index 7b0d9556cadf..3d83167d388e 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp +++ b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp @@ -129,7 +129,7 @@ void SensorAirspeedSim::Run() device_id.devid_s.address = 0; device_id.devid_s.devtype = DRV_DIFF_PRESS_DEVTYPE_SIM; - const float alt_amsl = gpos.alt; + const float alt_amsl = gpos.alt / 1000.0f; // mm to m const float temperature_local = TEMPERATURE_MSL - LAPSE_RATE * alt_amsl; const float density_ratio = powf(TEMPERATURE_MSL / temperature_local, 4.256f); const float air_density = AIR_DENSITY_MSL / density_ratio; @@ -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); diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp index 64305dcccada..3aafc9ca938e 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.hpp +++ b/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] From 0cbe6b1b81b85cc65eb9dffd94eed40cb27f3f2f Mon Sep 17 00:00:00 2001 From: Samuel Toledano Date: Thu, 12 Sep 2024 18:22:12 +0200 Subject: [PATCH 3/5] Fix HIGHRES_IMU pressure unit --- src/modules/mavlink/streams/HIGHRES_IMU.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/streams/HIGHRES_IMU.hpp b/src/modules/mavlink/streams/HIGHRES_IMU.hpp index a6682213bf93..d4866923a050 100644 --- a/src/modules/mavlink/streams/HIGHRES_IMU.hpp +++ b/src/modules/mavlink/streams/HIGHRES_IMU.hpp @@ -190,8 +190,8 @@ class MavlinkStreamHighresIMU : public MavlinkStream 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.baro_temp_celcius; msg.fields_updated = fields_updated; From ffe24c40c618df7055a0c01e38293778720f9fea Mon Sep 17 00:00:00 2001 From: Samuel Toledano Date: Thu, 9 Jan 2025 09:54:06 +0100 Subject: [PATCH 4/5] Don't build all INS drivers to avoid overflowing FLASH memory region --- boards/cubepilot/cubeorangeplus/default.px4board | 1 - boards/px4/fmu-v6c/default.px4board | 1 - 2 files changed, 2 deletions(-) diff --git a/boards/cubepilot/cubeorangeplus/default.px4board b/boards/cubepilot/cubeorangeplus/default.px4board index e3fc787417c6..0d158e3d5cf7 100644 --- a/boards/cubepilot/cubeorangeplus/default.px4board +++ b/boards/cubepilot/cubeorangeplus/default.px4board @@ -22,7 +22,6 @@ CONFIG_DRIVERS_IMU_INVENSENSE_ICM20649=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM20948=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM45686=y -CONFIG_COMMON_INS=y CONFIG_DRIVERS_IRLOCK=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y diff --git a/boards/px4/fmu-v6c/default.px4board b/boards/px4/fmu-v6c/default.px4board index dbb913d9e1d5..0c3e86abceb2 100644 --- a/boards/px4/fmu-v6c/default.px4board +++ b/boards/px4/fmu-v6c/default.px4board @@ -22,7 +22,6 @@ CONFIG_DRIVERS_HEATER=y CONFIG_DRIVERS_IMU_BOSCH_BMI055=y CONFIG_DRIVERS_IMU_BOSCH_BMI088=y CONFIG_DRIVERS_IMU_INVENSENSE_ICM42688P=y -CONFIG_COMMON_INS=y CONFIG_COMMON_LIGHT=y CONFIG_COMMON_MAGNETOMETER=y CONFIG_COMMON_OPTICAL_FLOW=y From 44dc55419e8e11bd22527e0413a386dceb302512 Mon Sep 17 00:00:00 2001 From: Samuel Toledano Date: Fri, 10 Jan 2025 19:07:45 +0100 Subject: [PATCH 5/5] Allow HIGHRES_IMU to support 4 IMUs --- src/modules/mavlink/streams/HIGHRES_IMU.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/modules/mavlink/streams/HIGHRES_IMU.hpp b/src/modules/mavlink/streams/HIGHRES_IMU.hpp index d4866923a050..694edefb5779 100644 --- a/src/modules/mavlink/streams/HIGHRES_IMU.hpp +++ b/src/modules/mavlink/streams/HIGHRES_IMU.hpp @@ -63,7 +63,7 @@ class MavlinkStreamHighresIMU : public MavlinkStream private: explicit MavlinkStreamHighresIMU(Mavlink *mavlink) : MavlinkStream(mavlink) {} - uORB::SubscriptionMultiArray _vehicle_imu_subs{ORB_ID::vehicle_imu}; + uORB::SubscriptionMultiArray _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)};