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

MAVLink Sim: Use time delta

Browse Source
This commit is contained in:
Lorenz Meier
2015-11-25 11:13:32 +01:00
parent 1216efc5b4
commit c1a2d19459
1 changed files with 12 additions and 13 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/simulator/simulator_mavlink.cpp
+12 -13
View File
@@ -237,23 +237,22 @@ void Simulator::update_gps(mavlink_hil_gps_t *gps_sim)
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);
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;
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);
perf_set(_perf_sim_delay, timestamp - sim_timestamp);
if (publish) {
publish_sensor_topics(&imu);
}
if (publish) {
publish_sensor_topics(&imu);
}
update_sensors(&imu);
update_sensors(&imu);
}
break;