From c16b93722106be19e6398e598831e83798bfcc22 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Thu, 4 Mar 2021 11:27:54 +0100 Subject: [PATCH] Takeoff: switch internally to use a ramp progress --- src/modules/mc_pos_control/Takeoff/Takeoff.cpp | 12 ++++++------ src/modules/mc_pos_control/Takeoff/Takeoff.hpp | 8 ++++---- 2 files changed, 10 insertions(+), 10 deletions(-) diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp index 68ee76a48b..3717106e17 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.cpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.cpp @@ -72,7 +72,7 @@ void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool case TakeoffState::ready_for_takeoff: if (want_takeoff) { _takeoff_state = TakeoffState::rampup; - _takeoff_ramp_vz = _takeoff_ramp_vz_init; + _takeoff_ramp_progress = 0.f; } else { break; @@ -80,7 +80,7 @@ void Takeoff::updateTakeoffState(const bool armed, const bool landed, const bool // FALLTHROUGH case TakeoffState::rampup: - if (_takeoff_ramp_vz >= takeoff_desired_vz) { + if (_takeoff_ramp_progress >= 1.f) { _takeoff_state = TakeoffState::flight; } else { @@ -119,14 +119,14 @@ float Takeoff::updateRamp(const float dt, const float takeoff_desired_vz) if (_takeoff_state == TakeoffState::rampup) { if (_takeoff_ramp_time > dt) { - _takeoff_ramp_vz += (takeoff_desired_vz - _takeoff_ramp_vz_init) * dt / _takeoff_ramp_time; + _takeoff_ramp_progress += dt / _takeoff_ramp_time; } else { - _takeoff_ramp_vz = takeoff_desired_vz; + _takeoff_ramp_progress = 1.f; } - if (_takeoff_ramp_vz < takeoff_desired_vz) { - upwards_velocity_limit = _takeoff_ramp_vz; + if (_takeoff_ramp_progress < 1.f) { + upwards_velocity_limit = _takeoff_ramp_vz_init + _takeoff_ramp_progress * (takeoff_desired_vz - _takeoff_ramp_vz_init); } } diff --git a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp index d396d19859..ab2a39ffc9 100644 --- a/src/modules/mc_pos_control/Takeoff/Takeoff.hpp +++ b/src/modules/mc_pos_control/Takeoff/Takeoff.hpp @@ -94,9 +94,9 @@ public: private: TakeoffState _takeoff_state = TakeoffState::disarmed; - systemlib::Hysteresis _spoolup_time_hysteresis{false}; /**< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed */ + systemlib::Hysteresis _spoolup_time_hysteresis{false}; ///< becomes true MPC_SPOOLUP_TIME seconds after the vehicle was armed - float _takeoff_ramp_time = 0.f; - float _takeoff_ramp_vz_init = 0.f; - float _takeoff_ramp_vz = 0.f; + float _takeoff_ramp_time{0.f}; + float _takeoff_ramp_vz_init{0.f}; ///< verticval velocity resulting in zero thrust + float _takeoff_ramp_progress{0.f}; ///< asecnding from 0 to 1 };