diff --git a/Tools/check_code_style_all.sh b/Tools/check_code_style_all.sh index 4530dff518..4d8ac78be8 100755 --- a/Tools/check_code_style_all.sh +++ b/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 \ diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp b/src/modules/position_estimator_inav/position_estimator_inav_main.cpp index 6c2f670762..70959bcf22 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.cpp +++ b/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; }