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

Takeoff: switch internally to use a ramp progress

Browse Source
This commit is contained in:
Matthias Grob
2021-03-31 10:03:08 +02:00
parent 8ca76feaba
commit c16b937221
2 changed files with 10 additions and 10 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/Takeoff/Takeoff.cpp
+6 -6
View File
@@ -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);
}
}
src/modules/mc_pos_control/Takeoff/Takeoff.hpp
+4 -4
View File
@@ -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
};