position_estimator_inav fix and enforce code style

Tools/check_code_style_all.sh

@@ -21,7 +21,6 @@ find src/ \
     -path src/modules/mc_att_control_multiplatform -prune -o \
     -path src/modules/mc_pos_control_multiplatform -prune -o \
     -path src/modules/navigator -prune -o \
-    -path src/modules/position_estimator_inav -prune -o \
     -path src/modules/sdlog2 -prune -o \
     -path src/modules/uavcan -prune -o \
     -path src/modules/uavcan/libuavcan -prune -o \

src/modules/position_estimator_inav/position_estimator_inav_main.cpp

@@ -417,11 +417,12 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		if (ret < 0) {
 			/* poll error */
 			mavlink_log_info(&mavlink_log_pub, "[inav] poll error on init");
+
 		} else if (hrt_absolute_time() - baro_wait_for_sample_time > MAX_WAIT_FOR_BARO_SAMPLE) {
 			wait_baro = false;
 			mavlink_log_info(&mavlink_log_pub, "[inav] timed out waiting for a baro sample");
-		}
-		else if (ret > 0) {
+
+		} else if (ret > 0) {
 			if (fds_init[0].revents & POLLIN) {
 				orb_copy(ORB_ID(sensor_combined), sensor_combined_sub, &sensor);
 
@@ -547,8 +548,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 			if (updated) { //check if altitude estimation for lidar is enabled and new sensor data
 
-				if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance && lidar.current_distance < lidar.max_distance
-			    		&& (PX4_R(att.R, 2, 2) > 0.7f)) {
+				if (params.enable_lidar_alt_est && lidar.current_distance > lidar.min_distance
+				    && lidar.current_distance < lidar.max_distance
+				    && (PX4_R(att.R, 2, 2) > 0.7f)) {
 
 					if (!use_lidar_prev && use_lidar) {
 						lidar_first = true;
@@ -584,6 +586,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						lidar_offset_count = 0;
 						lidar_valid_time = t;
 					}
+
 				} else {
 					lidar_valid = false;
 				}
@@ -654,16 +657,19 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 					/* check for vehicle rates setpoint - it below threshold -> dont subtract -> better hover */
 					orb_check(vehicle_rate_sp_sub, &updated);
-					if (updated)
+
+					if (updated) {
 						orb_copy(ORB_ID(vehicle_rates_setpoint), vehicle_rate_sp_sub, &rates_setpoint);
+					}
 
 					float rate_threshold = 0.15f;
 
 					if (fabsf(rates_setpoint.pitch) < rate_threshold) {
 						//warnx("[inav] test ohne comp");
-						flow_ang[0] = (flow.pixel_flow_x_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small)
-					}
-					else {
+						flow_ang[0] = (flow.pixel_flow_x_integral / (float)flow.integration_timespan * 1000000.0f) *
+							      params.flow_k;//for now the flow has to be scaled (to small)
+
+					} else {
 						//warnx("[inav] test mit comp");
 						//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)
 						flow_ang[0] = ((flow.pixel_flow_x_integral - flow.gyro_x_rate_integral) / (float)flow.integration_timespan * 1000000.0f
@@ -671,9 +677,10 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 					}
 
 					if (fabsf(rates_setpoint.roll) < rate_threshold) {
-						flow_ang[1] = (flow.pixel_flow_y_integral / (float)flow.integration_timespan * 1000000.0f) * params.flow_k;//for now the flow has to be scaled (to small)
-					}
-					else {
+						flow_ang[1] = (flow.pixel_flow_y_integral / (float)flow.integration_timespan * 1000000.0f) *
+							      params.flow_k;//for now the flow has to be scaled (to small)
+
+					} else {
 						//calculate flow [rad/s] and compensate for rotations (and offset of flow-gyro)
 						flow_ang[1] = ((flow.pixel_flow_y_integral - flow.gyro_y_rate_integral) / (float)flow.integration_timespan * 1000000.0f
 							       + gyro_offset_filtered[1]) * params.flow_k;//for now the flow has to be scaled (to small)
@@ -681,13 +688,16 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 					/* flow measurements vector */
 					float flow_m[3];
+
 					if (fabsf(rates_setpoint.yaw) < rate_threshold) {
 						flow_m[0] = -flow_ang[0] * flow_dist;
 						flow_m[1] = -flow_ang[1] * flow_dist;
+
 					} else {
 						flow_m[0] = -flow_ang[0] * flow_dist - yaw_comp[0] * params.flow_k;
 						flow_m[1] = -flow_ang[1] * flow_dist - yaw_comp[1] * params.flow_k;
 					}
+
 					flow_m[2] = z_est[1];
 
 					/* velocity in NED */
@@ -999,7 +1009,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		bool use_vision_xy = vision_valid && params.w_xy_vision_p > MIN_VALID_W;
 		bool use_vision_z = vision_valid && params.w_z_vision_p > MIN_VALID_W;
 		/* use MOCAP if it's valid and has a valid weight parameter */
-		bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W && params.att_ext_hdg_m == mocap_heading; //check if external heading is mocap
+		bool use_mocap = mocap_valid && params.w_mocap_p > MIN_VALID_W
+				 && params.att_ext_hdg_m == mocap_heading; //check if external heading is mocap
 
 		if (params.disable_mocap) { //disable mocap if fake gps is used
 			use_mocap = false;
@@ -1119,6 +1130,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 		if (use_lidar) {
 			accel_bias_corr[2] -= corr_lidar * params.w_z_lidar * params.w_z_lidar;
+
 		} else {
 			accel_bias_corr[2] -= corr_baro * params.w_z_baro * params.w_z_baro;
 		}