rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-18 08:54:15 +01:00
Code Issues Releases Activity

Scale the max velocity as a magnitude on teh vector instead of independently on

Browse Source 
each direction
This commit is contained in:
James Cotton 2012-03-15 04:25:37 -05:00
parent 5b3cc4876e
commit 6d1c36680f
1 changed files with 7 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

9
flight/Modules/Guidance/guidance.c
View File

@ -279,8 +279,13 @@ void updateVtolDesiredVelocity()
eastPosIntegral);
velocityDesired.North = bound(northCommand,-guidanceSettings.HorizontalVelMax,guidanceSettings.HorizontalVelMax);
velocityDesired.East = bound(eastCommand,-guidanceSettings.HorizontalVelMax,guidanceSettings.HorizontalVelMax);
float total_vel = sqrtf(powf(velocityDesired.North,2) + powf(velocityDesired.East,2));
float scale = 1;
if(total_vel > guidanceSettings.HorizontalVelMax)
scale = guidanceSettings.HorizontalVelMax / total_vel;
velocityDesired.North = northCommand * scale;
velocityDesired.East = eastCommand * scale;
downError = positionDesired.Down - positionActual.Down;
downPosIntegral = bound(downPosIntegral + downError * dT * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KI],