rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-01 18:29:16 +01:00
Code Issues Releases Activity

OP-1156 added vtolpathfollower velocity limits to attitude control

Browse Source
This commit is contained in:
Corvus Corax 2014-08-15 19:23:41 +02:00
parent b562fcb02e
commit b1675a2804
1 changed files with 10 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

10
flight/modules/PathFollower/pathfollower.c
View File

@ -1044,6 +1044,16 @@ static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction)
ManualControlCommandData manualControlData; ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData); ManualControlCommandGet(&manualControlData);
// scale velocity if it is above configured maximum
float velH = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
if (velH > vtolPathFollowerSettings.HorizontalVelMax) {
velocityDesired.North *= vtolPathFollowerSettings.HorizontalVelMax / velH;
velocityDesired.East *= vtolPathFollowerSettings.HorizontalVelMax / velH;
}
if (fabsf(velocityDesired.Down) > vtolPathFollowerSettings.VerticalVelMax) {
velocityDesired.Down *= vtolPathFollowerSettings.VerticalVelMax / fabsf(velocityDesired.Down);
}
// Compute desired north command // Compute desired north command
northError = velocityDesired.North - velocityState.North; northError = velocityDesired.North - velocityState.North;
i.vel[0] = boundf(i.vel[0] + northError * dT * vtolPathFollowerSettings.HorizontalVelPI.Ki, i.vel[0] = boundf(i.vel[0] + northError * dT * vtolPathFollowerSettings.HorizontalVelPI.Ki,