From 14f734ac15389dcf04c63255ea6d7143f87a4cf7 Mon Sep 17 00:00:00 2001 From: Daniel Agar Date: Fri, 7 Aug 2020 21:44:42 -0400 Subject: [PATCH] mc_pos_control: use local position timestamp for dt --- .../mc_pos_control/Takeoff/Takeoff.hpp | 2 +- .../mc_pos_control/mc_pos_control_main.cpp | 51 ++++++++----------- 2 files changed, 21 insertions(+), 32 deletions(-) diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp index 6b2cc6b94f..5253164c88 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp @@ -59,7 +59,7 @@ public: ~Takeoff() = default; // initialize parameters - void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, (hrt_abstime)(seconds * (float)1_s)); } + void setSpoolupTime(const float seconds) { _spoolup_time_hysteresis.set_hysteresis_time_from(false, seconds * 1_s); } void setTakeoffRampTime(const float seconds) { _takeoff_ramp_time = seconds; } /** diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 481a1dc6d3..3fc3cbb5bb 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -234,12 +234,6 @@ private: */ void limit_altitude(vehicle_local_position_setpoint_s &setpoint); - /** - * Prints a warning message at a lowered rate. - * @param str the message that has to be printed. - */ - void warn_rate_limited(const char *str); - /** * Adjust the setpoint during landing. * Thrust is adjusted to support the land-detector during detection. @@ -260,8 +254,8 @@ private: * setpoints. The failsafe will occur after LOITER_TIME_BEFORE_DESCEND. If force is set * to true, the failsafe will be initiated immediately. */ - void failsafe(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, const bool force, - bool warn); + void failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, + const bool force, bool warn); /** * Reset setpoints to NAN @@ -324,17 +318,6 @@ MulticopterPositionControl::init() return true; } -void -MulticopterPositionControl::warn_rate_limited(const char *string) -{ - hrt_abstime now = hrt_absolute_time(); - - if (now - _last_warn > 200_ms) { - PX4_WARN("%s", string); - _last_warn = now; - } -} - int MulticopterPositionControl::parameters_update(bool force) { @@ -542,11 +525,13 @@ MulticopterPositionControl::Run() poll_subscriptions(); parameters_update(false); - // set _dt in controllib Block - the time difference since the last loop iteration in seconds - const hrt_abstime time_stamp_now = hrt_absolute_time(); - setDt((time_stamp_now - _time_stamp_last_loop) / 1e6f); + const hrt_abstime time_stamp_now = _local_pos.timestamp; + 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; + // set _dt in controllib Block - the time difference since the last loop iteration in seconds + setDt(dt); + const bool was_in_failsafe = _in_failsafe; _in_failsafe = false; @@ -587,7 +572,7 @@ MulticopterPositionControl::Run() if (!_flight_tasks.update()) { // FAILSAFE // Task was not able to update correctly. Do Failsafe. - failsafe(setpoint, _states, false, !was_in_failsafe); + failsafe(time_stamp_now, setpoint, _states, false, !was_in_failsafe); } else { setpoint = _flight_tasks.getPositionSetpoint(); @@ -624,7 +609,7 @@ MulticopterPositionControl::Run() // handle smooth takeoff _takeoff.updateTakeoffState(_control_mode.flag_armed, _vehicle_land_detected.landed, constraints.want_takeoff, constraints.speed_up, !_control_mode.flag_control_climb_rate_enabled, time_stamp_now); - constraints.speed_up = _takeoff.updateRamp(_dt, constraints.speed_up); + constraints.speed_up = _takeoff.updateRamp(dt, constraints.speed_up); const bool not_taken_off = _takeoff.getTakeoffState() < TakeoffState::rampup; const bool flying = _takeoff.getTakeoffState() >= TakeoffState::flight; @@ -660,12 +645,16 @@ MulticopterPositionControl::Run() _control.setConstraints(constraints); _control.setInputSetpoint(setpoint); - if (!_control.update(_dt)) { - warn_rate_limited("PositionControl: invalid setpoints"); - failsafe(setpoint, _states, true, !was_in_failsafe); + if (!_control.update(dt)) { + if ((time_stamp_now - _last_warn) > 1_s) { + PX4_WARN("invalid setpoints"); + _last_warn = time_stamp_now; + } + + failsafe(time_stamp_now, setpoint, _states, true, !was_in_failsafe); _control.setInputSetpoint(setpoint); constraints = FlightTask::empty_constraints; - _control.update(_dt); + _control.update(dt); } // Fill local position, velocity and thrust setpoint. @@ -931,15 +920,15 @@ MulticopterPositionControl::start_flight_task() } void -MulticopterPositionControl::failsafe(vehicle_local_position_setpoint_s &setpoint, const PositionControlStates &states, - const bool force, bool warn) +MulticopterPositionControl::failsafe(const hrt_abstime &now, vehicle_local_position_setpoint_s &setpoint, + const PositionControlStates &states, const bool force, bool warn) { // do not warn while we are disarmed, as we might not have valid setpoints yet if (!_control_mode.flag_armed) { warn = false; } - _failsafe_land_hysteresis.set_state_and_update(true, hrt_absolute_time()); + _failsafe_land_hysteresis.set_state_and_update(true, now); if (!_failsafe_land_hysteresis.get_state() && !force) { // just keep current setpoint and don't do anything.