From 7735971900abe81cdcc3014d98838be9f8a1714c Mon Sep 17 00:00:00 2001 From: Valentin Bugrov Date: Fri, 9 May 2025 18:00:12 -0400 Subject: [PATCH] drivers/ins: Add driver for InertialLabs INS Signed-off-by: Valentin Bugrov --- src/drivers/drv_sensor.h | 2 + src/drivers/ins/Kconfig | 1 + src/drivers/ins/ilabs/CMakeLists.txt | 49 ++ src/drivers/ins/ilabs/ILabs.cpp | 525 ++++++++++++++++ src/drivers/ins/ilabs/ILabs.h | 126 ++++ src/drivers/ins/ilabs/Kconfig | 5 + src/drivers/ins/ilabs/libilabs/CMakeLists.txt | 54 ++ src/drivers/ins/ilabs/libilabs/include/data.h | 426 +++++++++++++ .../ins/ilabs/libilabs/include/sensor.h | 93 +++ src/drivers/ins/ilabs/libilabs/src/sensor.cpp | 562 ++++++++++++++++++ src/drivers/ins/ilabs/module.yaml | 24 + 11 files changed, 1867 insertions(+) create mode 100644 src/drivers/ins/ilabs/CMakeLists.txt create mode 100644 src/drivers/ins/ilabs/ILabs.cpp create mode 100644 src/drivers/ins/ilabs/ILabs.h create mode 100644 src/drivers/ins/ilabs/Kconfig create mode 100644 src/drivers/ins/ilabs/libilabs/CMakeLists.txt create mode 100644 src/drivers/ins/ilabs/libilabs/include/data.h create mode 100644 src/drivers/ins/ilabs/libilabs/include/sensor.h create mode 100644 src/drivers/ins/ilabs/libilabs/src/sensor.cpp create mode 100644 src/drivers/ins/ilabs/module.yaml diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index 87184924bc..d2ca4d86da 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -250,6 +250,8 @@ #define DRV_BARO_DEVTYPE_AUAV 0xE7 #define DRV_BARO_DEVTYPE_SPA06 0xE8 +#define DRV_INS_DEVTYPE_ILABS 0xE9 + #define DRV_DEVTYPE_UNUSED 0xff #endif /* _DRV_SENSOR_H */ diff --git a/src/drivers/ins/Kconfig b/src/drivers/ins/Kconfig index 5c215e3cca..808f1574e4 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_ILABS ---help--- Enable default set of INS sensors rsource "*/Kconfig" diff --git a/src/drivers/ins/ilabs/CMakeLists.txt b/src/drivers/ins/ilabs/CMakeLists.txt new file mode 100644 index 0000000000..7191cf43f3 --- /dev/null +++ b/src/drivers/ins/ilabs/CMakeLists.txt @@ -0,0 +1,49 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +add_subdirectory(libilabs) + +px4_add_module( + MODULE drivers__ins__ilabs + MAIN ilabs + INCLUDES + libilabs/include + COMPILE_FLAGS + SRCS + ILabs.cpp + ILabs.h + MODULE_CONFIG + module.yaml + DEPENDS + libilabs + ) diff --git a/src/drivers/ins/ilabs/ILabs.cpp b/src/drivers/ins/ilabs/ILabs.cpp new file mode 100644 index 0000000000..634415e490 --- /dev/null +++ b/src/drivers/ins/ilabs/ILabs.cpp @@ -0,0 +1,525 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "ILabs.h" + +#include +#include +#include +#include + +#ifndef MODULE_NAME +#define MODULE_NAME "ilabs_ins_driver" // NOLINT(cppcoreguidelines-macro-usage) +#endif + +using namespace time_literals; + +// GPS epoch: 1980-01-06 00:00:00 UTC +constexpr uint64_t GPS_EPOCH_SECS = 315964800ULL; + +constexpr uint8_t DECIMATION_VALUE = 20; + +uint64_t ToUtcMicroseconds(uint16_t gpsWeek, uint32_t msTow) { + const uint64_t gpsTimeSec = gpsWeek * 7ULL * 86400ULL + msTow / 1000ULL; + // 18 is a leap seconds correction + const uint64_t utcTimeSec = gpsTimeSec + GPS_EPOCH_SECS - 18; + const uint64_t timeUtcUsec = utcTimeSec * 1'000'000 + (msTow % 1000) * 1000ULL; + return timeUtcUsec; +} + +enum ILabsMode { + RAW_SENSORS_DATA = 0, + FULL_INS = 1, +}; + +ILabs::ILabs(const char *serialDeviceName) + : ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::serial_port_to_wq(serialDeviceName)), + _attitude_pub((_param_ilabs_mode.get() == ILabsMode::RAW_SENSORS_DATA) ? ORB_ID(external_ins_attitude) + : ORB_ID(vehicle_attitude)), + _local_position_pub((_param_ilabs_mode.get() == ILabsMode::RAW_SENSORS_DATA) ? ORB_ID(external_ins_local_position) + : ORB_ID(vehicle_local_position)), + _global_position_pub((_param_ilabs_mode.get() == ILabsMode::RAW_SENSORS_DATA) + ? ORB_ID(external_ins_global_position) + : ORB_ID(vehicle_global_position)) { + // store port name + strncpy(_serialDeviceName, serialDeviceName, sizeof(_serialDeviceName) - 1); + + // enforce null termination + _serialDeviceName[sizeof(_serialDeviceName) - 1] = '\0'; + + if (_param_ilabs_mode.get() == ILabsMode::FULL_INS) { + int32_t value = 0; + param_set(param_find("SENS_IMU_MODE"), &value); + param_set(param_find("SENS_MAG_MODE"), &value); + } + + _device_id.devid_s.devtype = DRV_INS_DEVTYPE_ILABS; + _device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL; + + _px4_accel.set_device_id(_device_id.devid); + _px4_gyro.set_device_id(_device_id.devid); + _px4_mag.set_device_id(_device_id.devid); + + _attitude_pub.advertise(); + _local_position_pub.advertise(); + _global_position_pub.advertise(); + _sensor_baro_pub.advertise(); + _sensor_gps_pub.advertise(); +} + +ILabs::~ILabs() { + _sensor.deinit(); + + perf_free(_sample_perf); + perf_free(_comms_errors); + + 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(_baro_pub_interval_perf); + + perf_free(_attitude_pub_interval_perf); + perf_free(_local_position_pub_interval_perf); + perf_free(_global_position_pub_interval_perf); +} + +int ILabs::task_spawn(int argc, char *argv[]) { + bool error_flag = false; + + int opt_index = 1; + const char *opt_arg = nullptr; + int opt_val = 0; + const char *device_name = nullptr; + + while ((opt_val = px4_getopt(argc, argv, "d:", &opt_index, &opt_arg)) != EOF) { + switch (opt_val) { + case 'd': + device_name = opt_arg; + break; + + case '?': + error_flag = true; + break; + + default: + PX4_WARN("Unrecognized flag"); + error_flag = true; + break; + } + } + + if (error_flag) { + return -1; + } + + if (device_name && (access(device_name, R_OK | W_OK) == 0)) { + ILabs *instance = new ILabs(device_name); + + if (instance == nullptr) { + PX4_ERR("Alloc failed"); + return PX4_ERROR; + } + + _object.store(instance); + _task_id = task_id_is_work_queue; + + instance->ScheduleNow(); + + return PX4_OK; + } + + if (device_name) { + PX4_ERR("Invalid device (-d) %s", device_name); + + } else { + PX4_INFO("Valid device required"); + } + + return PX4_ERROR; +} + +int ILabs::custom_command(int argc, char *argv[]) { + return print_usage("unknown command"); +} + +int ILabs::print_usage(const char *reason) { + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +Serial bus driver for the ILabs sensors. + +Most boards are configured to enable/start the driver on a specified UART using the SENS_ILABS_CFG. +After that you can use the ILABS_MODE parameter to config outputs: + +- Only raw sensor output (the default). +- Sensor output and INS data such as position and velocity estimates. + +Setup/usage information: https://docs.px4.io/main/en/sensor/ilabs.html + +### Examples + +Attempt to start driver on a specified serial device. +$ ilabs start -d /dev/ttyS1 +Stop driver +$ ilabs stop +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("ilabs", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("ins"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver"); + PRINT_MODULE_USAGE_PARAM_STRING('d', nullptr, nullptr, "Serial device", false); + PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Driver status"); + PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Print driver status"); + + return PX4_OK; +} + +int ILabs::print_status() { + if (_serialDeviceName[0] != '\0') { + PX4_INFO("UART device: %s", _serialDeviceName); + } + + perf_print_counter(_sample_perf); + perf_print_counter(_comms_errors); + + return 0; +} + +void ILabs::Run() { + if (should_exit()) { + _sensor.deinit(); + exit_and_cleanup(); + return; + } + + if (!_sensor.isInitialized()) { + const bool result = _sensor.init(_serialDeviceName, this, &ILabs::processDataProxy); + + if (!result) { + PX4_ERR("Sensor initializing error"); + ScheduleDelayed(1_s); + return; + } + _time_initialized.store(hrt_absolute_time()); + } + + // check for timeout + const hrt_abstime time_initialized = _time_initialized.load(); + const hrt_abstime time_last_valid_imu_data = _time_last_valid_imu_data.load(); + + if (_param_ilabs_mode.get() == ILabsMode::FULL_INS && time_last_valid_imu_data != 0 && + hrt_elapsed_time(&time_last_valid_imu_data) < 3_s) { + // update sensor_selection if configured in INS mode + if ((_px4_accel.get_device_id() != 0) && (_px4_gyro.get_device_id() != 0)) { + sensor_selection_s sensor_selection{}; + sensor_selection.accel_device_id = _px4_accel.get_device_id(); + sensor_selection.gyro_device_id = _px4_gyro.get_device_id(); + sensor_selection.timestamp = _time_initialized.load(); + _sensor_selection_pub.publish(sensor_selection); + } else { + PX4_ERR("Sensor not initialized"); + } + } + + if (time_initialized != 0 && hrt_elapsed_time(&time_last_valid_imu_data) > 5_s && + time_last_valid_imu_data != 0 && hrt_elapsed_time(&time_last_valid_imu_data) > 1_s) { + PX4_ERR("Timeout, reinitializing"); + _sensor.deinit(); + } + + ScheduleDelayed(100_ms); +} + +void ILabs::processData(InertialLabs::SensorsData *data) { + // PX4 by default uses FRD/NED frame position + // Inertial Labs INS by default uses in RFU/ENU frame position + + if (!data) { + PX4_ERR("Invalid sensor data"); + return; + } + + const bool isFilterOk = (data->ins.unitStatus & InertialLabs::USW::INITIAL_ALIGNMENT_FAIL) == 0 && + data->ins.solutionStatus != InertialLabs::InsSolution::INVALID; + const bool isAccelOk = (data->ins.unitStatus & InertialLabs::USW::ACCEL_FAIL) == 0; + const bool isGyroOk = (data->ins.unitStatus & InertialLabs::USW::GYRO_FAIL) == 0; + const bool isMagOk = (data->ins.unitStatus & InertialLabs::USW::MAG_FAIL) == 0; + + // true if received new GNSS position or velocity + const bool hasNewGpsData = (data->gps.newData & (InertialLabs::NewGpsData::NEW_GNSS_POSITION | InertialLabs::NewGpsData::NEW_GNSS_VELOCITY)); + const bool hasEnoughSatellites = data->gps.usedSatCount > 3; + + const bool isBaroOk = (data->ins.unitStatus2 & InertialLabs::USW2::ADU_BARO_FAIL) == 0; + + // TODO wind data: + // const bool isDiffPressureOk = (data->ins.unitStatus2 & InertialLabs::USW2::ADU_DIFF_PRESS_FAIL) == 0; + + const hrt_abstime time_now_us = hrt_absolute_time(); + _time_last_valid_imu_data.store(time_now_us); + + // update all temperatures + if (isFilterOk) { + _px4_accel.set_temperature(data->temperature); // degC + _px4_gyro.set_temperature(data->temperature); // degC + _px4_mag.set_temperature(data->temperature); // degC + } + + // publish accel + if (isFilterOk && isAccelOk) { + _px4_accel.update(time_now_us, data->accel(0), data->accel(1), data->accel(2)); // NED, in m/s^2 + perf_count(_accel_pub_interval_perf); + } + + // publish gyro + if (isFilterOk && isGyroOk) { + _px4_gyro.update(time_now_us, data->gyro(0), data->gyro(1), data->gyro(2)); // NED, in rad/s + perf_count(_gyro_pub_interval_perf); + } + + // publish mag + if (isFilterOk && isMagOk) { + _px4_mag.update(time_now_us, data->mag(0), data->mag(1), data->mag(2)); // NED, in Gauss + perf_count(_mag_pub_interval_perf); + } + + // publish baro + if (isFilterOk && isBaroOk) { + if (_average_sensors_data.count > DECIMATION_VALUE) { + sensor_baro_s sensor_baro{}; + sensor_baro.timestamp = time_now_us; + sensor_baro.timestamp_sample = time_now_us; + + sensor_baro.device_id = _device_id.devid; + sensor_baro.pressure = _average_sensors_data.pressure / static_cast(_average_sensors_data.count); // Pa + sensor_baro.temperature = _average_sensors_data.temperature / static_cast(_average_sensors_data.count); // degC + + _sensor_baro_pub.publish(sensor_baro); + perf_count(_baro_pub_interval_perf); + + _average_sensors_data.count = 0; + _average_sensors_data.pressure = 0.0f; + _average_sensors_data.temperature = 0.0f; + } + + _average_sensors_data.pressure += data->pressure; // Pa + _average_sensors_data.temperature += data->temperature; // degC + ++_average_sensors_data.count; + } + + // publish attitude + if (isFilterOk) { + // TODO: Use quaternion message in UDD? + const matrix::Quatf quat{matrix::Eulerf(math::radians(data->ins.roll), + math::radians(data->ins.pitch), + math::radians(data->ins.yaw))}; + vehicle_attitude_s attitude{}; + attitude.timestamp = time_now_us; + attitude.timestamp_sample = time_now_us; + + attitude.q[0] = quat(0); + attitude.q[1] = quat(1); + attitude.q[2] = quat(2); + attitude.q[3] = quat(3); + + _attitude_pub.publish(attitude); + perf_count(_attitude_pub_interval_perf); + } + + // publish local position + if (isFilterOk) { + vehicle_local_position_s local_position{}; + local_position.timestamp = time_now_us; + local_position.timestamp_sample = time_now_us; + + local_position.xy_valid = true; + local_position.z_valid = true; + local_position.v_xy_valid = true; + local_position.v_z_valid = true; + + if (!_pos_ref.isInitialized()) { + _pos_ref.initReference(data->ins.latitude, data->ins.longitude, time_now_us); + } + const matrix::Vector2f pos_ned = _pos_ref.project(data->ins.latitude, data->ins.longitude); + local_position.x = pos_ned(0); + local_position.y = pos_ned(1); + local_position.z = data->ins.altitude; + + local_position.vx = data->ins.velocity(0); + local_position.vy = data->ins.velocity(1); + local_position.vz = data->ins.velocity(2); + + local_position.ax = data->accel(0); + local_position.ay = data->accel(1); + local_position.az = data->accel(2); + + local_position.heading = static_cast(data->ext.headingData.heading) * + static_cast(M_DEG_TO_RAD) * 0.01f; // rad + local_position.unaided_heading = NAN; + local_position.heading_good_for_control = true; + + local_position.xy_global = true; + local_position.ref_timestamp = _pos_ref.getProjectionReferenceTimestamp(); + local_position.ref_lat = _pos_ref.getProjectionReferenceLat(); + local_position.ref_lon = _pos_ref.getProjectionReferenceLon(); + local_position.z_global = true; + + local_position.dist_bottom_valid = false; + + const float lat_err = static_cast(data->ins.accuracy.lat) * 0.001f; + const float lon_err = static_cast(data->ins.accuracy.lon) * 0.001f; + local_position.eph = sqrtf(lat_err * lat_err + lon_err * lon_err); + local_position.epv = static_cast(data->ins.accuracy.alt) * 0.001f; + + const float northVel_err = static_cast(data->ins.accuracy.northVel) * 0.001f; + const float eastVel_err = static_cast(data->ins.accuracy.eastVel) * 0.001f; + local_position.evh = sqrtf(northVel_err * northVel_err + eastVel_err * eastVel_err); + + local_position.evv = static_cast(data->ins.accuracy.verVel) * 0.001f; + + local_position.dead_reckoning = false; + + local_position.vxy_max = INFINITY; + local_position.vz_max = INFINITY; + local_position.hagl_min = INFINITY; + local_position.hagl_max_z = INFINITY; + local_position.hagl_max_xy = INFINITY; + + _local_position_pub.publish(local_position); + perf_count(_local_position_pub_interval_perf); + } + + // publish global_position + if (isFilterOk) { + vehicle_global_position_s global_position{}; + global_position.timestamp = time_now_us; + global_position.timestamp_sample = time_now_us; + + global_position.lat_lon_valid = true; + global_position.alt_valid = true; + global_position.lat = data->ins.latitude; + global_position.lon = data->ins.longitude; + global_position.alt = data->ins.altitude; + + const float lat_err = static_cast(data->ins.accuracy.lat) * 0.001f; + const float lon_err = static_cast(data->ins.accuracy.lon) * 0.001f; + global_position.eph = sqrtf(lat_err * lat_err + lon_err * lon_err); + global_position.epv = static_cast(data->ins.accuracy.alt) * 0.001f; + + global_position.dead_reckoning = false; + + _global_position_pub.publish(global_position); + perf_count(_global_position_pub_interval_perf); + } + + // publish GPS data + if (hasEnoughSatellites && isFilterOk && hasNewGpsData) { + sensor_gps_s sensor_gps{}; + sensor_gps.timestamp = time_now_us; + sensor_gps.timestamp_sample = time_now_us; + + sensor_gps.device_id = _device_id.devid; + + sensor_gps.latitude_deg = data->gps.latitude; + sensor_gps.longitude_deg = data->gps.longitude; + sensor_gps.altitude_ellipsoid_m = data->gps.altitude; + sensor_gps.altitude_msl_m = data->gps.altitude; + + sensor_gps.fix_type = data->gps.fixType + 1; + + const float lat_err = static_cast(data->ins.accuracy.lat) * 0.001f; + const float lon_err = static_cast(data->ins.accuracy.lon) * 0.001f; + sensor_gps.eph = sqrtf(lat_err * lat_err + lon_err * lon_err); + sensor_gps.epv = static_cast(data->ins.accuracy.alt) * 0.001f; + + sensor_gps.hdop = static_cast(data->gps.dop.hdop) * 0.001f; + sensor_gps.vdop = static_cast(data->gps.dop.vdop) * 0.001f; + + sensor_gps.jamming_state = data->gps.jamStatus; + sensor_gps.jamming_indicator = (sensor_gps.jamming_state != InertialLabs::JammingStatus::UNKOWN_OR_DISABLED) && + (sensor_gps.jamming_state != InertialLabs::JammingStatus::OK); + sensor_gps.spoofing_state = data->gps.spoofingStatus; + + sensor_gps.vel_m_s = + matrix::Vector3f(data->ins.velocity(0), data->ins.velocity(1), data->ins.velocity(2)).length(); + sensor_gps.vel_n_m_s = data->ins.velocity(0); + sensor_gps.vel_e_m_s = data->ins.velocity(1); + sensor_gps.vel_d_m_s = data->ins.velocity(2); + sensor_gps.vel_ned_valid = true; + + sensor_gps.time_utc_usec = ToUtcMicroseconds(data->gps.gpsWeek, data->gps.msTow); + sensor_gps.timestamp_time_relative = static_cast(sensor_gps.time_utc_usec - time_now_us); + + sensor_gps.satellites_used = data->gps.usedSatCount; + + sensor_gps.heading = NAN; + sensor_gps.heading_offset = NAN; + sensor_gps.heading_accuracy = NAN; + + // sensor_gps.s_variance_m_s = ...; // TODO: need 0x43 UDD Package? + + _sensor_gps_pub.publish(sensor_gps); + perf_count(_gnss_pub_interval_perf); + } + + if (_param_ilabs_mode.get() == ILabsMode::FULL_INS) { + estimator_status_s estimator_status{}; + estimator_status.timestamp = time_now_us; + estimator_status.timestamp_sample = time_now_us; + + const float test_ratio = 0.1f; + + estimator_status.hdg_test_ratio = test_ratio; + estimator_status.vel_test_ratio = test_ratio; + estimator_status.pos_test_ratio = test_ratio; + estimator_status.hgt_test_ratio = test_ratio; + + estimator_status.accel_device_id = _px4_accel.get_device_id(); + estimator_status.gyro_device_id = _px4_gyro.get_device_id(); + estimator_status.mag_device_id = _device_id.devid; + estimator_status.baro_device_id = _device_id.devid; + + _estimator_status_pub.publish(estimator_status); + } +} + +extern "C" __EXPORT int ilabs_main(int argc, char *argv[]) { + return ILabs::main(argc, argv); +} diff --git a/src/drivers/ins/ilabs/ILabs.h b/src/drivers/ins/ilabs/ILabs.h new file mode 100644 index 0000000000..82b94541db --- /dev/null +++ b/src/drivers/ins/ilabs/ILabs.h @@ -0,0 +1,126 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +/** + * + * Driver for the InertialLabs INS, AHRS + */ + +#pragma once + +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "sensor.h" + +class ILabs : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem { +public: + ILabs(const char *port); + ~ILabs() 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::print_status() */ + int print_status() override; + +private: + void Run() override; + void processData(InertialLabs::SensorsData *sensordata); + static void processDataProxy(void *context, InertialLabs::SensorsData *data) { + ILabs *self = static_cast(context); + self->processData(data); + } + + InertialLabs::Sensor _sensor{}; + + char _serialDeviceName[20]{}; + InertialLabs::AverageSensorsData _average_sensors_data{}; + device::Device::DeviceId _device_id{}; + + px4::atomic _time_initialized{0}; + px4::atomic _time_last_valid_imu_data{0}; + + PX4Accelerometer _px4_accel{0}; + PX4Gyroscope _px4_gyro{0}; + PX4Magnetometer _px4_mag{0}; + + MapProjection _pos_ref{}; + + 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::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + uORB::PublicationMulti _sensor_gps_pub{ORB_ID(sensor_gps)}; + uORB::Publication _sensor_selection_pub{ORB_ID(sensor_selection)}; + uORB::Publication _estimator_status_pub{ORB_ID(estimator_status)}; + + perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME ": com_err")}; + perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME ": read")}; + + 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 _baro_pub_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME ": baro 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")}; + + DEFINE_PARAMETERS((ParamInt)_param_ilabs_mode) +}; diff --git a/src/drivers/ins/ilabs/Kconfig b/src/drivers/ins/ilabs/Kconfig new file mode 100644 index 0000000000..d2b61929e9 --- /dev/null +++ b/src/drivers/ins/ilabs/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_INS_ILABS + bool "ilabs" + default n + ---help--- + Enable support for ilabs diff --git a/src/drivers/ins/ilabs/libilabs/CMakeLists.txt b/src/drivers/ins/ilabs/libilabs/CMakeLists.txt new file mode 100644 index 0000000000..3879c1a7f0 --- /dev/null +++ b/src/drivers/ins/ilabs/libilabs/CMakeLists.txt @@ -0,0 +1,54 @@ +############################################################################ +# +# Copyright (c) 2025 PX4 Development Team. All rights reserved. +# +# Redistribution and use in source and binary forms, with or without +# modification, are permitted provided that the following conditions +# are met: +# +# 1. Redistributions of source code must retain the above copyright +# notice, this list of conditions and the following disclaimer. +# 2. Redistributions in binary form must reproduce the above copyright +# notice, this list of conditions and the following disclaimer in +# the documentation and/or other materials provided with the +# distribution. +# 3. Neither the name PX4 nor the names of its contributors may be +# used to endorse or promote products derived from this software +# without specific prior written permission. +# +# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS +# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT +# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS +# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE +# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, +# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, +# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS +# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED +# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT +# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN +# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +# POSSIBILITY OF SUCH DAMAGE. +# +############################################################################ + +set(LIBRARY_NAME "libilabs") + +set(SOURCES + "${CMAKE_CURRENT_SOURCE_DIR}/src/sensor.cpp" +) + +add_library(${LIBRARY_NAME} ${SOURCES}) +target_compile_options(${LIBRARY_NAME} + PRIVATE + -Wno-unused-function +) + +target_include_directories(${LIBRARY_NAME} PUBLIC + "${CMAKE_CURRENT_SOURCE_DIR}/include" +) + +add_dependencies(${LIBRARY_NAME} prebuild_targets) + +if("${PX4_PLATFORM}" MATCHES "nuttx") + target_compile_definitions(${LIBRARY_NAME} PUBLIC __NUTTX__) +endif() diff --git a/src/drivers/ins/ilabs/libilabs/include/data.h b/src/drivers/ins/ilabs/libilabs/include/data.h new file mode 100644 index 0000000000..2d91355f28 --- /dev/null +++ b/src/drivers/ins/ilabs/libilabs/include/data.h @@ -0,0 +1,426 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include + +#define PACKED __attribute__((packed)) + +namespace InertialLabs { + +// NOLINTBEGIN(clang-analyzer-optin.performance.Padding, altera-struct-pack-align, misc-non-private-member-variables-in-classes) +struct PACKED vec3_16_t { + int16_t x, y, z; + + matrix::Vector3f toFloat() const { + return matrix::Vector3f(static_cast(x), static_cast(y), static_cast(z)); + } +}; + +struct PACKED vec3_32_t { + int32_t x, y, z; + + matrix::Vector3f toFloat() const { + return matrix::Vector3f(static_cast(x), static_cast(y), static_cast(z)); + } +}; + +enum DataType : uint8_t { + GPS_INS_TIME_MS = 0x01, + GPS_WEEK = 0x3C, + ACCEL_DATA_HR = 0x23, + GYRO_DATA_HR = 0x21, + BARO_DATA = 0x25, + MAG_DATA = 0x24, + SENSOR_BIAS = 0x26, + ORIENTATION_ANGLES = 0x07, + VELOCITIES = 0x12, + POSITION = 0x10, + UNIT_STATUS = 0x53, + GNSS_EXTENDED_INFO = 0x4A, + GNSS_POSITION = 0x30, + GNSS_VEL_TRACK = 0x32, + GNSS_POS_TIMESTAMP = 0x3E, + GNSS_NEW_DATA = 0x41, + GNSS_JAM_STATUS = 0xC0, + DIFFERENTIAL_PRESSURE = 0x28, + TRUE_AIRSPEED = 0x86, + CALIBRATED_AIRSPEED = 0x85, + WIND_SPEED = 0x8A, + AIR_DATA_STATUS = 0x8D, + SUPPLY_VOLTAGE = 0x50, + TEMPERATURE = 0x52, + UNIT_STATUS2 = 0x5A, + GNSS_DOP = 0x42, + INS_SOLUTION_STATUS = 0x54, + INS_POS_VEL_ACCURACY = 0x5F, + FULL_SAT_INFO = 0x37, + USED_SAT_COUNT = 0x3B, + GNSS_VEL_LATENCY = 0x3D, + GNSS_SOL_STATUS = 0x38, + GNSS_POS_VEL_TYPE = 0x39, + NEW_AIDING_DATA = 0x65, + NEW_AIDING_DATA2 = 0xA1, + EXT_SPEED = 0x61, + EXT_HOR_POS = 0x6E, + EXT_ALT = 0x6C, + EXT_HEADING = 0x66, + EXT_AMBIENT_DATA = 0x6B, + EXT_WIND_DATA = 0x62, + MAG_CLB_ACCURACY = 0x9A, +}; + +// Ins Solutiob Status +enum InsSolution { + GOOD = 0, + KF_CONVERGENCE_IN_PROGRESS = 1, + GNSS_ABSENCE = 2, + NO_HEADING_CORRECTION = 3, + AUTONOMOUS_MODE = 4, + NO_GNSS_AIDING_DATA = 5, + AUTONIMUS_TIME_EXCEEDED = 6, + ZUPT_MODE = 7, + INVALID = 8, +}; + +// Unit Status Word bits +enum USW { + INITIAL_ALIGNMENT_FAIL = (1U << 0), + OPERATION_FAIL = (1U << 1), + GYRO_FAIL = (1U << 2), + ACCEL_FAIL = (1U << 3), + MAG_FAIL = (1U << 4), + ELECTRONICS_FAIL = (1U << 5), + GNSS_FAIL = (1U << 6), + MAG_VG3D_CLB_RUNTIME = (1U << 7), + VOLTAGE_LOW = (1U << 8), + VOLTAGE_HIGH = (1U << 9), + GYRO_X_RATE_HIGH = (1U << 10), + GYRO_Y_RATE_HIGH = (1U << 11), + GYRO_Z_RATE_HIGH = (1U << 12), + MAG_FIELD_HIGH = (1U << 13), + TEMP_RANGE_ERR = (1U << 14), + MAG_VG3D_CLB_SUCCESS = (1U << 15), +}; + +// Unit Status Word2 bits +enum USW2 { + ACCEL_X_HIGH = (1U << 0), + ACCEL_Y_HIGH = (1U << 1), + ACCEL_Z_HIGH = (1U << 2), + ADU_BARO_FAIL = (1U << 3), + ADU_DIFF_PRESS_FAIL = (1U << 4), + MAG_AUTO_CAL_2D_RUNTIME = (1U << 5), + MAG_AUTO_CAL_3D_RUNTIME = (1U << 6), + GNSS_FUSION_OFF = (1U << 7), + DIFF_PRESS_FUSION_OFF = (1U << 8), + GNSS_POS_VALID = (1U << 10), +}; + +// Air Data Status bits +enum ADU { + BARO_INIT_FAIL = (1U << 0), + DIFF_PRESS_INIT_FAIL = (1U << 1), + BARO_FAIL = (1U << 2), + DIFF_PRESS_FAIL = (1U << 3), + BARO_RANGE_ERR = (1U << 4), + DIFF_PRESS_RANGE_ERR = (1U << 5), + BARO_ALT_FAIL = (1U << 8), + AIRSPEED_FAIL = (1U << 9), + AIRSPEED_BELOW_THRESHOLD = (1U << 10), +}; + +// New GPS indicator +enum NewGpsData { + NEW_GNSS_POSITION = (1U << 0), + NEW_GNSS_VELOCITY = (1U << 1), + NEW_GNSS_HEADING = (1U << 2), + NEW_VALID_PPS = (1U << 3), + NEW_GNSS_BESTXYZ_LOG = (1U << 4), + NEW_GNSS_PSRDOP_LOG = (1U << 5), + NEW_PPS = (1U << 6), + NEW_RANGE_LOG = (1U << 7), +}; + +enum NewAidingData { + NEW_AIRSPEED = (1U << 1), + NEW_WIND = (1U << 2), + NEW_EXT_POS = (1U << 3), + NEW_HEADING = (1U << 5), + NEW_AMBIENT = (1U << 10), + NEW_ALTITUDE = (1U << 11), + NEW_EXT_HOR_POS = (1U << 13), + NEW_ADC = (1U << 15), +}; + +enum JammingStatus : uint8_t { + UNKOWN_OR_DISABLED = 0, + OK = 1, // No significant jamming + WARNING = 2, // Interference visible but fix ok + CRITICAL = 3, // Interference visible and no fix +}; + +enum SpoofingStatus : uint8_t { + UNKOWN_OR_DEACTIVATED = 0, + NO = 1, + INDICATED = 2, + MULTIPLE_INCIDATIONS = 3, +}; + +/* + Every data package consist of: + MessageHeader (6 bytes) + Payload + Checksum (2 bytes) + + Whole data package size = MessageHeader->msgLen + 2(0x55AA) + + User Defined Data payload consist of: + Number of data structures + List of data structures ID + List of data structures Values + +*/ +struct PACKED MessageHeader { + uint16_t packageHeader; // 0x55AA + uint8_t msgType; // always 1 for INS data + uint8_t msgId; // always 0x95 + uint16_t msgLen; // payload + 6(msgType + msgId + msgLen + checksum). Not included packageHeader +}; + +struct PACKED SensorBias { + int8_t gyroX; // deg/s*0.5*1e5 + int8_t gyroY; // deg/s*0.5*1e5 + int8_t gyroZ; // deg/s*0.5*1e5 + int8_t accX; // g*0.5*1e6 + int8_t accY; // g*0.5*1e6 + int8_t accZ; // g*0.5*1e6 + int8_t reserved; +}; + +struct PACKED GnssExtendedInfo { + uint8_t fixType; + uint8_t spoofingStatus; +}; + +struct PACKED GnssDop { + uint16_t gdop; // *1000 + uint16_t pdop; // *1000 + uint16_t hdop; // *1000 + uint16_t vdop; // *1000 + uint16_t tdop; // *1000 +}; + +struct PACKED InsAccuracy { + int32_t lat; // m*1000 + int32_t lon; // m*1000 + int32_t alt; // m*1000 + int32_t eastVel; // m/s*1000 + int32_t northVel; // m/s*1000 + int32_t verVel; // m/s*1000 +}; + +struct PACKED ExtHorPos { + int32_t lat; // deg*1.0e7 + int32_t lon; // deg*1.0e7 + uint16_t latStd; // m*100 + uint16_t lonStd; // m*100 + uint16_t posLatency; // sec*1000 +}; + +struct PACKED ExtAlt { + int32_t alt; // m*1000, can be WGS84 or AMSL + uint16_t altStd; // m*100 +}; + +struct PACKED ExtHeading { + uint16_t heading; // deg*100 + uint16_t std; // deg*100 + uint16_t latency; // sec*1000 +}; + +struct PACKED ExtAmbientData { + int16_t airTemp; // degC*10 + int32_t alt; // m*100, can be WGS84 or AMSL + uint16_t absPress; // Pa/2 +}; + +struct PACKED ExtWindData { + int16_t nWindVel; // kt*100 + int16_t eWindVel; // kt*100 + uint16_t nStdWind; // kt*100 + uint16_t eStdWind; // kt*100 +}; + +union PACKED UDDMessageData { + uint32_t gpsTimeMs; // ms since start of GPS week + uint16_t gpsWeek; + vec3_32_t accelDataHr; // g * 1e6 + vec3_32_t gyroDataHr; // deg/s * 1e5 + struct PACKED { + uint16_t pressurePa2; // Pascals/2 + int32_t baroAlt; // meters*100, can be WGS84 or AMSL + } baroData; + vec3_16_t magData; // nT/10 + SensorBias sensorBias; + struct PACKED { + uint16_t yaw; // deg*100 + int16_t pitch; // deg*100 + int16_t roll; // deg*100 + } orientationAngles; // 321 euler order? + vec3_32_t velocity; // m/s * 100 + struct PACKED { + int32_t lat; // deg*1e7 + int32_t lon; // deg*1e7 + int32_t alt; // m*100, can be WGS84 or AMSL + } position; + uint16_t unitStatus; // set ILABS_UNIT_STATUS_* + GnssExtendedInfo gnssExtendedInfo; + struct PACKED { + int32_t lat; // deg*1e7 + int32_t lon; // deg*1e7 + int32_t alt; // m*100 can be WGS84 or AMSL + } gnssPosition; + struct PACKED { + int32_t horSpeed; // m/s*100 + uint16_t trackOverGround; // deg*100 + int32_t verSpeed; // m/s*100 + } gnssVelTrack; + uint32_t gnssPosTimestamp; // ms + uint8_t gnssNewData; + uint8_t gnssJamStatus; + int32_t differentialPressure; // mbar*1e4 + int16_t trueAirspeed; // m/s*100 + int16_t calibratedAirspeed; // m/s*100 + vec3_16_t windSpeed; // m/s*100 + uint16_t airDataStatus; + uint16_t supplyVoltage; // V*100 + int16_t temperature; // degC*10 + uint16_t unitStatus2; + GnssDop gnssDop; + uint8_t insSolStatus; + InsAccuracy insAccuracy; + uint8_t usedSatCount; + uint16_t gnssVelLatency; // ms + uint8_t gnssSolStatus; + uint8_t gnssPosVelType; + uint16_t newAidingData; + uint16_t newAidingData2; + int16_t externalSpeed; // kt*100 + ExtHorPos extHorPos; + ExtAlt extAlt; + ExtHeading extHeading; + ExtAmbientData extAmbientAirData; + ExtWindData extWindData; + uint8_t magClbAccuracy; // deg*10 +}; + +struct GpsData { + uint32_t msTow; // ms + uint16_t gpsWeek; + double latitude; // deg + double longitude; // deg + float altitude; // m, can be WGS84 or AMSL + float horSpeed; // m/s + float verSpeed; // m/s + float trackOverGround; // deg + GnssDop dop; + uint8_t newData; + uint8_t fixType; + uint8_t spoofingStatus; + uint8_t jamStatus; + uint8_t usedSatCount; + uint16_t velLatency; // ms + uint8_t gnssSolStatus; + uint8_t gnssPosVelType; +}; + +struct InsData { + float yaw; // deg + float pitch; // deg + float roll; // deg + uint32_t msTow; // ms + double latitude; // deg + double longitude; // deg + float altitude; // m, can be WGS84 or AMSL + matrix::Vector3f velocity; // NED, in m/s + uint16_t unitStatus; + uint16_t unitStatus2; + uint8_t solutionStatus; + InsAccuracy accuracy; + float baroAlt; // m + float trueAirspeed; // m/s + float calibratedAirspeed; // m/s + matrix::Vector3f windSpeed; // NED, in m/s + float airspeedSf; + uint16_t airDataStatus; + SensorBias sensorBias; + float magClbAccuracy; // deg +}; + +struct ExtData { + uint16_t newAidingData; + uint16_t newAidingData2; + int16_t speed; + ExtHorPos horPos; + ExtAlt altitudeData; + ExtHeading headingData; + ExtAmbientData ambientAirData; + ExtWindData windData; +}; + +struct SensorsData { + matrix::Vector3f accel; // NED, in m/s^2 + matrix::Vector3f gyro; // NED, in rad/s + matrix::Vector3f mag; // NED, in Gauss + InsData ins{}; + GpsData gps{}; + ExtData ext{}; + float pressure; // Pa + float differentialPressure; // Pa + float temperature; // degC + float supplyVoltage; // V +}; + +struct AverageSensorsData { + float pressure; // Pa + float temperature; // degC + uint8_t count; // number of samples +}; + +// NOLINTEND(clang-analyzer-optin.performance.Padding, altera-struct-pack-align, misc-non-private-member-variables-in-classes) + +} // namespace InertialLabs diff --git a/src/drivers/ins/ilabs/libilabs/include/sensor.h b/src/drivers/ins/ilabs/libilabs/include/sensor.h new file mode 100644 index 0000000000..f8c29bef0a --- /dev/null +++ b/src/drivers/ins/ilabs/libilabs/include/sensor.h @@ -0,0 +1,93 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#pragma once + +#include + +#include + +#include "data.h" + +namespace InertialLabs { + +constexpr uint16_t BUFFER_SIZE{512}; + +class Sensor { +public: + // Use C-style function pointer, because we can't use std::function on some platforms + using DataHandler = void (*)(void *, SensorsData *); + + Sensor() = default; + Sensor(const Sensor &) = delete; + Sensor &operator=(const Sensor &) = delete; + ~Sensor(); + + bool init(const char *serialDeviceName, void *context, DataHandler dataHandler); + void deinit(); + bool isInitialized() const; + + void updateData(); + +private: + static void *updateDataThreadHelper(void *context) { + Sensor *sensor = reinterpret_cast(context); + sensor->updateData(); + return nullptr; + } + void resetSerial(); + + bool moveToBufferStart(const uint8_t *pos); + bool skipPackageInBufferStart(); + bool movePackageHeaderToBufferStart(); + bool moveValidPackageToBufferStart(); + + bool parseUDDPayload(); + + device::Serial *_serial{nullptr}; + pthread_t _threadId; + bool _processInThread{false}; + + // callback. C-style class method pointer + void *_context{nullptr}; + DataHandler _dataHandler{nullptr}; + + bool _isInitialized{false}; + + uint8_t _buf[BUFFER_SIZE]{}; + uint16_t _bufOffset{0}; + + SensorsData _sensorData{}; +}; + +} // namespace InertialLabs diff --git a/src/drivers/ins/ilabs/libilabs/src/sensor.cpp b/src/drivers/ins/ilabs/libilabs/src/sensor.cpp new file mode 100644 index 0000000000..fa79551188 --- /dev/null +++ b/src/drivers/ins/ilabs/libilabs/src/sensor.cpp @@ -0,0 +1,562 @@ +/**************************************************************************** + * + * Copyright (c) 2025 PX4 Development Team. All rights reserved. + * + * Redistribution and use in source and binary forms, with or without + * modification, are permitted provided that the following conditions + * are met: + * + * 1. Redistributions of source code must retain the above copyright + * notice, this list of conditions and the following disclaimer. + * 2. Redistributions in binary form must reproduce the above copyright + * notice, this list of conditions and the following disclaimer in + * the documentation and/or other materials provided with the + * distribution. + * 3. Neither the name PX4 nor the names of its contributors may be + * used to endorse or promote products derived from this software + * without specific prior written permission. + * + * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS + * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT + * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS + * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE + * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, + * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, + * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS + * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED + * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT + * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN + * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE + * POSSIBILITY OF SUCH DAMAGE. + * + ****************************************************************************/ + +#include "sensor.h" + +#include +#include + +#include "data.h" + +#ifndef MODULE_NAME +#define MODULE_NAME "ilabs_ins_driver" // NOLINT(cppcoreguidelines-macro-usage) +#endif + +namespace { + +constexpr int BAUDRATE = 921600; + +// acceleration due to gravity in m/s^2 used in IL INS +constexpr float GRAVITY_MSS = 9.8106f; + +bool isMessageHeaderCorrect(const InertialLabs::MessageHeader *header) { + if (!header) { + PX4_ERR("Invalid header pointer"); + return false; + } + + return header->packageHeader == 0x55AA && header->msgType == 1 && header->msgId == 0x95 && + (header->msgLen + 2) <= InertialLabs::BUFFER_SIZE; //< 2 is size of PackageHeader, that not included in msgLen +} + +// sums the bytes in the supplied buffer, returns that sum mod 0xFFFF +uint16_t checksum(const uint8_t *data, uint16_t len) { + uint16_t sum = 0; + for (uint32_t i = 0; i < len; i++) { + sum += data[i]; + } + return sum; +} + +uint16_t readPackageChecksum(const uint8_t *checksumByte) { + return (checksumByte[1] << 8) | checksumByte[0]; +} + +// Convert from right-front-up to front-right-down or ENU to NED +matrix::Vector3f rfuToFrd(const matrix::Vector3f &vector3f) { + return matrix::Vector3f{vector3f(1), vector3f(0), -vector3f(2)}; +} + +} // namespace + +namespace InertialLabs { + +Sensor::~Sensor() { + deinit(); +} + +bool Sensor::init(const char *serialDeviceName, void *context, DataHandler dataHandler) { + if (_isInitialized) { + PX4_ERR("Serial device already initialized. Deinitialize first."); + return false; + } + + if (serialDeviceName[0] == '\0') { + PX4_ERR("Empty serial device name"); + return false; + } + + if (!context || !dataHandler) { + PX4_ERR("Empty data handler callback"); + return false; + } + + _serial = new device::Serial(serialDeviceName, BAUDRATE); + if (_serial == nullptr) { + PX4_ERR("Error creating serial device: %s", serialDeviceName); + px4_sleep(1); // NOLINT(concurrency-mt-unsafe) + return false; + } + + if (!_serial->isOpen() && !_serial->open()) { + // Open the UART. If this is successful then the UART is ready to use. + if (!_serial->open()) { + PX4_ERR("Error opening serial device: %s", serialDeviceName); + px4_sleep(1); // NOLINT(concurrency-mt-unsafe) + resetSerial(); + return false; + } + + PX4_INFO("Serial device opened sucessfully: %s", serialDeviceName); + + _serial->flush(); + } + _context = context; + _dataHandler = dataHandler; + + _processInThread = true; + const int result = pthread_create(&_threadId, nullptr, &Sensor::updateDataThreadHelper, this); + if (result) { + PX4_ERR("Cant create working thread"); + _processInThread = false; + px4_sleep(1); // NOLINT(concurrency-mt-unsafe) + resetSerial(); + return false; + } + + pthread_detach(_threadId); + + _isInitialized = true; + return true; +} + +void Sensor::deinit() { + _processInThread = false; + _isInitialized = false; + resetSerial(); +} + +bool Sensor::isInitialized() const { + return _isInitialized; +} + +void Sensor::updateData() { + while (_processInThread) { + bool result = moveValidPackageToBufferStart(); + if (!result) { + continue; + } + + result = parseUDDPayload(); + if (!result) { + continue; + } + + _dataHandler(_context, &_sensorData); + + skipPackageInBufferStart(); + } +} + +void Sensor::resetSerial() { + if (_serial) { + (void)_serial->close(); + delete _serial; + _serial = nullptr; + } +} + +bool Sensor::moveToBufferStart(const uint8_t *pos) { + if (!pos) { + PX4_ERR("Invalid position pointer"); + return false; + } + + const uint16_t bytesFromBufferStart = pos - _buf; + if (bytesFromBufferStart > BUFFER_SIZE) { + PX4_ERR("Invalid position pointer"); + return false; + } + + if (_bufOffset < bytesFromBufferStart) { + PX4_ERR("Buffer offset less than bytes diff need to move"); + return false; + } + + memmove(_buf, pos, _bufOffset - bytesFromBufferStart); + _bufOffset -= bytesFromBufferStart; + return true; +} + +bool Sensor::skipPackageInBufferStart() { + const MessageHeader *packageHeader = reinterpret_cast(_buf); + if (!isMessageHeaderCorrect(packageHeader)) { + PX4_ERR("Incorrect message header in buffer start. Can't skip the package correctly."); + return false; + } + + const uint8_t *endPackagePos = &_buf[packageHeader->msgLen + 2]; + + return moveToBufferStart(endPackagePos); +} + +bool Sensor::movePackageHeaderToBufferStart() { + if (_bufOffset < 3) { + PX4_WARN("Buffer offset is too small for the package"); + _bufOffset = 0; + return false; + } + + const uint16_t packageHeader = 0x55AA; + const uint8_t *startPackagePos = (const uint8_t *)memmem(&_buf[1], + _bufOffset - sizeof(packageHeader), + &packageHeader, + sizeof(packageHeader)); + if (!startPackagePos) { + PX4_WARN("Can't find the package start in the buffer"); + _bufOffset = 0; + return false; + } + + return moveToBufferStart(startPackagePos); +} + +bool Sensor::moveValidPackageToBufferStart() { + const uint16_t freeBufByteCount = BUFFER_SIZE - _bufOffset; + if (freeBufByteCount < sizeof(MessageHeader)) { + _bufOffset = 0; + PX4_DEBUG("Buffer free space not enough for the message header. Buffer was cleaned"); + return false; + } + + const ssize_t nRead = _serial->readAtLeast(&_buf[_bufOffset], freeBufByteCount, freeBufByteCount, 1000); + if (nRead != freeBufByteCount) { + PX4_ERR("Can't read requested data from the serial device. Bytes requested: %d. Bytes read: %d", + freeBufByteCount, static_cast(nRead)); + _bufOffset = 0; + return false; + } + + _bufOffset += nRead; + + if (_bufOffset > BUFFER_SIZE) { + PX4_ERR("Buffer read bytes: %d. Buffer size: %d. Offset will be reseted", _bufOffset, BUFFER_SIZE); + _bufOffset = 0; + return false; + } + + const MessageHeader *packageHeader = reinterpret_cast(_buf); + if (!isMessageHeaderCorrect(packageHeader)) { + PX4_DEBUG("Message header in the buffer start is incorrect"); + movePackageHeaderToBufferStart(); + return false; + } + + const uint16_t fullMessageLength = packageHeader->msgLen + 2; + if (fullMessageLength > _bufOffset) { + _bufOffset = 0; + PX4_ERR( + "The message was not read in full. This situation is not healthy. Is the buffer size enough?\n" + "Message was removed and buffer offset was reseted"); + return false; + } + + // Check checksum + const uint16_t calculatedChecksum = checksum(&_buf[2], packageHeader->msgLen - 2); + const uint16_t readChecksum = readPackageChecksum(&_buf[fullMessageLength - 2]); + if (calculatedChecksum != readChecksum) { + PX4_ERR("Invalid package checksum. Calculated checksum: %d. Read checksum: %d", calculatedChecksum, + readChecksum); + moveToBufferStart(&_buf[fullMessageLength]); + return false; + } + + return true; +} + +bool Sensor::parseUDDPayload() { + const MessageHeader *packageHeader = reinterpret_cast(_buf); + + if (!isMessageHeaderCorrect(packageHeader)) { + PX4_ERR("Package header in buffer start is incorrect"); + return false; + } + + const uint16_t payloadSize = packageHeader->msgLen - 6; + const uint8_t *payload = &_buf[6]; + + const uint8_t messageCount = payload[0]; + if (messageCount == 0 || messageCount > payloadSize - 1) { + PX4_ERR("Invalid data package. Number of messages or messages data are incorrect"); + movePackageHeaderToBufferStart(); + return false; + } + // 1 byle for message count + byte list of messages types + const uint8_t *messageDataOffset = &payload[1 + messageCount]; + + for (uint8_t i = 0; i < messageCount; i++) { + uint8_t messageLength = 0; + UDDMessageData &udd = *(UDDMessageData *)messageDataOffset; + uint8_t messageType = payload[1 + i]; + + switch (messageType) { + case DataType::GPS_INS_TIME_MS: { + // this is the GPS tow timestamp in ms for when the IMU data was sampled + _sensorData.ins.msTow = udd.gpsTimeMs; + messageLength = sizeof(udd.gpsTimeMs); + break; + } + case DataType::GPS_WEEK: { + _sensorData.gps.gpsWeek = udd.gpsWeek; + messageLength = sizeof(udd.gpsWeek); + break; + } + case DataType::ACCEL_DATA_HR: { + _sensorData.accel = + rfuToFrd(udd.accelDataHr.toFloat()) * GRAVITY_MSS * 1.0e-6f; // NED, in m/s^2 + messageLength = sizeof(udd.accelDataHr); + break; + } + case DataType::GYRO_DATA_HR: { + _sensorData.gyro = + rfuToFrd(udd.gyroDataHr.toFloat()) * M_DEG_TO_RAD * 1.0e-5f; // NED, in rad/s + messageLength = sizeof(udd.gyroDataHr); + break; + } + case DataType::BARO_DATA: { + _sensorData.pressure = static_cast(udd.baroData.pressurePa2 * 2); // Pa + _sensorData.ins.baroAlt = static_cast(udd.baroData.baroAlt) * 0.01f; // m + messageLength = sizeof(udd.baroData); + break; + } + case DataType::MAG_DATA: { + _sensorData.mag = + rfuToFrd(udd.magData.toFloat()) * 1.0e-6f; // NED, in Gauss + messageLength = sizeof(udd.magData); + break; + } + case DataType::SENSOR_BIAS: { + _sensorData.ins.sensorBias = udd.sensorBias; + messageLength = sizeof(udd.sensorBias); + break; + } + case DataType::ORIENTATION_ANGLES: { + _sensorData.ins.yaw = static_cast(udd.orientationAngles.yaw) * 0.01f; // deg + _sensorData.ins.pitch = static_cast(udd.orientationAngles.pitch) * 0.01f; // deg + _sensorData.ins.roll = static_cast(udd.orientationAngles.roll) * 0.01f; // deg + messageLength = sizeof(udd.orientationAngles); + break; + } + case DataType::VELOCITIES: { + _sensorData.ins.velocity = rfuToFrd(udd.velocity.toFloat()) * 0.01f; // NED, in m/s + messageLength = sizeof(udd.velocity); + break; + } + case DataType::POSITION: { + _sensorData.ins.latitude = static_cast(udd.position.lat) * 1.0e-7; // deg + _sensorData.ins.longitude = static_cast(udd.position.lon) * 1.0e-7; // deg + _sensorData.ins.altitude = static_cast(udd.position.alt) * 0.01f; // m + messageLength = sizeof(udd.position); + break; + } + case DataType::UNIT_STATUS: { + _sensorData.ins.unitStatus = udd.unitStatus; + messageLength = sizeof(udd.unitStatus); + break; + } + case DataType::GNSS_EXTENDED_INFO: { + _sensorData.gps.fixType = udd.gnssExtendedInfo.fixType; + _sensorData.gps.spoofingStatus = udd.gnssExtendedInfo.spoofingStatus; + messageLength = sizeof(udd.gnssExtendedInfo); + break; + } + case DataType::GNSS_POSITION: { + _sensorData.gps.latitude = static_cast(udd.gnssPosition.lat) * 1.0e-7; // deg + _sensorData.gps.longitude = static_cast(udd.gnssPosition.lon) * 1.0e-7; // deg + _sensorData.gps.altitude = static_cast(udd.gnssPosition.alt) * 0.01f; // m + messageLength = sizeof(udd.gnssPosition); + break; + } + case DataType::GNSS_VEL_TRACK: { + _sensorData.gps.horSpeed = + static_cast(udd.gnssVelTrack.horSpeed) * 0.01f; // m/s + _sensorData.gps.trackOverGround = + static_cast(udd.gnssVelTrack.trackOverGround) * 0.01f; // deg + _sensorData.gps.verSpeed = + static_cast(udd.gnssVelTrack.verSpeed) * 0.01f; // m/s + messageLength = sizeof(udd.gnssVelTrack); + break; + } + case DataType::GNSS_POS_TIMESTAMP: { + _sensorData.gps.msTow = udd.gnssPosTimestamp; + messageLength = sizeof(udd.gnssPosTimestamp); + break; + } + case DataType::GNSS_NEW_DATA: { + _sensorData.gps.newData = udd.gnssNewData; + messageLength = sizeof(udd.gnssNewData); + break; + } + case DataType::GNSS_JAM_STATUS: { + _sensorData.gps.jamStatus = udd.gnssJamStatus; + messageLength = sizeof(udd.gnssJamStatus); + break; + } + case DataType::DIFFERENTIAL_PRESSURE: { + _sensorData.differentialPressure = + static_cast(udd.differentialPressure) * 0.01f; // Pa + messageLength = sizeof(udd.differentialPressure); + break; + } + case DataType::TRUE_AIRSPEED: { + _sensorData.ins.trueAirspeed = static_cast(udd.trueAirspeed) * 0.01f; // m/s + messageLength = sizeof(udd.trueAirspeed); + break; + } + case DataType::CALIBRATED_AIRSPEED: { + _sensorData.ins.calibratedAirspeed = + static_cast(udd.calibratedAirspeed) * 0.01f; // m/s + messageLength = sizeof(udd.calibratedAirspeed); + break; + } + case DataType::WIND_SPEED: { + _sensorData.ins.airspeedSf = (udd.windSpeed.toFloat()(2)) * 1.0e-3f; + _sensorData.ins.windSpeed = rfuToFrd(udd.windSpeed.toFloat()) * 0.01f; // NED, in m/s + _sensorData.ins.windSpeed(2) = 0.0f; + messageLength = sizeof(udd.windSpeed); + break; + } + case DataType::AIR_DATA_STATUS: { + _sensorData.ins.airDataStatus = udd.airDataStatus; + messageLength = sizeof(udd.airDataStatus); + break; + } + case DataType::SUPPLY_VOLTAGE: { + _sensorData.supplyVoltage = static_cast(udd.supplyVoltage) * 0.01f; // V + messageLength = sizeof(udd.supplyVoltage); + break; + } + case DataType::TEMPERATURE: { + _sensorData.temperature = static_cast(udd.temperature) * 0.1f; // degC + messageLength = sizeof(udd.temperature); + break; + } + case DataType::UNIT_STATUS2: { + _sensorData.ins.unitStatus2 = udd.unitStatus2; + messageLength = sizeof(udd.unitStatus2); + break; + } + case DataType::GNSS_DOP: { + _sensorData.gps.dop.gdop = udd.gnssDop.gdop; + _sensorData.gps.dop.pdop = udd.gnssDop.pdop; + _sensorData.gps.dop.hdop = udd.gnssDop.hdop; + _sensorData.gps.dop.vdop = udd.gnssDop.vdop; + _sensorData.gps.dop.tdop = udd.gnssDop.tdop; + messageLength = sizeof(udd.gnssDop); + break; + } + case DataType::INS_SOLUTION_STATUS: { + _sensorData.ins.solutionStatus = udd.insSolStatus; + messageLength = sizeof(udd.insSolStatus); + break; + } + case DataType::INS_POS_VEL_ACCURACY: { + _sensorData.ins.accuracy.lat = udd.insAccuracy.lat; + _sensorData.ins.accuracy.lon = udd.insAccuracy.lon; + _sensorData.ins.accuracy.alt = udd.insAccuracy.alt; + _sensorData.ins.accuracy.eastVel = udd.insAccuracy.eastVel; + _sensorData.ins.accuracy.northVel = udd.insAccuracy.northVel; + _sensorData.ins.accuracy.verVel = udd.insAccuracy.verVel; + messageLength = sizeof(udd.insAccuracy); + break; + } + case USED_SAT_COUNT: { + _sensorData.gps.usedSatCount = udd.usedSatCount; + messageLength = sizeof(udd.usedSatCount); + break; + } + case DataType::GNSS_VEL_LATENCY: { + _sensorData.gps.velLatency = udd.gnssVelLatency; + messageLength = sizeof(udd.gnssVelLatency); + break; + } + case DataType::GNSS_SOL_STATUS: { + _sensorData.gps.gnssSolStatus = udd.gnssSolStatus; + messageLength = sizeof(udd.gnssSolStatus); + break; + } + case DataType::GNSS_POS_VEL_TYPE: { + _sensorData.gps.gnssPosVelType = udd.gnssPosVelType; + messageLength = sizeof(udd.gnssPosVelType); + break; + } + case DataType::NEW_AIDING_DATA: { + _sensorData.ext.newAidingData = udd.newAidingData; + messageLength = sizeof(udd.newAidingData); + break; + } + case DataType::NEW_AIDING_DATA2: { + _sensorData.ext.newAidingData2 = udd.newAidingData2; + messageLength = sizeof(udd.newAidingData2); + break; + } + case DataType::EXT_SPEED: { + _sensorData.ext.speed = udd.externalSpeed; + messageLength = sizeof(udd.externalSpeed); + break; + } + case DataType::EXT_HOR_POS: { + _sensorData.ext.horPos = udd.extHorPos; + messageLength = sizeof(udd.extHorPos); + break; + } + case DataType::EXT_ALT: { + _sensorData.ext.altitudeData = udd.extAlt; + messageLength = sizeof(udd.extAlt); + break; + } + case DataType::EXT_HEADING: { + _sensorData.ext.headingData = udd.extHeading; + messageLength = sizeof(udd.extHeading); + break; + } + case DataType::EXT_AMBIENT_DATA: { + _sensorData.ext.ambientAirData = udd.extAmbientAirData; + messageLength = sizeof(udd.extAmbientAirData); + break; + } + case DataType::EXT_WIND_DATA: { + _sensorData.ext.windData = udd.extWindData; + messageLength = sizeof(udd.extWindData); + break; + } + case DataType::MAG_CLB_ACCURACY: { + _sensorData.ins.magClbAccuracy = static_cast(udd.magClbAccuracy) * 0.1f; // deg + messageLength = sizeof(udd.magClbAccuracy); + break; + } + default: { + PX4_ERR("Unknown message type: %d. Further package parsing result will be incorrect!", + messageType); + messageLength = 0; + break; + } + } + + messageDataOffset += messageLength; + } + + return true; +} + +} // namespace InertialLabs diff --git a/src/drivers/ins/ilabs/module.yaml b/src/drivers/ins/ilabs/module.yaml new file mode 100644 index 0000000000..afd15163d6 --- /dev/null +++ b/src/drivers/ins/ilabs/module.yaml @@ -0,0 +1,24 @@ +module_name: InertialLabs + +serial_config: + - command: ilabs start -d ${SERIAL_DEV} + port_config_param: + name: SENS_ILABS_CFG + group: Sensors + +parameters: + - group: Sensors + definitions: + + ILABS_MODE: + description: + short: InertialLabs INS sensor mode configuration + long: | + Configures whether the driver outputs only raw sensor output (the default), + or additionally supplies INS data such as position and velocity estimates. + category: System + type: enum + values: + 0: Sensors Only (default) + 1: INS + default: 0