From 7659402fdb974b8a5b9acc2b9f8a3fcc8d69b370 Mon Sep 17 00:00:00 2001 From: James Goppert Date: Fri, 17 Feb 2017 12:36:52 -0500 Subject: [PATCH] WIP: valgrind runtime analysis and fixes (#6521) * Fix several valgrind identified mem leaks * Added callgrind target. * px4_posix_tasks use nullptr --- Tools/sitl_run.sh | 5 +- src/drivers/device/vdev_posix.cpp | 5 + src/firmware/posix/sitl_target.cmake | 2 +- src/modules/mavlink/mavlink_messages.cpp | 96 +++++++++---------- src/modules/simulator/simulator.h | 9 +- .../posix/px4_layer/px4_posix_tasks.cpp | 1 + 6 files changed, 67 insertions(+), 51 deletions(-) diff --git a/Tools/sitl_run.sh b/Tools/sitl_run.sh index 9c318f7b9b..954119e89a 100755 --- a/Tools/sitl_run.sh +++ b/Tools/sitl_run.sh @@ -136,7 +136,10 @@ then ddd --debugger gdb --args $sitl_command elif [ "$debugger" == "valgrind" ] then - valgrind $sitl_command + valgrind --track-origins=yes --leak-check=full -v $sitl_command +elif [ "$debugger" == "callgrind" ] +then + valgrind --tool=callgrind -v $sitl_command elif [ "$debugger" == "ide" ] then echo "######################################################################" diff --git a/src/drivers/device/vdev_posix.cpp b/src/drivers/device/vdev_posix.cpp index bfc57cf6a4..31de7129e8 100644 --- a/src/drivers/device/vdev_posix.cpp +++ b/src/drivers/device/vdev_posix.cpp @@ -168,6 +168,11 @@ extern "C" { if (dev) { pthread_mutex_lock(&filemutex); ret = dev->close(filemap[fd]); + + if (filemap[fd] != nullptr) { + delete filemap[fd]; + } + filemap[fd] = nullptr; pthread_mutex_unlock(&filemutex); PX4_DEBUG("px4_close fd = %d", fd); diff --git a/src/firmware/posix/sitl_target.cmake b/src/firmware/posix/sitl_target.cmake index 7c988992c2..1c2f7a07a2 100644 --- a/src/firmware/posix/sitl_target.cmake +++ b/src/firmware/posix/sitl_target.cmake @@ -68,7 +68,7 @@ set_target_properties(sitl_gazebo PROPERTIES EXCLUDE_FROM_ALL TRUE) # create targets for each viewer/model/debugger combination set(viewers none jmavsim gazebo replay) -set(debuggers none ide gdb lldb ddd valgrind) +set(debuggers none ide gdb lldb ddd valgrind callgrind) set(models none iris iris_opt_flow standard_vtol plane solo tailsitter typhoon_h480) set(all_posix_vmd_make_targets) foreach(viewer ${viewers}) diff --git a/src/modules/mavlink/mavlink_messages.cpp b/src/modules/mavlink/mavlink_messages.cpp index 6206fb4898..901602953c 100644 --- a/src/modules/mavlink/mavlink_messages.cpp +++ b/src/modules/mavlink/mavlink_messages.cpp @@ -318,7 +318,7 @@ protected: void send(const hrt_abstime t) { - struct vehicle_status_s status; + struct vehicle_status_s status = {}; /* always send the heartbeat, independent of the update status of the topics */ if (!_status_sub->update(&status)) { @@ -400,11 +400,11 @@ protected: { if (!_mavlink->get_logbuffer()->empty()) { - struct mavlink_log_s mavlink_log; + struct mavlink_log_s mavlink_log = {}; if (_mavlink->get_logbuffer()->get(&mavlink_log)) { - mavlink_statustext_t msg; + mavlink_statustext_t msg = {}; msg.severity = mavlink_log.severity; strncpy(msg.text, (const char *)mavlink_log.text, sizeof(msg.text)); msg.text[sizeof(msg.text) - 1] = '\0'; @@ -419,7 +419,7 @@ protected: timespec ts; px4_clock_gettime(CLOCK_REALTIME, &ts); time_t gps_time_sec = ts.tv_sec + (ts.tv_nsec / 1e9); - struct tm tt; + struct tm tt = {}; gmtime_r(&gps_time_sec, &tt); char tstamp[22]; strftime(tstamp, sizeof(tstamp) - 1, "%Y_%m_%d_%H_%M_%S", &tt); @@ -530,12 +530,12 @@ protected: void send(const hrt_abstime t) { - struct vehicle_command_s cmd; + struct vehicle_command_s cmd = {}; if (_cmd_sub->update(&_cmd_time, &cmd)) { /* only send commands for other systems/components */ if (cmd.target_system != mavlink_system.sysid || cmd.target_component != mavlink_system.compid) { - mavlink_command_long_t msg; + mavlink_command_long_t msg = {}; msg.target_system = cmd.target_system; msg.target_component = cmd.target_component; @@ -624,7 +624,7 @@ protected: } if (updated_status || updated_battery || updated_cpuload) { - mavlink_sys_status_t msg; + mavlink_sys_status_t msg = {}; msg.onboard_control_sensors_present = status.onboard_control_sensors_present; msg.onboard_control_sensors_enabled = status.onboard_control_sensors_enabled; @@ -732,8 +732,8 @@ protected: void send(const hrt_abstime t) { - struct sensor_combined_s sensor; - struct differential_pressure_s differential_pressure; + struct sensor_combined_s sensor = {}; + struct differential_pressure_s differential_pressure = {}; if (_sensor_sub->update(&_sensor_time, &sensor)) { uint16_t fields_updated = 0; @@ -764,7 +764,7 @@ protected: _differential_pressure_sub->update(&_differential_pressure_time, &differential_pressure); - mavlink_highres_imu_t msg; + mavlink_highres_imu_t msg = {}; msg.time_usec = sensor.timestamp; msg.xacc = sensor.accelerometer_m_s2[0]; @@ -841,7 +841,7 @@ protected: struct vehicle_attitude_s att; if (_att_sub->update(&_att_time, &att)) { - mavlink_attitude_t msg; + mavlink_attitude_t msg = {}; matrix::Eulerf euler = matrix::Quatf(att.q); msg.time_boot_ms = att.timestamp / 1000; msg.roll = euler.phi(); @@ -909,7 +909,7 @@ protected: struct vehicle_attitude_s att; if (_att_sub->update(&_att_time, &att)) { - mavlink_attitude_quaternion_t msg; + mavlink_attitude_quaternion_t msg = {}; msg.time_boot_ms = att.timestamp / 1000; msg.q1 = att.q[0]; @@ -1014,7 +1014,7 @@ protected: updated |= _airspeed_sub->update(&_airspeed_time, &airspeed); if (updated) { - mavlink_vfr_hud_t msg; + mavlink_vfr_hud_t msg = {}; matrix::Eulerf euler = matrix::Quatf(att.q); msg.airspeed = airspeed.indicated_airspeed_m_s; msg.groundspeed = sqrtf(pos.vel_n * pos.vel_n + pos.vel_e * pos.vel_e); @@ -1154,7 +1154,7 @@ protected: void send(const hrt_abstime t) { - mavlink_system_time_t msg; + mavlink_system_time_t msg = {}; timespec tv; px4_clock_gettime(CLOCK_REALTIME, &tv); @@ -1210,7 +1210,7 @@ protected: void send(const hrt_abstime t) { - mavlink_timesync_t msg; + mavlink_timesync_t msg = {}; msg.tc1 = 0; msg.ts1 = hrt_absolute_time() * 1000; // boot time in nanoseconds @@ -1411,7 +1411,7 @@ protected: struct camera_trigger_s trigger; if (_trigger_sub->update(&_trigger_time, &trigger)) { - mavlink_camera_trigger_t msg; + mavlink_camera_trigger_t msg = {}; msg.time_usec = trigger.timestamp; msg.seq = trigger.seq; @@ -1502,7 +1502,7 @@ protected: updated |= _home_sub->update(&_home_time, &home); if (updated) { - mavlink_global_position_int_t msg; + mavlink_global_position_int_t msg = {}; msg.time_boot_ms = pos.timestamp / 1000; msg.lat = pos.lat * 1e7; @@ -1580,7 +1580,7 @@ protected: bool pos_updated = _pos_sub->update(&_pos_time, &vpos); if (pos_updated || att_updated) { - mavlink_vision_position_estimate_t vmsg; + mavlink_vision_position_estimate_t vmsg = {}; vmsg.usec = vpos.timestamp; vmsg.x = vpos.x; vmsg.y = vpos.y; @@ -1648,7 +1648,7 @@ protected: struct vehicle_local_position_s pos; if (_pos_sub->update(&_pos_time, &pos)) { - mavlink_local_position_ned_t msg; + mavlink_local_position_ned_t msg = {}; msg.time_boot_ms = pos.timestamp / 1000; msg.x = pos.x; @@ -1875,7 +1875,7 @@ protected: struct att_pos_mocap_s mocap; if (_mocap_sub->update(&_mocap_time, &mocap)) { - mavlink_att_pos_mocap_t msg; + mavlink_att_pos_mocap_t msg = {}; msg.time_usec = mocap.timestamp; msg.q[0] = mocap.q[0]; @@ -1945,7 +1945,7 @@ protected: struct home_position_s home; if (_home_sub->update(&home)) { - mavlink_home_position_t msg; + mavlink_home_position_t msg = {}; msg.latitude = home.lat * 1e7; msg.longitude = home.lon * 1e7; @@ -2041,7 +2041,7 @@ protected: struct actuator_outputs_s act; if (_act_sub->update(&_act_time, &act)) { - mavlink_servo_output_raw_t msg; + mavlink_servo_output_raw_t msg = {}; msg.time_usec = act.timestamp; msg.port = N; @@ -2143,7 +2143,7 @@ protected: struct actuator_controls_s att_ctrl; if (_att_ctrl_sub->update(&_att_ctrl_time, &att_ctrl)) { - mavlink_actuator_control_target_t msg; + mavlink_actuator_control_target_t msg = {}; msg.time_usec = att_ctrl.timestamp; msg.group_mlx = N; @@ -2303,7 +2303,7 @@ protected: } } - mavlink_hil_controls_t msg; + mavlink_hil_controls_t msg = {}; msg.time_usec = hrt_absolute_time(); msg.roll_ailerons = out[0]; @@ -2387,7 +2387,7 @@ protected: uint8_t mavlink_state; uint8_t mavlink_base_mode; uint32_t mavlink_custom_mode; - mavlink_hil_actuator_controls_t msg; + mavlink_hil_actuator_controls_t msg = {}; get_mavlink_mode_state(&status, &mavlink_state, &mavlink_base_mode, &mavlink_custom_mode); @@ -2530,7 +2530,7 @@ protected: struct position_setpoint_triplet_s pos_sp_triplet; if (_pos_sp_triplet_sub->update(&pos_sp_triplet)) { - mavlink_position_target_global_int_t msg{}; + mavlink_position_target_global_int_t msg = {}; msg.time_boot_ms = hrt_absolute_time() / 1000; msg.coordinate_frame = MAV_FRAME_GLOBAL; @@ -2596,7 +2596,7 @@ protected: struct vehicle_local_position_setpoint_s pos_sp; if (_pos_sp_sub->update(&_pos_sp_time, &pos_sp)) { - mavlink_position_target_local_ned_t msg{}; + mavlink_position_target_local_ned_t msg = {}; msg.time_boot_ms = pos_sp.timestamp / 1000; msg.coordinate_frame = MAV_FRAME_LOCAL_NED; @@ -2670,14 +2670,14 @@ protected: void send(const hrt_abstime t) { - struct vehicle_attitude_setpoint_s att_sp; + struct vehicle_attitude_setpoint_s att_sp = {}; if (_att_sp_sub->update(&_att_sp_time, &att_sp)) { - struct vehicle_rates_setpoint_s att_rates_sp; + struct vehicle_rates_setpoint_s att_rates_sp = {}; (void)_att_rates_sp_sub->update(&_att_rates_sp_time, &att_rates_sp); - mavlink_attitude_target_t msg{}; + mavlink_attitude_target_t msg = {}; msg.time_boot_ms = att_sp.timestamp / 1000; @@ -2749,12 +2749,12 @@ protected: void send(const hrt_abstime t) { - struct rc_input_values rc; + struct rc_input_values rc = {}; if (_rc_sub->update(&_rc_time, &rc)) { /* send RC channel data and RSSI */ - mavlink_rc_channels_t msg; + mavlink_rc_channels_t msg = {}; msg.time_boot_ms = rc.timestamp / 1000; msg.chancount = rc.channel_count; @@ -2850,10 +2850,10 @@ protected: void send(const hrt_abstime t) { - struct manual_control_setpoint_s manual; + struct manual_control_setpoint_s manual = {}; if (_manual_sub->update(&_manual_time, &manual)) { - mavlink_manual_control_t msg; + mavlink_manual_control_t msg = {}; msg.target = mavlink_system.sysid; msg.x = manual.x * 1000; @@ -2923,10 +2923,10 @@ protected: void send(const hrt_abstime t) { - struct optical_flow_s flow; + struct optical_flow_s flow = {}; if (_flow_sub->update(&_flow_time, &flow)) { - mavlink_optical_flow_rad_t msg; + mavlink_optical_flow_rad_t msg = {}; msg.time_usec = flow.timestamp; msg.sensor_id = flow.sensor_id; @@ -2996,10 +2996,10 @@ protected: void send(const hrt_abstime t) { - struct debug_key_value_s debug; + struct debug_key_value_s debug = {}; if (_debug_sub->update(&_debug_time, &debug)) { - mavlink_named_value_float_t msg; + mavlink_named_value_float_t msg = {}; msg.time_boot_ms = debug.timestamp_ms; memcpy(msg.name, debug.key, sizeof(msg.name)); @@ -3135,7 +3135,7 @@ protected: struct vehicle_status_s status = {}; (void)_status_sub->update(&status); - mavlink_command_long_t msg; + mavlink_command_long_t msg = {}; msg.target_system = mavlink_system.sysid; msg.target_component = MAV_COMP_ID_ALL; @@ -3204,11 +3204,11 @@ protected: void send(const hrt_abstime t) { - struct distance_sensor_s dist_sensor; + struct distance_sensor_s dist_sensor = {}; if (_distance_sensor_sub->update(&_dist_sensor_time, &dist_sensor)) { - mavlink_distance_sensor_t msg; + mavlink_distance_sensor_t msg = {}; msg.time_boot_ms = dist_sensor.timestamp / 1000; /* us to ms */ @@ -3290,7 +3290,7 @@ protected: explicit MavlinkStreamExtendedSysState(Mavlink *mavlink) : MavlinkStream(mavlink), _status_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_status))), _landed_sub(_mavlink->add_orb_subscription(ORB_ID(vehicle_land_detected))), - _msg{} + _msg() { _msg.vtol_state = MAV_VTOL_STATE_UNDEFINED; @@ -3299,8 +3299,8 @@ protected: void send(const hrt_abstime t) { - struct vehicle_status_s status; - struct vehicle_land_detected_s land_detected; + struct vehicle_status_s status = {}; + struct vehicle_land_detected_s land_detected = {}; bool updated = false; if (_status_sub->update(&status)) { @@ -3403,7 +3403,7 @@ protected: void send(const hrt_abstime t) { - mavlink_altitude_t msg; + mavlink_altitude_t msg = {}; bool updated = false; float global_alt = 0.0f; @@ -3448,7 +3448,7 @@ protected: } { - struct home_position_s home; + struct home_position_s home = {}; updated |= _home_sub->update(&_home_time, &home); if (_global_pos_time > 0 && _home_time > 0) { @@ -3467,7 +3467,7 @@ protected: msg.time_usec = hrt_absolute_time(); { - struct sensor_combined_s sensor; + struct sensor_combined_s sensor = {}; (void)_sensor_sub->update(&_sensor_time, &sensor); msg.altitude_monotonic = (_sensor_time > 0) ? sensor.baro_alt_meter : NAN; } @@ -3548,7 +3548,7 @@ protected: msg.var_horiz = wind_estimate.covariance_north + wind_estimate.covariance_east; msg.var_vert = 0.0f; - struct vehicle_global_position_s global_pos; + struct vehicle_global_position_s global_pos = {}; _global_pos_sub->update(&_global_pos_time, &global_pos); msg.wind_alt = (_global_pos_time > 0) ? global_pos.alt : NAN; diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 3c57a928e9..24711e1130 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -278,7 +278,14 @@ private: _actuator_outputs_sub[i] = -1; } } - ~Simulator() { _instance = NULL; } + ~Simulator() + { + if (_instance != nullptr) { + delete _instance; + } + + _instance = NULL; + } void initializeSensorData(); diff --git a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp index adcd272d59..ee845b5fce 100644 --- a/src/platforms/posix/px4_layer/px4_posix_tasks.cpp +++ b/src/platforms/posix/px4_layer/px4_posix_tasks.cpp @@ -291,6 +291,7 @@ int px4_task_delete(px4_task_t id) // If current thread then exit, otherwise cancel if (pthread_self() == pid) { + pthread_join(pid, nullptr); taskmap[id].isused = false; pthread_mutex_unlock(&task_mutex); pthread_exit(nullptr);