mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-06-01 02:55:07 +08:00
fix(drvers/ins): Fix Microstrain codestyle according astyle-config
This commit is contained in:
committed by
Ramon Roche
parent
a7ebab58cb
commit
2b76573301
@@ -196,6 +196,7 @@ mip_cmd_result MicroStrain::forceIdle()
|
|||||||
|
|
||||||
while (set_to_idle_tries++ < 3) {
|
while (set_to_idle_tries++ < 3) {
|
||||||
res = mip_base_set_idle(&_device);
|
res = mip_base_set_idle(&_device);
|
||||||
|
|
||||||
if (mip_cmd_result_is_ack(res)) {
|
if (mip_cmd_result_is_ack(res)) {
|
||||||
break;
|
break;
|
||||||
|
|
||||||
@@ -831,6 +832,7 @@ 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)) {
|
||||||
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)) {
|
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;
|
||||||
@@ -845,6 +847,7 @@ mip_cmd_result MicroStrain::configureGnssAiding()
|
|||||||
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)) {
|
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;
|
||||||
@@ -879,6 +882,7 @@ mip_cmd_result MicroStrain::configureGnssAiding()
|
|||||||
_param_ms_int_heading_en.get(),
|
_param_ms_int_heading_en.get(),
|
||||||
0, 0, nullptr, mip_aiding_frame_config_command_rotation{},
|
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)) {
|
if (!mip_cmd_result_is_ack(res)) {
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
@@ -889,6 +893,7 @@ mip_cmd_result MicroStrain::configureGnssAiding()
|
|||||||
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)) {
|
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;
|
||||||
@@ -912,6 +917,7 @@ mip_cmd_result MicroStrain::configureAidingSources()
|
|||||||
_param_ms_int_mag_en.get(),
|
_param_ms_int_mag_en.get(),
|
||||||
0, 0, nullptr, mip_aiding_frame_config_command_rotation{},
|
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)) {
|
if (!mip_cmd_result_is_ack(res)) {
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
@@ -924,6 +930,7 @@ mip_cmd_result MicroStrain::configureAidingSources()
|
|||||||
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)) {
|
if (!mip_cmd_result_is_ack(res)) {
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
@@ -936,6 +943,7 @@ mip_cmd_result MicroStrain::configureAidingSources()
|
|||||||
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)) {
|
if (!mip_cmd_result_is_ack(res)) {
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
@@ -948,6 +956,7 @@ mip_cmd_result MicroStrain::configureAidingSources()
|
|||||||
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)) {
|
if (!mip_cmd_result_is_ack(res)) {
|
||||||
return res;
|
return res;
|
||||||
}
|
}
|
||||||
@@ -1016,6 +1025,7 @@ bool MicroStrain::initializeIns()
|
|||||||
PX4_INFO("Setting the baud to desired baud rate");
|
PX4_INFO("Setting the baud to desired baud rate");
|
||||||
|
|
||||||
res = writeBaudRate(DESIRED_BAUDRATE, 1);
|
res = writeBaudRate(DESIRED_BAUDRATE, 1);
|
||||||
|
|
||||||
if (!mip_cmd_result_is_ack(res)) {
|
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;
|
||||||
@@ -1031,6 +1041,7 @@ bool MicroStrain::initializeIns()
|
|||||||
|
|
||||||
// Configure IMU ranges
|
// Configure IMU ranges
|
||||||
res = configureImuRange();
|
res = configureImuRange();
|
||||||
|
|
||||||
if (!mip_cmd_result_is_ack(res)) {
|
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;
|
||||||
@@ -1038,6 +1049,7 @@ bool MicroStrain::initializeIns()
|
|||||||
|
|
||||||
// Configure the IMU message formt based on what descriptors are supported
|
// Configure the IMU message formt based on what descriptors are supported
|
||||||
res = configureImuMessageFormat();
|
res = configureImuMessageFormat();
|
||||||
|
|
||||||
if (!mip_cmd_result_is_ack(res)) {
|
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;
|
||||||
@@ -1050,6 +1062,7 @@ bool MicroStrain::initializeIns()
|
|||||||
|
|
||||||
// Configure the Filter message format based on what descriptors are supported
|
// Configure the Filter message format based on what descriptors are supported
|
||||||
res = configureFilterMessageFormat();
|
res = configureFilterMessageFormat();
|
||||||
|
|
||||||
if (!mip_cmd_result_is_ack(res)) {
|
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;
|
||||||
@@ -1062,6 +1075,7 @@ bool MicroStrain::initializeIns()
|
|||||||
|
|
||||||
// Configure the GNSS1 message format based on what descriptors are supported
|
// Configure the GNSS1 message format based on what descriptors are supported
|
||||||
res = configureGnssMessageFormat(MIP_GNSS1_DATA_DESC_SET);
|
res = configureGnssMessageFormat(MIP_GNSS1_DATA_DESC_SET);
|
||||||
|
|
||||||
if (!mip_cmd_result_is_ack(res)) {
|
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");
|
||||||
}
|
}
|
||||||
@@ -1073,6 +1087,7 @@ bool MicroStrain::initializeIns()
|
|||||||
|
|
||||||
// Configure the GNSS2 message format based on what descriptors are supported
|
// Configure the GNSS2 message format based on what descriptors are supported
|
||||||
res = configureGnssMessageFormat(MIP_GNSS2_DATA_DESC_SET);
|
res = configureGnssMessageFormat(MIP_GNSS2_DATA_DESC_SET);
|
||||||
|
|
||||||
if (!mip_cmd_result_is_ack(res)) {
|
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");
|
||||||
}
|
}
|
||||||
@@ -1084,6 +1099,7 @@ bool MicroStrain::initializeIns()
|
|||||||
|
|
||||||
// Configure the aiding sources based on what the sensor supports
|
// Configure the aiding sources based on what the sensor supports
|
||||||
res = configureAidingSources();
|
res = configureAidingSources();
|
||||||
|
|
||||||
if (!mip_cmd_result_is_ack(res)) {
|
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;
|
||||||
@@ -1091,6 +1107,7 @@ bool MicroStrain::initializeIns()
|
|||||||
|
|
||||||
// Initialize the filter
|
// Initialize the filter
|
||||||
res = writeFilterInitConfig();
|
res = writeFilterInitConfig();
|
||||||
|
|
||||||
if (!mip_cmd_result_is_ack(res)) {
|
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;
|
||||||
@@ -1101,9 +1118,10 @@ bool MicroStrain::initializeIns()
|
|||||||
PX4_DEBUG("Writing SVT");
|
PX4_DEBUG("Writing SVT");
|
||||||
|
|
||||||
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[1]),
|
||||||
math::radians<float>(rotation_sens.euler[2]));
|
math::radians<float>(rotation_sens.euler[2]));
|
||||||
|
|
||||||
if (!mip_cmd_result_is_ack(res)) {
|
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;
|
||||||
@@ -1115,6 +1133,7 @@ bool MicroStrain::initializeIns()
|
|||||||
PX4_DEBUG("Reseting filter");
|
PX4_DEBUG("Reseting filter");
|
||||||
|
|
||||||
res = mip_filter_reset(&_device);
|
res = mip_filter_reset(&_device);
|
||||||
|
|
||||||
if (!mip_cmd_result_is_ack(res)) {
|
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;
|
||||||
@@ -1126,6 +1145,7 @@ bool MicroStrain::initializeIns()
|
|||||||
|
|
||||||
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)) {
|
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;
|
||||||
@@ -1137,6 +1157,7 @@ bool MicroStrain::initializeIns()
|
|||||||
PX4_DEBUG("Resuming device");
|
PX4_DEBUG("Resuming device");
|
||||||
|
|
||||||
res = mip_base_resume(&_device);
|
res = mip_base_resume(&_device);
|
||||||
|
|
||||||
if (!mip_cmd_result_is_ack(res)) {
|
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;
|
||||||
|
|||||||
Reference in New Issue
Block a user