diff --git a/src/modules/simulator/simulator.h b/src/modules/simulator/simulator.h index 182be7916b..5da68d002a 100644 --- a/src/modules/simulator/simulator.h +++ b/src/modules/simulator/simulator.h @@ -230,6 +230,7 @@ private: _perf_gps(perf_alloc_once(PC_ELAPSED, "sim_gps_delay")), _perf_airspeed(perf_alloc_once(PC_ELAPSED, "sim_airspeed_delay")), _perf_sim_delay(perf_alloc_once(PC_ELAPSED, "sim_network_delay")), + _perf_sim_interval(perf_alloc(PC_INTERVAL, "sim_network_interval")), _accel_pub(nullptr), _baro_pub(nullptr), _gyro_pub(nullptr), @@ -270,6 +271,7 @@ private: perf_counter_t _perf_gps; perf_counter_t _perf_airspeed; perf_counter_t _perf_sim_delay; + perf_counter_t _perf_sim_interval; // uORB publisher handlers orb_advert_t _accel_pub; diff --git a/src/modules/simulator/simulator_mavlink.cpp b/src/modules/simulator/simulator_mavlink.cpp index 2b993c31f9..01899bf0ba 100644 --- a/src/modules/simulator/simulator_mavlink.cpp +++ b/src/modules/simulator/simulator_mavlink.cpp @@ -248,6 +248,7 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish) uint64_t timestamp = ts.tv_sec * 1000 * 1000 + ts.tv_nsec / 1000; perf_set(_perf_sim_delay, timestamp - sim_timestamp); + perf_count(_perf_sim_interval); if (publish) { publish_sensor_topics(&imu);