From 0b3889e2e3e9ab50dc9a37d8c7165322bbf80c27 Mon Sep 17 00:00:00 2001 From: Erik de Castro Lopo Date: Fri, 27 Nov 2015 11:30:24 +1100 Subject: [PATCH 1/4] INAV: Valgrind fix 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. --- .../position_estimator_inav_main.c | 15 ++++++++------- 1 file changed, 8 insertions(+), 7 deletions(-) diff --git a/src/modules/position_estimator_inav/position_estimator_inav_main.c b/src/modules/position_estimator_inav/position_estimator_inav_main.c index 56db8b4c0a..449b9c059f 100644 --- a/src/modules/position_estimator_inav/position_estimator_inav_main.c +++ b/src/modules/position_estimator_inav/position_estimator_inav_main.c @@ -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(¶ms, 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; From 6c2c2b19a7a588afbd26754fdabe0dc114d23fdc Mon Sep 17 00:00:00 2001 From: Erik de Castro Lopo Date: Fri, 27 Nov 2015 15:49:26 +1100 Subject: [PATCH 2/4] MAVLink: Only update rx count on successful read --- src/modules/mavlink/mavlink_receiver.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index 0c2180a4d8..d139ac99dd 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1837,8 +1837,10 @@ MavlinkReceiver::receive_thread(void *arg) } } - /* count received bytes */ - _mavlink->count_rxbytes(nread); + /* count received bytes (nread will be -1 on read error) */ + if (nread > 0) { + _mavlink->count_rxbytes(nread); + } } } From fb4bf7c59c47f9dd536dd4df87252b7d51727afe Mon Sep 17 00:00:00 2001 From: Erik de Castro Lopo Date: Fri, 27 Nov 2015 16:01:06 +1100 Subject: [PATCH 3/4] MAVLink: Fix call to orb_advertise_multi Previously, pointer to an uninitialized int was being passed as the instance pointer. Now we pass a NULL pointer instead and the code being called will use the default instance value of zero. --- src/modules/mavlink/mavlink_receiver.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/modules/mavlink/mavlink_receiver.cpp b/src/modules/mavlink/mavlink_receiver.cpp index d139ac99dd..37d070718d 100644 --- a/src/modules/mavlink/mavlink_receiver.cpp +++ b/src/modules/mavlink/mavlink_receiver.cpp @@ -1183,8 +1183,7 @@ MavlinkReceiver::handle_message_heartbeat(mavlink_message_t *msg) tstatus.heartbeat_time = tstatus.timestamp; if (_telemetry_status_pub == nullptr) { - int multi_instance; - _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, &multi_instance, ORB_PRIO_HIGH); + _telemetry_status_pub = orb_advertise_multi(ORB_ID(telemetry_status), &tstatus, NULL, ORB_PRIO_HIGH); } else { orb_publish(ORB_ID(telemetry_status), _telemetry_status_pub, &tstatus); From 67d61488da341d9311ef7b2f7ed8d970de6dd313 Mon Sep 17 00:00:00 2001 From: Erik de Castro Lopo Date: Fri, 27 Nov 2015 16:45:01 +1100 Subject: [PATCH 4/4] sensors: Initialize _parameters --- src/modules/sensors/sensors.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/modules/sensors/sensors.cpp b/src/modules/sensors/sensors.cpp index 3fbbdc80ba..827f1be916 100644 --- a/src/modules/sensors/sensors.cpp +++ b/src/modules/sensors/sensors.cpp @@ -542,6 +542,7 @@ Sensors::Sensors() : memset(&_rc, 0, sizeof(_rc)); memset(&_diff_pres, 0, sizeof(_diff_pres)); + memset(&_parameters, 0, sizeof(_parameters)); memset(&_rc_parameter_map, 0, sizeof(_rc_parameter_map)); /* basic r/c parameters */