flight/modules/PathFollower/pathfollower.c

   1 /**
   2  ******************************************************************************
   3  *
   4  * @file       pathfollower.c
   5  * @author     The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
   6  * @brief      This module compared @ref PositionActuatl to @ref ActiveWaypoint
   7  * and sets @ref AttitudeDesired.  It only does this when the FlightMode field
   8  * of @ref ManualControlCommand is Auto.
   9  *
  10  * @see        The GNU Public License (GPL) Version 3
  11  *
  12  *****************************************************************************/
  13 /*
  14  * This program is free software; you can redistribute it and/or modify
  15  * it under the terms of the GNU General Public License as published by
  16  * the Free Software Foundation; either version 3 of the License, or
  17  * (at your option) any later version.
  18  *
  19  * This program is distributed in the hope that it will be useful, but
  20  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  21  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  22  * for more details.
  23  *
  24  * You should have received a copy of the GNU General Public License along
  25  * with this program; if not, write to the Free Software Foundation, Inc.,
  26  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  27  */
  28
  29 /**
  30  * Input object: PathDesired
  31  * Input object: PositionState
  32  * Input object: ManualControlCommand
  33  * Output object: StabilizationDesired
  34  *
  35  * This module acts as "autopilot" - it controls the setpoints of stabilization
  36  * based on current flight situation and desired flight path (PathDesired) as
  37  * directed by flightmode selection or pathplanner
  38  * This is a periodic delayed callback module
  39  *
  40  * Modules have no API, all communication to other modules is done through UAVObjects.
  41  * However modules may use the API exposed by shared libraries.
  42  * See the OpenPilot wiki for more details.
  43  * http://www.openpilot.org/OpenPilot_Application_Architecture
  44  *
  45  */
  46
  47 #include <openpilot.h>
  48
  49 #include <callbackinfo.h>
  50
  51 #include <math.h>
  52 #include <pid.h>
  53 #include <CoordinateConversions.h>
  54 #include <pios_struct_helper.h>
  55 #include <sin_lookup.h>
  56 #include <pathdesired.h>
  57 #include <paths.h>
  58 #include <sanitycheck.h>
  59
  60
  61 #include <fixedwingpathfollowersettings.h>
  62 #include <fixedwingpathfollowerstatus.h>
  63 #include <vtolpathfollowersettings.h>
  64 #include <flightstatus.h>
  65 #include <pathstatus.h>
  66 #include <positionstate.h>
  67 #include <velocitystate.h>
  68 #include <velocitydesired.h>
  69 #include <stabilizationdesired.h>
  70 #include <airspeedstate.h>
  71 #include <attitudestate.h>
  72 #include <takeofflocation.h>
  73 #include <poilocation.h>
  74 #include <manualcontrolcommand.h>
  75 #include <systemsettings.h>
  76 #include <stabilizationbank.h>
  77
  78
  79 // Private constants
  80
  81 #define CALLBACK_PRIORITY      CALLBACK_PRIORITY_LOW
  82 #define CBTASK_PRIORITY        CALLBACK_TASK_FLIGHTCONTROL
  83
  84 #define PF_IDLE_UPDATE_RATE_MS 100
  85
  86 #define STACK_SIZE_BYTES       2048
  87
  88 #define DEADBAND_HIGH          0.10f
  89 #define DEADBAND_LOW           -0.10f
  90 // Private types
  91
  92 struct Globals {
  93     struct pid PIDposH[2];
  94     struct pid PIDposV;
  95     struct pid PIDvel[3];
  96     struct pid PIDcourse;
  97     struct pid PIDspeed;
  98     struct pid PIDpower;
  99     float poiRadius;
 100     float vtolEmergencyFallback;
 101     bool  vtolEmergencyFallbackSwitch;
 102 };
 103
 104
 105 // Private variables
 106 static DelayedCallbackInfo *pathFollowerCBInfo;
 107 static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS;
 108 static struct Globals global;
 109 static PathStatusData pathStatus;
 110 static PathDesiredData pathDesired;
 111 static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings;
 112 static VtolPathFollowerSettingsData vtolPathFollowerSettings;
 113
 114 // correct speed by measured airspeed
 115 static float indicatedAirspeedStateBias = 0.0f;
 116
 117
 118 // Private functions
 119 static void pathFollowerTask(void);
 120 static void resetGlobals();
 121 static void SettingsUpdatedCb(UAVObjEvent *ev);
 122 static uint8_t updateAutoPilotByFrameType();
 123 static uint8_t updateAutoPilotFixedWing();
 124 static uint8_t updateAutoPilotVtol();
 125 static float updateTailInBearing();
 126 static float updateCourseBearing();
 127 static float updatePathBearing();
 128 static float updatePOIBearing();
 129 static void processPOI();
 130 static void updatePathVelocity(float kFF, bool limited);
 131 static uint8_t updateFixedDesiredAttitude();
 132 static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction);
 133 static void updateFixedAttitude();
 134 static void updateVtolDesiredAttitudeEmergencyFallback();
 135 static void airspeedStateUpdatedCb(UAVObjEvent *ev);
 136 static bool correctCourse(float *C, float *V, float *F, float s);
 137
 138 /**
 139  * Initialise the module, called on startup
 140  * \returns 0 on success or -1 if initialisation failed
 141  */
 142 int32_t PathFollowerStart()
 143 {
 144     // Start main task
 145     PathStatusGet(&pathStatus);
 146     SettingsUpdatedCb(NULL);
 147     PIOS_CALLBACKSCHEDULER_Dispatch(pathFollowerCBInfo);
 148
 149     return 0;
 150 }
 151
 152 /**
 153  * Initialise the module, called on startup
 154  * \returns 0 on success or -1 if initialisation failed
 155  */
 156 int32_t PathFollowerInitialize()
 157 {
 158     // initialize objects
 159     FixedWingPathFollowerSettingsInitialize();
 160     FixedWingPathFollowerStatusInitialize();
 161     VtolPathFollowerSettingsInitialize();
 162     FlightStatusInitialize();
 163     PathStatusInitialize();
 164     PathDesiredInitialize();
 165     PositionStateInitialize();
 166     VelocityStateInitialize();
 167     VelocityDesiredInitialize();
 168     StabilizationDesiredInitialize();
 169     AirspeedStateInitialize();
 170     AttitudeStateInitialize();
 171     TakeOffLocationInitialize();
 172     PoiLocationInitialize();
 173     ManualControlCommandInitialize();
 174     SystemSettingsInitialize();
 175     StabilizationBankInitialize();
 176
 177     // reset integrals
 178     resetGlobals();
 179
 180     // Create object queue
 181     pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_PATHFOLLOWER, STACK_SIZE_BYTES);
 182     FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
 183     VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
 184     PathDesiredConnectCallback(SettingsUpdatedCb);
 185     AirspeedStateConnectCallback(airspeedStateUpdatedCb);
 186
 187     return 0;
 188 }
 189 MODULE_INITCALL(PathFollowerInitialize, PathFollowerStart);
 190
 191
 192 /**
 193  * Module thread, should not return.
 194  */
 195 static void pathFollowerTask(void)
 196 {
 197     FlightStatusData flightStatus;
 198
 199     FlightStatusGet(&flightStatus);
 200
 201     if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
 202         resetGlobals();
 203         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
 204         PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
 205         return;
 206     }
 207
 208     if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) { // TODO Hack from vtolpathfollower, move that shit into manualcontrol!
 209         processPOI();
 210     }
 211
 212     pathStatus.UID    = pathDesired.UID;
 213     pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
 214     switch (pathDesired.Mode) {
 215     case PATHDESIRED_MODE_FLYENDPOINT:
 216     case PATHDESIRED_MODE_FLYVECTOR:
 217     case PATHDESIRED_MODE_FLYCIRCLERIGHT:
 218     case PATHDESIRED_MODE_FLYCIRCLELEFT:
 219     {
 220         uint8_t result = updateAutoPilotByFrameType();
 221         if (result) {
 222             AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
 223         } else {
 224             pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
 225             AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_WARNING);
 226         }
 227     }
 228     break;
 229     case PATHDESIRED_MODE_FIXEDATTITUDE:
 230         updateFixedAttitude(pathDesired.ModeParameters);
 231         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
 232         break;
 233     case PATHDESIRED_MODE_DISARMALARM:
 234         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
 235         break;
 236     default:
 237         pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
 238         AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
 239         break;
 240     }
 241     PathStatusSet(&pathStatus);
 242
 243     PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER);
 244 }
 245
 246
 247 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 248 {
 249     FixedWingPathFollowerSettingsGet(&fixedWingPathFollowerSettings);
 250
 251     pid_configure(&global.PIDcourse, fixedWingPathFollowerSettings.CoursePI.Kp, fixedWingPathFollowerSettings.CoursePI.Ki, 0.0f, fixedWingPathFollowerSettings.CoursePI.ILimit);
 252     pid_configure(&global.PIDspeed, fixedWingPathFollowerSettings.SpeedPI.Kp, fixedWingPathFollowerSettings.SpeedPI.Ki, 0.0f, fixedWingPathFollowerSettings.SpeedPI.ILimit);
 253     pid_configure(&global.PIDpower, fixedWingPathFollowerSettings.PowerPI.Kp, fixedWingPathFollowerSettings.PowerPI.Ki, 0.0f, fixedWingPathFollowerSettings.PowerPI.ILimit);
 254
 255
 256     VtolPathFollowerSettingsGet(&vtolPathFollowerSettings);
 257
 258     pid_configure(&global.PIDvel[0], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit);
 259     pid_configure(&global.PIDvel[1], vtolPathFollowerSettings.HorizontalVelPID.Kp, vtolPathFollowerSettings.HorizontalVelPID.Ki, vtolPathFollowerSettings.HorizontalVelPID.Kd, vtolPathFollowerSettings.HorizontalVelPID.ILimit);
 260     pid_configure(&global.PIDvel[2], vtolPathFollowerSettings.VerticalVelPID.Kp, vtolPathFollowerSettings.VerticalVelPID.Ki, vtolPathFollowerSettings.VerticalVelPID.Kd, vtolPathFollowerSettings.VerticalVelPID.ILimit);
 261     pid_configure(&global.PIDvel[2], vtolPathFollowerSettings.VerticalVelPID.Kp, vtolPathFollowerSettings.VerticalVelPID.Ki, vtolPathFollowerSettings.VerticalVelPID.Kd, vtolPathFollowerSettings.VerticalVelPID.ILimit);
 262
 263     PathDesiredGet(&pathDesired);
 264 }
 265
 266
 267 static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 268 {
 269     AirspeedStateData airspeedState;
 270     VelocityStateData velocityState;
 271
 272     AirspeedStateGet(&airspeedState);
 273     VelocityStateGet(&velocityState);
 274     float airspeedVector[2];
 275     float yaw;
 276     AttitudeStateYawGet(&yaw);
 277     airspeedVector[0] = cos_lookup_deg(yaw);
 278     airspeedVector[1] = sin_lookup_deg(yaw);
 279     // vector projection of groundspeed on airspeed vector to handle both forward and backwards movement
 280     float groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
 281
 282     indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeedProjection;
 283     // note - we do fly by Indicated Airspeed (== calibrated airspeed) however
 284     // since airspeed is updated less often than groundspeed, we use sudden
 285     // changes to groundspeed to offset the airspeed by the same measurement.
 286     // This has a side effect that in the absence of any airspeed updates, the
 287     // pathfollower will fly using groundspeed.
 288 }
 289
 290
 291 /**
 292  * reset integrals
 293  */
 294 static void resetGlobals()
 295 {
 296     pid_zero(&global.PIDposH[0]);
 297     pid_zero(&global.PIDposH[1]);
 298     pid_zero(&global.PIDposV);
 299     pid_zero(&global.PIDvel[0]);
 300     pid_zero(&global.PIDvel[1]);
 301     pid_zero(&global.PIDvel[2]);
 302     pid_zero(&global.PIDcourse);
 303     pid_zero(&global.PIDspeed);
 304     pid_zero(&global.PIDpower);
 305     global.poiRadius = 0.0f;
 306     global.vtolEmergencyFallback = 0;
 307     global.vtolEmergencyFallbackSwitch = false;
 308 }
 309
 310 static uint8_t updateAutoPilotByFrameType()
 311 {
 312     FrameType_t frameType = GetCurrentFrameType();
 313
 314     if (frameType == FRAME_TYPE_CUSTOM || frameType == FRAME_TYPE_GROUND) {
 315         switch (vtolPathFollowerSettings.TreatCustomCraftAs) {
 316         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
 317             frameType = FRAME_TYPE_FIXED_WING;
 318             break;
 319         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
 320             frameType = FRAME_TYPE_MULTIROTOR;
 321             break;
 322         }
 323     }
 324     switch (frameType) {
 325     case FRAME_TYPE_MULTIROTOR:
 326     case FRAME_TYPE_HELI:
 327         updatePeriod = vtolPathFollowerSettings.UpdatePeriod;
 328         return updateAutoPilotVtol();
 329
 330         break;
 331     case FRAME_TYPE_FIXED_WING:
 332     default:
 333         updatePeriod = fixedWingPathFollowerSettings.UpdatePeriod;
 334         return updateAutoPilotFixedWing();
 335
 336         break;
 337     }
 338 }
 339
 340 /**
 341  * fixed wing autopilot:
 342  * straight forward:
 343  * 1. update path velocity for limited motion crafts
 344  * 2. update attitude according to default fixed wing pathfollower algorithm
 345  */
 346 static uint8_t updateAutoPilotFixedWing()
 347 {
 348     pid_configure(&global.PIDposH[0], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
 349     pid_configure(&global.PIDposH[1], fixedWingPathFollowerSettings.HorizontalPosP, 0.0f, 0.0f, 0.0f);
 350     pid_configure(&global.PIDposV, fixedWingPathFollowerSettings.VerticalPosP, 0.0f, 0.0f, 0.0f);
 351     updatePathVelocity(fixedWingPathFollowerSettings.CourseFeedForward, true);
 352     return updateFixedDesiredAttitude();
 353 }
 354
 355 /**
 356  * vtol autopilot
 357  * use hover capable algorithm with unlimeted movement calculation. if that fails (flyaway situation due to compass failure)
 358  * fall back to emergency fallback autopilot to keep minimum amount of flight control
 359  */
 360 static uint8_t updateAutoPilotVtol()
 361 {
 362     if (!global.vtolEmergencyFallbackSwitch) {
 363         if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) {
 364             pid_configure(&global.PIDposH[0], 1.0f, 0.0f, 0.0f, 0.0f);
 365             pid_configure(&global.PIDposH[1], 1.0f, 0.0f, 0.0f, 0.0f);
 366             pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit);
 367             updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, true);
 368             updateVtolDesiredAttitudeEmergencyFallback();
 369             return 1;
 370         } else {
 371             pid_configure(&global.PIDposH[0], vtolPathFollowerSettings.HorizontalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit);
 372             pid_configure(&global.PIDposH[1], vtolPathFollowerSettings.HorizontalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit);
 373             pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit);
 374             updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, false);
 375             uint8_t result    = 1;
 376             bool yaw_attitude = true;
 377             float yaw = 0.0f;
 378             switch (vtolPathFollowerSettings.YawControl) {
 379             case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL:
 380                 yaw_attitude = false;
 381                 break;
 382             case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN:
 383                 yaw = updateTailInBearing();
 384                 break;
 385             case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MOVEMENTDIRECTION:
 386                 yaw = updateCourseBearing();
 387                 break;
 388             case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_PATHDIRECTION:
 389                 yaw = updatePathBearing();
 390                 break;
 391             case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI:
 392                 yaw = updatePOIBearing();
 393                 break;
 394             }
 395             result = updateVtolDesiredAttitude(yaw_attitude, yaw);
 396             if (!result && (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ENABLED || vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST)) {
 397                 global.vtolEmergencyFallbackSwitch = true;
 398             }
 399             return result;
 400         }
 401     } else {
 402         pid_configure(&global.PIDposH[0], 1.0f, 0.0f, 0.0f, 0.0f);
 403         pid_configure(&global.PIDposH[1], 1.0f, 0.0f, 0.0f, 0.0f);
 404         pid_configure(&global.PIDposV, vtolPathFollowerSettings.VerticalPosPI.Kp, vtolPathFollowerSettings.HorizontalPosPI.Ki, 0.0f, vtolPathFollowerSettings.HorizontalPosPI.ILimit);
 405         updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, true);
 406         updateVtolDesiredAttitudeEmergencyFallback();
 407         return 0;
 408     }
 409 }
 410
 411
 412 /**
 413  * Compute bearing of current takeoff location
 414  */
 415 static float updateTailInBearing()
 416 {
 417     PositionStateData p;
 418
 419     PositionStateGet(&p);
 420     TakeOffLocationData t;
 421     TakeOffLocationGet(&t);
 422     // atan2f always returns in between + and - 180 degrees
 423     float yaw = RAD2DEG(atan2f(p.East - t.East, p.North - t.North));
 424     // result is in between 0 and 360 degrees
 425     if (yaw < 0.0f) {
 426         yaw += 360.0f;
 427     }
 428     return yaw;
 429 }
 430
 431 /**
 432  * Compute bearing of current movement direction
 433  */
 434 static float updateCourseBearing()
 435 {
 436     VelocityStateData v;
 437
 438     VelocityStateGet(&v);
 439     // atan2f always returns in between + and - 180 degrees
 440     float yaw = RAD2DEG(atan2f(v.East, v.North));
 441     // result is in between 0 and 360 degrees
 442     if (yaw < 0.0f) {
 443         yaw += 360.0f;
 444     }
 445     return yaw;
 446 }
 447
 448 /**
 449  * Compute bearing of current path direction
 450  */
 451 static float updatePathBearing()
 452 {
 453     PositionStateData positionState;
 454
 455     PositionStateGet(&positionState);
 456
 457     float cur[3] = { positionState.North,
 458                      positionState.East,
 459                      positionState.Down };
 460     struct path_status progress;
 461
 462     path_progress(&pathDesired, cur, &progress);
 463
 464     // atan2f always returns in between + and - 180 degrees
 465     float yaw = RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0]));
 466     // result is in between 0 and 360 degrees
 467     if (yaw < 0.0f) {
 468         yaw += 360.0f;
 469     }
 470     return yaw;
 471 }
 472
 473 /**
 474  * Compute bearing between current position and POI
 475  */
 476 static float updatePOIBearing()
 477 {
 478     PoiLocationData poi;
 479
 480     PoiLocationGet(&poi);
 481     PositionStateData positionState;
 482     PositionStateGet(&positionState);
 483
 484     const float dT = updatePeriod / 1000.0f;
 485     float dLoc[3];
 486     float yaw = 0;
 487     /*float elevation = 0;*/
 488
 489     dLoc[0] = positionState.North - poi.North;
 490     dLoc[1] = positionState.East - poi.East;
 491     dLoc[2] = positionState.Down - poi.Down;
 492
 493     if (dLoc[1] < 0) {
 494         yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
 495     } else {
 496         yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f;
 497     }
 498     ManualControlCommandData manualControlData;
 499     ManualControlCommandGet(&manualControlData);
 500
 501     float pathAngle = 0;
 502     if (manualControlData.Roll > DEADBAND_HIGH) {
 503         pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
 504     } else if (manualControlData.Roll < DEADBAND_LOW) {
 505         pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
 506     }
 507
 508     return yaw + (pathAngle / 2.0f);
 509 }
 510
 511 /**
 512  * process POI control logic TODO: this should most likely go into manualcontrol!!!!
 513  * TODO: the whole process of POI handling likely needs cleanup and rethinking, might be broken since manualcontrol was refactored currently
 514  **/
 515 static void processPOI()
 516 {
 517     const float dT = updatePeriod / 1000.0f;
 518
 519
 520     PositionStateData positionState;
 521
 522     PositionStateGet(&positionState);
 523     // CameraDesiredData cameraDesired;
 524     // CameraDesiredGet(&cameraDesired);
 525     StabilizationDesiredData stabDesired;
 526     StabilizationDesiredGet(&stabDesired);
 527     PoiLocationData poi;
 528     PoiLocationGet(&poi);
 529
 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
 544     // distance
 545     float distance = sqrtf(powf(dLoc[0], 2.0f) + powf(dLoc[1], 2.0f));
 546
 547     ManualControlCommandData manualControlData;
 548     ManualControlCommandGet(&manualControlData);
 549
 550     float changeRadius = 0;
 551     // Move closer or further, radially
 552     if (manualControlData.Pitch > DEADBAND_HIGH) {
 553         changeRadius = (manualControlData.Pitch - DEADBAND_HIGH) * dT * 100.0f;
 554     } else if (manualControlData.Pitch < DEADBAND_LOW) {
 555         changeRadius = (manualControlData.Pitch - DEADBAND_LOW) * dT * 100.0f;
 556     }
 557
 558     // move along circular path
 559     float pathAngle = 0;
 560     if (manualControlData.Roll > DEADBAND_HIGH) {
 561         pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
 562     } else if (manualControlData.Roll < DEADBAND_LOW) {
 563         pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
 564     } else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) {
 565         // change radius only when not circling
 566         global.poiRadius = distance + changeRadius;
 567     }
 568
 569     // don't try to move any closer
 570     if (global.poiRadius >= 3.0f || changeRadius > 0) {
 571         if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) {
 572             pathDesired.End.North = poi.North + (global.poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f)));
 573             pathDesired.End.East  = poi.East + (global.poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f)));
 574             pathDesired.StartingVelocity = 1.0f;
 575             pathDesired.EndingVelocity   = 0.0f;
 576             pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
 577             PathDesiredSet(&pathDesired);
 578         }
 579     }
 580     // not above
 581     if (distance >= 3.0f) {
 582         // You can feed this into camerastabilization
 583         /*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/
 584
 585         // cameraDesired.Yaw=yaw;
 586         // cameraDesired.PitchOrServo2=elevation;
 587
 588         // CameraDesiredSet(&cameraDesired);
 589     }
 590 }
 591
 592 /**
 593  * Compute desired velocity from the current position and path
 594  */
 595 static void updatePathVelocity(float kFF, bool limited)
 596 {
 597     PositionStateData positionState;
 598
 599     PositionStateGet(&positionState);
 600     VelocityStateData velocityState;
 601     VelocityStateGet(&velocityState);
 602
 603     const float dT = updatePeriod / 1000.0f;
 604
 605     // look ahead kFF seconds
 606     float cur[3]   = { positionState.North + (velocityState.North * kFF),
 607                        positionState.East + (velocityState.East * kFF),
 608                        positionState.Down + (velocityState.Down * kFF) };
 609     struct path_status progress;
 610
 611     path_progress(&pathDesired, cur, &progress);
 612
 613     // calculate velocity - can be zero if waypoints are too close
 614     VelocityDesiredData velocityDesired;
 615     velocityDesired.North = progress.path_vector[0];
 616     velocityDesired.East  = progress.path_vector[1];
 617     velocityDesired.Down  = progress.path_vector[2];
 618
 619     if (limited &&
 620         // if a plane is crossing its desired flightpath facing the wrong way (away from flight direction)
 621         // it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector
 622         // once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading)
 623         // leading to an S-shape snake course the wrong way
 624         // this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't
 625         // turn steep unless there is enough space complete the turn before crossing the flightpath
 626         // in this case the plane effectively needs to be turned around
 627         // indicators:
 628         // difference between correction_direction and velocitystate >90 degrees and
 629         // difference between path_direction and velocitystate >90 degrees  ( 4th sector, facing away from everything )
 630         // fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
 631         // calculating angles < 90 degrees through dot products
 632         (vector_lengthf(progress.path_vector, 2) > 1e-6f) &&
 633         ((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) &&
 634         ((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) {
 635         ;
 636     } else {
 637         // calculate correction
 638         velocityDesired.North += pid_apply(&global.PIDposH[0], progress.correction_vector[0], dT);
 639         velocityDesired.East  += pid_apply(&global.PIDposH[1], progress.correction_vector[1], dT);
 640     }
 641     velocityDesired.Down += pid_apply(&global.PIDposV, progress.correction_vector[2], dT);
 642
 643     // update pathstatus
 644     pathStatus.error      = progress.error;
 645     pathStatus.fractional_progress  = progress.fractional_progress;
 646     pathStatus.path_direction_north = progress.path_vector[0];
 647     pathStatus.path_direction_east  = progress.path_vector[1];
 648     pathStatus.path_direction_down  = progress.path_vector[2];
 649
 650     pathStatus.correction_direction_north = progress.correction_vector[0];
 651     pathStatus.correction_direction_east  = progress.correction_vector[1];
 652     pathStatus.correction_direction_down  = progress.correction_vector[2];
 653
 654     VelocityDesiredSet(&velocityDesired);
 655 }
 656
 657
 658 /**
 659  * Compute desired attitude from the desired velocity for fixed wing craft
 660  */
 661 static uint8_t updateFixedDesiredAttitude()
 662 {
 663     uint8_t result = 1;
 664
 665     const float dT = updatePeriod / 1000.0f; // Convert from [ms] to [s]
 666
 667     VelocityDesiredData velocityDesired;
 668     VelocityStateData velocityState;
 669     StabilizationDesiredData stabDesired;
 670     AttitudeStateData attitudeState;
 671     FixedWingPathFollowerStatusData fixedWingPathFollowerStatus;
 672     AirspeedStateData airspeedState;
 673     SystemSettingsData systemSettings;
 674
 675     float groundspeedProjection;
 676     float indicatedAirspeedState;
 677     float indicatedAirspeedDesired;
 678     float airspeedError;
 679
 680     float pitchCommand;
 681
 682     float descentspeedDesired;
 683     float descentspeedError;
 684     float powerCommand;
 685
 686     float airspeedVector[2];
 687     float fluidMovement[2];
 688     float courseComponent[2];
 689     float courseError;
 690     float courseCommand;
 691
 692     FixedWingPathFollowerStatusGet(&fixedWingPathFollowerStatus);
 693
 694     VelocityStateGet(&velocityState);
 695     StabilizationDesiredGet(&stabDesired);
 696     VelocityDesiredGet(&velocityDesired);
 697     AttitudeStateGet(&attitudeState);
 698     AirspeedStateGet(&airspeedState);
 699     SystemSettingsGet(&systemSettings);
 700
 701     /**
 702      * Compute speed error and course
 703      */
 704     // missing sensors for airspeed-direction we have to assume within
 705     // reasonable error that measured airspeed is actually the airspeed
 706     // component in forward pointing direction
 707     // airspeedVector is normalized
 708     airspeedVector[0]     = cos_lookup_deg(attitudeState.Yaw);
 709     airspeedVector[1]     = sin_lookup_deg(attitudeState.Yaw);
 710
 711     // current ground speed projected in forward direction
 712     groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
 713
 714     // note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
 715     // but thanks to accelerometers,  groundspeedProjection reacts faster to changes in direction
 716     // than airspeed and gps sensors alone
 717     indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
 718
 719     // fluidMovement is a vector describing the aproximate movement vector of
 720     // the surrounding fluid in 2d space (aka wind vector)
 721     fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]);
 722     fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]);
 723
 724     // calculate the movement vector we need to fly to reach velocityDesired -
 725     // taking fluidMovement into account
 726     courseComponent[0] = velocityDesired.North - fluidMovement[0];
 727     courseComponent[1] = velocityDesired.East - fluidMovement[1];
 728
 729     indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]),
 730                                       fixedWingPathFollowerSettings.HorizontalVelMin,
 731                                       fixedWingPathFollowerSettings.HorizontalVelMax);
 732
 733     // if we could fly at arbitrary speeds, we'd just have to move towards the
 734     // courseComponent vector as previously calculated and we'd be fine
 735     // unfortunately however we are bound by min and max air speed limits, so
 736     // we need to recalculate the correct course to meet at least the
 737     // velocityDesired vector direction at our current speed
 738     // this overwrites courseComponent
 739     bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired);
 740
 741     // Error condition: wind speed too high, we can't go where we want anymore
 742     fixedWingPathFollowerStatus.Errors.Wind = 0;
 743     if ((!valid) &&
 744         fixedWingPathFollowerSettings.Safetymargins.Wind > 0.5f) { // alarm switched on
 745         fixedWingPathFollowerStatus.Errors.Wind = 1;
 746         result = 0;
 747     }
 748
 749     // Airspeed error
 750     airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
 751
 752     // Vertical speed error
 753     descentspeedDesired = boundf(
 754         velocityDesired.Down,
 755         -fixedWingPathFollowerSettings.VerticalVelMax,
 756         fixedWingPathFollowerSettings.VerticalVelMax);
 757     descentspeedError = descentspeedDesired - velocityState.Down;
 758
 759     // Error condition: plane too slow or too fast
 760     fixedWingPathFollowerStatus.Errors.Highspeed = 0;
 761     fixedWingPathFollowerStatus.Errors.Lowspeed  = 0;
 762     if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingPathFollowerSettings.Safetymargins.Overspeed) {
 763         fixedWingPathFollowerStatus.Errors.Overspeed = 1;
 764         result = 0;
 765     }
 766     if (indicatedAirspeedState > fixedWingPathFollowerSettings.HorizontalVelMax * fixedWingPathFollowerSettings.Safetymargins.Highspeed) {
 767         fixedWingPathFollowerStatus.Errors.Highspeed = 1;
 768         result = 0;
 769     }
 770     if (indicatedAirspeedState < fixedWingPathFollowerSettings.HorizontalVelMin * fixedWingPathFollowerSettings.Safetymargins.Lowspeed) {
 771         fixedWingPathFollowerStatus.Errors.Lowspeed = 1;
 772         result = 0;
 773     }
 774     if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingPathFollowerSettings.Safetymargins.Stallspeed) {
 775         fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
 776         result = 0;
 777     }
 778
 779     /**
 780      * Compute desired thrust command
 781      */
 782
 783     // Compute the cross feed from vertical speed to pitch, with saturation
 784     float speedErrorToPowerCommandComponent = boundf(
 785         (airspeedError / fixedWingPathFollowerSettings.HorizontalVelMin) * fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Kp,
 786         -fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max,
 787         fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max
 788         );
 789
 790     // Compute final thrust response
 791     powerCommand = pid_apply(&global.PIDpower, -descentspeedError, dT) +
 792                    speedErrorToPowerCommandComponent;
 793
 794     // Output internal state to telemetry
 795     fixedWingPathFollowerStatus.Error.Power    = descentspeedError;
 796     fixedWingPathFollowerStatus.ErrorInt.Power = global.PIDpower.iAccumulator;
 797     fixedWingPathFollowerStatus.Command.Power  = powerCommand;
 798
 799     // set thrust
 800     stabDesired.Thrust = boundf(fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand,
 801                                 fixedWingPathFollowerSettings.ThrustLimit.Min,
 802                                 fixedWingPathFollowerSettings.ThrustLimit.Max);
 803
 804     // Error condition: plane cannot hold altitude at current speed.
 805     fixedWingPathFollowerStatus.Errors.Lowpower = 0;
 806     if (fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand >= fixedWingPathFollowerSettings.ThrustLimit.Max && // thrust at maximum
 807         velocityState.Down > 0.0f && // we ARE going down
 808         descentspeedDesired < 0.0f && // we WANT to go up
 809         airspeedError > 0.0f && // we are too slow already
 810         fixedWingPathFollowerSettings.Safetymargins.Lowpower > 0.5f) { // alarm switched on
 811         fixedWingPathFollowerStatus.Errors.Lowpower = 1;
 812         result = 0;
 813     }
 814     // Error condition: plane keeps climbing despite minimum thrust (opposite of above)
 815     fixedWingPathFollowerStatus.Errors.Highpower = 0;
 816     if (fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand <= fixedWingPathFollowerSettings.ThrustLimit.Min && // thrust at minimum
 817         velocityState.Down < 0.0f && // we ARE going up
 818         descentspeedDesired > 0.0f && // we WANT to go down
 819         airspeedError < 0.0f && // we are too fast already
 820         fixedWingPathFollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on
 821         fixedWingPathFollowerStatus.Errors.Highpower = 1;
 822         result = 0;
 823     }
 824
 825     /**
 826      * Compute desired pitch command
 827      */
 828     // Compute the cross feed from vertical speed to pitch, with saturation
 829     float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Kp,
 830                                                         -fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max,
 831                                                         fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max
 832                                                         );
 833
 834     // Compute the pitch command as err*Kp + errInt*Ki + X_feed.
 835     pitchCommand = -pid_apply(&global.PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent;
 836
 837     fixedWingPathFollowerStatus.Error.Speed    = airspeedError;
 838     fixedWingPathFollowerStatus.ErrorInt.Speed = global.PIDspeed.iAccumulator;
 839     fixedWingPathFollowerStatus.Command.Speed  = pitchCommand;
 840
 841     stabDesired.Pitch = boundf(fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand,
 842                                fixedWingPathFollowerSettings.PitchLimit.Min,
 843                                fixedWingPathFollowerSettings.PitchLimit.Max);
 844
 845     // Error condition: high speed dive
 846     fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0;
 847     if (fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand >= fixedWingPathFollowerSettings.PitchLimit.Max && // pitch demand is full up
 848         velocityState.Down > 0.0f && // we ARE going down
 849         descentspeedDesired < 0.0f && // we WANT to go up
 850         airspeedError < 0.0f && // we are too fast already
 851         fixedWingPathFollowerSettings.Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
 852         fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1;
 853         result = 0;
 854     }
 855
 856     /**
 857      * Compute desired roll command
 858      */
 859     courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
 860
 861     if (courseError < -180.0f) {
 862         courseError += 360.0f;
 863     }
 864     if (courseError > 180.0f) {
 865         courseError -= 360.0f;
 866     }
 867
 868     // overlap calculation. Theres a dead zone behind the craft where the
 869     // counter-yawing of some craft while rolling could render a desired right
 870     // turn into a desired left turn. Making the turn direction based on
 871     // current roll angle keeps the plane committed to a direction once chosen
 872     if (courseError < -180.0f + (fixedWingPathFollowerSettings.ReverseCourseOverlap * 0.5f)
 873         && attitudeState.Roll > 0.0f) {
 874         courseError += 360.0f;
 875     }
 876     if (courseError > 180.0f - (fixedWingPathFollowerSettings.ReverseCourseOverlap * 0.5f)
 877         && attitudeState.Roll < 0.0f) {
 878         courseError -= 360.0f;
 879     }
 880
 881     courseCommand = pid_apply(&global.PIDcourse, courseError, dT);
 882
 883     fixedWingPathFollowerStatus.Error.Course    = courseError;
 884     fixedWingPathFollowerStatus.ErrorInt.Course = global.PIDcourse.iAccumulator;
 885     fixedWingPathFollowerStatus.Command.Course  = courseCommand;
 886
 887     stabDesired.Roll = boundf(fixedWingPathFollowerSettings.RollLimit.Neutral +
 888                               courseCommand,
 889                               fixedWingPathFollowerSettings.RollLimit.Min,
 890                               fixedWingPathFollowerSettings.RollLimit.Max);
 891
 892     // TODO: find a check to determine loss of directional control. Likely needs some check of derivative
 893
 894
 895     /**
 896      * Compute desired yaw command
 897      */
 898     // TODO implement raw control mode for yaw and base on Accels.Y
 899     stabDesired.Yaw = 0.0f;
 900
 901
 902     stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 903     stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
 904     stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
 905     stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
 906
 907     StabilizationDesiredSet(&stabDesired);
 908
 909     FixedWingPathFollowerStatusSet(&fixedWingPathFollowerStatus);
 910
 911     return result;
 912 }
 913
 914
 915 /**
 916  * Function to calculate course vector C based on airspeed s, fluid movement F
 917  * and desired movement vector V
 918  * parameters in: V,F,s
 919  * parameters out: C
 920  * returns true if a valid solution could be found for V,F,s, false if not
 921  * C will be set to a best effort attempt either way
 922  */
 923 static bool correctCourse(float *C, float *V, float *F, float s)
 924 {
 925     // Approach:
 926     // Let Sc be a circle around origin marking possible movement vectors
 927     // of the craft with airspeed s (all possible options for C)
 928     // Let Vl be a line through the origin along movement vector V where fr any
 929     // point v on line Vl v = k * (V / |V|) = k' * V
 930     // Let Wl be a line parallel to Vl where for any point v on line Vl exists
 931     // a point w on WL with w = v - F
 932     // Then any intersection between circle Sc and line Wl represents course
 933     // vector which would result in a movement vector
 934     // V' = k * ( V / |V|) = k' * V
 935     // If there is no intersection point, S is insufficient to compensate
 936     // for F and we can only try to fly in direction of V (thus having wind drift
 937     // but at least making progress orthogonal to wind)
 938
 939     s = fabsf(s);
 940     float f = vector_lengthf(F, 2);
 941
 942     // normalize Cn=V/|V|, |V| must be >0
 943     float v = vector_lengthf(V, 2);
 944     if (v < 1e-6f) {
 945         // if |V|=0, we aren't supposed to move, turn into the wind
 946         // (this allows hovering)
 947         C[0] = -F[0];
 948         C[1] = -F[1];
 949         // if desired airspeed matches fluidmovement a hover is actually
 950         // intended so return true
 951         return fabsf(f - s) < 1e-3f;
 952     }
 953     float Vn[2] = { V[0] / v, V[1] / v };
 954
 955     // project F on V
 956     float fp    = F[0] * Vn[0] + F[1] * Vn[1];
 957
 958     // find component Fo of F that is orthogonal to V
 959     // (which is exactly the distance between Vl and Wl)
 960     float Fo[2] = { F[0] - (fp * Vn[0]), F[1] - (fp * Vn[1]) };
 961     float fo2   = Fo[0] * Fo[0] + Fo[1] * Fo[1];
 962
 963     // find k where k * Vn = C - Fo
 964     // |C|=s is the hypothenuse in any rectangular triangle formed by k * Vn and Fo
 965     // so k^2 + fo^2 = s^2 (since |Vn|=1)
 966     float k2 = s * s - fo2;
 967     if (k2 <= -1e-3f) {
 968         // there is no solution, we will be drifted off either way
 969         // fallback: fly stupidly in direction of V and hope for the best
 970         C[0] = V[0];
 971         C[1] = V[1];
 972         return false;
 973     } else if (k2 <= 1e-3f) {
 974         // there is exactly one solution: -Fo
 975         C[0] = -Fo[0];
 976         C[1] = -Fo[1];
 977         return true;
 978     }
 979     // we have two possible solutions k positive and k negative as there are
 980     // two intersection points between Wl and Sc
 981     // which one is better? two criteria:
 982     // 1. we MUST move in the right direction, if any k leads to -v its invalid
 983     // 2. we should minimize the speed error
 984     float k     = sqrt(k2);
 985     float C1[2] = { -k * Vn[0] - Fo[0], -k * Vn[1] - Fo[1] };
 986     float C2[2] = { k *Vn[0] - Fo[0], k * Vn[1] - Fo[1] };
 987     // project C+F on Vn to find signed resulting movement vector length
 988     float vp1   = (C1[0] + F[0]) * Vn[0] + (C1[1] + F[1]) * Vn[1];
 989     float vp2   = (C2[0] + F[0]) * Vn[0] + (C2[1] + F[1]) * Vn[1];
 990     if (vp1 >= 0.0f && fabsf(v - vp1) < fabsf(v - vp2)) {
 991         // in this case the angle between course and resulting movement vector
 992         // is greater than 90 degrees - so we actually fly backwards
 993         C[0] = C1[0];
 994         C[1] = C1[1];
 995         return true;
 996     }
 997     C[0] = C2[0];
 998     C[1] = C2[1];
 999     if (vp2 >= 0.0f) {
1000         // in this case the angle between course and movement vector is less than
1001         // 90 degrees, but we do move in the right direction
1002         return true;
1003     } else {
1004         // in this case we actually get driven in the opposite direction of V
1005         // with both solutions for C
1006         // this might be reached in headwind stronger than maximum allowed
1007         // airspeed.
1008         return false;
1009     }
1010 }
1011
1012 /**
1013  * Compute desired attitude from the desired velocity
1014  *
1015  * Takes in @ref NedState which has the acceleration in the
1016  * NED frame as the feedback term and then compares the
1017  * @ref VelocityState against the @ref VelocityDesired
1018  */
1019 static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction)
1020 {
1021     const float dT = updatePeriod / 1000.0f;
1022     uint8_t result = 1;
1023
1024     VelocityDesiredData velocityDesired;
1025     VelocityStateData velocityState;
1026     StabilizationDesiredData stabDesired;
1027     AttitudeStateData attitudeState;
1028     StabilizationBankData stabSettings;
1029     SystemSettingsData systemSettings;
1030
1031     float northError;
1032     float northCommand;
1033
1034     float eastError;
1035     float eastCommand;
1036
1037     float downError;
1038     float downCommand;
1039
1040     SystemSettingsGet(&systemSettings);
1041     VelocityStateGet(&velocityState);
1042     VelocityDesiredGet(&velocityDesired);
1043     StabilizationDesiredGet(&stabDesired);
1044     VelocityDesiredGet(&velocityDesired);
1045     AttitudeStateGet(&attitudeState);
1046     StabilizationBankGet(&stabSettings);
1047
1048     // Testing code - refactor into manual control command
1049     ManualControlCommandData manualControlData;
1050     ManualControlCommandGet(&manualControlData);
1051
1052     // scale velocity if it is above configured maximum
1053     float velH = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East);
1054     if (velH > vtolPathFollowerSettings.HorizontalVelMax) {
1055         velocityDesired.North *= vtolPathFollowerSettings.HorizontalVelMax / velH;
1056         velocityDesired.East  *= vtolPathFollowerSettings.HorizontalVelMax / velH;
1057     }
1058     if (fabsf(velocityDesired.Down) > vtolPathFollowerSettings.VerticalVelMax) {
1059         velocityDesired.Down *= vtolPathFollowerSettings.VerticalVelMax / fabsf(velocityDesired.Down);
1060     }
1061
1062     // Compute desired north command
1063     northError   = velocityDesired.North - velocityState.North;
1064     northCommand = pid_apply(&global.PIDvel[0], northError, dT) + velocityDesired.North * vtolPathFollowerSettings.VelocityFeedforward;
1065
1066     // Compute desired east command
1067     eastError    = velocityDesired.East - velocityState.East;
1068     eastCommand  = pid_apply(&global.PIDvel[1], eastError, dT) + velocityDesired.East * vtolPathFollowerSettings.VelocityFeedforward;
1069
1070     // Compute desired down command
1071     downError    = velocityDesired.Down - velocityState.Down;
1072     // Must flip this sign
1073     downError    = -downError;
1074     downCommand  = pid_apply(&global.PIDvel[2], downError, dT);
1075
1076     stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max);
1077
1078
1079     // DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in
1080     if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST) {
1081         attitudeState.Yaw += 120.0f;
1082         if (attitudeState.Yaw > 180.0f) {
1083             attitudeState.Yaw -= 360.0f;
1084         }
1085     }
1086
1087     if ( // emergency flyaway detection
1088         ( // integral already at its limit
1089             vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f ||
1090             vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[1].iAccumulator) < 1e-6f
1091         ) &&
1092         // angle between desired and actual velocity >90 degrees (by dot product)
1093         (velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) &&
1094         // quad is moving at significant speed (during flyaway it would keep speeding up)
1095         squaref(velocityState.North) + squaref(velocityState.East) > 1.0f
1096         ) {
1097         global.vtolEmergencyFallback += dT;
1098         if (global.vtolEmergencyFallback >= vtolPathFollowerSettings.FlyawayEmergencyFallbackTriggerTime) {
1099             // after emergency timeout, trigger alarm - everything else is handled by callers
1100             // (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...)
1101             result = 0;
1102         }
1103     } else {
1104         global.vtolEmergencyFallback = 0.0f;
1105     }
1106
1107     // Project the north and east command signals into the pitch and roll based on yaw.  For this to behave well the
1108     // craft should move similarly for 5 deg roll versus 5 deg pitch
1109     stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
1110                                -eastCommand * sinf(DEG2RAD(attitudeState.Yaw)),
1111                                -vtolPathFollowerSettings.MaxRollPitch, vtolPathFollowerSettings.MaxRollPitch);
1112     stabDesired.Roll  = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
1113                                eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
1114                                -vtolPathFollowerSettings.MaxRollPitch, vtolPathFollowerSettings.MaxRollPitch);
1115
1116     if (vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
1117         // For now override thrust with manual control.  Disable at your risk, quad goes to China.
1118         ManualControlCommandData manualControl;
1119         ManualControlCommandGet(&manualControl);
1120         stabDesired.Thrust = manualControl.Thrust;
1121     }
1122
1123     stabDesired.StabilizationMode.Roll  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
1124     stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
1125     if (yaw_attitude) {
1126         stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
1127         stabDesired.Yaw = yaw_direction;
1128     } else {
1129         stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
1130         stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControlData.Yaw;
1131     }
1132     stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
1133     StabilizationDesiredSet(&stabDesired);
1134
1135     return result;
1136 }
1137
1138 /**
1139  * Compute desired attitude for vtols - emergency fallback
1140  */
1141 static void updateVtolDesiredAttitudeEmergencyFallback()
1142 {
1143     const float dT = updatePeriod / 1000.0f;
1144
1145     VelocityDesiredData velocityDesired;
1146     VelocityStateData velocityState;
1147     StabilizationDesiredData stabDesired;
1148
1149     float courseError;
1150     float courseCommand;
1151
1152     float downError;
1153     float downCommand;
1154
1155     VelocityStateGet(&velocityState);
1156     VelocityDesiredGet(&velocityDesired);
1157
1158     ManualControlCommandData manualControlData;
1159     ManualControlCommandGet(&manualControlData);
1160
1161     courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North));
1162
1163     if (courseError < -180.0f) {
1164         courseError += 360.0f;
1165     }
1166     if (courseError > 180.0f) {
1167         courseError -= 360.0f;
1168     }
1169
1170
1171     courseCommand   = (courseError * vtolPathFollowerSettings.EmergencyFallbackYawRate.kP);
1172
1173     stabDesired.Yaw = boundf(courseCommand, -vtolPathFollowerSettings.EmergencyFallbackYawRate.Max, vtolPathFollowerSettings.EmergencyFallbackYawRate.Max);
1174
1175     // Compute desired down command
1176     downError   = velocityDesired.Down - velocityState.Down;
1177     // Must flip this sign
1178     downError   = -downError;
1179     downCommand = pid_apply(&global.PIDvel[2], downError, dT);
1180
1181     stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max);
1182
1183
1184     stabDesired.Roll   = vtolPathFollowerSettings.EmergencyFallbackAttitude.Roll;
1185     stabDesired.Pitch  = vtolPathFollowerSettings.EmergencyFallbackAttitude.Pitch;
1186
1187     if (vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
1188         // For now override thrust with manual control.  Disable at your risk, quad goes to China.
1189         ManualControlCommandData manualControl;
1190         ManualControlCommandGet(&manualControl);
1191         stabDesired.Thrust = manualControl.Thrust;
1192     }
1193
1194     stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
1195     stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
1196     stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
1197     stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
1198     StabilizationDesiredSet(&stabDesired);
1199 }
1200
1201
1202 /**
1203  * Compute desired attitude from a fixed preset
1204  *
1205  */
1206 static void updateFixedAttitude(float *attitude)
1207 {
1208     StabilizationDesiredData stabDesired;
1209
1210     StabilizationDesiredGet(&stabDesired);
1211     stabDesired.Roll   = attitude[0];
1212     stabDesired.Pitch  = attitude[1];
1213     stabDesired.Yaw    = attitude[2];
1214     stabDesired.Thrust = attitude[3];
1215     stabDesired.StabilizationMode.Roll   = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
1216     stabDesired.StabilizationMode.Pitch  = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
1217     stabDesired.StabilizationMode.Yaw    = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
1218     stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
1219     StabilizationDesiredSet(&stabDesired);
1220 }