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

PositionControl: auto takeoff with constant speed

Browse Source 
PositionControl: remove takeoff logic

mc_pos_control: takeoff logic for flighttask

mc_pos_control: smooth takeoff throttle start at 0
This commit is contained in:
Dennis Mannhart
2018-01-23 18:24:43 +01:00
committed by Lorenz Meier
parent 6e62beb560
commit 558a0f892c
2 changed files with 1 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl.cpp
-50
View File
@@ -108,23 +108,8 @@ void PositionControl::generateThrustYawSetpoint(const float &dt)
*/
if (!_skipController) {
_positionController();
if (_smooth_takeoff) {
_setTakeoffVelocity(dt);
} else {
_takeoff_vel_sp = 0.5f; // Reset to pointing downwards.
}
_velocityController(dt);
} else {
if (_smooth_takeoff) {
_setTakeoffThrust(dt);
} else {
_takeoff_vel_sp = 0.5f; // Reset to pointing downwards.
}
}
}
@@ -135,13 +120,6 @@ void PositionControl::_interfaceMapping()
* do not require control.
*/
if (PX4_ISFINITE(_pos_sp(2)) && _smooth_takeoff) {
_takeoff_max_speed = 0.6f;
} else {
_takeoff_max_speed = _VelMaxZ[0];
}
/* Loop through x,y and z components of all setpoints. */
for (int i = 0; i <= 2; i++) {
@@ -314,7 +292,6 @@ void PositionControl::_velocityController(const float &dt)
}
}
}
void PositionControl::updateConstraints(const Controller::Constraints &constraints)
@@ -334,33 +311,6 @@ void PositionControl::_updateParams()
}
}
void PositionControl::_setTakeoffVelocity(const float dt)
{
/* Limit velocity setpoint to maximum takeoff velocity */
_vel_sp(2) = math::max(_vel_sp(2), -_takeoff_max_speed);
/* Smooth takeoff is achieved once target velocity is reached. (NED frame). */
_smooth_takeoff = _takeoff_vel_sp >= _vel_sp(2);
/* ramp vertical velocity limit up to takeoff speed */
_takeoff_vel_sp += _vel_sp(2) * dt / _TakeoffRampTime;
/* limit vertical velocity to the current ramp value */
_vel_sp(2) = math::max(_vel_sp(2), _takeoff_vel_sp);
}
void PositionControl::_setTakeoffThrust(const float dt)
{
/* Smooth takeoff is achieved once target velocity is reached. (NED frame). */
_smooth_takeoff = _takeoff_vel_sp >= _thr_sp(2);
/* ramp vertical velocity limit up to takeoff speed */
_takeoff_vel_sp += _thr_sp(2) * dt / _TakeoffRampTime;
/* limit vertical velocity to the current ramp value */
_thr_sp(2) = math::max(_thr_sp(2), _takeoff_vel_sp);
}
void PositionControl::_setParams()
{
param_get(_Pxy_h, &Pp(0));
src/modules/mc_pos_control/PositionControl.hpp
+1 -6
View File
@@ -64,8 +64,7 @@ public:
~PositionControl() {};
void updateState(const vehicle_local_position_s &state, const matrix::Vector3f &vel_dot,
const bool smooth_takeoff);
void updateState(const vehicle_local_position_s &state, const matrix::Vector3f &vel_dot);
void updateSetpoint(const vehicle_local_position_setpoint_s &setpoint);
void updateConstraints(const Controller::Constraints &constraints);
void generateThrustYawSetpoint(const float &dt);
@@ -75,7 +74,6 @@ public:
float getYawspeedSetpoint() {return _yawspeed_sp;}
matrix::Vector3f getVelSp() {return _vel_sp;}
matrix::Vector3f getPosSp() {return _pos_sp;}
bool getSmoothTakeoff() {return _smooth_takeoff;};
private:
@@ -93,9 +91,6 @@ private:
matrix::Vector3f _thr_sp{};
float _yaw_sp{};
float _yawspeed_sp{};
float _takeoff_vel_sp = 0.5f; // starts positive (NED frame)
float _takeoff_max_speed = 1.0f;
bool _smooth_takeoff{false};
/* Other variables */
matrix::Vector3f _thr_int{};