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

FlightTaskAuto: abstract class for mapping triplets to quadruple

Browse Source 
FlightTaskAuto: add type that corresponds to triplet type

FligthTaskAuto: set all setpoints if invalid in xy

FlightTaskAuto: cast triplet type to WaypointType

FlightTaskAutoLine: class for px4 legacy auto

FlightTaskAutoLine: methods prototype

FlightTaskAuto: change sp to wp (=Waypoint)
add params

FlightTaskAutoLine: follow waypoints along line based on flight state
This commit is contained in:
Dennis Mannhart
2018-01-12 17:27:36 +01:00
committed by Lorenz Meier
parent 89a902524a
commit 6e62beb560
11 changed files with 963 additions and 17 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_pos_control/PositionControl.cpp
+54 -4
View File
@@ -108,7 +108,23 @@ 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.
}
}
}
@@ -119,6 +135,13 @@ 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++) {
@@ -176,11 +199,11 @@ void PositionControl::_positionController()
_vel_sp = (_pos_sp - _pos).emult(Pp) + _vel_sp;
/* Make sure velocity setpoint is constrained in all directions (xyz). */
float vel_norm_xy = sqrtf(_vel_sp(0) * _vel_sp(0) + _vel_sp(1) * _vel_sp(1));
Vector2f vel_sp_xy(&_vel_sp(0));
if (vel_norm_xy > _VelMaxXY) {
_vel_sp(0) = _vel_sp(0) * _VelMaxXY / vel_norm_xy;
_vel_sp(1) = _vel_sp(1) * _VelMaxXY / vel_norm_xy;
if (vel_sp_xy.length() >= _VelMaxXY) {
_vel_sp(0) = _vel_sp(0) * _VelMaxXY / vel_sp_xy.length();
_vel_sp(1) = _vel_sp(1) * _VelMaxXY / vel_sp_xy.length();
}
/* Saturate velocity in D-direction */
@@ -311,6 +334,33 @@ 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));