flight/modules/PathFollower/vtolflycontroller.cpp

   1 /*
   2  ******************************************************************************
   3  *
   4  * @file       vtolflycontroller.cpp
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
   6  * @brief      Class implements the fly controller for vtols
   7  * @see        The GNU Public License (GPL) Version 3
   8  *
   9  *****************************************************************************/
  10 /*
  11  * This program is free software; you can redistribute it and/or modify
  12  * it under the terms of the GNU General Public License as published by
  13  * the Free Software Foundation; either version 3 of the License, or
  14  * (at your option) any later version.
  15  *
  16  * This program is distributed in the hope that it will be useful, but
  17  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  18  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  19  * for more details.
  20  *
  21  * You should have received a copy of the GNU General Public License along
  22  * with this program; if not, write to the Free Software Foundation, Inc.,
  23  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  24  */
  25
  26 extern "C" {
  27 #include <openpilot.h>
  28
  29 #include <math.h>
  30 #include <pid.h>
  31 #include <CoordinateConversions.h>
  32 #include <sin_lookup.h>
  33 #include <pathdesired.h>
  34 #include <paths.h>
  35 #include "plans.h"
  36 #include <sanitycheck.h>
  37
  38 #include <accelstate.h>
  39 #include <vtolpathfollowersettings.h>
  40 #include <flightstatus.h>
  41 #include <flightmodesettings.h>
  42 #include <pathstatus.h>
  43 #include <positionstate.h>
  44 #include <velocitystate.h>
  45 #include <velocitydesired.h>
  46 #include <stabilizationdesired.h>
  47 #include <attitudestate.h>
  48 #include <takeofflocation.h>
  49 #include <poilocation.h>
  50 #include <manualcontrolcommand.h>
  51 #include <systemsettings.h>
  52 #include <stabilizationbank.h>
  53 #include <stabilizationdesired.h>
  54 #include <vtolselftuningstats.h>
  55 #include <pathsummary.h>
  56 }
  57
  58 // C++ includes
  59 #include "vtolflycontroller.h"
  60 #include "pathfollowerfsm.h"
  61 #include "pidcontroldown.h"
  62 #include "pidcontrolne.h"
  63
  64 // Private constants
  65 #define DEADBAND_HIGH                                        0.10f
  66 #define DEADBAND_LOW                                         -0.10f
  67 #define RTB_LAND_FRACTIONAL_PROGRESS_START_CHECKS            0.95f
  68 #define RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE 2.0f
  69
  70 // pointer to a singleton instance
  71 VtolFlyController *VtolFlyController::p_inst = 0;
  72
  73 VtolFlyController::VtolFlyController()
  74     : vtolPathFollowerSettings(NULL), mActive(false), mManualThrust(false), mMode(0), vtolEmergencyFallback(0.0f), vtolEmergencyFallbackSwitch(false)
  75 {}
  76
  77 // Called when mode first engaged
  78 void VtolFlyController::Activate(void)
  79 {
  80     if (!mActive) {
  81         mActive = true;
  82         mManualThrust = false;
  83         SettingsUpdated();
  84         controlDown.Activate();
  85         controlNE.UpdateVelocitySetpoint(0.0f, 0.0f);
  86         controlNE.Activate();
  87         mMode = pathDesired->Mode;
  88
  89         vtolEmergencyFallback = 0.0f;
  90         vtolEmergencyFallbackSwitch = false;
  91     }
  92 }
  93
  94 uint8_t VtolFlyController::IsActive(void)
  95 {
  96     return mActive;
  97 }
  98
  99 uint8_t VtolFlyController::Mode(void)
 100 {
 101     return mMode;
 102 }
 103
 104 // Objective updated in pathdesired
 105 void VtolFlyController::ObjectiveUpdated(void)
 106 {}
 107
 108
 109 void VtolFlyController::Deactivate(void)
 110 {
 111     if (mActive) {
 112         mActive = false;
 113         mManualThrust = false;
 114         controlDown.Deactivate();
 115         controlNE.Deactivate();
 116         vtolEmergencyFallback = 0.0f;
 117         vtolEmergencyFallbackSwitch = false;
 118     }
 119 }
 120
 121
 122 void VtolFlyController::SettingsUpdated(void)
 123 {
 124     const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
 125
 126     controlNE.UpdateParameters(vtolPathFollowerSettings->HorizontalVelPID.Kp,
 127                                vtolPathFollowerSettings->HorizontalVelPID.Ki,
 128                                vtolPathFollowerSettings->HorizontalVelPID.Kd,
 129                                vtolPathFollowerSettings->HorizontalVelPID.Beta,
 130                                dT,
 131                                vtolPathFollowerSettings->HorizontalVelMax);
 132     controlNE.UpdatePositionalParameters(vtolPathFollowerSettings->HorizontalPosP);
 133     controlNE.UpdateCommandParameters(-vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->MaxRollPitch, vtolPathFollowerSettings->VelocityFeedforward);
 134
 135     controlDown.UpdateParameters(vtolPathFollowerSettings->VerticalVelPID.Kp,
 136                                  vtolPathFollowerSettings->VerticalVelPID.Ki,
 137                                  vtolPathFollowerSettings->VerticalVelPID.Kd,
 138                                  vtolPathFollowerSettings->VerticalVelPID.Beta,
 139                                  dT,
 140                                  vtolPathFollowerSettings->VerticalVelMax);
 141     controlDown.UpdatePositionalParameters(vtolPathFollowerSettings->VerticalPosP);
 142
 143     VtolSelfTuningStatsData vtolSelfTuningStats;
 144     VtolSelfTuningStatsGet(&vtolSelfTuningStats);
 145     controlDown.UpdateNeutralThrust(vtolSelfTuningStats.NeutralThrustOffset + vtolPathFollowerSettings->ThrustLimits.Neutral);
 146     controlDown.SetThrustLimits(vtolPathFollowerSettings->ThrustLimits.Min, vtolPathFollowerSettings->ThrustLimits.Max);
 147
 148     // disable neutral thrust calcs which should only be done in a hold mode.
 149     controlDown.DisableNeutralThrustCalc();
 150 }
 151
 152 /**
 153  * Initialise the module, called on startup
 154  * \returns 0 on success or -1 if initialisation failed
 155  */
 156 int32_t VtolFlyController::Initialize(VtolPathFollowerSettingsData *ptr_vtolPathFollowerSettings)
 157 {
 158     PIOS_Assert(ptr_vtolPathFollowerSettings);
 159
 160     vtolPathFollowerSettings = ptr_vtolPathFollowerSettings;
 161
 162     return 0;
 163 }
 164
 165
 166 /**
 167  * Compute desired velocity from the current position and path
 168  */
 169 void VtolFlyController::UpdateVelocityDesired()
 170 {
 171     PositionStateData positionState;
 172
 173     PositionStateGet(&positionState);
 174
 175     VelocityStateData velocityState;
 176     VelocityStateGet(&velocityState);
 177
 178     VelocityDesiredData velocityDesired;
 179
 180     // look ahead kFF seconds
 181     float cur[3] = { positionState.North + (velocityState.North * vtolPathFollowerSettings->CourseFeedForward),
 182                      positionState.East + (velocityState.East * vtolPathFollowerSettings->CourseFeedForward),
 183                      positionState.Down + (velocityState.Down * vtolPathFollowerSettings->CourseFeedForward) };
 184     struct path_status progress;
 185     path_progress(pathDesired, cur, &progress, true);
 186
 187     controlNE.ControlPositionWithPath(&progress);
 188     if (!mManualThrust) {
 189         controlDown.ControlPositionWithPath(&progress);
 190     }
 191
 192     controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
 193     controlDown.UpdateVelocityState(velocityState.Down);
 194
 195     float north, east;
 196     controlNE.GetVelocityDesired(&north, &east);
 197     velocityDesired.North = north;
 198     velocityDesired.East  = east;
 199     if (!mManualThrust) {
 200         velocityDesired.Down = controlDown.GetVelocityDesired();
 201     } else { velocityDesired.Down = 0.0f; }
 202
 203     // update pathstatus
 204     pathStatus->error = progress.error;
 205     pathStatus->fractional_progress  = progress.fractional_progress;
 206     pathStatus->path_direction_north = progress.path_vector[0];
 207     pathStatus->path_direction_east  = progress.path_vector[1];
 208     pathStatus->path_direction_down  = progress.path_vector[2];
 209
 210     pathStatus->correction_direction_north = progress.correction_vector[0];
 211     pathStatus->correction_direction_east  = progress.correction_vector[1];
 212     pathStatus->correction_direction_down  = progress.correction_vector[2];
 213
 214     VelocityDesiredSet(&velocityDesired);
 215 }
 216
 217
 218 int8_t VtolFlyController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction)
 219 {
 220     uint8_t result = 1;
 221     StabilizationDesiredData stabDesired;
 222     AttitudeStateData attitudeState;
 223     StabilizationBankData stabSettings;
 224     float northCommand;
 225     float eastCommand;
 226
 227     StabilizationDesiredGet(&stabDesired);
 228     AttitudeStateGet(&attitudeState);
 229     StabilizationBankGet(&stabSettings);
 230
 231     controlNE.GetNECommand(&northCommand, &eastCommand);
 232
 233     float angle_radians = DEG2RAD(attitudeState.Yaw);
 234     float cos_angle     = cosf(angle_radians);
 235     float sine_angle    = sinf(angle_radians);
 236     float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
 237     stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 238     stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch); // this should be in the controller
 239     stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 240     stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
 241
 242     ManualControlCommandData manualControl;
 243     ManualControlCommandGet(&manualControl);
 244
 245     // TODO The below need to be rewritten because the PID implementation has changed.
 246 #if 0
 247     // DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in
 248     if (vtolPathFollowerSettings->FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST) {
 249         attitudeState.Yaw += 120.0f;
 250         if (attitudeState.Yaw > 180.0f) {
 251             attitudeState.Yaw -= 360.0f;
 252         }
 253     }
 254
 255
 256     if ( // emergency flyaway detection
 257         ( // integral already at its limit
 258             vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f ||
 259             vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[1].iAccumulator) < 1e-6f
 260         ) &&
 261         // angle between desired and actual velocity >90 degrees (by dot product)
 262         (velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) &&
 263         // quad is moving at significant speed (during flyaway it would keep speeding up)
 264         squaref(velocityState.North) + squaref(velocityState.East) > 1.0f
 265         ) {
 266         vtolEmergencyFallback += dT;
 267         if (vtolEmergencyFallback >= vtolPathFollowerSettings->FlyawayEmergencyFallbackTriggerTime) {
 268             // after emergency timeout, trigger alarm - everything else is handled by callers
 269             // (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...)
 270             result = 0;
 271         }
 272     } else {
 273         vtolEmergencyFallback = 0.0f;
 274     }
 275 #endif // if 0
 276
 277     // Yaw Attitude will be disabled without velocity requested.
 278     // PositionHold, AutoTakeoff or AutoCruise still using manual Yaw.
 279     if (yaw_attitude && ((fabsf(pathDesired->StartingVelocity) > 0.0f) && (fabsf(pathDesired->EndingVelocity) > 0.0f))) {
 280         stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 281         stabDesired.Yaw = yaw_direction;
 282     } else {
 283         stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
 284         stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
 285     }
 286
 287     // default thrust mode to cruise control
 288     stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
 289
 290     if (mManualThrust) {
 291         stabDesired.Thrust = manualControl.Thrust;
 292     } else {
 293         stabDesired.Thrust = controlDown.GetDownCommand();
 294     }
 295
 296     StabilizationDesiredSet(&stabDesired);
 297
 298     return result;
 299 }
 300
 301 /**
 302  * Compute desired attitude for vtols - emergency fallback
 303  */
 304 void VtolFlyController::UpdateDesiredAttitudeEmergencyFallback()
 305 {
 306     VelocityDesiredData velocityDesired;
 307     VelocityStateData velocityState;
 308     StabilizationDesiredData stabDesired;
 309
 310     float courseError;
 311     float courseCommand;
 312
 313     VelocityStateGet(&velocityState);
 314     VelocityDesiredGet(&velocityDesired);
 315
 316     ManualControlCommandData manualControlData;
 317     ManualControlCommandGet(&manualControlData);
 318
 319     courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North));
 320
 321     if (courseError < -180.0f) {
 322         courseError += 360.0f;
 323     }
 324     if (courseError > 180.0f) {
 325         courseError -= 360.0f;
 326     }
 327
 328
 329     courseCommand   = (courseError * vtolPathFollowerSettings->EmergencyFallbackYawRate.kP);
 330     stabDesired.Yaw = boundf(courseCommand, -vtolPathFollowerSettings->EmergencyFallbackYawRate.Max, vtolPathFollowerSettings->EmergencyFallbackYawRate.Max);
 331
 332     controlDown.UpdateVelocitySetpoint(velocityDesired.Down);
 333     controlDown.UpdateVelocityState(velocityState.Down);
 334     stabDesired.Thrust = controlDown.GetDownCommand();
 335
 336
 337     stabDesired.Roll   = vtolPathFollowerSettings->EmergencyFallbackAttitude.Roll;
 338     stabDesired.Pitch  = vtolPathFollowerSettings->EmergencyFallbackAttitude.Pitch;
 339
 340     if (vtolPathFollowerSettings->ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
 341         stabDesired.Thrust = manualControlData.Thrust;
 342     }
 343
 344     stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 345     stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 346     stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
 347     stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
 348     StabilizationDesiredSet(&stabDesired);
 349 }
 350
 351
 352 void VtolFlyController::UpdateAutoPilot()
 353 {
 354     if (vtolPathFollowerSettings->ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
 355         mManualThrust = true;
 356     }
 357
 358     uint8_t result = RunAutoPilot();
 359
 360     if (result) {
 361         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
 362     } else {
 363         pathStatus->Status = PATHSTATUS_STATUS_CRITICAL;
 364         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
 365     }
 366
 367     PathStatusSet(pathStatus);
 368
 369     // If rtbl, detect arrival at the endpoint and then triggers a change
 370     // to the pathDesired to initiate a Landing sequence. This is the simpliest approach. plans.c
 371     // can't manage this.  And pathplanner whilst similar does not manage this as it is not a
 372     // waypoint traversal and is not aware of flight modes other than path plan.
 373     if (flightStatus->FlightMode == FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE) {
 374         if ((uint8_t)pathDesired->ModeParameters[PATHDESIRED_MODEPARAMETER_GOTOENDPOINT_NEXTCOMMAND] == FLIGHTMODESETTINGS_RETURNTOBASENEXTCOMMAND_LAND) {
 375             if (pathStatus->fractional_progress > RTB_LAND_FRACTIONAL_PROGRESS_START_CHECKS) {
 376                 if (fabsf(pathStatus->correction_direction_north) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE && fabsf(pathStatus->correction_direction_east) < RTB_LAND_NE_DISTANCE_REQUIRED_TO_START_LAND_SEQUENCE) {
 377                     plan_setup_land();
 378                 }
 379             }
 380         }
 381     }
 382 }
 383 /**
 384  * vtol autopilot
 385  * use hover capable algorithm with unlimeted movement calculation. if that fails (flyaway situation due to compass failure)
 386  * fall back to emergency fallback autopilot to keep minimum amount of flight control
 387  */
 388 uint8_t VtolFlyController::RunAutoPilot()
 389 {
 390     enum { RETURN_0 = 0, RETURN_1 = 1, RETURN_RESULT } returnmode;
 391     enum { FOLLOWER_REGULAR, FOLLOWER_FALLBACK } followermode;
 392     uint8_t result = 0;
 393
 394     // decide on behaviour based on settings and system state
 395     if (vtolEmergencyFallbackSwitch) {
 396         returnmode   = RETURN_0;
 397         followermode = FOLLOWER_FALLBACK;
 398     } else {
 399         if (vtolPathFollowerSettings->FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) {
 400             returnmode   = RETURN_1;
 401             followermode = FOLLOWER_FALLBACK;
 402         } else {
 403             returnmode   = RETURN_RESULT;
 404             followermode = FOLLOWER_REGULAR;
 405         }
 406     }
 407
 408     switch (followermode) {
 409     case FOLLOWER_REGULAR:
 410     {
 411         // horizontal position control PID loop works according to settings in regular mode, allowing integral terms
 412         UpdateVelocityDesired();
 413
 414         // yaw behaviour is configurable in vtolpathfollower, select yaw control algorithm
 415         bool yaw_attitude = true;
 416         float yaw = 0.0f;
 417
 418         switch (vtolPathFollowerSettings->YawControl) {
 419         case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL:
 420             yaw_attitude = false;
 421             break;
 422         case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN:
 423             yaw = updateTailInBearing();
 424             break;
 425         case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION:
 426             yaw = updateCourseBearing();
 427             break;
 428         case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION:
 429             yaw = updatePathBearing();
 430             break;
 431         case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI:
 432             yaw = updatePOIBearing();
 433             break;
 434         }
 435
 436         result = UpdateStabilizationDesired(yaw_attitude, yaw);
 437
 438         if (!result) {
 439             if (vtolPathFollowerSettings->FlyawayEmergencyFallback != VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DISABLED) {
 440                 // switch to emergency follower if follower indicates problems
 441                 vtolEmergencyFallbackSwitch = true;
 442             }
 443         }
 444     }
 445     break;
 446     case FOLLOWER_FALLBACK:
 447     {
 448         // fallback loop only cares about intended horizontal flight direction, simplify control behaviour accordingly
 449         controlNE.UpdatePositionalParameters(1.0f);
 450         UpdateVelocityDesired();
 451
 452         // emergency follower has no return value
 453         UpdateDesiredAttitudeEmergencyFallback();
 454     }
 455     break;
 456     }
 457
 458     switch (returnmode) {
 459     case RETURN_RESULT:
 460         return result;
 461
 462     default:
 463         // returns either 0 or 1 according to enum definition above
 464         return returnmode;
 465     }
 466 }
 467
 468
 469 /**
 470  * Compute bearing of current takeoff location
 471  */
 472 float VtolFlyController::updateTailInBearing()
 473 {
 474     PositionStateData p;
 475
 476     PositionStateGet(&p);
 477     TakeOffLocationData t;
 478     TakeOffLocationGet(&t);
 479     // atan2f always returns in between + and - 180 degrees
 480     return RAD2DEG(atan2f(p.East - t.East, p.North - t.North));
 481 }
 482
 483
 484 /**
 485  * Compute bearing of current movement direction
 486  */
 487 float VtolFlyController::updateCourseBearing()
 488 {
 489     VelocityStateData v;
 490
 491     VelocityStateGet(&v);
 492     // atan2f always returns in between + and - 180 degrees
 493     return RAD2DEG(atan2f(v.East, v.North));
 494 }
 495
 496
 497 /**
 498  * Compute bearing of current path direction
 499  */
 500 float VtolFlyController::updatePathBearing()
 501 {
 502     PositionStateData positionState;
 503
 504     PositionStateGet(&positionState);
 505
 506     float cur[3] = { positionState.North,
 507                      positionState.East,
 508                      positionState.Down };
 509     struct path_status progress;
 510
 511     path_progress(pathDesired, cur, &progress, true);
 512
 513     // atan2f always returns in between + and - 180 degrees
 514     return RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0]));
 515 }
 516
 517
 518 /**
 519  * Compute bearing between current position and POI
 520  */
 521 float VtolFlyController::updatePOIBearing()
 522 {
 523     PoiLocationData poi;
 524
 525     PoiLocationGet(&poi);
 526     PositionStateData positionState;
 527     PositionStateGet(&positionState);
 528
 529     const float dT = vtolPathFollowerSettings->UpdatePeriod / 1000.0f;
 530     float dLoc[3];
 531     float yaw = 0;
 532     /*float elevation = 0;*/
 533
 534     dLoc[0] = positionState.North - poi.North;
 535     dLoc[1] = positionState.East - poi.East;
 536     dLoc[2] = positionState.Down - poi.Down;
 537
 538     if (dLoc[1] < 0) {
 539         yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
 540     } else {
 541         yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f;
 542     }
 543     ManualControlCommandData manualControlData;
 544     ManualControlCommandGet(&manualControlData);
 545
 546     float pathAngle = 0;
 547     if (manualControlData.Roll > DEADBAND_HIGH) {
 548         pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
 549     } else if (manualControlData.Roll < DEADBAND_LOW) {
 550         pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
 551     }
 552
 553     return yaw + (pathAngle / 2.0f);
 554 }