From 019eaf70382ab1352dcf0a0223eb3f7c29a9cd81 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Wed, 15 Dec 2021 10:30:17 -0500 Subject: [PATCH] multicopter: minimize dt jitter by using estimate timestamp_sample --- src/modules/flight_mode_manager/FlightModeManager.cpp | 2 +- src/modules/mc_att_control/mc_att_control_main.cpp | 4 ++-- src/modules/mc_pos_control/MulticopterPositionControl.cpp | 2 +- 3 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/modules/flight_mode_manager/FlightModeManager.cpp b/src/modules/flight_mode_manager/FlightModeManager.cpp index f6d98b3038..1901cd8ece 100644 --- a/src/modules/flight_mode_manager/FlightModeManager.cpp +++ b/src/modules/flight_mode_manager/FlightModeManager.cpp @@ -99,7 +99,7 @@ void FlightModeManager::Run() vehicle_local_position_s vehicle_local_position; if (_vehicle_local_position_sub.update(&vehicle_local_position)) { - const hrt_abstime time_stamp_now = hrt_absolute_time(); + const hrt_abstime time_stamp_now = vehicle_local_position.timestamp_sample; // Guard against too small (< 0.2ms) and too large (> 100ms) dt's. const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) / 1e6f), 0.0002f, 0.1f); _time_stamp_last_loop = time_stamp_now; diff --git a/src/modules/mc_att_control/mc_att_control_main.cpp b/src/modules/mc_att_control/mc_att_control_main.cpp index f24d7df603..d614d1349a 100644 --- a/src/modules/mc_att_control/mc_att_control_main.cpp +++ b/src/modules/mc_att_control/mc_att_control_main.cpp @@ -263,8 +263,8 @@ MulticopterAttitudeControl::Run() } // Guard against too small (< 0.2ms) and too large (> 20ms) dt's. - const float dt = math::constrain(((v_att.timestamp - _last_run) * 1e-6f), 0.0002f, 0.02f); - _last_run = v_att.timestamp; + const float dt = math::constrain(((v_att.timestamp_sample - _last_run) * 1e-6f), 0.0002f, 0.02f); + _last_run = v_att.timestamp_sample; /* check for updates in other topics */ _manual_control_setpoint_sub.update(&_manual_control_setpoint); diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 5b3352c831..7f754e86bc 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -291,7 +291,7 @@ void MulticopterPositionControl::Run() vehicle_local_position_s local_pos; if (_local_pos_sub.update(&local_pos)) { - const hrt_abstime time_stamp_now = local_pos.timestamp; + const hrt_abstime time_stamp_now = local_pos.timestamp_sample; const float dt = math::constrain(((time_stamp_now - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f); _time_stamp_last_loop = time_stamp_now;