From 6d1c36680fdf37e494fa8bd1446e43e7bad04b3e Mon Sep 17 00:00:00 2001 From: James Cotton Date: Thu, 15 Mar 2012 04:25:37 -0500 Subject: [PATCH] Scale the max velocity as a magnitude on teh vector instead of independently on each direction --- flight/Modules/Guidance/guidance.c | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/flight/Modules/Guidance/guidance.c b/flight/Modules/Guidance/guidance.c index da4cb7268..8cae6a290 100644 --- a/flight/Modules/Guidance/guidance.c +++ b/flight/Modules/Guidance/guidance.c @@ -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],