fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity
PX4-Autopilot/src/drivers/ins/sbgecom/sbgecom.cpp
Samuel Toledano b2672910da
sbgecom: Implement sbgECom INS driver (#24137) 
* Add new INS driver sbgECom

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

* Fix sensor airspeed simulator units

* Fix HIGHRES_IMU pressure unit

* Allow HIGHRES_IMU to support 4 IMUs

* sbgECom: Add documentation

* Use submodule instead of fetching sbgECom using CMake

* Remove patch strategy

* Fix dates

* Remove patch file

* Update SBG dev type ID

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

---------

Co-authored-by: Samuel Toledano <samuel.toledano@sbg-systems.com>
Co-authored-by: Jacob Dahl <37091262+dakejahl@users.noreply.github.com>
2025-09-11 00:37:41 -08:00

1126 lines
34 KiB
C++
Raw Blame History

/****************************************************************************
*
* Copyright (c) 2012-2025 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
* are met:
*
* 1. Redistributions of source code must retain the above copyright
* notice, this list of conditions and the following disclaimer.
* 2. Redistributions in binary form must reproduce the above copyright
* notice, this list of conditions and the following disclaimer in
* the documentation and/or other materials provided with the
* distribution.
* 3. Neither the name PX4 nor the names of its contributors may be
* used to endorse or promote products derived from this software
* without specific prior written permission.
*
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
* POSSIBILITY OF SUCH DAMAGE.
*
****************************************************************************/
/**
* @file sbgecom.cpp
* Driver for the SBG Systems products
*
* @author SBG Systems <contact@sbg-systems.com>
*/
#include "sbgecom.hpp"
#include <lib/drivers/device/Device.hpp>
#include <px4_platform_common/getopt.h>
#include <fcntl.h>
#include <sys/stat.h>
#include <termios.h>
#define DEFAULT_DEVNAME "/dev/ttyS0"
#define SBG_MODE_SENSOR 0
#define SBG_MODE_GNSS 1
#define SBG_MODE_INS 2
#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<SbgEcom *>(user_arg);
const float temperature = sbgEComLogImuShortGetTemperature(&ref_sbg_data->imuShort);
// publish sensor_accel
instance->_px4_accel.update(hrt_absolute_time(),
sbgEComLogImuShortGetDeltaVelocity(&ref_sbg_data->imuShort, 0),
sbgEComLogImuShortGetDeltaVelocity(&ref_sbg_data->imuShort, 1),
sbgEComLogImuShortGetDeltaVelocity(&ref_sbg_data->imuShort, 2));
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(hrt_absolute_time(),
sbgEComLogImuShortGetDeltaAngle(&ref_sbg_data->imuShort, 0),
sbgEComLogImuShortGetDeltaAngle(&ref_sbg_data->imuShort, 1),
sbgEComLogImuShortGetDeltaAngle(&ref_sbg_data->imuShort, 2));
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<SbgEcom *>(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 = sbgEComLogEkfGetSolutionMode(ekf_status);
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<SbgEcom *>(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<SbgEcom *>(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 = sbgEComLogEkfGetSolutionMode(ref_sbg_data->ekfNavData.status);
// 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<SbgEcom *>(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 (((time_now_us - pos_time) < max_time_diff) &&
((time_now_us - vel_time) < max_time_diff) &&
((time_now_us - hdt_time) < max_time_diff) &&
((pos_time - vel_time) < max_time_diff) &&
((pos_time - hdt_time) < max_time_diff) &&
((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<SbgEcom *>(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 = hrt_absolute_time() - air_data.timestamp;
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;
struct termios options;
int *pSerialHandle;
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);
}
pSerialHandle = (int *)_sbg_interface.handle;
if (tcgetattr((*pSerialHandle), &options) != -1) {
// add custom options
options.c_cflag &= CSIZE;
options.c_iflag &= ~(IXON | IXOFF | IXANY);
options.c_iflag &= ~(IGNBRK | BRKINT | PARMRK | ISTRIP | INLCR | ICRNL | INPCK);
options.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG | ECHONL | IEXTEN);
if (tcsetattr((*pSerialHandle), TCSANOW, &options) != -1) {
error_code = sbgInterfaceFlush(&_sbg_interface, SBG_IF_FLUSH_ALL);
} else {
error_code = SBG_ERROR;
}
} else {
error_code = SBG_ERROR;
}
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;
}
} 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;
}
error_code = sendAirDataLog(&_com_handle, this);
if (error_code != SBG_NO_ERROR) {
PX4_WARN("Unable to send AirData log %d", error_code);
}
error_code = sendMagLog(&_com_handle, this);
if (error_code != SBG_NO_ERROR) {
PX4_WARN("Unable to send Mag log %d", error_code);
}
}
}
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);
}
Reference in New Issue View Git Blame Copy Permalink