Simulator: Add performance counters for delay

src/modules/simulator/simulator.h

@@ -50,6 +50,7 @@
 #include <drivers/drv_mag.h>
 #include <drivers/drv_hrt.h>
 #include <drivers/drv_rc_input.h>
+#include <systemlib/perf_counter.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/optical_flow.h>
 #include <v1.0/mavlink_types.h>
@@ -222,6 +223,13 @@ private:
 		_mag(1),
 		_gps(1),
 		_airspeed(1),
+		_perf_accel(perf_alloc_once(PC_ELAPSED, "sim_accel_delay")),
+		_perf_mpu(perf_alloc_once(PC_ELAPSED, "sim_mpu_delay")),
+		_perf_baro(perf_alloc_once(PC_ELAPSED, "sim_baro_delay")),
+		_perf_mag(perf_alloc_once(PC_ELAPSED, "sim_mag_delay")),
+		_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")),
 		_accel_pub(nullptr),
 		_baro_pub(nullptr),
 		_gyro_pub(nullptr),
@@ -255,6 +263,14 @@ private:
 	simulator::Report<simulator::RawGPSData>	_gps;
 	simulator::Report<simulator::RawAirspeedData> _airspeed;
 
+	perf_counter_t _perf_accel;
+	perf_counter_t _perf_mpu;
+	perf_counter_t _perf_baro;
+	perf_counter_t _perf_mag;
+	perf_counter_t _perf_gps;
+	perf_counter_t _perf_airspeed;
+	perf_counter_t _perf_sim_delay;
+
 	// uORB publisher handlers
 	orb_advert_t _accel_pub;
 	orb_advert_t _baro_pub;

src/modules/simulator/simulator_mavlink.cpp

@@ -182,6 +182,7 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu)
 	mpu.gyro_z = imu->zgyro;
 
 	write_MPU_data((void *)&mpu);
+	perf_begin(_perf_mpu);
 
 	RawAccelData accel = {};
 	accel.x = imu->xacc;
@@ -189,6 +190,7 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu)
 	accel.z = imu->zacc;
 
 	write_accel_data((void *)&accel);
+	perf_begin(_perf_accel);
 
 	RawMagData mag = {};
 	mag.x = imu->xmag;
@@ -196,6 +198,7 @@ void Simulator::update_sensors(mavlink_hil_sensor_t *imu)
 	mag.z = imu->zmag;
 
 	write_mag_data((void *)&mag);
+	perf_begin(_perf_mag);
 
 	RawBaroData baro = {};
 	// calculate air pressure from altitude (valid for low altitude)
@@ -235,14 +238,23 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 {
 	switch (msg->msgid) {
 	case MAVLINK_MSG_ID_HIL_SENSOR:
+		{
 		mavlink_hil_sensor_t imu;
 		mavlink_msg_hil_sensor_decode(msg, &imu);
 
+		uint64_t sim_timestamp = imu.time_usec;
+		struct timespec ts;
+		px4_clock_gettime(CLOCK_REALTIME, &ts);
+		uint64_t timestamp = ts.tv_sec * 1000 * 1000 + ts.tv_nsec / 1000;
+
+		perf_set(_perf_sim_delay, timestamp - sim_timestamp);
+
 		if (publish) {
 			publish_sensor_topics(&imu);
 		}
 
 		update_sensors(&imu);
+		}
 		break;
 
 	case MAVLINK_MSG_ID_HIL_OPTICAL_FLOW:
@@ -379,12 +391,12 @@ void Simulator::initializeSensorData()
 	RawMPUData mpu = {};
 	mpu.accel_z = 9.81f;
 
-	write_MPU_data((void *)&mpu);
+	write_MPU_data(&mpu);
 
 	RawAccelData accel = {};
 	accel.z = 9.81f;
 
-	write_accel_data((void *)&accel);
+	write_accel_data(&accel);
 
 	RawMagData mag = {};
 	mag.x = 0.4f;
@@ -399,11 +411,11 @@ void Simulator::initializeSensorData()
 	baro.altitude = 0.0f;
 	baro.temperature = 25.0f;
 
-	write_baro_data((void *)&baro);
+	write_baro_data(&baro);
 
 	RawAirspeedData airspeed {};
 
-	write_airspeed_data((void *)&airspeed);
+	write_airspeed_data(&airspeed);
 }
 
 void Simulator::pollForMAVLinkMessages(bool publish)
@@ -664,8 +676,6 @@ int openUart(const char *uart_name, int baud)
 int Simulator::publish_sensor_topics(mavlink_hil_sensor_t *imu)
 {
 
-
-	//uint64_t timestamp = imu->time_usec;
 	uint64_t timestamp = hrt_absolute_time();
 
 	if ((imu->fields_updated & 0x1FFF) != 0x1FFF) {