yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-22 01:12:31 +00:00
Code Issues Packages Projects Releases Wiki Activity

INAV: Valgrind fix

Browse Source 
Make sure `struct position_estimator_inav_params params` is properly initialized.

At first there was no indication that this struct was un-initialized because valgrind
was issuing warnings about values that were derived (via float calculations) from
values in this struct. Maybe instruction re-ordering during optimization caused
this disconnect between the source of the problem and the symptom.
This commit is contained in:
Erik de Castro Lopo
2015-11-27 11:30:24 +11:00
parent 1c6a7fb360
commit 0b3889e2e3
1 changed files with 8 additions and 7 deletions
Download Patch File Download Diff File Expand all files Collapse all files

15
src/modules/position_estimator_inav/position_estimator_inav_main.c
View File

@@ -216,7 +216,7 @@ static void write_debug_log(const char *msg, float dt, float x_est[2], float y_e
fclose(f);
}
#else
#define write_debug_log(...)
#define write_debug_log(...)
#endif
/****************************************************************************
@@ -380,6 +380,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
orb_advert_t vehicle_global_position_pub = NULL;
struct position_estimator_inav_params params;
memset(&params, 0, sizeof(params));
struct position_estimator_inav_param_handles pos_inav_param_handles;
/* initialize parameter handles */
inav_parameters_init(&pos_inav_param_handles);
@@ -595,7 +596,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
flow_gyrospeed[0] = flow.gyro_x_rate_integral/(float)flow.integration_timespan*1000000.0f;
flow_gyrospeed[1] = flow.gyro_y_rate_integral/(float)flow.integration_timespan*1000000.0f;
flow_gyrospeed[2] = flow.gyro_z_rate_integral/(float)flow.integration_timespan*1000000.0f;
//moving average
if (n_flow >= 100) {
gyro_offset_filtered[0] = flow_gyrospeed_filtered[0] - att_gyrospeed_filtered[0];
@@ -617,7 +618,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
att_gyrospeed_filtered[2] = (att.yawspeed + n_flow * att_gyrospeed_filtered[2]) / (n_flow + 1);
n_flow++;
}
/*yaw compensation (flow sensor is not in center of rotation) -> params in QGC*/
yaw_comp[0] = params.flow_module_offset_x * (flow_gyrospeed[2] - gyro_offset_filtered[2]);
@@ -841,7 +842,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
} else if (t > ref_init_start + ref_init_delay) {
ref_inited = true;
/* set position estimate to (0, 0, 0), use GPS velocity for XY */
x_est[0] = 0.0f;
x_est[1] = gps.vel_n_m_s;
@@ -981,7 +982,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
/* use flow if it's valid and (accurate or no GPS available) */
bool use_flow = flow_valid && (flow_accurate || !use_gps_xy);
bool can_estimate_xy = (eph < max_eph_epv) || use_gps_xy || use_flow || use_vision_xy || use_mocap;
bool dist_bottom_valid = (t < lidar_valid_time + lidar_valid_timeout);
@@ -1147,7 +1148,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
if (!(isfinite(z_est[0]) && isfinite(z_est[1]))) {
write_debug_log("BAD ESTIMATE AFTER Z CORRECTION", dt, x_est, y_est, z_est, x_est_prev, y_est_prev, z_est_prev,
acc, corr_gps, w_xy_gps_p, w_xy_gps_v, corr_mocap, w_mocap_p,
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
corr_vision, w_xy_vision_p, w_z_vision_p, w_xy_vision_v);
memcpy(z_est, z_est_prev, sizeof(z_est));
memset(corr_gps, 0, sizeof(corr_gps));
memset(corr_vision, 0, sizeof(corr_vision));
@@ -1274,7 +1275,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
buf_ptr = 0;
}
/* publish local position */
local_pos.xy_valid = can_estimate_xy;
local_pos.v_xy_valid = can_estimate_xy;