From 424fd8b3042bb7beb3d35aeab180059a86c75284 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Wed, 6 Apr 2022 16:23:31 +0200 Subject: [PATCH] MulticoperPositionControl: remove time_stamp_now alias for timestamp_sample of the local position to make it explicit what is used is not a fresh hrt_absolute_time() call by this module. --- .../MulticopterPositionControl.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/mc_pos_control/MulticopterPositionControl.cpp b/src/modules/mc_pos_control/MulticopterPositionControl.cpp index 95478a10c3..0ae2828567 100644 --- a/src/modules/mc_pos_control/MulticopterPositionControl.cpp +++ b/src/modules/mc_pos_control/MulticopterPositionControl.cpp @@ -316,9 +316,9 @@ void MulticopterPositionControl::Run() vehicle_local_position_s vehicle_local_position; if (_local_pos_sub.update(&vehicle_local_position)) { - const hrt_abstime time_stamp_now = vehicle_local_position.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; + const float dt = + math::constrain(((vehicle_local_position.timestamp_sample - _time_stamp_last_loop) * 1e-6f), 0.002f, 0.04f); + _time_stamp_last_loop = vehicle_local_position.timestamp_sample; // set _dt in controllib Block for BlockDerivative setDt(dt); @@ -400,9 +400,9 @@ void MulticopterPositionControl::Run() // set failsafe setpoint if there hasn't been a new // trajectory setpoint since position control started if ((_setpoint.timestamp < _time_position_control_enabled) - && (time_stamp_now > _time_position_control_enabled)) { + && (vehicle_local_position.timestamp_sample > _time_position_control_enabled)) { - failsafe(time_stamp_now, _setpoint, states); + failsafe(vehicle_local_position.timestamp_sample, _setpoint, states); _setpoint.timestamp = vehicle_local_position.timestamp; } } @@ -422,7 +422,7 @@ void MulticopterPositionControl::Run() if (_vehicle_control_mode.flag_control_offboard_enabled) { bool want_takeoff = _vehicle_control_mode.flag_armed && _vehicle_land_detected.landed - && (time_stamp_now < _setpoint.timestamp + 1_s); + && (vehicle_local_position.timestamp_sample < _setpoint.timestamp + 1_s); if (want_takeoff && PX4_ISFINITE(_setpoint.z) && (_setpoint.z < states.position(2))) { @@ -451,7 +451,7 @@ void MulticopterPositionControl::Run() // handle smooth takeoff _takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.landed, _vehicle_constraints.want_takeoff, - _vehicle_constraints.speed_up, false, time_stamp_now); + _vehicle_constraints.speed_up, false, vehicle_local_position.timestamp_sample); const bool flying = (_takeoff.getTakeoffState() >= TakeoffState::flight); @@ -515,13 +515,13 @@ void MulticopterPositionControl::Run() // Run position control if (_control.update(dt)) { - _failsafe_land_hysteresis.set_state_and_update(false, time_stamp_now); + _failsafe_land_hysteresis.set_state_and_update(false, vehicle_local_position.timestamp_sample); } else { // Failsafe vehicle_local_position_setpoint_s failsafe_setpoint{}; - failsafe(time_stamp_now, failsafe_setpoint, states); + failsafe(vehicle_local_position.timestamp_sample, failsafe_setpoint, states); // reset constraints _vehicle_constraints = {0, NAN, NAN, false, {}}; @@ -548,7 +548,7 @@ void MulticopterPositionControl::Run() } else { // an update is necessary here because otherwise the takeoff state doesn't get skiped with non-altitude-controlled modes _takeoff.updateTakeoffState(_vehicle_control_mode.flag_armed, _vehicle_land_detected.landed, false, 10.f, true, - time_stamp_now); + vehicle_local_position.timestamp_sample); } // Publish takeoff status