diff --git a/src/drivers/ins/microstrain/MicroStrain.cpp b/src/drivers/ins/microstrain/MicroStrain.cpp index ab6cb5ba8e..ebf3287713 100755 --- a/src/drivers/ins/microstrain/MicroStrain.cpp +++ b/src/drivers/ins/microstrain/MicroStrain.cpp @@ -195,7 +195,8 @@ mip_cmd_result MicroStrain::forceIdle() 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)))) { + res = mip_base_set_idle(&_device); + if (mip_cmd_result_is_ack(res)) { break; } else { @@ -210,7 +211,7 @@ 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); + PX4_ERR("Failed to set UART %lu baud", static_cast(baud)); } } else { @@ -220,7 +221,7 @@ int MicroStrain::connectAtBaud(int32_t baud) } 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(baud)); 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(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); + &mipInterfaceUserSendToDevice, &mipInterfaceUserRecvFromDevice, &mip_interface_default_update, nullptr); if (!mip_cmd_result_is_ack(forceIdle())) { PX4_ERR("Could not set device to idle"); @@ -827,7 +830,8 @@ mip_cmd_result MicroStrain::configureGnssAiding() // 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()))) { + 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"); return res; } @@ -838,9 +842,10 @@ mip_cmd_result MicroStrain::configureGnssAiding() // 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))) { + res = mip_aiding_write_frame_config(&_device, 1, + MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER, false, + gnss_antenna_offset1, &rotation_gnss); + if (!mip_cmd_result_is_ack(res)) { PX4_ERR("Could not write aiding frame config"); return res; } @@ -870,20 +875,21 @@ mip_cmd_result MicroStrain::configureGnssAiding() } // 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"))) { + 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, _int_aiding, "dual antenna heading"); + if (!mip_cmd_result_is_ack(res)) { 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))) { + res = mip_aiding_write_frame_config(&_device, 1, + MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER, false, + gnss_antenna_offset1, &rotation_gnss); + if (!mip_cmd_result_is_ack(res)) { PX4_ERR("Could not write aiding frame config"); return res; } @@ -902,47 +908,47 @@ mip_cmd_result MicroStrain::configureAidingSources() mip_cmd_result res; // 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(), - 0, 0, nullptr, mip_aiding_frame_config_command_rotation{0}, - 0, _int_aiding, "internal magnetometer"))) { + res = enableAidingSource(MIP_FILTER_AIDING_MEASUREMENT_ENABLE_COMMAND_AIDING_SOURCE_MAGNETOMETER, + _param_ms_int_mag_en.get(), + 0, 0, nullptr, mip_aiding_frame_config_command_rotation{}, + 0, _int_aiding, "internal magnetometer"); + if (!mip_cmd_result_is_ack(res)) { return res; } // 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"))) { + 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"); + if (!mip_cmd_result_is_ack(res)) { 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"))) { + 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"); + if (!mip_cmd_result_is_ack(res)) { 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"))) { + 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"); + if (!mip_cmd_result_is_ack(res)) { return res; } @@ -989,7 +995,7 @@ bool MicroStrain::initializeIns() for (auto &baudrate : BAUDRATES) { if (connectAtBaud(baudrate) == PX4_OK) { - PX4_INFO("found baudrate %lu", baudrate); + PX4_INFO("found baudrate %lu", static_cast(baudrate)); is_connected = true; break; } @@ -1009,7 +1015,8 @@ bool MicroStrain::initializeIns() // 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))) { + res = writeBaudRate(DESIRED_BAUDRATE, 1); + if (!mip_cmd_result_is_ack(res)) { PX4_ERR("Could not set the baudrate!"); return false; } @@ -1018,18 +1025,20 @@ bool MicroStrain::initializeIns() // Connecting using the desired baudrate if (connectAtBaud(DESIRED_BAUDRATE) != PX4_OK) { - PX4_ERR("Could not Connect at %lu", DESIRED_BAUDRATE); + PX4_ERR("Could not Connect at %lu", static_cast(DESIRED_BAUDRATE)); return false; } // 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"); return false; } // 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"); return false; } @@ -1040,7 +1049,8 @@ bool MicroStrain::initializeIns() this); // 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"); return false; } @@ -1051,7 +1061,8 @@ bool MicroStrain::initializeIns() 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))) { + res = configureGnssMessageFormat(MIP_GNSS1_DATA_DESC_SET); + if (!mip_cmd_result_is_ack(res)) { MS_PX4_ERROR(res, "Could not write GNSS1 message format"); } @@ -1061,7 +1072,8 @@ bool MicroStrain::initializeIns() 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))) { + res = configureGnssMessageFormat(MIP_GNSS2_DATA_DESC_SET); + if (!mip_cmd_result_is_ack(res)) { MS_PX4_ERROR(res, "Could not write GNSS2 message format"); } @@ -1071,13 +1083,15 @@ bool MicroStrain::initializeIns() this); // 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!"); return false; } // 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!"); 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)) { PX4_DEBUG("Writing SVT"); - if (!mip_cmd_result_is_ack(res = mip_3dm_write_sensor_2_vehicle_transform_euler(&_device, - math::radians(rotation_sens.euler[0]), - math::radians(rotation_sens.euler[1]), math::radians(rotation_sens.euler[2])))) { + res = mip_3dm_write_sensor_2_vehicle_transform_euler(&_device, + math::radians(rotation_sens.euler[0]), + math::radians(rotation_sens.euler[1]), + math::radians(rotation_sens.euler[2])); + if (!mip_cmd_result_is_ack(res)) { MS_PX4_ERROR(res, "Could not set sensor-to-vehicle transformation!"); return false; } @@ -1098,7 +1114,8 @@ bool MicroStrain::initializeIns() 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))) { + res = mip_filter_reset(&_device); + if (!mip_cmd_result_is_ack(res)) { MS_PX4_ERROR(res, "Could not reset the filter!"); return false; } @@ -1107,8 +1124,9 @@ bool MicroStrain::initializeIns() 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))) { + res = mip_3dm_write_datastream_control(&_device, + 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"); return false; } @@ -1118,7 +1136,8 @@ bool MicroStrain::initializeIns() 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))) { + res = mip_base_resume(&_device); + if (!mip_cmd_result_is_ack(res)) { MS_PX4_ERROR(res, "Could not resume the device!"); return false; } diff --git a/src/drivers/ins/microstrain/MicroStrain.hpp b/src/drivers/ins/microstrain/MicroStrain.hpp index cad2c2830a..74055fccd4 100755 --- a/src/drivers/ins/microstrain/MicroStrain.hpp +++ b/src/drivers/ins/microstrain/MicroStrain.hpp @@ -201,11 +201,11 @@ private: 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}; + mip_aiding_frame_config_command_rotation rotation_sens = {}; + mip_aiding_frame_config_command_rotation rotation_gnss = {}; + mip_aiding_frame_config_command_rotation rotation_ext_mag = {}; + mip_aiding_frame_config_command_rotation rotation_oflow = {}; + mip_aiding_frame_config_command_rotation rotation_ext_heading = {}; float ext_mag_uncert = 0.0; float opt_flow_uncert = 0.0;