Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-06-02 11:59:17 +08:00
Code Issues Actions 21 Packages Projects Releases Wiki Activity

fix(drvers/ins): Fix Microstrain clang-tidy errors

Browse Source
This commit is contained in:
Valentin Bugrov
2026-04-02 21:07:51 +07:00
committed by Ramon Roche
parent 2c693e4113
commit a7ebab58cb
2 changed files with 87 additions and 68 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/drivers/ins/microstrain/MicroStrain.cpp
+62 -43
View File
@@ -195,7 +195,8 @@ mip_cmd_result MicroStrain::forceIdle()
uint8_t set_to_idle_tries = 0; uint8_t set_to_idle_tries = 0;
while (set_to_idle_tries++ < 3) { while (set_to_idle_tries++ < 3) {
if (mip_cmd_result_is_ack((res = mip_base_set_idle(&_device)))) { res = mip_base_set_idle(&_device);
if (mip_cmd_result_is_ack(res)) {
break; break;
} else { } else {
@@ -210,7 +211,7 @@ int MicroStrain::connectAtBaud(int32_t baud)
{ {
if (device_uart.isOpen()) { if (device_uart.isOpen()) {
if (device_uart.setBaudrate(baud) == false) { if (device_uart.setBaudrate(baud) == false) {
PX4_ERR("Failed to set UART %lu baud", baud); PX4_ERR("Failed to set UART %lu baud", static_cast<unsigned long>(baud));
} }
} else { } else {
@@ -220,7 +221,7 @@ int MicroStrain::connectAtBaud(int32_t baud)
} }
if (device_uart.setBaudrate(baud) == false) { if (device_uart.setBaudrate(baud) == false) {
PX4_ERR("Failed to set UART %lu baud", baud); PX4_ERR("Failed to set UART %lu baud", static_cast<unsigned long>(baud));
return PX4_ERROR; return PX4_ERROR;
} }
@@ -230,11 +231,13 @@ int MicroStrain::connectAtBaud(int32_t baud)
} }
} }
PX4_INFO("Serial Port %s with baud of %lu baud", (device_uart.isOpen() ? "CONNECTED" : "NOT CONNECTED"), baud); PX4_INFO("Serial Port %s with baud of %lu baud",
(device_uart.isOpen() ? "CONNECTED" : "NOT CONNECTED"),
static_cast<unsigned long>(baud));
// Re-init the interface with the correct timeouts // 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, 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); &mipInterfaceUserSendToDevice, &mipInterfaceUserRecvFromDevice, &mip_interface_default_update, nullptr);
if (!mip_cmd_result_is_ack(forceIdle())) { if (!mip_cmd_result_is_ack(forceIdle())) {
PX4_ERR("Could not set device to idle"); PX4_ERR("Could not set device to idle");
@@ -827,7 +830,8 @@ mip_cmd_result MicroStrain::configureGnssAiding()
// Sets up the GNSS aiding source // Sets up the GNSS aiding source
if (supportsDescriptor(MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GNSS_SOURCE_CONTROL)) { 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()))) { res = mip_filter_write_gnss_source(&_device, (uint8_t)_param_ms_gnss_aid_src_ctrl.get());
if (!mip_cmd_result_is_ack(res)) {
PX4_ERR("Could not write the gnss aiding source"); PX4_ERR("Could not write the gnss aiding source");
return res; return res;
} }
@@ -838,9 +842,10 @@ mip_cmd_result MicroStrain::configureGnssAiding()
// Sets up the aiding frame for the external source // Sets up the aiding frame for the external source
if (supportsDescriptor(MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG)) { 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, res = mip_aiding_write_frame_config(&_device, 1,
MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER, false, MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER, false,
gnss_antenna_offset1, &rotation_gnss))) { gnss_antenna_offset1, &rotation_gnss);
if (!mip_cmd_result_is_ack(res)) {
PX4_ERR("Could not write aiding frame config"); PX4_ERR("Could not write aiding frame config");
return res; return res;
} }
@@ -870,20 +875,21 @@ mip_cmd_result MicroStrain::configureGnssAiding()
} }
// Selectively enables dual antenna heading as an aiding measurement // Selectively enables dual antenna heading as an aiding measurement
if (!mip_cmd_result_is_ack(res = enableAidingSource( res = enableAidingSource(MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_GNSS_HEADING,
MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_GNSS_HEADING,
_param_ms_int_heading_en.get(), _param_ms_int_heading_en.get(),
0, 0, nullptr, mip_aiding_frame_config_command_rotation{0}, 0, 0, nullptr, mip_aiding_frame_config_command_rotation{},
0, _int_aiding, "dual antenna heading"))) { 0, _int_aiding, "dual antenna heading");
if (!mip_cmd_result_is_ack(res)) {
return res; return res;
} }
} }
// Otherwise sets up the aiding frame // Otherwise sets up the aiding frame
else if (supportsDescriptor(MIP_AIDING_CMD_DESC_SET, MIP_CMD_DESC_AIDING_FRAME_CONFIG)) { 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, res = mip_aiding_write_frame_config(&_device, 1,
MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER, false, MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER, false,
gnss_antenna_offset1, &rotation_gnss))) { gnss_antenna_offset1, &rotation_gnss);
if (!mip_cmd_result_is_ack(res)) {
PX4_ERR("Could not write aiding frame config"); PX4_ERR("Could not write aiding frame config");
return res; return res;
} }
@@ -902,47 +908,47 @@ mip_cmd_result MicroStrain::configureAidingSources()
mip_cmd_result res; mip_cmd_result res;
// Selectively turn on internal magnetometer as an aiding source // Selectively turn on internal magnetometer as an aiding source
if (!mip_cmd_result_is_ack(res = enableAidingSource( res = enableAidingSource(MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_MAGNETOMETER,
MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_MAGNETOMETER,
_param_ms_int_mag_en.get(), _param_ms_int_mag_en.get(),
0, 0, nullptr, mip_aiding_frame_config_command_rotation{0}, 0, 0, nullptr, mip_aiding_frame_config_command_rotation{},
0, _int_aiding, "internal magnetometer"))) { 0, _int_aiding, "internal magnetometer");
if (!mip_cmd_result_is_ack(res)) {
return res; return res;
} }
// Selectively turn on external magnetometer as an aiding source // Selectively turn on external magnetometer as an aiding source
if (!mip_cmd_result_is_ack(res = enableAidingSource( res = enableAidingSource(MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_MAGNETOMETER,
MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_MAGNETOMETER,
_param_ms_ext_mag_en.get(), _param_ms_ext_mag_en.get(),
2, MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER, 2, MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER,
ext_mag_offset, rotation_ext_mag, ext_mag_offset, rotation_ext_mag,
MIP_CMD_DESC_AIDING_MAGNETIC_FIELD, MIP_CMD_DESC_AIDING_MAGNETIC_FIELD,
_ext_mag_aiding, _ext_mag_aiding,
"external magnetometer"))) { "external magnetometer");
if (!mip_cmd_result_is_ack(res)) {
return res; return res;
} }
// Selectively turn on body frame velocity as an aiding source // Selectively turn on body frame velocity as an aiding source
if (!mip_cmd_result_is_ack(res = enableAidingSource( res = enableAidingSource(MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_VEHICLE_FRAME_VEL,
MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_VEHICLE_FRAME_VEL,
_param_ms_ext_opt_flow_en.get(), _param_ms_ext_opt_flow_en.get(),
3, MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER, 3, MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER,
optical_flow_offset, rotation_oflow, optical_flow_offset, rotation_oflow,
MIP_CMD_DESC_AIDING_VEL_ODOM, MIP_CMD_DESC_AIDING_VEL_ODOM,
_ext_optical_flow_aiding, _ext_optical_flow_aiding,
"optical flow"))) { "optical flow");
if (!mip_cmd_result_is_ack(res)) {
return res; return res;
} }
// Selectively turn on external heading as an aiding source // Selectively turn on external heading as an aiding source
if (!mip_cmd_result_is_ack(res = enableAidingSource( res = enableAidingSource(MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_HEADING,
MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_EXTERNAL_HEADING,
_param_ms_ext_heading_en.get(), _param_ms_ext_heading_en.get(),
4, MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER, 4, MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER,
ext_heading_offset, rotation_ext_heading, ext_heading_offset, rotation_ext_heading,
MIP_CMD_DESC_AIDING_HEADING_TRUE, MIP_CMD_DESC_AIDING_HEADING_TRUE,
_ext_heading_aiding, _ext_heading_aiding,
"external heading"))) { "external heading");
if (!mip_cmd_result_is_ack(res)) {
return res; return res;
} }
@@ -989,7 +995,7 @@ bool MicroStrain::initializeIns()
for (auto &baudrate : BAUDRATES) { for (auto &baudrate : BAUDRATES) {
if (connectAtBaud(baudrate) == PX4_OK) { if (connectAtBaud(baudrate) == PX4_OK) {
PX4_INFO("found baudrate %lu", baudrate); PX4_INFO("found baudrate %lu", static_cast<unsigned long>(baudrate));
is_connected = true; is_connected = true;
break; break;
} }
@@ -1009,7 +1015,8 @@ bool MicroStrain::initializeIns()
// Setting the device baudrate to the desired value // Setting the device baudrate to the desired value
PX4_INFO("Setting the baud to desired baud rate"); PX4_INFO("Setting the baud to desired baud rate");
if (!mip_cmd_result_is_ack(res = writeBaudRate(DESIRED_BAUDRATE, 1))) { res = writeBaudRate(DESIRED_BAUDRATE, 1);
if (!mip_cmd_result_is_ack(res)) {
PX4_ERR("Could not set the baudrate!"); PX4_ERR("Could not set the baudrate!");
return false; return false;
} }
@@ -1018,18 +1025,20 @@ bool MicroStrain::initializeIns()
// Connecting using the desired baudrate // Connecting using the desired baudrate
if (connectAtBaud(DESIRED_BAUDRATE) != PX4_OK) { if (connectAtBaud(DESIRED_BAUDRATE) != PX4_OK) {
PX4_ERR("Could not Connect at %lu", DESIRED_BAUDRATE); PX4_ERR("Could not Connect at %lu", static_cast<unsigned long>(DESIRED_BAUDRATE));
return false; return false;
} }
// Configure IMU ranges // Configure IMU ranges
if (!mip_cmd_result_is_ack(res = configureImuRange())) { res = configureImuRange();
if (!mip_cmd_result_is_ack(res)) {
MS_PX4_ERROR(res, "Could not configure IMU range"); MS_PX4_ERROR(res, "Could not configure IMU range");
return false; return false;
} }
// Configure the IMU message formt based on what descriptors are supported // Configure the IMU message formt based on what descriptors are supported
if (!mip_cmd_result_is_ack(res = configureImuMessageFormat())) { res = configureImuMessageFormat();
if (!mip_cmd_result_is_ack(res)) {
MS_PX4_ERROR(res, "Could not write IMU message format"); MS_PX4_ERROR(res, "Could not write IMU message format");
return false; return false;
} }
@@ -1040,7 +1049,8 @@ bool MicroStrain::initializeIns()
this); this);
// Configure the Filter message format based on what descriptors are supported // Configure the Filter message format based on what descriptors are supported
if (!mip_cmd_result_is_ack(res = configureFilterMessageFormat())) { res = configureFilterMessageFormat();
if (!mip_cmd_result_is_ack(res)) {
MS_PX4_ERROR(res, "Could not write filter message format"); MS_PX4_ERROR(res, "Could not write filter message format");
return false; return false;
} }
@@ -1051,7 +1061,8 @@ bool MicroStrain::initializeIns()
this); this);
// Configure the GNSS1 message format based on what descriptors are supported // Configure the GNSS1 message format based on what descriptors are supported
if (!mip_cmd_result_is_ack(res = configureGnssMessageFormat(MIP_GNSS1_DATA_DESC_SET))) { res = configureGnssMessageFormat(MIP_GNSS1_DATA_DESC_SET);
if (!mip_cmd_result_is_ack(res)) {
MS_PX4_ERROR(res, "Could not write GNSS1 message format"); MS_PX4_ERROR(res, "Could not write GNSS1 message format");
} }
@@ -1061,7 +1072,8 @@ bool MicroStrain::initializeIns()
this); this);
// Configure the GNSS2 message format based on what descriptors are supported // Configure the GNSS2 message format based on what descriptors are supported
if (!mip_cmd_result_is_ack(res = configureGnssMessageFormat(MIP_GNSS2_DATA_DESC_SET))) { res = configureGnssMessageFormat(MIP_GNSS2_DATA_DESC_SET);
if (!mip_cmd_result_is_ack(res)) {
MS_PX4_ERROR(res, "Could not write GNSS2 message format"); MS_PX4_ERROR(res, "Could not write GNSS2 message format");
} }
@@ -1071,13 +1083,15 @@ bool MicroStrain::initializeIns()
this); this);
// Configure the aiding sources based on what the sensor supports // Configure the aiding sources based on what the sensor supports
if (!mip_cmd_result_is_ack(res = configureAidingSources())) { res = configureAidingSources();
if (!mip_cmd_result_is_ack(res)) {
MS_PX4_ERROR(res, "Could not configure aiding frames!"); MS_PX4_ERROR(res, "Could not configure aiding frames!");
return false; return false;
} }
// Initialize the filter // Initialize the filter
if (!mip_cmd_result_is_ack(res = writeFilterInitConfig())) { res = writeFilterInitConfig();
if (!mip_cmd_result_is_ack(res)) {
MS_PX4_ERROR(res, "Could not configure filter initialization!"); MS_PX4_ERROR(res, "Could not configure filter initialization!");
return false; return false;
} }
@@ -1086,9 +1100,11 @@ bool MicroStrain::initializeIns()
if (_param_ms_svt_en.get() && supportsDescriptor(MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_SENSOR2VEHICLE_TRANSFORM_EUL)) { if (_param_ms_svt_en.get() && supportsDescriptor(MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_SENSOR2VEHICLE_TRANSFORM_EUL)) {
PX4_DEBUG("Writing SVT"); PX4_DEBUG("Writing SVT");
if (!mip_cmd_result_is_ack(res = mip_3dm_write_sensor_2_vehicle_transform_euler(&_device, res = mip_3dm_write_sensor_2_vehicle_transform_euler(&_device,
math::radians<float>(rotation_sens.euler[0]), math::radians<float>(rotation_sens.euler[0]),
math::radians<float>(rotation_sens.euler[1]), math::radians<float>(rotation_sens.euler[2])))) { math::radians<float>(rotation_sens.euler[1]),
math::radians<float>(rotation_sens.euler[2]));
if (!mip_cmd_result_is_ack(res)) {
MS_PX4_ERROR(res, "Could not set sensor-to-vehicle transformation!"); MS_PX4_ERROR(res, "Could not set sensor-to-vehicle transformation!");
return false; return false;
} }
@@ -1098,7 +1114,8 @@ bool MicroStrain::initializeIns()
if (supportsDescriptor(MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_RESET_FILTER)) { if (supportsDescriptor(MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_RESET_FILTER)) {
PX4_DEBUG("Reseting filter"); PX4_DEBUG("Reseting filter");
if (!mip_cmd_result_is_ack(res = mip_filter_reset(&_device))) { res = mip_filter_reset(&_device);
if (!mip_cmd_result_is_ack(res)) {
MS_PX4_ERROR(res, "Could not reset the filter!"); MS_PX4_ERROR(res, "Could not reset the filter!");
return false; return false;
} }
@@ -1107,8 +1124,9 @@ bool MicroStrain::initializeIns()
if (supportsDescriptor(MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_CONTROL_DATA_STREAM)) { if (supportsDescriptor(MIP_3DM_CMD_DESC_SET, MIP_CMD_DESC_3DM_CONTROL_DATA_STREAM)) {
PX4_DEBUG("Writing datastream control"); PX4_DEBUG("Writing datastream control");
if (!mip_cmd_result_is_ack(res = mip_3dm_write_datastream_control(&_device, res = mip_3dm_write_datastream_control(&_device,
MIP_3DM_DATASTREAM_CONTROL_COMMAND_ALL_STREAMS, true))) { MIP_3DM_DATASTREAM_CONTROL_COMMAND_ALL_STREAMS, true);
if (!mip_cmd_result_is_ack(res)) {
MS_PX4_ERROR(res, "Could not enable the data stream"); MS_PX4_ERROR(res, "Could not enable the data stream");
return false; return false;
} }
@@ -1118,7 +1136,8 @@ bool MicroStrain::initializeIns()
if (supportsDescriptor(MIP_BASE_CMD_DESC_SET, MIP_CMD_DESC_BASE_RESUME)) { if (supportsDescriptor(MIP_BASE_CMD_DESC_SET, MIP_CMD_DESC_BASE_RESUME)) {
PX4_DEBUG("Resuming device"); PX4_DEBUG("Resuming device");
if (!mip_cmd_result_is_ack(res = mip_base_resume(&_device))) { res = mip_base_resume(&_device);
if (!mip_cmd_result_is_ack(res)) {
MS_PX4_ERROR(res, "Could not resume the device!"); MS_PX4_ERROR(res, "Could not resume the device!");
return false; return false;
} }
src/drivers/ins/microstrain/MicroStrain.hpp
+5 -5
View File
@@ -201,11 +201,11 @@ private:
float ext_mag_offset[3] = {0}; float ext_mag_offset[3] = {0};
float optical_flow_offset[3] = {0}; float optical_flow_offset[3] = {0};
float ext_heading_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_sens = {};
mip_aiding_frame_config_command_rotation rotation_gnss = {0}; mip_aiding_frame_config_command_rotation rotation_gnss = {};
mip_aiding_frame_config_command_rotation rotation_ext_mag = {0}; mip_aiding_frame_config_command_rotation rotation_ext_mag = {};
mip_aiding_frame_config_command_rotation rotation_oflow = {0}; mip_aiding_frame_config_command_rotation rotation_oflow = {};
mip_aiding_frame_config_command_rotation rotation_ext_heading = {0}; mip_aiding_frame_config_command_rotation rotation_ext_heading = {};
float ext_mag_uncert = 0.0; float ext_mag_uncert = 0.0;
float opt_flow_uncert = 0.0; float opt_flow_uncert = 0.0;