@@ -196,6 +196,7 @@ mip_cmd_result MicroStrain::forceIdle()
196196
197197 while (set_to_idle_tries++ < 3 ) {
198198 res = mip_base_set_idle (&_device);
199+
199200 if (mip_cmd_result_is_ack (res)) {
200201 break ;
201202
@@ -833,6 +834,7 @@ mip_cmd_result MicroStrain::configureGnssAiding()
833834 // Sets up the GNSS aiding source
834835 if (supportsDescriptor (MIP_FILTER_CMD_DESC_SET , MIP_CMD_DESC_FILTER_GNSS_SOURCE_CONTROL )) {
835836 res = mip_filter_write_gnss_source (&_device, (uint8_t )_param_ms_gnss_aid_src_ctrl.get ());
837+
836838 if (!mip_cmd_result_is_ack (res)) {
837839 PX4_ERR (" Could not write the gnss aiding source" );
838840 return res;
@@ -847,6 +849,7 @@ mip_cmd_result MicroStrain::configureGnssAiding()
847849 res = mip_aiding_write_frame_config (&_device, 1 ,
848850 MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER , false ,
849851 gnss_antenna_offset1, &rotation_gnss);
852+
850853 if (!mip_cmd_result_is_ack (res)) {
851854 PX4_ERR (" Could not write aiding frame config" );
852855 return res;
@@ -881,6 +884,7 @@ mip_cmd_result MicroStrain::configureGnssAiding()
881884 _param_ms_int_heading_en.get (),
882885 0 , 0 , nullptr , mip_aiding_frame_config_command_rotation{},
883886 0 , _int_aiding, " dual antenna heading" );
887+
884888 if (!mip_cmd_result_is_ack (res)) {
885889 return res;
886890 }
@@ -891,6 +895,7 @@ mip_cmd_result MicroStrain::configureGnssAiding()
891895 res = mip_aiding_write_frame_config (&_device, 1 ,
892896 MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER , false ,
893897 gnss_antenna_offset1, &rotation_gnss);
898+
894899 if (!mip_cmd_result_is_ack (res)) {
895900 PX4_ERR (" Could not write aiding frame config" );
896901 return res;
@@ -914,6 +919,7 @@ mip_cmd_result MicroStrain::configureAidingSources()
914919 _param_ms_int_mag_en.get (),
915920 0 , 0 , nullptr , mip_aiding_frame_config_command_rotation{},
916921 0 , _int_aiding, " internal magnetometer" );
922+
917923 if (!mip_cmd_result_is_ack (res)) {
918924 return res;
919925 }
@@ -926,6 +932,7 @@ mip_cmd_result MicroStrain::configureAidingSources()
926932 MIP_CMD_DESC_AIDING_MAGNETIC_FIELD ,
927933 _ext_mag_aiding,
928934 " external magnetometer" );
935+
929936 if (!mip_cmd_result_is_ack (res)) {
930937 return res;
931938 }
@@ -938,6 +945,7 @@ mip_cmd_result MicroStrain::configureAidingSources()
938945 MIP_CMD_DESC_AIDING_VEL_ODOM ,
939946 _ext_optical_flow_aiding,
940947 " optical flow" );
948+
941949 if (!mip_cmd_result_is_ack (res)) {
942950 return res;
943951 }
@@ -950,6 +958,7 @@ mip_cmd_result MicroStrain::configureAidingSources()
950958 MIP_CMD_DESC_AIDING_HEADING_TRUE ,
951959 _ext_heading_aiding,
952960 " external heading" );
961+
953962 if (!mip_cmd_result_is_ack (res)) {
954963 return res;
955964 }
@@ -1018,6 +1027,7 @@ bool MicroStrain::initializeIns()
10181027 PX4_INFO (" Setting the baud to desired baud rate" );
10191028
10201029 res = writeBaudRate (DESIRED_BAUDRATE , 1 );
1030+
10211031 if (!mip_cmd_result_is_ack (res)) {
10221032 PX4_ERR (" Could not set the baudrate!" );
10231033 return false ;
@@ -1033,13 +1043,15 @@ bool MicroStrain::initializeIns()
10331043
10341044 // Configure IMU ranges
10351045 res = configureImuRange ();
1046+
10361047 if (!mip_cmd_result_is_ack (res)) {
10371048 MS_PX4_ERROR (res, " Could not configure IMU range" );
10381049 return false ;
10391050 }
10401051
10411052 // Configure the IMU message formt based on what descriptors are supported
10421053 res = configureImuMessageFormat ();
1054+
10431055 if (!mip_cmd_result_is_ack (res)) {
10441056 MS_PX4_ERROR (res, " Could not write IMU message format" );
10451057 return false ;
@@ -1052,6 +1064,7 @@ bool MicroStrain::initializeIns()
10521064
10531065 // Configure the Filter message format based on what descriptors are supported
10541066 res = configureFilterMessageFormat ();
1067+
10551068 if (!mip_cmd_result_is_ack (res)) {
10561069 MS_PX4_ERROR (res, " Could not write filter message format" );
10571070 return false ;
@@ -1064,6 +1077,7 @@ bool MicroStrain::initializeIns()
10641077
10651078 // Configure the GNSS1 message format based on what descriptors are supported
10661079 res = configureGnssMessageFormat (MIP_GNSS1_DATA_DESC_SET );
1080+
10671081 if (!mip_cmd_result_is_ack (res)) {
10681082 MS_PX4_ERROR (res, " Could not write GNSS1 message format" );
10691083 }
@@ -1075,6 +1089,7 @@ bool MicroStrain::initializeIns()
10751089
10761090 // Configure the GNSS2 message format based on what descriptors are supported
10771091 res = configureGnssMessageFormat (MIP_GNSS2_DATA_DESC_SET );
1092+
10781093 if (!mip_cmd_result_is_ack (res)) {
10791094 MS_PX4_ERROR (res, " Could not write GNSS2 message format" );
10801095 }
@@ -1086,13 +1101,15 @@ bool MicroStrain::initializeIns()
10861101
10871102 // Configure the aiding sources based on what the sensor supports
10881103 res = configureAidingSources ();
1104+
10891105 if (!mip_cmd_result_is_ack (res)) {
10901106 MS_PX4_ERROR (res, " Could not configure aiding frames!" );
10911107 return false ;
10921108 }
10931109
10941110 // Initialize the filter
10951111 res = writeFilterInitConfig ();
1112+
10961113 if (!mip_cmd_result_is_ack (res)) {
10971114 MS_PX4_ERROR (res, " Could not configure filter initialization!" );
10981115 return false ;
@@ -1103,9 +1120,10 @@ bool MicroStrain::initializeIns()
11031120 PX4_DEBUG (" Writing SVT" );
11041121
11051122 res = mip_3dm_write_sensor_2_vehicle_transform_euler (&_device,
1106- math::radians<float >(rotation_sens.euler [0 ]),
1107- math::radians<float >(rotation_sens.euler [1 ]),
1108- math::radians<float >(rotation_sens.euler [2 ]));
1123+ math::radians<float >(rotation_sens.euler [0 ]),
1124+ math::radians<float >(rotation_sens.euler [1 ]),
1125+ math::radians<float >(rotation_sens.euler [2 ]));
1126+
11091127 if (!mip_cmd_result_is_ack (res)) {
11101128 MS_PX4_ERROR (res, " Could not set sensor-to-vehicle transformation!" );
11111129 return false ;
@@ -1117,6 +1135,7 @@ bool MicroStrain::initializeIns()
11171135 PX4_DEBUG (" Reseting filter" );
11181136
11191137 res = mip_filter_reset (&_device);
1138+
11201139 if (!mip_cmd_result_is_ack (res)) {
11211140 MS_PX4_ERROR (res, " Could not reset the filter!" );
11221141 return false ;
@@ -1128,6 +1147,7 @@ bool MicroStrain::initializeIns()
11281147
11291148 res = mip_3dm_write_datastream_control (&_device,
11301149 MIP_3DM_DATASTREAM_CONTROL_COMMAND_ALL_STREAMS , true );
1150+
11311151 if (!mip_cmd_result_is_ack (res)) {
11321152 MS_PX4_ERROR (res, " Could not enable the data stream" );
11331153 return false ;
@@ -1139,6 +1159,7 @@ bool MicroStrain::initializeIns()
11391159 PX4_DEBUG (" Resuming device" );
11401160
11411161 res = mip_base_resume (&_device);
1162+
11421163 if (!mip_cmd_result_is_ack (res)) {
11431164 MS_PX4_ERROR (res, " Could not resume the device!" );
11441165 return false ;
0 commit comments