fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-03 12:58:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

MicroStrain driver: Expanded aiding support (#25673)

Browse Source 
* External mag + Optical flow aiding

* Adding autostart

* global position eph fix

* write fix

* configureAidingSources split + frame fixes + params cleanup

* configureGnssAiding fix + params cleanup

* Redundant param removal

* External Heading fix
This commit is contained in:
JoelJ18
2025-10-03 19:13:04 -08:00
committed by GitHub
parent 1a3cdecb39
commit 6be7abb13d
3 changed files with 587 additions and 321 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/ins/microstrain/MicroStrain.cpp
+289 -148
View File
@@ -91,13 +91,35 @@ MicroStrain::MicroStrain(const char *uart_port) :
(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();
optical_flow_offset[0] = _param_ms_oflow_offset_x.get();
optical_flow_offset[1] = _param_ms_oflow_offset_y.get();
optical_flow_offset[2] = _param_ms_oflow_offset_z.get();
PX4_DEBUG("Optical flow offset: %f/%f/%f", (double)_param_ms_oflow_offset_x.get(),
(double)_param_ms_oflow_offset_y.get(),
(double)_param_ms_oflow_offset_z.get());
rotation_sens.euler[0] = _param_ms_sensor_roll.get();
rotation_sens.euler[1] = _param_ms_sensor_pitch.get();
rotation_sens.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());
rotation_ext_mag.euler[0] = _param_ms_emag_roll.get();
rotation_ext_mag.euler[1] = _param_ms_emag_pitch.get();
rotation_ext_mag.euler[2] = _param_ms_emag_yaw.get();
PX4_DEBUG("External magnetometer Roll/Pitch/Yaw: %f/%f/%f", (double)_param_ms_emag_roll.get(),
(double)_param_ms_emag_pitch.get(),
(double)_param_ms_emag_yaw.get());
rotation_ext_heading.euler[2] = _param_ms_ehead_yaw.get();
PX4_DEBUG("External heading yaw: %f", (double)_param_ms_ehead_yaw.get());
ext_mag_uncert = _param_ms_emag_uncert.get();
opt_flow_uncert = _param_ms_oflow_uncert.get();
PX4_DEBUG("External Mag Uncertainty: %f", (double)_param_ms_emag_uncert.get());
PX4_DEBUG("Optical Flow Uncertainty: %f", (double)_param_ms_oflow_uncert.get());
_sensor_baro_pub.advertise();
_sensor_selection_pub.advertise();
_vehicle_local_position_pub.advertise();
@@ -106,9 +128,6 @@ MicroStrain::MicroStrain(const char *uart_port) :
_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()
@@ -156,9 +175,9 @@ bool mipInterfaceUserRecvFromDevice(mip_interface *device, uint8_t *buffer, size
bool mipInterfaceUserSendToDevice(mip_interface *device, const uint8_t *data, size_t length)
{
int res = device_uart.write(const_cast<uint8_t *>(data), length);
size_t res = device_uart.write(const_cast<uint8_t *>(data), length);
if (res >= 0) {
if (res == length) {
return true;
}
@@ -239,7 +258,7 @@ mip_cmd_result MicroStrain::getSupportedDescriptors()
return res;
}
if (mip_cmd_result_is_ack(res_extended)) {
if (!mip_cmd_result_is_ack(res_extended)) {
PX4_DEBUG("Device does not support the extended descriptors command.");
}
@@ -531,7 +550,6 @@ mip_cmd_result MicroStrain::configureImuMessageFormat()
imu_descriptors);
return res;
}
mip_cmd_result MicroStrain::configureFilterMessageFormat()
@@ -635,6 +653,11 @@ mip_cmd_result MicroStrain::configureFilterMessageFormat()
mip_cmd_result MicroStrain::configureGnssMessageFormat(uint8_t descriptor_set)
{
if (_param_ms_gnss_aid_src_ctrl.get() == MIP_FILTER_GNSS_SOURCE_COMMAND_SOURCE_EXT) {
PX4_DEBUG("External GNSS aiding source detected, skipping GNSS message format config");
return MIP_ACK_OK;
}
PX4_DEBUG("Configuring GNSS Message Format");
uint8_t num_gnss_descriptors = 0;
@@ -700,34 +723,174 @@ mip_cmd_result MicroStrain::configureGnssMessageFormat(uint8_t descriptor_set)
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 {
// If the device doesn’t support aiding measurements
if (!supportsDescriptor(MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_AIDING_MEASUREMENT_ENABLE)) {
PX4_WARN("Aiding measurements are not supported");
// If the command was to disable, we consider it a success
if (!enable) {
res = MIP_ACK_OK;
// disabling is always OK
return enable ? MIP_PX4_ERROR : MIP_ACK_OK;
}
// Try to configure aiding measurement
mip_cmd_result 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) {
return MIP_ACK_OK;
}
return res;
}
mip_cmd_result MicroStrain::enableAidingSource(uint16_t source,
bool enabled,
uint8_t frame_id,
uint8_t frame_format,
const float offset[3],
mip_aiding_frame_config_command_rotation rotation,
uint16_t aiding_cmd_desc,
bool &aiding_flag,
const char *name)
{
mip_cmd_result res = configureAidingMeasurement(source, enabled);
if (!mip_cmd_result_is_ack(res)) {
PX4_ERR("Could not configure %s aiding", name);
return res;
}
// Frame 0 is used to handle the internal aiding scenario
if (frame_id == 0) {
return res;
}
// True if the aiding message supported & requested
aiding_flag = supportsDescriptor(MIP_AIDING_CMD_DESC_SET, aiding_cmd_desc) && enabled;
// Error if requested but unsupported.
if (enabled && !aiding_flag) {
PX4_ERR("Sending %s aiding messages not supported", name);
return MIP_PX4_ERROR;
}
// Write the frame config if enabled
if (aiding_flag) {
res = mip_aiding_write_frame_config(&_device, frame_id, frame_format, false, offset, &rotation);
if (!mip_cmd_result_is_ack(res)) {
PX4_ERR("Could not write %s frame config", name);
return res;
}
}
return res;
}
mip_cmd_result MicroStrain::configureGnssAiding()
{
// Enables GNSS Position & Velocity as an aiding measurement
mip_cmd_result 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 {
res = MIP_PX4_ERROR;
// AR and AHRS edge case
PX4_WARN("Could not enable GNSS Position & Velocity aiding");
return MIP_ACK_OK;
}
}
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;
}
}
// Prioritizing setting up multi antenna offsets if it is supported
if (supportsDescriptor(MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_MULTI_ANTENNA_OFFSET)) {
// Sets up the GNSS aiding source
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;
}
// Checks if the gnss aiding source is external
if (_param_ms_gnss_aid_src_ctrl.get() == MIP_FILTER_GNSS_SOURCE_COMMAND_SOURCE_EXT) {
_ext_pos_vel_aiding = true;
// Sets up the aiding frame for the external source
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_gnss))) {
PX4_ERR("Could not write aiding frame config");
return res;
}
}
}
else {
// Sets up the antenna offsets if the source is internal
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 (1) offsets");
return res1;
}
else if (!mip_cmd_result_is_ack(res2)) {
PX4_ERR("Could not write multi antenna (2) offsets");
return res2;
}
}
}
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 = enableAidingSource(
MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_GNSS_HEADING,
_param_ms_int_heading_en.get(),
0, 0, nullptr, mip_aiding_frame_config_command_rotation{0},
0, _int_aiding, "dual antenna heading"))) {
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_gnss))) {
PX4_ERR("Could not write aiding frame config");
return res;
}
}
else {
PX4_WARN("Aiding frames are not supported");
}
return res;
}
@@ -736,116 +899,53 @@ 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(
// Selectively turn on internal magnetometer as an aiding source
if (!mip_cmd_result_is_ack(res = enableAidingSource(
MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_MAGNETOMETER,
_param_ms_int_mag_en.get()))) {
PX4_ERR("Could not configure magnetometer aiding");
_param_ms_int_mag_en.get(),
0, 0, nullptr, mip_aiding_frame_config_command_rotation{0},
0, _int_aiding, "internal magnetometer"))) {
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");
// Selectively turn on external magnetometer as an aiding source
if (!mip_cmd_result_is_ack(res = enableAidingSource(
MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_MAGNETOMETER,
_param_ms_ext_mag_en.get(),
2, MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER,
ext_mag_offset, rotation_ext_mag,
MIP_CMD_DESC_AIDING_MAGNETIC_FIELD,
_ext_mag_aiding,
"external magnetometer"))) {
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;
}
// Selectively turn on body frame velocity as an aiding source
if (!mip_cmd_result_is_ack(res = enableAidingSource(
MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_VEHICLE_FRAME_VEL,
_param_ms_ext_opt_flow_en.get(),
3, MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER,
optical_flow_offset, rotation_oflow,
MIP_CMD_DESC_AIDING_VEL_ODOM,
_ext_optical_flow_aiding,
"optical flow"))) {
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;
}
// Selectively turn on external heading as an aiding source
if (!mip_cmd_result_is_ack(res = enableAidingSource(
MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_HEADING,
_param_ms_ext_heading_en.get(),
4, MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER,
ext_heading_offset, rotation_ext_heading,
MIP_CMD_DESC_AIDING_HEADING_TRUE,
_ext_heading_aiding,
"external heading"))) {
return res;
}
else {
PX4_WARN("Aiding frames are not supported");
}
_ext_aiding = (_ext_pos_vel_aiding || _ext_heading_aiding);
// Configures GNSS Aiding
res = configureGnssAiding();
return res;
}
@@ -985,8 +1085,8 @@ bool MicroStrain::initializeIns()
PX4_DEBUG("Writing SVT");
if (!mip_cmd_result_is_ack(res = mip_3dm_write_sensor_2_vehicle_transform_euler(&_device,
math::radians<float>(rotation.euler[0]),
math::radians<float>(rotation.euler[1]), math::radians<float>(rotation.euler[2])))) {
math::radians<float>(rotation_sens.euler[0]),
math::radians<float>(rotation_sens.euler[1]), math::radians<float>(rotation_sens.euler[2])))) {
MS_PX4_ERROR(res, "Could not set sensor-to-vehicle transformation!");
return false;
}
@@ -1047,29 +1147,23 @@ void MicroStrain::sensorCallback(void *user, const mip_packet *packet, mip::Time
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;
}
}
@@ -1078,7 +1172,6 @@ void MicroStrain::sensorCallback(void *user, const mip_packet *packet, mip::Time
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) {
@@ -1201,8 +1294,8 @@ void MicroStrain::filterCallback(void *user, const mip_packet *packet, mip::Time
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]));
gp.eph = sqrtf(sq(llh_uncert.sample.north) + sq(llh_uncert.sample.east));
gp.epv = llh_uncert.sample.down;
// ------- Fields we cannot obtain -------
gp.delta_alt = 0;
@@ -1625,9 +1718,6 @@ void MicroStrain::initializeRefPos()
_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");
}
@@ -1646,7 +1736,7 @@ void MicroStrain::updateGeoidHeight(float geoid_height, float t)
}
}
void MicroStrain::sendAidingMeasurements()
void MicroStrain::sendGPSAiding()
{
sensor_gps_s gps{0};
@@ -1667,13 +1757,13 @@ void MicroStrain::sendAidingMeasurements()
mip_time t;
t.timebase = MIP_TIME_TIMEBASE_TIME_OF_ARRIVAL;
t.reserved = 0x01;
t.reserved = 0x00;
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,
mip_aiding_llh_pos(&_device, &t, 1, gps.latitude_deg,
gps.longitude_deg,
gps.altitude_ellipsoid_m, llh_uncertainty, MIP_AIDING_LLH_POS_COMMAND_VALID_FLAGS_ALL);
@@ -1684,7 +1774,7 @@ void MicroStrain::sendAidingMeasurements()
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(&_device, &t, 1, ned_v, ned_velocity_uncertainty,
MIP_AIDING_NED_VEL_COMMAND_VALID_FLAGS_ALL);
}
}
@@ -1692,7 +1782,60 @@ void MicroStrain::sendAidingMeasurements()
// 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);
mip_aiding_true_heading(&_device, &t, 4, heading, gps.heading_accuracy, 0xff);
}
}
void MicroStrain::sendMagAiding()
{
vehicle_magnetometer_s mag{0};
if (!_vehicle_magnetometer_sub.update(&mag)) {
return;
}
mip_time t;
t.timebase = MIP_TIME_TIMEBASE_TIME_OF_ARRIVAL;
t.reserved = 0x00;
t.nanoseconds = 0;
float uncert[3] = {ext_mag_uncert, ext_mag_uncert, ext_mag_uncert};
mip_aiding_magnetic_field(&_device, &t, 2, mag.magnetometer_ga, uncert,
MIP_AIDING_MAGNETIC_FIELD_COMMAND_VALID_FLAGS_ALL);
}
void MicroStrain::sendOpticalFlowAiding()
{
vehicle_optical_flow_vel_s ofv{0};
if (!_vehicle_optical_flow_vel_sub.update(&ofv)) {
return;
}
mip_time t;
t.timebase = MIP_TIME_TIMEBASE_TIME_OF_ARRIVAL;
t.reserved = 0x00;
t.nanoseconds = 0;
float vel[3] = {ofv.vel_body[0], ofv.vel_body[1], 0};
float uncert[3] = {opt_flow_uncert, opt_flow_uncert, 0.0};
mip_aiding_vehicle_fixed_frame_velocity(&_device, &t, 3, vel, uncert, 0x0003);
}
void MicroStrain::sendAidingMeasurements()
{
if (_ext_pos_vel_aiding || _ext_heading_aiding) {
sendGPSAiding();
}
if (_ext_mag_aiding) {
sendMagAiding();
}
if (_ext_optical_flow_aiding) {
sendOpticalFlowAiding();
}
}
@@ -1706,7 +1849,6 @@ bool MicroStrain::init()
void MicroStrain::Run()
{
if (should_exit()) {
ScheduleClear();
exit_and_cleanup();
@@ -1742,8 +1884,7 @@ void MicroStrain::Run()
//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();}
sendAidingMeasurements();
perf_end(_loop_perf);
}
src/drivers/ins/microstrain/MicroStrain.hpp
+48 -4
View File
@@ -62,6 +62,8 @@
#include <uORB/topics/vehicle_angular_velocity.h>
#include <uORB/topics/vehicle_attitude.h>
#include <uORB/topics/vehicle_odometry.h>
#include <uORB/topics/vehicle_magnetometer.h>
#include <uORB/topics/vehicle_optical_flow_vel.h>
#include <uORB/topics/debug_array.h>
#include <uORB/topics/estimator_status.h>
@@ -145,6 +147,18 @@ private:
mip_cmd_result configureAidingMeasurement(uint16_t aiding_source, bool enable);
mip_cmd_result enableAidingSource(uint16_t source,
bool enabled,
uint8_t frame_id,
uint8_t frame_format,
const float offset[3],
mip_aiding_frame_config_command_rotation rotation,
uint16_t aiding_cmd_desc,
bool &aiding_flag,
const char *name);
mip_cmd_result configureGnssAiding();
mip_cmd_result configureAidingSources();
mip_cmd_result writeFilterInitConfig();
@@ -153,6 +167,12 @@ private:
void updateGeoidHeight(float geoid_height, float t);
void sendGPSAiding();
void sendMagAiding();
void sendOpticalFlowAiding();
void sendAidingMeasurements();
bool init();
@@ -170,11 +190,23 @@ private:
bool _ext_pos_vel_aiding{false};
bool _ext_heading_aiding{false};
bool _ext_aiding{false};
bool _ext_mag_aiding{false};
bool _ext_optical_flow_aiding{false};
bool _int_aiding{false};
float gnss_antenna_offset1[3] = {0};
float gnss_antenna_offset2[3] = {0};
mip_aiding_frame_config_command_rotation rotation = {0};
float ext_mag_offset[3] = {0};
float optical_flow_offset[3] = {0};
float ext_heading_offset[3] = {0};
mip_aiding_frame_config_command_rotation rotation_sens = {0};
mip_aiding_frame_config_command_rotation rotation_gnss = {0};
mip_aiding_frame_config_command_rotation rotation_ext_mag = {0};
mip_aiding_frame_config_command_rotation rotation_oflow = {0};
mip_aiding_frame_config_command_rotation rotation_ext_heading = {0};
float ext_mag_uncert = 0.0;
float opt_flow_uncert = 0.0;
AlphaFilter<float> _geoid_height_lpf;
uint64_t _last_geoid_height_update_us{0};
@@ -182,7 +214,6 @@ private:
MapProjection _pos_ref{};
double _ref_alt = 0;
float _gps_origin_ep[2] = {0};
template <typename T>
struct SensorSample {
@@ -217,8 +248,10 @@ private:
(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_EXT_MAG_EN>) _param_ms_ext_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_OPT_FLOW_EN>) _param_ms_ext_opt_flow_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,
@@ -230,7 +263,16 @@ private:
(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
(ParamFloat<px4::params::MS_SENSOR_YAW>) _param_ms_sensor_yaw,
(ParamFloat<px4::params::MS_EMAG_ROLL>) _param_ms_emag_roll,
(ParamFloat<px4::params::MS_EMAG_PTCH>) _param_ms_emag_pitch,
(ParamFloat<px4::params::MS_EMAG_YAW>) _param_ms_emag_yaw,
(ParamFloat<px4::params::MS_OFLW_OFF_X>) _param_ms_oflow_offset_x,
(ParamFloat<px4::params::MS_OFLW_OFF_Y>) _param_ms_oflow_offset_y,
(ParamFloat<px4::params::MS_OFLW_OFF_Z>) _param_ms_oflow_offset_z,
(ParamFloat<px4::params::MS_EHEAD_YAW>) _param_ms_ehead_yaw,
(ParamFloat<px4::params::MS_EMAG_UNCERT>) _param_ms_emag_uncert,
(ParamFloat<px4::params::MS_OFLW_UNCERT>) _param_ms_oflow_uncert
)
// Sensor types needed for message creation / updating / publishing
@@ -256,4 +298,6 @@ private:
// 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)};
uORB::Subscription _vehicle_magnetometer_sub{ORB_ID(vehicle_magnetometer)};
uORB::Subscription _vehicle_optical_flow_vel_sub{ORB_ID(vehicle_optical_flow_vel)};
};
src/drivers/ins/microstrain/module.yaml
+250 -169
View File
@@ -1,310 +1,391 @@
module_name: MICROSTRAIN
serial_config:
- command: sleep 1; microstrain start -d ${SERIAL_DEV}
port_config_param:
name: SENS_MS_CFG
group: Sensors
parameters:
- group: Sensors
definitions:
MS_MODE:
description:
short: Toggles using the device as the primary EKF
short: MicroStrain device mode
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
Sensor mode publishes raw IMU data to be used by EKF2. INS data from the device is published to the external INS topics.
INS mode publishes the INS data to the vehicle topics to be used for navigation.
type: enum
values:
0: Sensor Mode
1: INS Mode
reboot_required: true
default: 0
MS_IMU_RATE_HZ:
description:
short: IMU Data Rate
short: MicroStrain 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.
Accelerometer and Gyroscope data rate (Hz).
Valid rates: 0 or any factor of 1000.
type: int32
max: 1000
min: 0
reboot_required: true
default: 500
MS_MAG_RATE_HZ:
description:
short: Magnetometer Data Rate
short: MicroStrain 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.
Magnetometer data rate (Hz).
Valid rates: 0 or any factor of 1000.
type: int32
max: 1000
min: 0
reboot_required: true
default: 50
MS_BARO_RATE_HZ:
description:
short: Barometer data rate
short: MicroStrain 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.
Barometer data rate (Hz).
Valid rates: 0 or any factor of 1000.
type: int32
max: 1000
min: 0
reboot_required: true
default: 50
MS_FILT_RATE_HZ:
description:
short: EKF data Rate
short: MicroStrain 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.
The rate at which the INS data is published (Hz).
Valid rates: 0 or any factor of 1000.
type: int32
max: 1000
min: 0
reboot_required: true
default: 250
MS_GNSS_RATE_HZ:
description:
short: GNSS data Rate
short: MicroStrain 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.
GNSS receiver 1 and 2 data rate (Hz).
Valid rates: 0, 1 or 5.
type: int32
max: 5
min: 0
reboot_required: true
default: 5
MS_ALIGNMENT:
description:
short: Alignment type
short: MicroStrain heading 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
Select the source of heading alignment.
type: bitmask
bit:
0: Dual-antenna GNSS
1: GNSS kinematic (requires motion, e.g. a GNSS velocity)
2: Magnetometer
3: External Heading (first valid external heading will be used to initialize the filter)
max: 15
min: 1
reboot_required: true
default: 2
MS_GNSS_AID_SRC:
description:
short: GNSS aiding source control
short: MicroStrain 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
Select the source of gnss aiding (GNSS/INS).
type: enum
values:
1: All internal receivers
2: External GNSS messages
3: GNSS receiver 1 only
4: GNSS receiver 2 only
reboot_required: true
default: 1
MS_INT_MAG_EN:
description:
short: Toggles internal magnetometer aiding in the device filter
short: Enable MicroStrain internal magnetometer
long: |
0 = Disabled,
1 = Enabled
Restart required
Toggles internal magnetometer aiding in the device filter.
type: enum
values:
0: Disabled
1: Enabled
reboot_required: true
default: 0
This parameter is specific to the MicroStrain driver.
type: int32
MS_EXT_MAG_EN:
description:
short: Enable MicroStrain external magnetometer aiding
long: |
Toggles external magnetometer aiding in the device filter.
type: enum
values:
0: Disabled
1: Enabled
reboot_required: true
default: 0
MS_INT_HEAD_EN:
description:
short: Toggles internal heading as an aiding measurement
short: Enable MicroStrain internal heading aiding
long: |
0 = Disabled,
1 = Enabled
Toggles internal heading as an aiding measurement.
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
type: enum
values:
0: Disabled
1: Enabled
reboot_required: true
default: 0
MS_EXT_HEAD_EN:
description:
short: Toggles external heading as an aiding measurement
short: Enable MicroStrain external heading aiding
long: |
0 = Disabled,
1 = Enabled
Toggles external heading as an aiding measurement.
If enabled, the filter will be configured to accept external heading as an aiding meaurement.
Restart required
type: enum
values:
0: Disabled
1: Enabled
reboot_required: true
default: 0
This parameter is specific to the MicroStrain driver.
type: int32
MS_OPT_FLOW_EN:
description:
short: Enable MicroStrain optical flow aiding
long: |
Toggles body frame velocity as an aiding measurement.
The driver uses the body frame velocity from the optical flow sensor as the aiding measurements.
type: enum
values:
0: Disabled
1: Enabled
reboot_required: true
default: 0
MS_SVT_EN:
description:
short: Enables sensor to vehicle transform
short: Enables Microstrain 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
The transform is described by MS_SENSOR_ROLL, MS_SENSOR_PITCH, MS_SENSOR_YAW.
type: enum
values:
0: Disabled
1: Enabled
reboot_required: true
default: 0
MS_ACCEL_RANGE:
description:
short: Sets the range of the accelerometer
short: MicroStrain accelerometer range
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.
-1 = Will not be configured, and will use the device default range.
Ranges vary by device and map to integer codes. Check the device's [User Manual](https://www.hbkworld.com/en/products/transducers/inertial-sensors#!ref_microstrain.com) for supported ranges and set the corresponding integer.
type: int32
reboot_required: true
default: -1
MS_GYRO_RANGE:
description:
short: Sets the range of the gyro
short: MicroStrain gyroscope range
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.
-1 = Will not be configured, and will use the device default range.
Ranges vary by device and map to integer codes. Check the device's [User Manual](https://www.hbkworld.com/en/products/transducers/inertial-sensors#!ref_microstrain.com) for supported ranges and set the corresponding integer.
type: int32
reboot_required: true
default: -1
MS_GNSS_OFF1_X:
description:
short: GNSS lever arm offset 1 (X)
short: MicroStrain 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.
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.
type: float
reboot_required: true
default: 0.0
MS_GNSS_OFF1_Y:
description:
short: GNSS lever arm offset 1 (Y)
short: MicroStrain 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.
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.
type: float
reboot_required: true
default: 0.0
MS_GNSS_OFF1_Z:
description:
short: GNSS lever arm offset 1 (Z)
short: MicroStrain 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.
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.
type: float
reboot_required: true
default: 0.0
MS_GNSS_OFF2_X:
description:
short: GNSS lever arm offset 2 (X)
short: MicroStrain 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.
This will only be used if the device supports a dual antenna setup.
type: float
reboot_required: true
default: 0.0
MS_GNSS_OFF2_Y:
description:
short: GNSS lever arm offset 2 (Y)
short: MicroStrain 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.
Lever arm offset (m) in the Y direction for antenna 2.
This will only be used if the device supports a dual antenna setup.
type: float
reboot_required: true
default: 0.0
MS_GNSS_OFF2_Z:
description:
short: GNSS lever arm offset 2 (Z)
short: MicroStrain 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.
Lever arm offset (m) in the X direction for antenna 2.
This will only be used if the device supports a dual antenna setup.
type: float
reboot_required: true
default: 0.0
MS_SENSOR_ROLL:
description:
short: Sensor to Vehicle Transform (Roll)
short: MicroStrain 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.
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.
type: float
reboot_required: true
default: 0.0
MS_SENSOR_PTCH:
description:
short: Sensor to Vehicle Transform (Pitch)
short: MicroStrain 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.
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.
type: float
reboot_required: true
default: 0.0
MS_SENSOR_YAW:
description:
short: Sensor to Vehicle Transform (Yaw)
short: MicroStrain 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.
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.
type: float
reboot_required: true
default: 0.0
MS_EMAG_ROLL:
description:
short: MicroStrain External Magnetometer Orientation (Roll)
long: |
The orientation of the device (Radians) with respect to the vehicle frame around the x axis.
Requires MS_EXT_MAG_EN to be enabled to be used.
type: float
reboot_required: true
default: 0.0
MS_EMAG_PTCH:
description:
short: MicroStrain External Magnetometer Orientation (Pitch)
long: |
The orientation of the device (Radians) with respect to the vehicle frame around the y axis.
Requires MS_EXT_MAG_EN to be enabled to be used.
type: float
reboot_required: true
default: 0.0
MS_EMAG_YAW:
description:
short: MicroStrain External Magnetometer Orientation (Yaw)
long: |
The orientation of the device (Radians) with respect to the vehicle frame around the z axis.
Requires MS_EXT_MAG_EN to be enabled to be used.
type: float
reboot_required: true
default: 0.0
MS_OFLW_OFF_X:
description:
short: MicroStrain optical flow offset (X)
long: |
Offset (m) in the X direction if an Optical Flow sensor is connected.
Requires MS_OPT_FLOW_EN to be enabled to be used.
type: float
reboot_required: true
default: 0.0
MS_OFLW_OFF_Y:
description:
short: MicroStrain optical flow offset (Y)
long: |
Offset (m) in the Y direction if an Optical Flow sensor is connected.
Requires MS_OPT_FLOW_EN to be enabled to be used.
type: float
reboot_required: true
default: 0.0
MS_OFLW_OFF_Z:
description:
short: MicroStrain optical flow offset (Z)
long: |
Offset (m) in the Z direction if an Optical Flow sensor is connected.
Requires MS_OPT_FLOW_EN to be enabled to be used.
type: float
reboot_required: true
default: 0.0
MS_EHEAD_YAW:
description:
short: MicroStrain External Heading Orientation (Yaw)
long: |
The orientation of the device (Radians) with respect to the vehicle frame around the z axis.
Requires MS_EXT_HEAD_EN to be enabled to be used.
type: float
reboot_required: true
default: 0.0
MS_EMAG_UNCERT:
description:
short: MicroStrain external magnetometer uncertainty
long: |
The 1-sigma uncertainty (in Gauss) for all axes, which will remain constant across all aiding measurements.
Requires MS_EXT_MAG_EN to be enabled to be used.
type: float
reboot_required: true
default: 0.1
MS_OFLW_UNCERT:
description:
short: MicroStrain optical flow uncertainty
long: |
The 1-sigma uncertainty (in m/s) for the X and Y axes, which will remain constant across all aiding measurements.
The Z axis is not used for aiding.
Requires MS_OPT_FLOW_EN to be enabled to be used.
type: float
reboot_required: true
default: 0.1