@@ -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
@@ -831,6 +832,7 @@ mip_cmd_result MicroStrain::configureGnssAiding()
831832 // Sets up the GNSS aiding source
832833 if (supportsDescriptor (MIP_FILTER_CMD_DESC_SET, MIP_CMD_DESC_FILTER_GNSS_SOURCE_CONTROL)) {
833834 res = mip_filter_write_gnss_source (&_device, (uint8_t )_param_ms_gnss_aid_src_ctrl.get ());
835+
834836 if (!mip_cmd_result_is_ack (res)) {
835837 PX4_ERR (" Could not write the gnss aiding source" );
836838 return res;
@@ -845,6 +847,7 @@ mip_cmd_result MicroStrain::configureGnssAiding()
845847 res = mip_aiding_write_frame_config (&_device, 1 ,
846848 MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER, false ,
847849 gnss_antenna_offset1, &rotation_gnss);
850+
848851 if (!mip_cmd_result_is_ack (res)) {
849852 PX4_ERR (" Could not write aiding frame config" );
850853 return res;
@@ -879,6 +882,7 @@ mip_cmd_result MicroStrain::configureGnssAiding()
879882 _param_ms_int_heading_en.get (),
880883 0 , 0 , nullptr , mip_aiding_frame_config_command_rotation{},
881884 0 , _int_aiding, " dual antenna heading" );
885+
882886 if (!mip_cmd_result_is_ack (res)) {
883887 return res;
884888 }
@@ -889,6 +893,7 @@ mip_cmd_result MicroStrain::configureGnssAiding()
889893 res = mip_aiding_write_frame_config (&_device, 1 ,
890894 MIP_AIDING_FRAME_CONFIG_COMMAND_FORMAT_EULER, false ,
891895 gnss_antenna_offset1, &rotation_gnss);
896+
892897 if (!mip_cmd_result_is_ack (res)) {
893898 PX4_ERR (" Could not write aiding frame config" );
894899 return res;
@@ -912,6 +917,7 @@ mip_cmd_result MicroStrain::configureAidingSources()
912917 _param_ms_int_mag_en.get (),
913918 0 , 0 , nullptr , mip_aiding_frame_config_command_rotation{},
914919 0 , _int_aiding, " internal magnetometer" );
920+
915921 if (!mip_cmd_result_is_ack (res)) {
916922 return res;
917923 }
@@ -924,6 +930,7 @@ mip_cmd_result MicroStrain::configureAidingSources()
924930 MIP_CMD_DESC_AIDING_MAGNETIC_FIELD,
925931 _ext_mag_aiding,
926932 " external magnetometer" );
933+
927934 if (!mip_cmd_result_is_ack (res)) {
928935 return res;
929936 }
@@ -936,6 +943,7 @@ mip_cmd_result MicroStrain::configureAidingSources()
936943 MIP_CMD_DESC_AIDING_VEL_ODOM,
937944 _ext_optical_flow_aiding,
938945 " optical flow" );
946+
939947 if (!mip_cmd_result_is_ack (res)) {
940948 return res;
941949 }
@@ -948,6 +956,7 @@ mip_cmd_result MicroStrain::configureAidingSources()
948956 MIP_CMD_DESC_AIDING_HEADING_TRUE,
949957 _ext_heading_aiding,
950958 " external heading" );
959+
951960 if (!mip_cmd_result_is_ack (res)) {
952961 return res;
953962 }
@@ -1016,6 +1025,7 @@ bool MicroStrain::initializeIns()
10161025 PX4_INFO (" Setting the baud to desired baud rate" );
10171026
10181027 res = writeBaudRate (DESIRED_BAUDRATE, 1 );
1028+
10191029 if (!mip_cmd_result_is_ack (res)) {
10201030 PX4_ERR (" Could not set the baudrate!" );
10211031 return false ;
@@ -1031,13 +1041,15 @@ bool MicroStrain::initializeIns()
10311041
10321042 // Configure IMU ranges
10331043 res = configureImuRange ();
1044+
10341045 if (!mip_cmd_result_is_ack (res)) {
10351046 MS_PX4_ERROR (res, " Could not configure IMU range" );
10361047 return false ;
10371048 }
10381049
10391050 // Configure the IMU message formt based on what descriptors are supported
10401051 res = configureImuMessageFormat ();
1052+
10411053 if (!mip_cmd_result_is_ack (res)) {
10421054 MS_PX4_ERROR (res, " Could not write IMU message format" );
10431055 return false ;
@@ -1050,6 +1062,7 @@ bool MicroStrain::initializeIns()
10501062
10511063 // Configure the Filter message format based on what descriptors are supported
10521064 res = configureFilterMessageFormat ();
1065+
10531066 if (!mip_cmd_result_is_ack (res)) {
10541067 MS_PX4_ERROR (res, " Could not write filter message format" );
10551068 return false ;
@@ -1062,6 +1075,7 @@ bool MicroStrain::initializeIns()
10621075
10631076 // Configure the GNSS1 message format based on what descriptors are supported
10641077 res = configureGnssMessageFormat (MIP_GNSS1_DATA_DESC_SET);
1078+
10651079 if (!mip_cmd_result_is_ack (res)) {
10661080 MS_PX4_ERROR (res, " Could not write GNSS1 message format" );
10671081 }
@@ -1073,6 +1087,7 @@ bool MicroStrain::initializeIns()
10731087
10741088 // Configure the GNSS2 message format based on what descriptors are supported
10751089 res = configureGnssMessageFormat (MIP_GNSS2_DATA_DESC_SET);
1090+
10761091 if (!mip_cmd_result_is_ack (res)) {
10771092 MS_PX4_ERROR (res, " Could not write GNSS2 message format" );
10781093 }
@@ -1084,13 +1099,15 @@ bool MicroStrain::initializeIns()
10841099
10851100 // Configure the aiding sources based on what the sensor supports
10861101 res = configureAidingSources ();
1102+
10871103 if (!mip_cmd_result_is_ack (res)) {
10881104 MS_PX4_ERROR (res, " Could not configure aiding frames!" );
10891105 return false ;
10901106 }
10911107
10921108 // Initialize the filter
10931109 res = writeFilterInitConfig ();
1110+
10941111 if (!mip_cmd_result_is_ack (res)) {
10951112 MS_PX4_ERROR (res, " Could not configure filter initialization!" );
10961113 return false ;
@@ -1101,9 +1118,10 @@ bool MicroStrain::initializeIns()
11011118 PX4_DEBUG (" Writing SVT" );
11021119
11031120 res = mip_3dm_write_sensor_2_vehicle_transform_euler (&_device,
1104- math::radians<float >(rotation_sens.euler [0 ]),
1105- math::radians<float >(rotation_sens.euler [1 ]),
1106- math::radians<float >(rotation_sens.euler [2 ]));
1121+ math::radians<float >(rotation_sens.euler [0 ]),
1122+ math::radians<float >(rotation_sens.euler [1 ]),
1123+ math::radians<float >(rotation_sens.euler [2 ]));
1124+
11071125 if (!mip_cmd_result_is_ack (res)) {
11081126 MS_PX4_ERROR (res, " Could not set sensor-to-vehicle transformation!" );
11091127 return false ;
@@ -1115,6 +1133,7 @@ bool MicroStrain::initializeIns()
11151133 PX4_DEBUG (" Reseting filter" );
11161134
11171135 res = mip_filter_reset (&_device);
1136+
11181137 if (!mip_cmd_result_is_ack (res)) {
11191138 MS_PX4_ERROR (res, " Could not reset the filter!" );
11201139 return false ;
@@ -1126,6 +1145,7 @@ bool MicroStrain::initializeIns()
11261145
11271146 res = mip_3dm_write_datastream_control (&_device,
11281147 MIP_3DM_DATASTREAM_CONTROL_COMMAND_ALL_STREAMS, true );
1148+
11291149 if (!mip_cmd_result_is_ack (res)) {
11301150 MS_PX4_ERROR (res, " Could not enable the data stream" );
11311151 return false ;
@@ -1137,6 +1157,7 @@ bool MicroStrain::initializeIns()
11371157 PX4_DEBUG (" Resuming device" );
11381158
11391159 res = mip_base_resume (&_device);
1160+
11401161 if (!mip_cmd_result_is_ack (res)) {
11411162 MS_PX4_ERROR (res, " Could not resume the device!" );
11421163 return false ;
0 commit comments