drivers: MicroStrain Inertial Sensors (#23858)

This commit is contained in:
JoelJ18 2025-08-05 11:05:46 -04:00 committed by GitHub
parent e021a1c946
commit b5e5d214fe
No known key found for this signature in database
GPG Key ID: B5690EEEBB952194
9 changed files with 2497 additions and 0 deletions

4
.gitmodules vendored
View File

@ -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

View File

@ -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 */

View File

@ -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"

View File

@ -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
)

View File

@ -0,0 +1,5 @@
menuconfig DRIVERS_INS_MICROSTRAIN
bool "microstrain"
default n
---help---
Enable support for Microstrain Sensors

File diff suppressed because it is too large Load Diff

View File

@ -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 <px4_platform_common/module.h>
#include <px4_platform_common/module_params.h>
#include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
#include <px4_platform_common/getopt.h>
#include <px4_platform_common/Serial.hpp>
#include <drivers/drv_hrt.h>
#include <lib/perf/perf_counter.h>
#include <lib/drivers/accelerometer/PX4Accelerometer.hpp>
#include <lib/drivers/gyroscope/PX4Gyroscope.hpp>
#include <lib/drivers/magnetometer/PX4Magnetometer.hpp>
#include "mathlib/math/filter/AlphaFilter.hpp"
#include <uORB/Publication.hpp>
#include <uORB/SubscriptionCallback.hpp>
#include <uORB/topics/parameter_update.h>
#include <uORB/topics/sensor_accel.h>
#include <uORB/topics/sensor_mag.h>
#include <uORB/topics/sensor_gyro.h>
#include <uORB/topics/sensor_baro.h>
#include <uORB/topics/sensor_gps.h>
#include <uORB/topics/sensor_selection.h>
#include <uORB/topics/vehicle_global_position.h>
#include <uORB/topics/vehicle_local_position.h>
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/debug_array.h>
#include <uORB/topics/estimator_status.h>
#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<MicroStrain>, 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<float> _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 <typename T>
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<px4::params::MS_MODE>) _param_ms_mode,
(ParamInt<px4::params::MS_IMU_RATE_HZ>) _param_ms_imu_rate_hz,
(ParamInt<px4::params::MS_MAG_RATE_HZ>) _param_ms_mag_rate_hz,
(ParamInt<px4::params::MS_BARO_RATE_HZ>) _param_ms_baro_rate_hz,
(ParamInt<px4::params::MS_FILT_RATE_HZ>) _param_ms_filter_rate_hz,
(ParamInt<px4::params::MS_GNSS_RATE_HZ>) _param_ms_gnss_rate_hz,
(ParamInt<px4::params::MS_ALIGNMENT>) _param_ms_alignment,
(ParamInt<px4::params::MS_GNSS_AID_SRC>) _param_ms_gnss_aid_src_ctrl,
(ParamInt<px4::params::MS_INT_MAG_EN>) _param_ms_int_mag_en,
(ParamInt<px4::params::MS_INT_HEAD_EN>) _param_ms_int_heading_en,
(ParamInt<px4::params::MS_EXT_HEAD_EN>) _param_ms_ext_heading_en,
(ParamInt<px4::params::MS_SVT_EN>) _param_ms_svt_en,
(ParamInt<px4::params::MS_ACCEL_RANGE>) _param_ms_accel_range_setting,
(ParamInt<px4::params::MS_GYRO_RANGE>) _param_ms_gyro_range_setting,
(ParamFloat<px4::params::MS_GNSS_OFF1_X>) _param_ms_gnss_offset1_x,
(ParamFloat<px4::params::MS_GNSS_OFF1_Y>) _param_ms_gnss_offset1_y,
(ParamFloat<px4::params::MS_GNSS_OFF1_Z>) _param_ms_gnss_offset1_z,
(ParamFloat<px4::params::MS_GNSS_OFF2_X>) _param_ms_gnss_offset2_x,
(ParamFloat<px4::params::MS_GNSS_OFF2_Y>) _param_ms_gnss_offset2_y,
(ParamFloat<px4::params::MS_GNSS_OFF2_Z>) _param_ms_gnss_offset2_z,
(ParamFloat<px4::params::MS_SENSOR_ROLL>) _param_ms_sensor_roll,
(ParamFloat<px4::params::MS_SENSOR_PTCH>) _param_ms_sensor_pitch,
(ParamFloat<px4::params::MS_SENSOR_YAW>) _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_s> _sensor_baro_pub{ORB_ID(sensor_baro)};
uORB::PublicationMulti<sensor_gps_s> _sensor_gps_pub[2] {ORB_ID(sensor_gps), ORB_ID(sensor_gps)};
uORB::Publication<sensor_selection_s> _sensor_selection_pub{ORB_ID(sensor_selection)};
uORB::Publication<vehicle_global_position_s> _vehicle_global_position_pub;
uORB::Publication<vehicle_attitude_s> _vehicle_attitude_pub;
uORB::Publication<vehicle_local_position_s> _vehicle_local_position_pub;
uORB::Publication<vehicle_odometry_s> _vehicle_odometry_pub{ORB_ID(vehicle_odometry)};
uORB::Publication<vehicle_angular_velocity_s> _vehicle_angular_velocity_pub{ORB_ID(vehicle_angular_velocity)};
// Needed for health checks
uORB::Publication<estimator_status_s> _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)};
};

@ -0,0 +1 @@
Subproject commit 35596994b60ba89fe02f71ce5127baa5e7ff2bbf

View File

@ -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