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..cd7a5677c1ba --- /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.2.590-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..c32971e18302 --- /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 + 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 diff --git a/src/drivers/ins/sbgecom/sbgecom.cpp b/src/drivers/ins/sbgecom/sbgecom.cpp new file mode 100644 index 000000000000..8b6835433f7a --- /dev/null +++ b/src/drivers/ins/sbgecom/sbgecom.cpp @@ -0,0 +1,1127 @@ +/**************************************************************************** + * + * 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_DELTA_ANGLE_HIGH_RANGE 12304174.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(hrt_absolute_time(), + (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 + if ((ref_sbg_data->imuShort.status & SBG_ECOM_IMU_GYROS_USE_HIGH_SCALE) == 0) { + instance->_px4_gyro.update(hrt_absolute_time(), + (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)); + + } else { + instance->_px4_gyro.update(hrt_absolute_time(), + (ref_sbg_data->imuShort.deltaAngle[0] / LSB_DELTA_ANGLE_HIGH_RANGE), + (ref_sbg_data->imuShort.deltaAngle[1] / LSB_DELTA_ANGLE_HIGH_RANGE), + (ref_sbg_data->imuShort.deltaAngle[2] / LSB_DELTA_ANGLE_HIGH_RANGE)); + } + + 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; + estimator_status.accel_device_id = instance->get_device_id(); + estimator_status.gyro_device_id = instance->get_device_id(); + estimator_status.mag_device_id = instance->get_device_id(); + + 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); + + matrix::Quatf q{attitude.q}; + instance->_heading = matrix::Eulerf{q}.psi(); +} + +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); + + SbgEComSolutionMode ekf_nav_status = (SbgEComSolutionMode)(ref_sbg_data->ekfNavData.status & + SBG_NAV_SOLUTION_MODE_MASK); + + // don't publish local and global positions if not reliable + if (ekf_nav_status != SBG_ECOM_SOL_MODE_NAV_POSITION) { + return; + } + + 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.heading = instance->_heading; + local_position.heading_good_for_control = true;; + + 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.evh = sqrt(pow(ref_sbg_data->ekfNavData.velocityStdDev[0], 2) + + pow(ref_sbg_data->ekfNavData.velocityStdDev[1], 2)); + local_position.evv = ref_sbg_data->ekfNavData.velocityStdDev[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_xy = INFINITY; + local_position.hagl_max_z = 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.lat_lon_valid = math::isFinite(latitude) && math::isFinite(longitude); + global_position.alt_valid = math::isFinite(altitude); + + 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.ambient_temperature; + 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..81bf3382d18c --- /dev/null +++ b/src/drivers/ins/sbgecom/sbgecom.hpp @@ -0,0 +1,294 @@ +/**************************************************************************** + * + * 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; + float _heading; + + 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 + + /*! diff --git a/src/modules/mavlink/streams/HIGHRES_IMU.hpp b/src/modules/mavlink/streams/HIGHRES_IMU.hpp index 7a3d96cb3891..c6e4d7580bcb 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)}; @@ -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.ambient_temperature; msg.fields_updated = fields_updated; diff --git a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp b/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp index 7b0d9556cadf..5566efbb1c7d 100644 --- a/src/modules/simulation/sensor_airspeed_sim/SensorAirspeedSim.cpp +++ b/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); 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]