fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

Simulator: Add performance counters for delay

Browse Source
This commit is contained in:
Lorenz Meier 2015-11-24 14:23:59 +01:00
parent 1de4403686
commit bd4497f883
2 changed files with 32 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

16
src/modules/simulator/simulator.h
View File

@ -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;

22
src/modules/simulator/simulator_mavlink.cpp
View File

@ -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) {