From b5e5d214fe0ebde9b6aabd9f9bc704e5a1723649 Mon Sep 17 00:00:00 2001 From: JoelJ18 <101486509+JoelJ18@users.noreply.github.com> Date: Tue, 5 Aug 2025 11:05:46 -0400 Subject: [PATCH] drivers: MicroStrain Inertial Sensors (#23858) --- .gitmodules | 4 + src/drivers/drv_sensor.h | 2 + src/drivers/ins/Kconfig | 1 + src/drivers/ins/microstrain/CMakeLists.txt | 57 + src/drivers/ins/microstrain/Kconfig | 5 + src/drivers/ins/microstrain/MicroStrain.cpp | 1858 +++++++++++++++++++ src/drivers/ins/microstrain/MicroStrain.hpp | 259 +++ src/drivers/ins/microstrain/mip_sdk | 1 + src/drivers/ins/microstrain/module.yaml | 310 ++++ 9 files changed, 2497 insertions(+) create mode 100644 src/drivers/ins/microstrain/CMakeLists.txt create mode 100644 src/drivers/ins/microstrain/Kconfig create mode 100755 src/drivers/ins/microstrain/MicroStrain.cpp create mode 100755 src/drivers/ins/microstrain/MicroStrain.hpp create mode 160000 src/drivers/ins/microstrain/mip_sdk create mode 100644 src/drivers/ins/microstrain/module.yaml diff --git a/.gitmodules b/.gitmodules index 0758caf3e8..c9cebae704 100644 --- a/.gitmodules +++ b/.gitmodules @@ -95,3 +95,7 @@ [submodule "src/lib/tensorflow_lite_micro/tflite_micro"] path = src/lib/tensorflow_lite_micro/tflite_micro url = https://github.com/PX4/tflite-micro.git +[submodule "src/drivers/ins/microstrain/mip_sdk"] + path = src/drivers/ins/microstrain/mip_sdk + url = https://github.com/PX4/LORD-MicroStrain_mip_sdk.git + branch = feature/px4_compilation diff --git a/src/drivers/drv_sensor.h b/src/drivers/drv_sensor.h index e7509dc0bd..2d943397ea 100644 --- a/src/drivers/drv_sensor.h +++ b/src/drivers/drv_sensor.h @@ -253,6 +253,8 @@ #define DRV_INS_DEVTYPE_ILABS 0xE9 +#define DRV_INS_DEVTYPE_MICROSTRAIN 0xEA + #define DRV_DEVTYPE_UNUSED 0xff #endif /* _DRV_SENSOR_H */ diff --git a/src/drivers/ins/Kconfig b/src/drivers/ins/Kconfig index 808f1574e4..e5f5f6324a 100644 --- a/src/drivers/ins/Kconfig +++ b/src/drivers/ins/Kconfig @@ -4,6 +4,7 @@ menu "Inertial Navigation Systems (INS)" default n select DRIVERS_INS_VECTORNAV select DRIVERS_INS_ILABS + select DRIVERS_INS_MICROSTRAIN ---help--- Enable default set of INS sensors rsource "*/Kconfig" diff --git a/src/drivers/ins/microstrain/CMakeLists.txt b/src/drivers/ins/microstrain/CMakeLists.txt new file mode 100644 index 0000000000..9848bd09fd --- /dev/null +++ b/src/drivers/ins/microstrain/CMakeLists.txt @@ -0,0 +1,57 @@ +############################################################################ +# +# 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(MIP_SDK_DIR ${CMAKE_CURRENT_SOURCE_DIR}/mip_sdk) +px4_add_git_submodule(TARGET git_mip PATH ${MIP_SDK_DIR}) + +# Set some options for the MIP SDK which we will reset after building. +set(MIP_USE_TCP "OFF" CACHE BOOL "" FORCE) +set(MIP_DISABLE_CPP "ON" CACHE BOOL "" FORCE) +set(BUILD_EXAMPLES "OFF" CACHE BOOL "" FORCE) +set(BUILD_TESTING "OFF" CACHE BOOL "" FORCE) +add_subdirectory(mip_sdk) + +px4_add_module( + MODULE drivers__ins__microstrain + MAIN microstrain + COMPILE_FLAGS + SRCS + MicroStrain.cpp + MicroStrain.hpp + MODULE_CONFIG + module.yaml + DEPENDS + px4_work_queue + mip + ) diff --git a/src/drivers/ins/microstrain/Kconfig b/src/drivers/ins/microstrain/Kconfig new file mode 100644 index 0000000000..fe4f794ec7 --- /dev/null +++ b/src/drivers/ins/microstrain/Kconfig @@ -0,0 +1,5 @@ +menuconfig DRIVERS_INS_MICROSTRAIN + bool "microstrain" + default n + ---help--- + Enable support for Microstrain Sensors diff --git a/src/drivers/ins/microstrain/MicroStrain.cpp b/src/drivers/ins/microstrain/MicroStrain.cpp new file mode 100755 index 0000000000..428e404b36 --- /dev/null +++ b/src/drivers/ins/microstrain/MicroStrain.cpp @@ -0,0 +1,1858 @@ +/**************************************************************************** + * + * 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 + +#include "MicroStrain.hpp" + +device::Serial device_uart{}; + +MicroStrain::MicroStrain(const char *uart_port) : + ModuleParams(nullptr), + ScheduledWorkItem(MODULE_NAME, px4::wq_configurations::INS3), + _vehicle_global_position_pub((_param_ms_mode.get() == 0) ? ORB_ID(external_ins_global_position) : ORB_ID( + vehicle_global_position)), + _vehicle_attitude_pub((_param_ms_mode.get() == 0) ? ORB_ID(external_ins_attitude) : ORB_ID(vehicle_attitude)), + _vehicle_local_position_pub((_param_ms_mode.get() == 0) ? ORB_ID(external_ins_local_position) : ORB_ID( + vehicle_local_position)) +{ + // Store port name + memset(_port, '\0', sizeof(_port)); + const size_t max_len = math::min(sizeof(_port), strnlen(uart_port, sizeof(_port))); + strncpy(_port, uart_port, max_len); + + // Enforce null termination + _port[max_len] = '\0'; + + // The scheduling rate (in microseconds) is set higher than the highest data rate param (in Hz) + const int max_param_rate = math::max(math::max(_param_ms_imu_rate_hz.get(), _param_ms_mag_rate_hz.get(), + _param_ms_baro_rate_hz.get()), _param_ms_filter_rate_hz.get()); + + // We always want the schedule rate faster than the highest sensor data rate + _ms_schedule_rate_us = (1e6) / (2 * max_param_rate); + PX4_DEBUG("Schedule rate: %i", _ms_schedule_rate_us); + + device::Device::DeviceId device_id{}; + device_id.devid_s.devtype = DRV_INS_DEVTYPE_MICROSTRAIN; + device_id.devid_s.bus_type = device::Device::DeviceBusType_SERIAL; + device_id.devid_s.bus = 2; + _dev_id = device_id.devid; + + _px4_accel.set_device_id(_dev_id); + _px4_gyro.set_device_id(_dev_id); + _px4_mag.set_device_id(_dev_id); + + _sensor_baro.device_id = _dev_id; + _sensor_baro.pressure = 0; + _sensor_baro.temperature = 0; + _sensor_baro.error_count = 0; + + gnss_antenna_offset1[0] = _param_ms_gnss_offset1_x.get(); + gnss_antenna_offset1[1] = _param_ms_gnss_offset1_y.get(); + gnss_antenna_offset1[2] = _param_ms_gnss_offset1_z.get(); + PX4_DEBUG("GNSS antenna offset 1: %f/%f/%f", (double)_param_ms_gnss_offset1_x.get(), + (double)_param_ms_gnss_offset1_y.get(), + (double)_param_ms_gnss_offset1_z.get()); + + gnss_antenna_offset2[0] = _param_ms_gnss_offset2_x.get(); + gnss_antenna_offset2[1] = _param_ms_gnss_offset2_y.get(); + gnss_antenna_offset2[2] = _param_ms_gnss_offset2_z.get(); + PX4_DEBUG("GNSS antenna offset 2: %f/%f/%f", (double)_param_ms_gnss_offset2_x.get(), + (double)_param_ms_gnss_offset2_y.get(), + (double)_param_ms_gnss_offset2_z.get()); + + rotation.euler[0] = _param_ms_sensor_roll.get(); + rotation.euler[1] = _param_ms_sensor_pitch.get(); + rotation.euler[2] = _param_ms_sensor_yaw.get(); + PX4_DEBUG("Device Roll/Pitch/Yaw: %f/%f/%f", (double)_param_ms_sensor_roll.get(), + (double)_param_ms_sensor_pitch.get(), + (double)_param_ms_sensor_yaw.get()); + + _sensor_baro_pub.advertise(); + _sensor_selection_pub.advertise(); + _vehicle_local_position_pub.advertise(); + _vehicle_angular_velocity_pub.advertise(); + _vehicle_attitude_pub.advertise(); + _vehicle_global_position_pub.advertise(); + _vehicle_odometry_pub.advertise(); + _estimator_status_pub.advertise(); + _sensor_gps_pub[0].advertise(); + _sensor_gps_pub[1].advertise(); + +} + +MicroStrain::~MicroStrain() +{ + if (device_uart.isOpen()) { + device_uart.close(); + } + + PX4_DEBUG("Destructor"); + _sensor_baro_pub.unadvertise(); + _sensor_selection_pub.unadvertise(); + _vehicle_local_position_pub.unadvertise(); + _vehicle_angular_velocity_pub.unadvertise(); + _vehicle_attitude_pub.unadvertise(); + _vehicle_global_position_pub.unadvertise(); + _vehicle_odometry_pub.unadvertise(); + _estimator_status_pub.unadvertise(); + + perf_free(_loop_perf); + perf_free(_loop_interval_perf); +} + +bool mipInterfaceUserRecvFromDevice(mip_interface *device, uint8_t *buffer, size_t max_length, + timeout_type wait_time, + size_t *out_length, timestamp_type *timestamp_out) +{ + (void)device; + + *timestamp_out = hrt_absolute_time(); + + int res = device_uart.read(buffer, max_length); + + if (res == -1 && errno != EAGAIN) { + PX4_ERR("MicroStrain driver failed to read(%d): %s", errno, strerror(errno)); + *out_length = 0; + return false; + } + + if (res >= 0) { + *out_length = res; + } + + return true; +} + +bool mipInterfaceUserSendToDevice(mip_interface *device, const uint8_t *data, size_t length) +{ + int res = device_uart.write(const_cast(data), length); + + if (res >= 0) { + return true; + } + + PX4_ERR("MicroStrain driver failed to write(%d): %s", errno, strerror(errno)); + return false; +} + +mip_cmd_result MicroStrain::forceIdle() +{ + // Setting to idle may fail the first couple times, so call it a few times in case the device is streaming too much data + PX4_DEBUG("Setting device to idle"); + mip_cmd_result res = MIP_PX4_ERROR; + uint8_t set_to_idle_tries = 0; + + while (set_to_idle_tries++ < 3) { + if (mip_cmd_result_is_ack((res = mip_base_set_idle(&_device)))) { + break; + + } else { + usleep(1_s); + } + } + + return res; +} + +int MicroStrain::connectAtBaud(int32_t baud) +{ + if (device_uart.isOpen()) { + if (device_uart.setBaudrate(baud) == false) { + PX4_ERR("Failed to set UART %lu baud", baud); + } + + } else { + if (device_uart.setPort(_port) == false) { + PX4_ERR("Could not set device port!"); + return PX4_ERROR; + } + + if (device_uart.setBaudrate(baud) == false) { + PX4_ERR("Failed to set UART %lu baud", baud); + return PX4_ERROR; + } + + if (device_uart.open() == false) { + PX4_ERR("Could not open device port!"); + return PX4_ERROR; + } + } + + PX4_INFO("Serial Port %s with baud of %lu baud", (device_uart.isOpen() ? "CONNECTED" : "NOT CONNECTED"), baud); + + // Re-init the interface with the correct timeouts + mip_interface_init(&_device, _parse_buffer, sizeof(_parse_buffer), mip_timeout_from_baudrate(baud) * 1_ms, 250_ms, + &mipInterfaceUserSendToDevice, &mipInterfaceUserRecvFromDevice, &mip_interface_default_update, NULL); + + if (!mip_cmd_result_is_ack(forceIdle())) { + PX4_ERR("Could not set device to idle"); + return PX4_ERROR; + } + + PX4_INFO("Successfully opened and pinged"); + return PX4_OK; +} + +mip_cmd_result MicroStrain::getSupportedDescriptors() +{ + const size_t descriptors_max_size = sizeof(_supported_descriptors) / sizeof(_supported_descriptors[0]); + uint8_t descriptors_count, extended_descriptors_count; + + // Pull the descriptors and the extended descriptors from the device + mip_cmd_result res = mip_base_get_device_descriptors(&_device, _supported_descriptors, descriptors_max_size, + &descriptors_count); + mip_cmd_result res_extended = mip_base_get_extended_descriptors(&_device, &(_supported_descriptors[descriptors_count]), + descriptors_max_size - descriptors_count, &extended_descriptors_count); + + if (!mip_cmd_result_is_ack(res)) { + return res; + } + + if (mip_cmd_result_is_ack(res_extended)) { + PX4_DEBUG("Device does not support the extended descriptors command."); + } + + _supported_desc_len = descriptors_count + extended_descriptors_count; + + // Get the supported descriptor sets from the obtained descriptors + for (uint16_t i = 0; i < _supported_desc_len; i++) { + auto descriptor_set = static_cast((_supported_descriptors[i] & 0xFF00) >> 8); + bool unique = true; + + uint16_t j; + + for (j = 0; j < _supported_desc_set_len; j++) { + if (_supported_descriptor_sets[j] == descriptor_set) { + unique = false; + break; + + } else if (_supported_descriptor_sets[j] == 0) { + break; + } + } + + if (unique) {_supported_descriptor_sets[j] = descriptor_set; _supported_desc_set_len++;} + } + + return res; +} + +bool MicroStrain::supportsDescriptorSet(uint8_t descriptor_set) +{ + for (uint16_t i = 0; i < _supported_desc_set_len; i++) { + if (_supported_descriptor_sets[i] == descriptor_set) { + return true; + } + } + + return false; +} + +bool MicroStrain::supportsDescriptor(uint8_t descriptor_set, uint8_t field_descriptor) +{ + // Check if the descriptor set is supported + if (!supportsDescriptorSet(descriptor_set)) {return false;} + + // Check if the field descriptor is supported + const uint16_t full_descriptor = (descriptor_set << 8) | field_descriptor; + + for (uint16_t i = 0; i < _supported_desc_len; i++) { + if (_supported_descriptors[i] == full_descriptor) { + return true; + } + } + + return false; +} + +mip_cmd_result MicroStrain::writeBaudRate(uint32_t baudrate, uint8_t port) +{ + mip_cmd_result res; + + // Writes the baudrate using whichever command the device supports + if (supportsDescriptor(MIP_BASE_CMD_DESC_SET, MIP_CMD_DESC_BASE_COMM_SPEED)) { + res = mip_base_write_comm_speed(&_device, port, baudrate); + + } + + else if (supportsDescriptor(MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_UART_BAUDRATE)) { + res = mip_3dm_write_uart_baudrate(&_device, baudrate); + } + + else { + PX4_ERR("Does not support write baudrate commands"); + res = MIP_PX4_ERROR; + } + + return res; +} + +mip_cmd_result MicroStrain::configureImuRange() +{ + mip_cmd_result res = MIP_ACK_OK; + + // Checks if the accel range is to be configured + if (_param_ms_accel_range_setting.get() != -1) { + if (supportsDescriptor(MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_SENSOR_RANGE)) { + res = mip_3dm_write_sensor_range(&_device, MIP_SENSOR_RANGE_TYPE_ACCEL, _param_ms_accel_range_setting.get()); + + } else { + PX4_ERR("Accel sensor range write command is not supported"); + res = MIP_PX4_ERROR; + } + } + + if (!mip_cmd_result_is_ack(res)) { + return res; + } + + // Checks if the gyro range is to be configured + if (_param_ms_gyro_range_setting.get() != -1) { + if (supportsDescriptor(MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_SENSOR_RANGE)) { + res = mip_3dm_write_sensor_range(&_device, MIP_SENSOR_RANGE_TYPE_GYRO, _param_ms_gyro_range_setting.get()); + + } else { + PX4_ERR("Gyro sensor range write command is not supported"); + res = MIP_PX4_ERROR; + } + } + + return res; +} + +mip_cmd_result MicroStrain::getBaseRate(uint8_t descriptor_set, uint16_t *base_rate) +{ + mip_cmd_result res; + + // Gets the base rate using whichever command the device supports + if (supportsDescriptor(MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_GET_BASE_RATE)) { + res = mip_3dm_get_base_rate(&_device, descriptor_set, base_rate); + + } else { + switch (descriptor_set) { + case MIP_SENSOR_DATA_DESC_SET: { + if (supportsDescriptor(MIP_SENSOR_DATA_DESC_SET, MIP_CMD_DESC_3DM_GET_IMU_BASE_RATE)) { + res = mip_3dm_imu_get_base_rate(&_device, base_rate); + + } else { + PX4_ERR("IMU base rate command is not supported"); + res = MIP_PX4_ERROR; + } + + break; + } + + case MIP_GNSS1_DATA_DESC_SET: + case MIP_GNSS2_DATA_DESC_SET: + case MIP_GNSS_DATA_DESC_SET: { + if (supportsDescriptor(descriptor_set, MIP_CMD_DESC_3DM_GET_GNSS_BASE_RATE)) { + res = mip_3dm_gps_get_base_rate(&_device, base_rate); + + } else { + PX4_ERR("GNSS base rate command is not supported"); + res = MIP_PX4_ERROR; + } + + break; + } + + case MIP_FILTER_DATA_DESC_SET: { + if (supportsDescriptor(MIP_FILTER_DATA_DESC_SET, MIP_CMD_DESC_3DM_GET_FILTER_BASE_RATE)) { + res = mip_3dm_filter_get_base_rate(&_device, base_rate); + + } else { + PX4_ERR("Filter base rate command is not supported"); + res = MIP_PX4_ERROR; + } + + break; + } + + default: + PX4_ERR("Descriptor set for base rate is not supported"); + res = MIP_PX4_ERROR; + break; + } + } + + return res; +} + +mip_cmd_result MicroStrain::writeMessageFormat(uint8_t descriptor_set, uint8_t num_descriptors, + const mip::DescriptorRate *descriptors) +{ + mip_cmd_result res; + + // Writes the message format using whichever command the device supports + if (supportsDescriptor(MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_MESSAGE_FORMAT)) { + return mip_3dm_write_message_format(&_device, descriptor_set, num_descriptors, + descriptors); + + } else { + switch (descriptor_set) { + case MIP_SENSOR_DATA_DESC_SET: { + if (supportsDescriptor(MIP_SENSOR_DATA_DESC_SET, MIP_CMD_DESC_3DM_IMU_MESSAGE_FORMAT)) { + res = mip_3dm_write_imu_message_format(&_device, num_descriptors, descriptors); + + } else { + PX4_ERR("IMU message format command is not supported"); + res = MIP_PX4_ERROR; + } + + break; + } + + case MIP_GNSS1_DATA_DESC_SET: + case MIP_GNSS2_DATA_DESC_SET: + case MIP_GNSS_DATA_DESC_SET: { + if (supportsDescriptor(descriptor_set, MIP_CMD_DESC_3DM_GNSS_MESSAGE_FORMAT)) { + res = mip_3dm_write_gps_message_format(&_device, num_descriptors, descriptors); + + } else { + PX4_ERR("GNSS messaage format command is not supported"); + res = MIP_PX4_ERROR; + } + + break; + } + + case MIP_FILTER_DATA_DESC_SET: { + if (supportsDescriptor(MIP_FILTER_DATA_DESC_SET, MIP_CMD_DESC_3DM_FILTER_MESSAGE_FORMAT)) { + res = mip_3dm_write_filter_message_format(&_device, num_descriptors, descriptors); + + + } else { + PX4_ERR("Filter message format command is not supported"); + res = MIP_PX4_ERROR; + } + + break; + } + + default: + PX4_ERR("Descriptor set for writing message format is not supported"); + res = MIP_PX4_ERROR; + break; + } + } + + return res; +} + +mip_cmd_result MicroStrain::configureImuMessageFormat() +{ + PX4_DEBUG("Configuring IMU Message Format"); + + uint8_t num_imu_descriptors = 0; + mip_descriptor_rate imu_descriptors[5]; + + // Get the base rate + uint16_t base_rate; + mip_cmd_result res = getBaseRate(MIP_SENSOR_DATA_DESC_SET, &base_rate); + + PX4_DEBUG("The IMU base rate is %d", base_rate); + + if (!mip_cmd_result_is_ack(res)) { + PX4_ERR("Could not get the IMU base rate"); + return res; + } + + // Configure the Message Format depending on if the device supports the descriptor + if (supportsDescriptor(MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_ACCEL_SCALED) + && _param_ms_imu_rate_hz.get() > 0) { + uint16_t imu_decimation = base_rate / (uint16_t)_param_ms_imu_rate_hz.get(); + imu_descriptors[num_imu_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_SENSOR_ACCEL_SCALED, imu_decimation}; + PX4_DEBUG("IMU: Scaled accel enabled with decimation %i", imu_decimation); + } + + if (supportsDescriptor(MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_GYRO_SCALED) + && _param_ms_imu_rate_hz.get() > 0) { + uint16_t imu_decimation = base_rate / (uint16_t)_param_ms_imu_rate_hz.get(); + imu_descriptors[num_imu_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_SENSOR_GYRO_SCALED, imu_decimation}; + PX4_DEBUG("IMU: Scaled gyro enabled with decimation %i", imu_decimation); + } + + if (supportsDescriptor(MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_MAG_SCALED) + && _param_ms_mag_rate_hz.get() > 0) { + uint16_t mag_decimation = base_rate / (uint16_t)_param_ms_mag_rate_hz.get(); + imu_descriptors[num_imu_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_SENSOR_MAG_SCALED, mag_decimation}; + PX4_DEBUG("IMU: Scaled mag enabled with decimation %i", mag_decimation); + } + + if (supportsDescriptor(MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SENSOR_PRESSURE_SCALED) + && _param_ms_baro_rate_hz.get() > 0) { + uint16_t baro_decimation = base_rate / (uint16_t)_param_ms_baro_rate_hz.get(); + imu_descriptors[num_imu_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_SENSOR_PRESSURE_SCALED, baro_decimation}; + PX4_DEBUG("IMU: Scaled pressure enabled with decimation %i", baro_decimation); + } + + if (supportsDescriptor(MIP_SENSOR_DATA_DESC_SET, MIP_DATA_DESC_SHARED_GPS_TIME) + && _param_ms_filter_rate_hz.get() > 0) { + const int max_param_rate = math::max(_param_ms_imu_rate_hz.get(), _param_ms_mag_rate_hz.get(), + _param_ms_baro_rate_hz.get()); + uint16_t gps_time_decimation = base_rate / max_param_rate; + imu_descriptors[num_imu_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_SHARED_GPS_TIME, gps_time_decimation}; + PX4_DEBUG("IMU: GPS Time enabled"); + } + + // Write the settings + res = writeMessageFormat(MIP_SENSOR_DATA_DESC_SET, num_imu_descriptors, + imu_descriptors); + + return res; + +} + +mip_cmd_result MicroStrain::configureFilterMessageFormat() +{ + PX4_DEBUG("Configuring Filter Message Format"); + + uint8_t num_filter_descriptors = 0; + mip_descriptor_rate filter_descriptors[11]; + + // Get the base rate + uint16_t base_rate; + mip_cmd_result res = getBaseRate(MIP_FILTER_DATA_DESC_SET, &base_rate); + + PX4_DEBUG("The filter base rate is %d", base_rate); + + if (!mip_cmd_result_is_ack(res)) { + PX4_ERR("Could not get the filter base rate"); + return res; + } + + // Configure the Message Format depending on if the device supports the descriptor + uint16_t filter_decimation = 250; + + if (_param_ms_filter_rate_hz.get() != 0) { + filter_decimation = base_rate / (uint16_t)_param_ms_filter_rate_hz.get(); + PX4_DEBUG("Filter decimation: %i", filter_decimation); + } + + if (supportsDescriptor(MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_POS_LLH) + && _param_ms_filter_rate_hz.get() > 0) { + filter_descriptors[num_filter_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_FILTER_POS_LLH, filter_decimation}; + PX4_DEBUG("Filter: LLH pos enabled"); + } + + if (supportsDescriptor(MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_ATT_QUATERNION) + && _param_ms_filter_rate_hz.get() > 0) { + filter_descriptors[num_filter_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_FILTER_ATT_QUATERNION, filter_decimation}; + PX4_DEBUG("Filter: Attitude quaternion enabled"); + } + + if (supportsDescriptor(MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_VEL_NED) + && _param_ms_filter_rate_hz.get() > 0) { + filter_descriptors[num_filter_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_FILTER_VEL_NED, filter_decimation}; + PX4_DEBUG("Filter: Velocity NED enabled"); + } + + if (supportsDescriptor(MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_LINEAR_ACCELERATION) + && _param_ms_filter_rate_hz.get() > 0) { + filter_descriptors[num_filter_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_FILTER_LINEAR_ACCELERATION, filter_decimation}; + PX4_DEBUG("Filter: Linear accel FRD enabled"); + } + + if (supportsDescriptor(MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_POS_UNCERTAINTY) + && _param_ms_filter_rate_hz.get() > 0) { + filter_descriptors[num_filter_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_FILTER_POS_UNCERTAINTY, filter_decimation}; + PX4_DEBUG("Filter: Position uncertainty enabled"); + } + + if (supportsDescriptor(MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_VEL_UNCERTAINTY) + && _param_ms_filter_rate_hz.get() > 0) { + filter_descriptors[num_filter_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_FILTER_VEL_UNCERTAINTY, filter_decimation}; + PX4_DEBUG("Filter: Velocity uncertainty enabled"); + } + + if (supportsDescriptor(MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_ATT_UNCERTAINTY_EULER) + && _param_ms_filter_rate_hz.get() > 0) { + filter_descriptors[num_filter_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_FILTER_ATT_UNCERTAINTY_EULER, filter_decimation}; + PX4_DEBUG("Filter: Attitude euler uncertainty enabled"); + } + + if (supportsDescriptor(MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_COMPENSATED_ANGULAR_RATE) + && _param_ms_filter_rate_hz.get() > 0) { + filter_descriptors[num_filter_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_FILTER_COMPENSATED_ANGULAR_RATE, filter_decimation}; + PX4_DEBUG("Filter: Compensated angular rate enabled"); + } + + if (supportsDescriptor(MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_FILTER_STATUS) + && _param_ms_filter_rate_hz.get() > 0) { + filter_descriptors[num_filter_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_FILTER_FILTER_STATUS, filter_decimation}; + PX4_DEBUG("Filter: Status enabled"); + } + + if (supportsDescriptor(MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_FILTER_GNSS_DUAL_ANTENNA_STATUS) + && _param_ms_filter_rate_hz.get() > 0) { + filter_descriptors[num_filter_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_FILTER_GNSS_DUAL_ANTENNA_STATUS, filter_decimation}; + PX4_DEBUG("Filter: GNSS Dual antenna status enabled"); + } + + if (supportsDescriptor(MIP_FILTER_DATA_DESC_SET, MIP_DATA_DESC_SHARED_GPS_TIME) + && _param_ms_filter_rate_hz.get() > 0) { + filter_descriptors[num_filter_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_SHARED_GPS_TIME, filter_decimation}; + PX4_DEBUG("Filter: GPS Time enabled"); + } + + // Write the settings + res = writeMessageFormat(MIP_FILTER_DATA_DESC_SET, num_filter_descriptors, + filter_descriptors); + + return res; +} + +mip_cmd_result MicroStrain::configureGnssMessageFormat(uint8_t descriptor_set) +{ + PX4_DEBUG("Configuring GNSS Message Format"); + + uint8_t num_gnss_descriptors = 0; + mip_descriptor_rate gnss_descriptors[6]; + + // Get the base rate + uint16_t base_rate; + mip_cmd_result res = getBaseRate(descriptor_set, &base_rate); + + PX4_DEBUG("The GNSS base rate is %d", base_rate); + + if (!mip_cmd_result_is_ack(res)) { + PX4_ERR("Could not get the GNSS base rate"); + return res; + } + + uint16_t gnss_decimation = 5; + + if (_param_ms_gnss_rate_hz.get() != 0) { + gnss_decimation = base_rate / (uint16_t)_param_ms_gnss_rate_hz.get(); + PX4_DEBUG("GNSS decimation: %i", gnss_decimation); + } + + // Configure the Message Format depending on if the device supports the descriptor + if (supportsDescriptor(descriptor_set, MIP_DATA_DESC_GNSS_POSITION_LLH) + && _param_ms_gnss_rate_hz.get() > 0) { + gnss_descriptors[num_gnss_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_GNSS_POSITION_LLH, gnss_decimation}; + PX4_DEBUG("GNSS: LLH Pos enabled"); + } + + if (supportsDescriptor(descriptor_set, MIP_DATA_DESC_GNSS_DOP) + && _param_ms_gnss_rate_hz.get() > 0) { + gnss_descriptors[num_gnss_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_GNSS_DOP, gnss_decimation}; + PX4_DEBUG("GNSS: DOP enabled"); + } + + if (supportsDescriptor(descriptor_set, MIP_DATA_DESC_GNSS_VELOCITY_NED) + && _param_ms_gnss_rate_hz.get() > 0) { + gnss_descriptors[num_gnss_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_GNSS_VELOCITY_NED, gnss_decimation}; + PX4_DEBUG("GNSS: Velocity NED enabled"); + } + + if (supportsDescriptor(descriptor_set, MIP_DATA_DESC_SHARED_GPS_TIME) + && _param_ms_gnss_rate_hz.get() > 0) { + gnss_descriptors[num_gnss_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_SHARED_GPS_TIME, gnss_decimation}; + PX4_DEBUG("GNSS: GPS Time enabled"); + } + + if (supportsDescriptor(descriptor_set, MIP_DATA_DESC_GNSS_GPS_LEAP_SECONDS) + && _param_ms_gnss_rate_hz.get() > 0) { + gnss_descriptors[num_gnss_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_GNSS_GPS_LEAP_SECONDS, gnss_decimation}; + PX4_DEBUG("GNSS: GPS Leap seconds enabled"); + } + + if (supportsDescriptor(descriptor_set, MIP_DATA_DESC_GNSS_FIX_INFO) + && _param_ms_gnss_rate_hz.get() > 0) { + gnss_descriptors[num_gnss_descriptors++] = mip_descriptor_rate { MIP_DATA_DESC_GNSS_FIX_INFO, gnss_decimation}; + PX4_DEBUG("GNSS: Fix info enabled"); + } + + // Write the settings + res = writeMessageFormat(descriptor_set, num_gnss_descriptors, + gnss_descriptors); + + return res; + +} + +mip_cmd_result MicroStrain::configureAidingMeasurement(uint16_t aiding_source, bool enable) +{ + mip_cmd_result res; + + // Configures the aiding measurement if the device support it + if (supportsDescriptor(MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_AIDING_MEASUREMENT_ENABLE)) { + res = mip_filter_write_aiding_measurement_enable(&_device, aiding_source, enable); + + // If the device doesnt support the aiding source but the command is to disable it, we consider it a success + if (res == MIP_NACK_INVALID_PARAM && !enable) { + res = MIP_ACK_OK; + } + + } else { + PX4_WARN("Aiding measurements are not supported"); + + // If the command was to disable, we consider it a success + if (!enable) { + res = MIP_ACK_OK; + + } else { + res = MIP_PX4_ERROR; + } + } + + return res; +} + +mip_cmd_result MicroStrain::configureAidingSources() +{ + PX4_DEBUG("Configuring aiding sources"); + mip_cmd_result res; + + // Selectively turn on the magnetometer aiding source + if (!mip_cmd_result_is_ack(res = configureAidingMeasurement( + MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_MAGNETOMETER, + _param_ms_int_mag_en.get()))) { + PX4_ERR("Could not configure magnetometer aiding"); + return res; + } + + // Enables GNSS Position & Velocity as an aiding measurement + res = configureAidingMeasurement(MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_GNSS_POS_VEL, + true); + + if (!mip_cmd_result_is_ack(res)) { + if (res != MIP_NACK_INVALID_PARAM && res != MIP_PX4_ERROR) { + PX4_ERR("Error enabling GNSS Position & Velocity aiding"); + return res; + + } else { + PX4_WARN("Could not enable GNSS Position & Velocity aiding"); + } + + } else { + // Check to see if sending GNSS position and velocity as an aiding measurement is supported + bool pos_aiding = supportsDescriptor(MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_POS_LLH); + bool vel_aiding = supportsDescriptor(MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_VEL_NED); + _ext_pos_vel_aiding = pos_aiding && vel_aiding; + + if (!_ext_pos_vel_aiding) { + PX4_ERR("Sending GNSS pos/vel aiding messages is not supported"); + return MIP_PX4_ERROR; + } + + } + + // Selectively turn on external heading as an aiding measurement + if (!mip_cmd_result_is_ack(res = configureAidingMeasurement( + MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_GNSS_HEADING, + _param_ms_ext_heading_en.get()))) { + PX4_ERR("Could not configure external heading aiding"); + return res; + + } else { + // Check to see if sending external heading as an aiding measurement is supported + _ext_heading_aiding = supportsDescriptor(MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_HEADING_TRUE) + && _param_ms_ext_heading_en.get(); + + if (!_ext_heading_aiding && _param_ms_ext_heading_en.get()) { + PX4_ERR("Sending external heading aiding messages is not supported"); + return MIP_PX4_ERROR; + } + + } + + // Prioritizing setting up multi antenna offsets if it is supported + if (supportsDescriptor(MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MULTI_ANTENNA_OFFSET)) { + mip_cmd_result res1 = mip_filter_write_multi_antenna_offset(&_device, 1, gnss_antenna_offset1); + mip_cmd_result res2 = mip_filter_write_multi_antenna_offset(&_device, 2, gnss_antenna_offset2); + + if (!mip_cmd_result_is_ack(res1)) { + PX4_ERR("Could not write multi antenna offsets"); + return res1; + + } + + else if (!mip_cmd_result_is_ack(res2)) { + PX4_ERR("Could not write multi antenna offsets"); + return res2; + + } + + if (supportsDescriptor(MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GNSS_SOURCE_CONTROL)) { + if (!mip_cmd_result_is_ack(res = mip_filter_write_gnss_source(&_device, (uint8_t)_param_ms_gnss_aid_src_ctrl.get()))) { + PX4_ERR("Could not write the gnss aiding source"); + return res; + + } + + _ext_pos_vel_aiding = (_param_ms_gnss_aid_src_ctrl.get() == MIP_FILTER_GNSS_SOURCE_COMMAND_SOURCE_EXT); + + + } else { + PX4_ERR("Does not support GNSS source control"); + return MIP_PX4_ERROR; + } + + // Selectively enables dual antenna heading as an aiding measurement + if (!mip_cmd_result_is_ack(res = configureAidingMeasurement( + MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_GNSS_HEADING, + _param_ms_int_heading_en.get()))) { + PX4_ERR("Could not configure dual antenna heading aiding"); + return res; + } + + } + + // Otherwise sets up the aiding frame + else if (supportsDescriptor(MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG)) { + if (!mip_cmd_result_is_ack(res = mip_aiding_write_frame_config(&_device, 1, + MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER, false, + gnss_antenna_offset1, &rotation))) { + PX4_ERR("Could not write aiding frame config"); + return res; + } + } + + else { + PX4_WARN("Aiding frames are not supported"); + } + + _ext_aiding = (_ext_pos_vel_aiding || _ext_heading_aiding); + + return res; +} + +mip_cmd_result MicroStrain::writeFilterInitConfig() +{ + PX4_DEBUG("Initializing filter"); + mip_cmd_result res; + + float filter_init_pos[3] = {0}; + float filter_init_vel[3] = {0}; + + const uint8_t initial_alignment = _param_ms_alignment.get(); + + // Filter initialization configuration + if (supportsDescriptor(MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_INITIALIZATION_CONFIGURATION)) { + res = mip_filter_write_initialization_configuration(&_device, 0, + MIP_FILTER_INITIALIZATION_CONFIGURATION_COMMAND_INITIAL_CONDITION_SOURCE_AUTO_POS_VEL_ATT, + initial_alignment, + 0.0, 0.0, 0.0, filter_init_pos, filter_init_vel, MIP_FILTER_REFERENCE_FRAME_LLH); + } + + else { + PX4_WARN("Filter initialization is not supported"); + res = MIP_ACK_OK; + } + + return res; +} + +bool MicroStrain::initializeIns() +{ + mip_cmd_result res; + + const uint32_t DESIRED_BAUDRATE = 921600; + + static constexpr uint32_t BAUDRATES[] {115200, 921600, 460800, 230400, 128000, 38400, 19200, 57600, 9600}; + bool is_connected = false; + + for (auto &baudrate : BAUDRATES) { + if (connectAtBaud(baudrate) == PX4_OK) { + PX4_INFO("found baudrate %lu", baudrate); + is_connected = true; + break; + } + } + + if (!is_connected) { + PX4_ERR("Could not connect to the device, exiting"); + return false; + } + + // Get the supported descriptors for the device in use + if (!mip_cmd_result_is_ack(getSupportedDescriptors())) { + PX4_ERR("Could not get descriptors"); + return false; + } + + // Setting the device baudrate to the desired value + PX4_INFO("Setting the baud to desired baud rate"); + + if (!mip_cmd_result_is_ack(res = writeBaudRate(DESIRED_BAUDRATE, 1))) { + PX4_ERR("Could not set the baudrate!"); + return false; + } + + device_uart.flush(); + + // Connecting using the desired baudrate + if (connectAtBaud(DESIRED_BAUDRATE) != PX4_OK) { + PX4_ERR("Could not Connect at %lu", DESIRED_BAUDRATE); + return false; + } + + // Configure IMU ranges + if (!mip_cmd_result_is_ack(res = configureImuRange())) { + MS_PX4_ERROR(res, "Could not configure IMU range"); + return false; + } + + // Configure the IMU message formt based on what descriptors are supported + if (!mip_cmd_result_is_ack(res = configureImuMessageFormat())) { + MS_PX4_ERROR(res, "Could not write IMU message format"); + return false; + } + + // Register data callbacks + mip_interface_register_packet_callback(&_device, &_sensor_data_handler, MIP_SENSOR_DATA_DESC_SET, false, + &sensorCallback, + this); + + // Configure the Filter message format based on what descriptors are supported + if (!mip_cmd_result_is_ack(res = configureFilterMessageFormat())) { + MS_PX4_ERROR(res, "Could not write filter message format"); + return false; + } + + // Register data callbacks + mip_interface_register_packet_callback(&_device, &_filter_data_handler, MIP_FILTER_DATA_DESC_SET, false, + &filterCallback, + this); + + // Configure the GNSS1 message format based on what descriptors are supported + if (!mip_cmd_result_is_ack(res = configureGnssMessageFormat(MIP_GNSS1_DATA_DESC_SET))) { + MS_PX4_ERROR(res, "Could not write GNSS1 message format"); + } + + // Register data callbacks + mip_interface_register_packet_callback(&_device, &_gnss_data_handler[0], MIP_GNSS1_DATA_DESC_SET, false, + &gnssCallback, + this); + + // Configure the GNSS2 message format based on what descriptors are supported + if (!mip_cmd_result_is_ack(res = configureGnssMessageFormat(MIP_GNSS2_DATA_DESC_SET))) { + MS_PX4_ERROR(res, "Could not write GNSS2 message format"); + } + + // Register data callbacks + mip_interface_register_packet_callback(&_device, &_gnss_data_handler[1], MIP_GNSS2_DATA_DESC_SET, false, + &gnssCallback, + this); + + // Configure the aiding sources based on what the sensor supports + if (!mip_cmd_result_is_ack(res = configureAidingSources())) { + MS_PX4_ERROR(res, "Could not configure aiding frames!"); + return false; + } + + // Initialize the filter + if (!mip_cmd_result_is_ack(res = writeFilterInitConfig())) { + MS_PX4_ERROR(res, "Could not configure filter initialization!"); + return false; + } + + // Setup the rotation based on PX4 standard rotation sets + if (_param_ms_svt_en.get() && supportsDescriptor(MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_SENSOR2VEHICLE_TRANSFORM_EUL)) { + PX4_DEBUG("Writing SVT"); + + if (!mip_cmd_result_is_ack(res = mip_3dm_write_sensor_2_vehicle_transform_euler(&_device, + math::radians(rotation.euler[0]), + math::radians(rotation.euler[1]), math::radians(rotation.euler[2])))) { + MS_PX4_ERROR(res, "Could not set sensor-to-vehicle transformation!"); + return false; + } + } + + // Reset the filter + if (supportsDescriptor(MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_RESET_FILTER)) { + PX4_DEBUG("Reseting filter"); + + if (!mip_cmd_result_is_ack(res = mip_filter_reset(&_device))) { + MS_PX4_ERROR(res, "Could not reset the filter!"); + return false; + } + } + + if (supportsDescriptor(MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_CONTROL_DATA_STREAM)) { + PX4_DEBUG("Writing datastream control"); + + if (!mip_cmd_result_is_ack(res = mip_3dm_write_datastream_control(&_device, + MIP_3DM_DATASTREAM_CONTROL_COMMAND_ALL_STREAMS, true))) { + MS_PX4_ERROR(res, "Could not enable the data stream"); + return false; + } + } + + // Resume the device + if (supportsDescriptor(MIP_BASE_CMD_DESC_SET, MIP_CMD_DESC_BASE_RESUME)) { + PX4_DEBUG("Resuming device"); + + if (!mip_cmd_result_is_ack(res = mip_base_resume(&_device))) { + MS_PX4_ERROR(res, "Could not resume the device!"); + return false; + } + } + + return true; +} + +void MicroStrain::sensorCallback(void *user, const mip_packet *packet, mip::Timestamp timestamp) +{ + MicroStrain *ref = static_cast(user); + + assert(mip_packet_descriptor_set(packet) == MIP_SENSOR_DATA_DESC_SET); + + SensorSample accel; + SensorSample gyro; + SensorSample mag; + SensorSample baro; + + // Iterate through the packet and extract based on the descriptor present + auto t = hrt_absolute_time(); + + for (mip_field field = mip_field_first_from_packet(packet); mip_field_is_valid(&field); mip_field_next(&field)) { + switch (mip_field_field_descriptor(&field)) { + + case MIP_DATA_DESC_SENSOR_ACCEL_SCALED: + extract_mip_sensor_scaled_accel_data_from_field(&field, &accel.sample); + accel.updated = true; + break; + + + case MIP_DATA_DESC_SENSOR_GYRO_SCALED: + extract_mip_sensor_scaled_gyro_data_from_field(&field, &gyro.sample); + gyro.updated = true; + break; + + + case MIP_DATA_DESC_SENSOR_MAG_SCALED: + extract_mip_sensor_scaled_mag_data_from_field(&field, &mag.sample); + mag.updated = true; + break; + + + case MIP_DATA_DESC_SENSOR_PRESSURE_SCALED: + extract_mip_sensor_scaled_pressure_data_from_field(&field, &baro.sample); + baro.updated = true; + break; + + + default: + break; + + + } + } + + // Publish only if the corresponding data was extracted from the packet + if (accel.updated) { + ref->_px4_accel.update(t, accel.sample.scaled_accel[0]*CONSTANTS_ONE_G, + accel.sample.scaled_accel[1]*CONSTANTS_ONE_G, + accel.sample.scaled_accel[2]*CONSTANTS_ONE_G); + + } + + if (gyro.updated) { + ref->_px4_gyro.update(t, gyro.sample.scaled_gyro[0], + gyro.sample.scaled_gyro[1], + gyro.sample.scaled_gyro[2]); + } + + if (mag.updated) { + ref->_px4_mag.update(t, mag.sample.scaled_mag[0], + mag.sample.scaled_mag[1], + mag.sample.scaled_mag[2]); + } + + if (baro.updated) { + ref->_sensor_baro.timestamp_sample = t; + ref->_sensor_baro.pressure = baro.sample.scaled_pressure * 100.f; // convert [Pa] to [mBar] + ref->_sensor_baro.timestamp = hrt_absolute_time(); + ref->_sensor_baro_pub.publish(ref->_sensor_baro); + } +} + +void MicroStrain::filterCallback(void *user, const mip_packet *packet, mip::Timestamp timestamp) +{ + MicroStrain *ref = static_cast(user); + + assert(mip_packet_descriptor_set(packet) == MIP_FILTER_DATA_DESC_SET); + + SensorSample pos_llh; + SensorSample att_quat; + SensorSample ang_rate; + SensorSample vel_ned; + SensorSample stat; + SensorSample llh_uncert; + SensorSample vel_uncert; + SensorSample att_euler_uncert; + SensorSample lin_accel; + + // Iterate through the packet and extract based on the descriptor present + auto t = hrt_absolute_time(); + + for (mip_field field = mip_field_first_from_packet(packet); mip_field_is_valid(&field); mip_field_next(&field)) { + switch (mip_field_field_descriptor(&field)) { + case MIP_DATA_DESC_FILTER_POS_LLH: + extract_mip_filter_position_llh_data_from_field(&field, &pos_llh.sample); + pos_llh.updated = true; + break; + + case MIP_DATA_DESC_FILTER_ATT_QUATERNION: + extract_mip_filter_attitude_quaternion_data_from_field(&field, &att_quat.sample); + att_quat.updated = true; + break; + + case MIP_DATA_DESC_FILTER_VEL_NED: + extract_mip_filter_velocity_ned_data_from_field(&field, &vel_ned.sample); + vel_ned.updated = true ; + break; + + case MIP_DATA_DESC_FILTER_LINEAR_ACCELERATION: + extract_mip_filter_linear_accel_data_from_field(&field, &lin_accel.sample); + lin_accel.updated = true; + break; + + case MIP_DATA_DESC_FILTER_POS_UNCERTAINTY: + extract_mip_filter_position_llh_uncertainty_data_from_field(&field, &llh_uncert.sample); + llh_uncert.updated = true; + break; + + case MIP_DATA_DESC_FILTER_VEL_UNCERTAINTY: + extract_mip_filter_velocity_ned_uncertainty_data_from_field(&field, &vel_uncert.sample); + vel_uncert.updated = true; + break; + + case MIP_DATA_DESC_FILTER_ATT_UNCERTAINTY_EULER: + extract_mip_filter_euler_angles_uncertainty_data_from_field(&field, &att_euler_uncert.sample); + att_euler_uncert.updated = true; + break; + + case MIP_DATA_DESC_FILTER_COMPENSATED_ANGULAR_RATE: + extract_mip_filter_comp_angular_rate_data_from_field(&field, &ang_rate.sample); + ang_rate.updated = true; + break; + + case MIP_DATA_DESC_FILTER_FILTER_STATUS: + extract_mip_filter_status_data_from_field(&field, &stat.sample); + stat.updated = true; + break; + + case MIP_DATA_DESC_FILTER_GNSS_DUAL_ANTENNA_STATUS: + extract_mip_filter_gnss_dual_antenna_status_data_from_field(&field, &ref->dual_ant_stat); + break; + + default: + break; + } + } + + // Publish only if the required data was extracted from the packet + bool vehicle_global_position_valid = pos_llh.updated && llh_uncert.updated && stat.updated; + bool vehicle_attitude_valid = att_quat.updated; + bool vehicle_local_position_valid = pos_llh.updated && att_quat.updated && vel_ned.updated && lin_accel.updated + && llh_uncert.updated && vel_uncert.updated && att_euler_uncert.updated && stat.updated; + bool vehicle_odometry_valid = pos_llh.updated && att_quat.updated && vel_ned.updated && llh_uncert.updated + && vel_uncert.updated && att_euler_uncert.updated && ang_rate.updated; + bool estimator_status_valid = stat.updated && llh_uncert.updated; + + if (vehicle_global_position_valid) { + vehicle_global_position_s gp{0}; + gp.timestamp_sample = t; + bool is_fullnav = (stat.sample.filter_state == MIP_FILTER_MODE_FULL_NAV); + + gp.lat = pos_llh.sample.latitude; + gp.lon = pos_llh.sample.longitude; + + gp.lat_lon_valid = is_fullnav && pos_llh.sample.valid_flags; + + // gp.alt is supposed to be in MSL, since the filter only estimates ellipsoid, we publish that instead. + gp.alt_ellipsoid = pos_llh.sample.ellipsoid_height; + gp.alt = pos_llh.sample.ellipsoid_height - (double)ref->_geoid_height_lpf.getState(); + + gp.alt_valid = is_fullnav && pos_llh.sample.valid_flags; + + gp.eph = sqrtf(sq(llh_uncert.sample.north) + sq(llh_uncert.sample.east) + sq(ref->_gps_origin_ep[0])); + gp.epv = sqrtf(sq(llh_uncert.sample.down) + sq(ref->_gps_origin_ep[1])); + + // ------- Fields we cannot obtain ------- + gp.delta_alt = 0; + gp.delta_terrain = 0; + gp.lat_lon_reset_counter = 0; + gp.alt_reset_counter = 0; + gp.terrain_reset_counter = 0; + + gp.dead_reckoning = false; + gp.terrain_alt_valid = false; + gp.terrain_alt = 0; + // --------------------------------------- + + gp.timestamp = hrt_absolute_time(); + ref->_vehicle_global_position_pub.publish(gp); + } + + if (vehicle_attitude_valid) { + vehicle_attitude_s att_data{0}; + att_data.timestamp_sample = t; + + att_data.q[0] = att_quat.sample.q[0]; + att_data.q[1] = att_quat.sample.q[1]; + att_data.q[2] = att_quat.sample.q[2]; + att_data.q[3] = att_quat.sample.q[3]; + + // ------- Fields we cannot obtain ------- + att_data.delta_q_reset[0] = 0; + att_data.delta_q_reset[0] = 0; + att_data.delta_q_reset[0] = 0; + att_data.delta_q_reset[0] = 0; + att_data.quat_reset_counter = 0; + // --------------------------------------- + + att_data.timestamp = hrt_absolute_time(); + ref->_vehicle_attitude_pub.publish(att_data); + } + + if (vehicle_local_position_valid) { + vehicle_local_position_s vp{0}; + vp.timestamp_sample = t; + bool is_fullNav = (stat.sample.filter_state == MIP_FILTER_MODE_FULL_NAV); + + const Vector2f pos_ned = ref->_pos_ref.project(pos_llh.sample.latitude, pos_llh.sample.longitude); + + vp.x = pos_ned(0); + vp.y = pos_ned(1); + vp.z = -((pos_llh.sample.ellipsoid_height - (double)ref->_geoid_height_lpf.getState()) - ref->_ref_alt); + + vp.xy_valid = is_fullNav && ref->_pos_ref.isInitialized(); + + vp.z_valid = is_fullNav && ref->_pos_ref.isInitialized() && pos_llh.sample.valid_flags; + + vp.vx = vel_ned.sample.north; + vp.vy = vel_ned.sample.east; + vp.vz = vel_ned.sample.down; + vp.z_deriv = vp.vz; + + vp.v_xy_valid = is_fullNav && vel_ned.sample.valid_flags; + + vp.v_z_valid = is_fullNav && vel_ned.sample.valid_flags; + + // Conversion of linear acceleration from body frame to NED frame + matrix::Quatf quat{att_quat.sample.q[0], att_quat.sample.q[1], att_quat.sample.q[2], att_quat.sample.q[3]}; + matrix::Vector3f acc_body{lin_accel.sample.accel[0], lin_accel.sample.accel[1], lin_accel.sample.accel[2]}; + matrix::Vector3f acc_ned = quat.rotateVectorInverse(acc_body); + + vp.ax = acc_ned(0); + vp.ay = acc_ned(1); + vp.az = acc_ned(2); + + // Get yaw from attitude for heading + matrix::Eulerf euler_attitude(matrix::Quatf(att_quat.sample.q[0], att_quat.sample.q[1], att_quat.sample.q[2], + att_quat.sample.q[3])); + float yaw = euler_attitude.psi(); + + vp.heading_good_for_control = (!std::isnan(yaw)); + vp.heading = yaw; + vp.heading_var = att_euler_uncert.sample.yaw; + + // vp.ref_alt is supposed to be in MSL, since the filter only estimates ellipsoid, we publish that instead. + vp.xy_global = (ref->_pos_ref.isInitialized() == true); + vp.z_global = (ref->_pos_ref.isInitialized() == true); + vp.ref_timestamp = ref->_pos_ref.getProjectionReferenceTimestamp(); + vp.ref_lat = ref->_pos_ref.getProjectionReferenceLat(); + vp.ref_lon = ref->_pos_ref.getProjectionReferenceLon(); + vp.ref_alt = ref->_ref_alt; + + vp.eph = sqrtf(sq(llh_uncert.sample.north) + sq(llh_uncert.sample.east)); + vp.epv = llh_uncert.sample.down; + vp.evh = sqrtf(sq(vel_uncert.sample.north) + sq(vel_uncert.sample.east)); + vp.evv = vel_uncert.sample.down; + + vp.vxy_max = INFINITY; + vp.vz_max = INFINITY; + vp.hagl_min = INFINITY; + vp.hagl_max_xy = INFINITY; + vp.hagl_max_z = INFINITY; + + // ------- Fields we cannot obtain ------- + vp.delta_xy[0] = 0; + vp.delta_xy[1] = 0; + vp.xy_reset_counter = 0; + vp.delta_z = 0; + vp.z_reset_counter = 0; + + vp.delta_vxy[0] = 0; + vp.delta_vxy[1] = 0; + vp.vxy_reset_counter = 0; + vp.delta_vz = 0; + vp.vz_reset_counter = 0; + + vp.unaided_heading = NAN; + vp.delta_heading = 0; + vp.heading_reset_counter = 0; + + vp.tilt_var = 0; + + vp.dist_bottom_valid = 0; + vp.dist_bottom = 0; + vp.dist_bottom_var = 0; + vp.delta_dist_bottom = 0; + vp.dist_bottom_reset_counter = 0; + vp.dist_bottom_sensor_bitfield = 0; + + vp.dead_reckoning = false; + // --------------------------------------- + + vp.timestamp = hrt_absolute_time(); + ref->_vehicle_local_position_pub.publish(vp); + } + + if (vehicle_odometry_valid && ref->_param_ms_mode.get()) { + vehicle_odometry_s vo{0}; + vo.timestamp_sample = t; + + const Vector2f pos_ned = ref->_pos_ref.project(pos_llh.sample.latitude, pos_llh.sample.longitude); + + vo.pose_frame = 1; + vo.position[0] = pos_ned(0); + vo.position[1] = pos_ned(1); + vo.position[2] = -((pos_llh.sample.ellipsoid_height - (double)ref->_geoid_height_lpf.getState()) - ref->_ref_alt); + + vo.q[0] = att_quat.sample.q[0]; + vo.q[1] = att_quat.sample.q[1]; + vo.q[2] = att_quat.sample.q[2]; + vo.q[3] = att_quat.sample.q[3]; + + vo.velocity_frame = 1; + vo.velocity[0] = vel_ned.sample.north; + vo.velocity[1] = vel_ned.sample.east; + vo.velocity[2] = vel_ned.sample.down; + + vo.angular_velocity[0] = ang_rate.sample.gyro[0]; + vo.angular_velocity[1] = ang_rate.sample.gyro[1]; + vo.angular_velocity[2] = ang_rate.sample.gyro[2]; + + vo.position_variance[0] = sq(llh_uncert.sample.north); + vo.position_variance[1] = sq(llh_uncert.sample.east); + vo.position_variance[2] = sq(llh_uncert.sample.down); + + vo.velocity_variance[0] = sq(vel_uncert.sample.north); + vo.velocity_variance[1] = sq(vel_uncert.sample.east); + vo.velocity_variance[2] = sq(vel_uncert.sample.down); + + vo.orientation_variance[0] = sq(att_euler_uncert.sample.roll); + vo.orientation_variance[1] = sq(att_euler_uncert.sample.pitch); + vo.orientation_variance[2] = sq(att_euler_uncert.sample.yaw); + + // ------- Fields we cannot obtain ------- + vo.reset_counter = 0; + vo.quality = 0; + // --------------------------------------- + + vo.timestamp = hrt_absolute_time(); + ref->_vehicle_odometry_pub.publish(vo); + } + + if (estimator_status_valid && ref->_param_ms_mode.get()) { + estimator_status_s status{0}; + status.timestamp_sample = t; + + status.output_tracking_error[0] = 0; + status.output_tracking_error[1] = 0; + status.output_tracking_error[2] = 0; + + status.gps_check_fail_flags = 0; + + // Minimal mapping of error flags from device to the PX4 health flags + status.control_mode_flags = stat.sample.filter_state == 4 ? (0x1ULL << estimator_status_s::CS_GNSS_POS) | + (0x1ULL << estimator_status_s::CS_GNSS_VEL) | + (0x1ULL << estimator_status_s::CS_GPS_HGT) : 0x00; + status.filter_fault_flags = stat.sample.status_flags; + + status.pos_horiz_accuracy = sqrtf(sq(llh_uncert.sample.north) + sq(llh_uncert.sample.east)); + status.pos_vert_accuracy = llh_uncert.sample.down; + + status.hdg_test_ratio = 0.1f; + status.vel_test_ratio = 0.1f; + status.pos_test_ratio = 0.1f; + status.hgt_test_ratio = 0.1f; + status.tas_test_ratio = 0.1f; + status.hagl_test_ratio = 0.1f; + status.beta_test_ratio = 0.1f; + + status.solution_status_flags = 0; + + status.reset_count_vel_ne = 0; + status.reset_count_vel_d = 0; + status.reset_count_pos_ne = 0; + status.reset_count_pod_d = 0; + status.reset_count_quat = 0; + + status.time_slip = 0; + + status.pre_flt_fail_innov_heading = false; + status.pre_flt_fail_innov_height = false; + status.pre_flt_fail_innov_pos_horiz = false; + status.pre_flt_fail_innov_vel_horiz = false; + status.pre_flt_fail_innov_vel_vert = false; + status.pre_flt_fail_mag_field_disturbed = false; + + status.accel_device_id = ref->_dev_id; + status.gyro_device_id = ref->_dev_id; + status.mag_device_id = ref->_dev_id; + status.baro_device_id = ref->_dev_id; + + status.health_flags = 0; + status.timeout_flags = 0; + + status.mag_inclination_deg = 0; + status.mag_inclination_ref_deg = 0; + status.mag_strength_gs = 0; + status.mag_strength_ref_gs = 0; + + + status.timestamp = hrt_absolute_time(); + ref->_estimator_status_pub.publish(status); + + sensor_selection_s sensor_selection{}; + sensor_selection.accel_device_id = ref->_dev_id; + sensor_selection.gyro_device_id = ref->_dev_id; + + sensor_selection.timestamp = hrt_absolute_time(); + ref->_sensor_selection_pub.publish(sensor_selection); + } +} + +void MicroStrain::gnssCallback(void *user, const mip_packet *packet, mip::Timestamp timestamp) +{ + MicroStrain *ref = static_cast(user); + + int instance = 0; + + assert((mip_packet_descriptor_set(packet) == MIP_GNSS1_DATA_DESC_SET) + || (mip_packet_descriptor_set(packet) == MIP_GNSS2_DATA_DESC_SET)); + + if (mip_packet_descriptor_set(packet) == MIP_GNSS2_DATA_DESC_SET) { + instance = 1; + } + + SensorSample pos_llh; + SensorSample dop; + SensorSample vel_ned; + SensorSample gps_time; + SensorSample gps_leap_sec; + SensorSample sat; + SensorSample fix_info; + + + // Iterate through the packet and extract based on the descriptor present + auto t = hrt_absolute_time(); + + for (mip_field field = mip_field_first_from_packet(packet); mip_field_is_valid(&field); mip_field_next(&field)) { + switch (mip_field_field_descriptor(&field)) { + + case MIP_DATA_DESC_GNSS_POSITION_LLH: + extract_mip_gnss_pos_llh_data_from_field(&field, &pos_llh.sample); + pos_llh.updated = true; + break; + + case MIP_DATA_DESC_GNSS_DOP: + extract_mip_gnss_dop_data_from_field(&field, &dop.sample); + dop.updated = true; + break; + + case MIP_DATA_DESC_GNSS_VELOCITY_NED: + extract_mip_gnss_vel_ned_data_from_field(&field, &vel_ned.sample); + vel_ned.updated = true; + break; + + case MIP_DATA_DESC_SHARED_GPS_TIME: + extract_mip_shared_gps_timestamp_data_from_field(&field, &gps_time.sample); + gps_time.updated = true; + break; + + case MIP_DATA_DESC_GNSS_GPS_LEAP_SECONDS: + extract_mip_gnss_gps_leap_seconds_data_from_field(&field, &gps_leap_sec.sample); + gps_leap_sec.updated = true; + break; + + case MIP_DATA_DESC_GNSS_FIX_INFO: + extract_mip_gnss_fix_info_data_from_field(&field, &fix_info.sample); + fix_info.updated = true; + break; + + default: + break; + + } + } + + bool gnss_valid = pos_llh.updated && dop.updated && vel_ned.updated && gps_leap_sec.updated && fix_info.updated; + + // Publish only if the corresponding data was extracted from the packet + if (gnss_valid) { + + sensor_gps_s gps{0}; + gps.timestamp_sample = t; + + gps.device_id = ref->_dev_id; + + gps.latitude_deg = pos_llh.sample.latitude; + gps.longitude_deg = pos_llh.sample.longitude; + gps.altitude_msl_m = pos_llh.sample.msl_height; + gps.altitude_ellipsoid_m = pos_llh.sample.ellipsoid_height; + + const float _geoid_height = pos_llh.sample.ellipsoid_height - pos_llh.sample.msl_height; + + gps.s_variance_m_s = vel_ned.sample.speed_accuracy; + gps.c_variance_rad = 0; + + switch (fix_info.sample.fix_type) { + case 0: + gps.fix_type = sensor_gps_s::FIX_TYPE_3D; + break; + + case 1: + gps.fix_type = sensor_gps_s::FIX_TYPE_2D; + break; + + case 5: + gps.fix_type = sensor_gps_s::FIX_TYPE_RTK_FLOAT; + break; + + case 6: + gps.fix_type = sensor_gps_s::FIX_TYPE_RTK_FIXED; + break; + + case 7: + gps.fix_type = sensor_gps_s::FIX_TYPE_RTCM_CODE_DIFFERENTIAL; + break; + + default: + gps.fix_type = sensor_gps_s::FIX_TYPE_NONE; + } + + gps.eph = pos_llh.sample.horizontal_accuracy; + gps.epv = pos_llh.sample.vertical_accuracy; + + gps.hdop = dop.sample.hdop; + gps.vdop = dop.sample.vdop; + + gps.noise_per_ms = 0; + gps.automatic_gain_control = 0; + + gps.jamming_state = 0; + gps.jamming_indicator = 0; + + gps.spoofing_state = 0; + + gps.vel_m_s = vel_ned.sample.speed; + gps.vel_n_m_s = vel_ned.sample.v[0]; + gps.vel_e_m_s = vel_ned.sample.v[1]; + gps.vel_d_m_s = vel_ned.sample.v[2]; + gps.cog_rad = 0; + gps.vel_ned_valid = (vel_ned.sample.valid_flags >> 1) & 1; + + gps.timestamp_time_relative = 0; // + gps.time_utc_usec = ((gps_time.sample.week_number * 604800) + gps_time.sample.tow + 315964800 - + gps_leap_sec.sample.leap_seconds) * 1000000; + + gps.satellites_used = fix_info.sample.num_sv; + + gps.heading = ref->dual_ant_stat.heading; + gps.heading_offset = 0; + gps.heading_accuracy = 0; + + gps.rtcm_injection_rate = 0; + gps.selected_rtcm_instance = 0; + gps.rtcm_crc_failed = 0; + + gps.rtcm_msg_used = 0; + + gps.timestamp = hrt_absolute_time(); + + if (instance == 0) {ref->updateGeoidHeight(_geoid_height, gps.timestamp);} + + ref->_sensor_gps_pub[instance].publish(gps); + } +} + +void MicroStrain::initializeRefPos() +{ + sensor_gps_s gps{0}; + + _vehicle_gps_position_sub.update(&gps); + + // Fix isn't 3D or RTK or RTCM + if ((gps.fix_type < 3) || (gps.fix_type > 6)) { + return; + } + + // If the timestamp has not been set, then don't send any data into the filter + if (gps.time_utc_usec == 0) { + return; + } + + const hrt_abstime t = hrt_absolute_time(); + _pos_ref.initReference(gps.latitude_deg, gps.longitude_deg, t); + _ref_alt = gps.altitude_msl_m; + + _gps_origin_ep[0] = gps.eph; + _gps_origin_ep[1] = gps.epv; + + PX4_DEBUG("Reference position initialized"); +} + +void MicroStrain::updateGeoidHeight(float geoid_height, float t) +{ + // Updates the low pass filter for geoid height + if (_last_geoid_height_update_us == 0) { + _geoid_height_lpf.reset(geoid_height); + _last_geoid_height_update_us = t; + + } else if (t > _last_geoid_height_update_us) { + const float dt = 1e-6f * (t - _last_geoid_height_update_us); + _geoid_height_lpf.setParameters(dt, kGeoidHeightLpfTimeConstant); + _geoid_height_lpf.update(geoid_height); + _last_geoid_height_update_us = t; + } +} + +void MicroStrain::sendAidingMeasurements() +{ + sensor_gps_s gps{0}; + + // No new data + if (!_vehicle_gps_position_sub.update(&gps)) { + return; + } + + // Fix isn't 3D or RTK or RTCM + if ((gps.fix_type < 3) || (gps.fix_type > 6)) { + return; + } + + // If the timestamp has not been set, then don't send any data into the filter + if (gps.time_utc_usec == 0) { + return; + } + + mip_time t; + t.timebase = MIP_TIME_TIMEBASE_TIME_OF_ARRIVAL; + t.reserved = 0x01; + t.nanoseconds = 0; + + // Sends GNSS position and velocity aiding data if they are both supported + if (_ext_pos_vel_aiding) { + float llh_uncertainty[3] = {gps.eph, gps.eph, gps.epv}; + mip_aiding_llh_pos(&_device, &t, MIP_FILTER_REFERENCE_FRAME_LLH, gps.latitude_deg, + gps.longitude_deg, + gps.altitude_ellipsoid_m, llh_uncertainty, MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_ALL); + + // Calculate the geoid height and update the low pass filter + const float _geoid_height = gps.altitude_ellipsoid_m - gps.altitude_msl_m; + updateGeoidHeight(_geoid_height, gps.timestamp); + + if (gps.vel_ned_valid) { + float ned_v[3] = {gps.vel_n_m_s, gps.vel_e_m_s, gps.vel_d_m_s}; + float ned_velocity_uncertainty[3] = {sqrtf(gps.s_variance_m_s), sqrtf(gps.s_variance_m_s), sqrtf(gps.s_variance_m_s)}; + mip_aiding_ned_vel(&_device, &t, MIP_FILTER_REFERENCE_FRAME_LLH, ned_v, ned_velocity_uncertainty, + MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_ALL); + } + } + + // Sends external heading aiding data if they are both supported + if (_ext_heading_aiding && PX4_ISFINITE(gps.heading)) { + float heading = gps.heading + gps.heading_offset; + mip_aiding_true_heading(&_device, &t, MIP_FILTER_REFERENCE_FRAME_LLH, heading, gps.heading_accuracy, 0xff); + } +} + +bool MicroStrain::init() +{ + // Run on fixed interval + ScheduleOnInterval(_ms_schedule_rate_us); + + return true; +} + +void MicroStrain::Run() +{ + + if (should_exit()) { + ScheduleClear(); + exit_and_cleanup(); + return; + } + + perf_begin(_loop_perf); + perf_count(_loop_interval_perf); + + if (!_is_initialized) { + _is_initialized = initializeIns(); + PX4_INFO("Initialization complete"); + } + + // Initialization failed, stop the module + if (!_is_initialized) { + request_stop(); + perf_end(_loop_perf); + return; + } + + // Check if parameters have changed + if (_parameter_update_sub.updated()) { + // clear update + parameter_update_s param_update; + _parameter_update_sub.copy(¶m_update); + updateParams(); // update module parameters (in DEFINE_PARAMETERS) + + } + + mip_interface_update(&_device, false); + + //Initializes reference position if there is gps data + if (_vehicle_gps_position_sub.updated() && !_pos_ref.isInitialized()) {initializeRefPos();} + + // Sends aiding data only if external aiding was set up + if (_ext_aiding) {sendAidingMeasurements();} + + perf_end(_loop_perf); +} + +int MicroStrain::task_spawn(int argc, char *argv[]) +{ + int ch; + int myoptind = 1; + const char *myoptarg = nullptr; + + const char *dev = "/dev/ttyS4"; + + while ((ch = px4_getopt(argc, argv, "d:", &myoptind, &myoptarg)) != EOF) { + switch (ch) { + case 'd': + dev = myoptarg; + break; + + default: + PX4_WARN("Unrecognized option, Using defaults"); + break; + } + } + + if (dev == nullptr || strlen(dev) == 0) { + print_usage("no device specified"); + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; + } + + PX4_INFO("Opening device port %s", dev); + MicroStrain *instance = new MicroStrain(dev); + + if (instance) { + _object.store(instance); + _task_id = task_id_is_work_queue; + + if (instance->init()) { + return PX4_OK; + } + + } else { + PX4_ERR("alloc failed"); + } + + delete instance; + _object.store(nullptr); + _task_id = -1; + + return PX4_ERROR; +} + +int MicroStrain::print_status() +{ + perf_print_counter(_loop_perf); + perf_print_counter(_loop_interval_perf); + return 0; +} + +int MicroStrain::custom_command(int argc, char *argv[]) +{ + return print_usage("unknown command"); +} + +int MicroStrain::print_usage(const char *reason) +{ + if (reason) { + PX4_WARN("%s\n", reason); + } + + PRINT_MODULE_DESCRIPTION( + R"DESCR_STR( +### Description + +MicroStrain by HBK Inertial Sensor Driver. +Currently supports the following sensors: + +-[CV7-AR](https://www.hbkworld.com/en/products/transducers/inertial-sensors/vertical-reference-units--vru-/3dm-cv7-ar) +-[CV7-AHRS](https://www.hbkworld.com/en/products/transducers/inertial-sensors/attitude-and-heading-reference-systems--ahrs-/3dm-cv7-ahrs) +-[CV7-INS](https://www.hbkworld.com/en/products/transducers/inertial-sensors/inertial-navigation-systems--ins-/3dm-cv7-ins) +-[CV7-GNSS/INS](https://www.hbkworld.com/en/products/transducers/inertial-sensors/inertial-navigation-systems--ins-/3dm-cv7-gnss-ins) + +This driver is not included in the firmware by default. +Include the module in firmware by setting the +[kconfig](../hardware/porting_guide_config.md#px4-board-configuration-kconfig) variables: +`CONFIG_DRIVERS_INS_MICROSTRAIN` or `CONFIG_COMMON_INS`. + +### Examples + +Attempt to start driver on a specified serial device. +The driver will choose /dev/ttyS4 by default if no port is specified +$ microstrain start -d /dev/ttyS1 +Stop driver +$ microstrain stop +)DESCR_STR"); + + PRINT_MODULE_USAGE_NAME("MicroStrain", "driver"); + PRINT_MODULE_USAGE_SUBCATEGORY("ins"); + PRINT_MODULE_USAGE_COMMAND_DESCR("start", "Start driver"); + PRINT_MODULE_USAGE_PARAM_STRING('d', "/dev/ttyS4", "", "Port", true); + PRINT_MODULE_USAGE_COMMAND_DESCR("stop", "Stop driver"); + PRINT_MODULE_USAGE_COMMAND_DESCR("status", "Driver status"); + + return PX4_OK; +} + +extern "C" __EXPORT int microstrain_main(int argc, char *argv[]) +{ + return MicroStrain::main(argc, argv); +} diff --git a/src/drivers/ins/microstrain/MicroStrain.hpp b/src/drivers/ins/microstrain/MicroStrain.hpp new file mode 100755 index 0000000000..af4c707d82 --- /dev/null +++ b/src/drivers/ins/microstrain/MicroStrain.hpp @@ -0,0 +1,259 @@ +/**************************************************************************** + * + * 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 +#include +#include + +#include +#include +#include +#include +#include + +#include "mathlib/math/filter/AlphaFilter.hpp" + +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +#include "mip_sdk/src/mip/mip_all.h" +#include "mip_sdk/src/mip/definitions/commands_aiding.h" + +using namespace mip::C; + +using namespace time_literals; + +using matrix::Vector2f; + +#define MS_PX4_ERROR(res, ...) \ + do \ + { \ + PX4_ERR(__VA_ARGS__); \ + PX4_ERR(" Error: %s", mip_cmd_result_to_string(res)); \ + } while (0) + +static constexpr float sq(float x) { return x * x; }; + +class MicroStrain : public ModuleBase, public ModuleParams, public px4::ScheduledWorkItem +{ +public: + MicroStrain(const char *_device); + ~MicroStrain() override; + + /** @see ModuleBase */ + static int task_spawn(int argc, char *argv[]); + + /** @see ModuleBase */ + static int custom_command(int argc, char *argv[]); + + /** @see ModuleBase */ + static int print_usage(const char *reason = nullptr); + + /** @see ModuleBase */ + int print_status() override; + + /* Callbacks */ + + // Sensor Callbacks + static void sensorCallback(void *user, const mip_packet *packet, mip::Timestamp timestamp); + + static void filterCallback(void *user, const mip_packet *packet, mip::Timestamp timestamp); + + static void gnssCallback(void *user, const mip_packet *packet, mip::Timestamp timestamp); + +private: + /** @see ModuleBase */ + void Run() override; + + /// @brief Attempt to connect to the Sensor and set in known configuration + bool initializeIns(); + + int connectAtBaud(int32_t baud); + + mip_cmd_result forceIdle(); + + mip_cmd_result getSupportedDescriptors(); + + bool supportsDescriptor(uint8_t descriptor_set, uint8_t field_descriptor); + + bool supportsDescriptorSet(uint8_t descriptor_set); + + mip_cmd_result writeBaudRate(uint32_t baudrate, uint8_t port); + + mip_cmd_result configureImuRange(); + + mip_cmd_result getBaseRate(uint8_t descriptor_set, uint16_t *base_rate); + + mip_cmd_result configureImuMessageFormat(); + + mip_cmd_result configureFilterMessageFormat(); + + mip_cmd_result configureGnssMessageFormat(uint8_t descriptor_set); + + mip_cmd_result writeMessageFormat(uint8_t descriptor_set, uint8_t num_descriptors, + const mip::DescriptorRate *descriptors); + + mip_cmd_result configureAidingMeasurement(uint16_t aiding_source, bool enable); + + mip_cmd_result configureAidingSources(); + + mip_cmd_result writeFilterInitConfig(); + + void initializeRefPos(); + + void updateGeoidHeight(float geoid_height, float t); + + void sendAidingMeasurements(); + + bool init(); + + uint32_t _dev_id{0}; + + // Performance (perf) counters + perf_counter_t _loop_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": cycle")}; + perf_counter_t _loop_interval_perf{perf_alloc(PC_INTERVAL, MODULE_NAME": interval")}; + + mip_cmd_result MIP_PX4_ERROR = MIP_STATUS_USER_START; + uint8_t _parse_buffer[2048]; + bool _is_initialized{false}; + int _ms_schedule_rate_us{0}; + + bool _ext_pos_vel_aiding{false}; + bool _ext_heading_aiding{false}; + bool _ext_aiding{false}; + + float gnss_antenna_offset1[3] = {0}; + float gnss_antenna_offset2[3] = {0}; + mip_aiding_frame_config_command_rotation rotation = {0}; + + AlphaFilter _geoid_height_lpf; + uint64_t _last_geoid_height_update_us{0}; + static constexpr float kGeoidHeightLpfTimeConstant = 10.f; + + MapProjection _pos_ref{}; + double _ref_alt = 0; + float _gps_origin_ep[2] = {0}; + + template + struct SensorSample { + T sample; + bool updated = false; + }; + + mip_filter_gnss_dual_antenna_status_data dual_ant_stat{0}; + + uint16_t _supported_descriptors[1024] = {0}; + uint16_t _supported_desc_len = 0; + uint16_t _supported_descriptor_sets[1024] = {0}; + uint16_t _supported_desc_set_len = 0; + + mip::C::mip_interface _device; + + // Handlers + mip_dispatch_handler _sensor_data_handler; + mip_dispatch_handler _filter_data_handler; + mip_dispatch_handler _gnss_data_handler[2]; + + char _port[128]; + + // Parameters + DEFINE_PARAMETERS( + (ParamInt) _param_ms_mode, + (ParamInt) _param_ms_imu_rate_hz, + (ParamInt) _param_ms_mag_rate_hz, + (ParamInt) _param_ms_baro_rate_hz, + (ParamInt) _param_ms_filter_rate_hz, + (ParamInt) _param_ms_gnss_rate_hz, + (ParamInt) _param_ms_alignment, + (ParamInt) _param_ms_gnss_aid_src_ctrl, + (ParamInt) _param_ms_int_mag_en, + (ParamInt) _param_ms_int_heading_en, + (ParamInt) _param_ms_ext_heading_en, + (ParamInt) _param_ms_svt_en, + (ParamInt) _param_ms_accel_range_setting, + (ParamInt) _param_ms_gyro_range_setting, + (ParamFloat) _param_ms_gnss_offset1_x, + (ParamFloat) _param_ms_gnss_offset1_y, + (ParamFloat) _param_ms_gnss_offset1_z, + (ParamFloat) _param_ms_gnss_offset2_x, + (ParamFloat) _param_ms_gnss_offset2_y, + (ParamFloat) _param_ms_gnss_offset2_z, + (ParamFloat) _param_ms_sensor_roll, + (ParamFloat) _param_ms_sensor_pitch, + (ParamFloat) _param_ms_sensor_yaw + ) + + // Sensor types needed for message creation / updating / publishing + PX4Accelerometer _px4_accel{0}; + PX4Gyroscope _px4_gyro{0}; + PX4Magnetometer _px4_mag{0}; + sensor_baro_s _sensor_baro{0}; + + // Must publish to prevent sensor stale failure (sensors module) + uORB::PublicationMulti _sensor_baro_pub{ORB_ID(sensor_baro)}; + uORB::PublicationMulti _sensor_gps_pub[2] {ORB_ID(sensor_gps), ORB_ID(sensor_gps)}; + uORB::Publication _sensor_selection_pub{ORB_ID(sensor_selection)}; + + uORB::Publication _vehicle_global_position_pub; + uORB::Publication _vehicle_attitude_pub; + uORB::Publication _vehicle_local_position_pub; + uORB::Publication _vehicle_odometry_pub{ORB_ID(vehicle_odometry)}; + uORB::Publication _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)}; + + // Needed for health checks + uORB::Publication _estimator_status_pub{ORB_ID(estimator_status)}; + + // Subscriptions + uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s}; // subscription limited to 1 Hz updates + uORB::Subscription _vehicle_gps_position_sub{ORB_ID(vehicle_gps_position)}; +}; diff --git a/src/drivers/ins/microstrain/mip_sdk b/src/drivers/ins/microstrain/mip_sdk new file mode 160000 index 0000000000..35596994b6 --- /dev/null +++ b/src/drivers/ins/microstrain/mip_sdk @@ -0,0 +1 @@ +Subproject commit 35596994b60ba89fe02f71ce5127baa5e7ff2bbf diff --git a/src/drivers/ins/microstrain/module.yaml b/src/drivers/ins/microstrain/module.yaml new file mode 100644 index 0000000000..6c91ee0d40 --- /dev/null +++ b/src/drivers/ins/microstrain/module.yaml @@ -0,0 +1,310 @@ +module_name: MICROSTRAIN + +parameters: + - group: Sensors + definitions: + + MS_MODE: + description: + short: Toggles using the device as the primary EKF + long: | + Setting to 1 will publish data from the device to the vehicle topics (global_position, attitude, local_position, odometry), estimator_status and sensor_selection + Setting to 0 will publish data from the device to the external_ins topics (global position, attitude, local position) + Restart Required + + This parameter is specific to the MicroStrain driver. + type: int32 + default: 1 + + MS_IMU_RATE_HZ: + description: + short: IMU Data Rate + long: | + IMU (Accelerometer and Gyroscope) data rate + The INS driver will be scheduled at a rate 2*MS_IMU_RATE_HZ + Max Limit: 1000 + 0 - Disable IMU datastream + The max limit should be divisible by the rate + eg: 1000 % MS_IMU_RATE_HZ = 0 + Restart required + + This parameter is specific to the MicroStrain driver. + type: int32 + default: 500 + + MS_MAG_RATE_HZ: + description: + short: Magnetometer Data Rate + long: | + Magnetometer data rate + Max Limit: 1000 + 0 - Disable magnetometer datastream + The max limit should be divisible by the rate + eg: 1000 % MS_MAG_RATE_HZ = 0 + Restart required + + This parameter is specific to the MicroStrain driver. + type: int32 + default: 50 + + MS_BARO_RATE_HZ: + description: + short: Barometer data rate + long: | + Barometer data rate + Max Limit: 1000 + 0 - Disable barometer datastream + The max limit should be divisible by the rate + eg: 1000 % MS_BARO_RATE_HZ = 0 + Restart required + + This parameter is specific to the MicroStrain driver. + type: int32 + default: 50 + + MS_FILT_RATE_HZ: + description: + short: EKF data Rate + long: | + EKF data rate + Max Limit: 1000 + 0 - Disable EKF datastream + The max limit should be divisible by the rate + eg: 1000 % MS_FILT_RATE_HZ = 0 + Restart required + + This parameter is specific to the MicroStrain driver. + type: int32 + default: 250 + + MS_GNSS_RATE_HZ: + description: + short: GNSS data Rate + long: | + GNSS receiver 1 and 2 data rate + Max Limit: 5 + The max limit should be divisible by the rate + 0 - Disable GNSS datastream + eg: 5 % MS_GNSS_RATE_HZ = 0 + Restart required + + This parameter is specific to the MicroStrain driver. + type: int32 + default: 5 + + MS_ALIGNMENT: + description: + short: Alignment type + long: | + Select the source of heading alignment + This is a bitfield, you can use more than 1 source + Bit 0 - Dual-antenna GNSS + Bit 1 - GNSS kinematic (requires motion, e.g. a GNSS velocity) + Bit 2 - Magnetometer + Bit 3 - External Heading (first valid external heading will be used to initialize the filter) + Restart required + + This parameter is specific to the MicroStrain driver. + type: int32 + default: 2 + + MS_GNSS_AID_SRC: + description: + short: GNSS aiding source control + long: | + Select the source of gnss aiding (GNSS/INS) + 1 = All internal receivers, + 2 = External GNSS messages, + 3 = GNSS receiver 1 only + 4 = GNSS receiver 2 only + Restart required + + This parameter is specific to the MicroStrain driver. + type: int32 + default: 1 + + MS_INT_MAG_EN: + description: + short: Toggles internal magnetometer aiding in the device filter + long: | + 0 = Disabled, + 1 = Enabled + Restart required + + This parameter is specific to the MicroStrain driver. + type: int32 + default: 0 + + MS_INT_HEAD_EN: + description: + short: Toggles internal heading as an aiding measurement + long: | + 0 = Disabled, + 1 = Enabled + If dual antennas are supported (CV7-GNSS/INS). The filter will be configured to use dual antenna heading as an aiding measurement. + Restart required + + This parameter is specific to the MicroStrain driver. + type: int32 + default: 0 + + MS_EXT_HEAD_EN: + description: + short: Toggles external heading as an aiding measurement + long: | + 0 = Disabled, + 1 = Enabled + If enabled, the filter will be configured to accept external heading as an aiding meaurement. + Restart required + + This parameter is specific to the MicroStrain driver. + type: int32 + default: 0 + + MS_SVT_EN: + description: + short: Enables sensor to vehicle transform + long: | + 0 = Disabled, + 1 = Enabled + If the sensor has a different orientation with respect to the vehicle. This will enable a transform to correct itself. + The transform is described by MS_SENSOR_ROLL, MS_SENSOR_PITCH, MS_SENSOR_YAW + Restart required + + This parameter is specific to the MicroStrain driver. + type: int32 + default: 0 + + MS_ACCEL_RANGE: + description: + short: Sets the range of the accelerometer + long: | + -1 = Will not be configured, and will use the device default range, + Each adjustable range has a corresponding integer setting. Refer to the device's User Manual to check the available adjustment ranges. + https://www.hbkworld.com/en/products/transducers/inertial-sensors#!ref_microstrain.com + Restart required + + This parameter is specific to the MicroStrain driver. + type: int32 + default: -1 + + MS_GYRO_RANGE: + description: + short: Sets the range of the gyro + long: | + -1 = Will not be configured, and will use the device default range, + Each adjustable range has a corresponding integer setting. Refer to the device's User Manual to check the available adjustment ranges. + https://www.hbkworld.com/en/products/transducers/inertial-sensors#!ref_microstrain.com + Restart required + + This parameter is specific to the MicroStrain driver. + type: int32 + default: -1 + + MS_GNSS_OFF1_X: + description: + short: GNSS lever arm offset 1 (X) + long: | + Lever arm offset (m) in the X direction for the external GNSS receiver + In the case of a dual antenna setup, this is antenna 1 + Restart required + + This parameter is specific to the MicroStrain driver. + type: float + default: 0.0 + + MS_GNSS_OFF1_Y: + description: + short: GNSS lever arm offset 1 (Y) + long: | + Lever arm offset (m) in the Y direction for the external GNSS receiver + In the case of a dual antenna setup, this is antenna 1 + Restart required + + This parameter is specific to the MicroStrain driver. + type: float + default: 0.0 + + MS_GNSS_OFF1_Z: + description: + short: GNSS lever arm offset 1 (Z) + long: | + Lever arm offset (m) in the Z direction for the external GNSS receiver + In the case of a dual antenna setup, this is antenna 1 + Restart required + + This parameter is specific to the MicroStrain driver. + type: float + default: 0.0 + + MS_GNSS_OFF2_X: + description: + short: GNSS lever arm offset 2 (X) + long: | + Lever arm offset (m) in the X direction for antenna 2 + This will only be used if the device supports a dual antenna setup + Restart required + + This parameter is specific to the MicroStrain driver. + type: float + default: 0.0 + + MS_GNSS_OFF2_Y: + description: + short: GNSS lever arm offset 2 (Y) + long: | + Lever arm offset (m) in the Y direction for antenna 2 + This will only be used if the device supports a dual antenna setup + Restart required + + This parameter is specific to the MicroStrain driver. + type: float + default: 0.0 + + MS_GNSS_OFF2_Z: + description: + short: GNSS lever arm offset 2 (Z) + long: | + Lever arm offset (m) in the X direction for antenna 2 + This will only be used if the device supports a dual antenna setup + Restart required + + This parameter is specific to the MicroStrain driver. + type: float + default: 0.0 + + MS_SENSOR_ROLL: + description: + short: Sensor to Vehicle Transform (Roll) + long: | + The orientation of the device (Radians) with respect to the vehicle frame around the x axis + Requires MS_SVT_EN to be enabled to be used + Restart required + + This parameter is specific to the MicroStrain driver. + type: float + default: 0.0 + + MS_SENSOR_PTCH: + description: + short: Sensor to Vehicle Transform (Pitch) + long: | + The orientation of the device (Radians) with respect to the vehicle frame around the y axis + Requires MS_SVT_EN to be enabled to be used + Restart required + + This parameter is specific to the MicroStrain driver. + type: float + default: 0.0 + + MS_SENSOR_YAW: + description: + short: Sensor to Vehicle Transform (Yaw) + long: | + The orientation of the device (Radians) with respect to the vehicle frame around the z axis + Requires MS_SVT_EN to be enabled to be used + Restart required + + This parameter is specific to the MicroStrain driver. + type: float + default: 0.0