flight/modules/PathPlanner/pathplanner.c

   1 /**
   2  ******************************************************************************
   3  * @addtogroup LibrePilotModules LibrePilot Modules
   4  * @{
   5  * @addtogroup PathPlanner Path Planner Module
   6  * @brief Executes a series of waypoints
   7  * @{
   8  *
   9  * @file       pathplanner.c
  10  * @author     The LibrePilot Project, http://www.librepilot.org Copyright (C) 2016.
  11  *             The OpenPilot Team, http://www.openpilot.org Copyright (C) 2015.
  12  * @brief      Executes a series of waypoints
  13  *
  14  * @see        The GNU Public License (GPL) Version 3
  15  *
  16  *****************************************************************************/
  17 /*
  18  * This program is free software; you can redistribute it and/or modify
  19  * it under the terms of the GNU General Public License as published by
  20  * the Free Software Foundation; either version 3 of the License, or
  21  * (at your option) any later version.
  22  *
  23  * This program is distributed in the hope that it will be useful, but
  24  * WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
  25  * or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
  26  * for more details.
  27  *
  28  * You should have received a copy of the GNU General Public License along
  29  * with this program; if not, write to the Free Software Foundation, Inc.,
  30  * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
  31  */
  32
  33 #include "openpilot.h"
  34
  35 #include "callbackinfo.h"
  36 #include "pathplan.h"
  37 #include "flightstatus.h"
  38 #include "airspeedstate.h"
  39 #include "pathaction.h"
  40 #include "pathdesired.h"
  41 #include "pathstatus.h"
  42 #include "positionstate.h"
  43 #include "velocitystate.h"
  44 #include "waypoint.h"
  45 #include "waypointactive.h"
  46 #include "flightmodesettings.h"
  47 #include <systemsettings.h>
  48 #include "paths.h"
  49 #include "plans.h"
  50 #include <sanitycheck.h>
  51 #include <vtolpathfollowersettings.h>
  52 #include <manualcontrolcommand.h>
  53
  54 // Private constants
  55 #define STACK_SIZE_BYTES            1024
  56 #define TASK_PRIORITY               CALLBACK_TASK_NAVIGATION
  57 #define MAX_QUEUE_SIZE              2
  58 #define PATH_PLANNER_UPDATE_RATE_MS 100 // can be slow, since we listen to status updates as well
  59
  60 // Private types
  61
  62 // Private functions
  63 static void pathPlannerTask();
  64 static void commandUpdated(UAVObjEvent *ev);
  65 static void statusUpdated(UAVObjEvent *ev);
  66 static void updatePathDesired();
  67 static void setWaypoint(uint16_t num);
  68
  69 static uint8_t checkPathPlan();
  70 static uint8_t pathConditionCheck();
  71 static uint8_t conditionNone();
  72 static uint8_t conditionTimeOut();
  73 static uint8_t conditionDistanceToTarget();
  74 static uint8_t conditionLegRemaining();
  75 static uint8_t conditionBelowError();
  76 static uint8_t conditionAboveAltitude();
  77 static uint8_t conditionAboveSpeed();
  78 static uint8_t conditionPointingTowardsNext();
  79 static uint8_t conditionPythonScript();
  80 static uint8_t conditionImmediate();
  81 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev);
  82
  83
  84 // Private variables
  85 static DelayedCallbackInfo *pathPlannerHandle;
  86 static DelayedCallbackInfo *pathDesiredUpdaterHandle;
  87 static WaypointActiveData waypointActive;
  88 static WaypointData waypoint;
  89 static PathActionData pathAction;
  90 static bool pathplanner_active = false;
  91 static FrameType_t frameType;
  92 static bool mode3D;
  93
  94 extern FrameType_t GetCurrentFrameType();
  95
  96 /**
  97  * Module initialization
  98  */
  99 int32_t PathPlannerStart()
 100 {
 101     plan_initialize();
 102     // when the active waypoint changes, update pathDesired
 103     WaypointConnectCallback(commandUpdated);
 104     WaypointActiveConnectCallback(commandUpdated);
 105     PathActionConnectCallback(commandUpdated);
 106     PathStatusConnectCallback(statusUpdated);
 107     SettingsUpdatedCb(NULL);
 108     SystemSettingsConnectCallback(&SettingsUpdatedCb);
 109     VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
 110
 111     // Start main task callback
 112     PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
 113
 114     return 0;
 115 }
 116
 117 /**
 118  * Module initialization
 119  */
 120 int32_t PathPlannerInitialize()
 121 {
 122     PathPlanInitialize();
 123     PathActionInitialize();
 124     PathStatusInitialize();
 125     PathDesiredInitialize();
 126     PositionStateInitialize();
 127     AirspeedStateInitialize();
 128     VelocityStateInitialize();
 129     WaypointInitialize();
 130     WaypointActiveInitialize();
 131
 132     pathPlannerHandle = PIOS_CALLBACKSCHEDULER_Create(&pathPlannerTask, CALLBACK_PRIORITY_REGULAR, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER0, STACK_SIZE_BYTES);
 133     pathDesiredUpdaterHandle = PIOS_CALLBACKSCHEDULER_Create(&updatePathDesired, CALLBACK_PRIORITY_CRITICAL, TASK_PRIORITY, CALLBACKINFO_RUNNING_PATHPLANNER1, STACK_SIZE_BYTES);
 134
 135     return 0;
 136 }
 137
 138 MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart);
 139
 140
 141 static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
 142 {
 143     VtolPathFollowerSettingsTreatCustomCraftAsOptions TreatCustomCraftAs;
 144
 145     VtolPathFollowerSettingsTreatCustomCraftAsGet(&TreatCustomCraftAs);
 146
 147     frameType = GetCurrentFrameType();
 148
 149     if (frameType == FRAME_TYPE_CUSTOM) {
 150         switch (TreatCustomCraftAs) {
 151         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
 152             frameType = FRAME_TYPE_FIXED_WING;
 153             break;
 154         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
 155             frameType = FRAME_TYPE_MULTIROTOR;
 156             break;
 157         case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_GROUND:
 158             frameType = FRAME_TYPE_GROUND;
 159             break;
 160         }
 161     }
 162
 163     switch (frameType) {
 164     case FRAME_TYPE_GROUND:
 165         mode3D = false;
 166         break;
 167     default:
 168         mode3D = true;
 169     }
 170 }
 171
 172 #define AUTOTAKEOFF_THROTTLE_LIMIT_TO_ALLOW_TAKEOFF_START 0.3f
 173
 174 /**
 175  * Module task
 176  */
 177 static void pathPlannerTask()
 178 {
 179     PIOS_CALLBACKSCHEDULER_Schedule(pathPlannerHandle, PATH_PLANNER_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
 180
 181     bool endCondition = false;
 182
 183     // check path plan validity early to raise alarm
 184     // even if not in guided mode
 185     uint8_t validPathPlan = checkPathPlan();
 186
 187     FlightStatusData flightStatus;
 188     FlightStatusGet(&flightStatus);
 189     if (flightStatus.ControlChain.PathPlanner != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
 190         pathplanner_active = false;
 191         AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
 192         return;
 193     }
 194
 195     PathDesiredData pathDesired;
 196     PathDesiredGet(&pathDesired);
 197
 198     static uint8_t failsafeRTHset = 0;
 199     if (!validPathPlan) {
 200         pathplanner_active = false;
 201
 202         if (!failsafeRTHset) {
 203             failsafeRTHset = 1;
 204             plan_setup_positionHold();
 205         }
 206         AlarmsSet(SYSTEMALARMS_ALARM_PATHPLAN, SYSTEMALARMS_ALARM_CRITICAL);
 207
 208         return;
 209     }
 210
 211     failsafeRTHset = 0;
 212     AlarmsClear(SYSTEMALARMS_ALARM_PATHPLAN);
 213
 214     WaypointActiveGet(&waypointActive);
 215
 216     // with the introduction of takeoff, we allow for arming
 217     // whilst in pathplanner mode.  Previously it was just an assumption that
 218     // a user never armed in pathplanner mode.  This check allows a user to select
 219     // pathplanner, to upload waypoints, and then arm in pathplanner.
 220     if (!flightStatus.Armed) {
 221         return;
 222     }
 223
 224     // the transition from pathplanner to another flightmode back to pathplanner
 225     // triggers a reset back either to 0 index in the waypoint list,
 226     // or to current index in the waypoint list, depending on FlightModeChangeRestartsPathPlan setting
 227     if (pathplanner_active == false) {
 228         pathplanner_active = true;
 229
 230         FlightModeSettingsFlightModeChangeRestartsPathPlanOptions restart;
 231         FlightModeSettingsFlightModeChangeRestartsPathPlanGet(&restart);
 232         if (restart == FLIGHTMODESETTINGS_FLIGHTMODECHANGERESTARTSPATHPLAN_TRUE) {
 233             setWaypoint(0);
 234         } else {
 235             setWaypoint(waypointActive.Index);
 236         }
 237         return;
 238     }
 239
 240     WaypointInstGet(waypointActive.Index, &waypoint);
 241     PathActionInstGet(waypoint.Action, &pathAction);
 242     PathStatusData pathStatus;
 243     PathStatusGet(&pathStatus);
 244
 245     // delay next step until path follower has acknowledged the path mode
 246     if (pathStatus.UID != pathDesired.UID) {
 247         return;
 248     }
 249
 250     // negative destinations DISABLE this feature
 251     if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) {
 252         setWaypoint(pathAction.ErrorDestination);
 253         return;
 254     }
 255
 256
 257     // check if condition has been met
 258     endCondition = pathConditionCheck();
 259     // decide what to do
 260     switch (pathAction.Command) {
 261     case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT:
 262         endCondition = !endCondition;
 263     case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT:
 264         if (endCondition) {
 265             setWaypoint(waypointActive.Index + 1);
 266         }
 267         break;
 268     case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT:
 269         endCondition = !endCondition;
 270     case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT:
 271         if (endCondition) {
 272             if (pathAction.JumpDestination < 0) {
 273                 // waypoint ids <0 code relative jumps
 274                 setWaypoint(waypointActive.Index - pathAction.JumpDestination);
 275             } else {
 276                 setWaypoint(pathAction.JumpDestination);
 277             }
 278         }
 279         break;
 280     case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT:
 281         if (endCondition) {
 282             if (pathAction.JumpDestination < 0) {
 283                 // waypoint ids <0 code relative jumps
 284                 setWaypoint(waypointActive.Index - pathAction.JumpDestination);
 285             } else {
 286                 setWaypoint(pathAction.JumpDestination);
 287             }
 288         } else {
 289             setWaypoint(waypointActive.Index + 1);
 290         }
 291         break;
 292     }
 293 }
 294
 295 // callback function when waypoints changed in any way, update pathDesired
 296 void updatePathDesired()
 297 {
 298     // only ever touch pathDesired if pathplanner is enabled
 299     if (!pathplanner_active) {
 300         return;
 301     }
 302
 303     // find out current waypoint
 304     WaypointActiveGet(&waypointActive);
 305     WaypointInstGet(waypointActive.Index, &waypoint);
 306
 307     PathActionInstGet(waypoint.Action, &pathAction);
 308
 309     PathDesiredData pathDesired;
 310
 311     pathDesired.End.North = waypoint.Position.North;
 312     pathDesired.End.East  = waypoint.Position.East;
 313     pathDesired.End.Down  = waypoint.Position.Down;
 314     pathDesired.EndingVelocity    = waypoint.Velocity;
 315     pathDesired.Mode = pathAction.Mode;
 316     pathDesired.ModeParameters[0] = pathAction.ModeParameters[0];
 317     pathDesired.ModeParameters[1] = pathAction.ModeParameters[1];
 318     pathDesired.ModeParameters[2] = pathAction.ModeParameters[2];
 319     pathDesired.ModeParameters[3] = pathAction.ModeParameters[3];
 320     pathDesired.UID = waypointActive.Index;
 321
 322
 323     if (waypointActive.Index == 0) {
 324         PositionStateData positionState;
 325         PositionStateGet(&positionState);
 326         // First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
 327
 328         /*pathDesired.Start[PATHDESIRED_START_NORTH] =  waypoint.Position[WAYPOINT_POSITION_NORTH];
 329            pathDesired.Start[PATHDESIRED_START_EAST] =  waypoint.Position[WAYPOINT_POSITION_EAST];
 330            pathDesired.Start[PATHDESIRED_START_DOWN] =  waypoint.Position[WAYPOINT_POSITION_DOWN];*/
 331         // note: if certain flightmodes need to override Start, End or mode parameters, that should happen within
 332         // the pathfollower as needed. This also holds if Start is replaced by current position for takeoff and landing
 333         pathDesired.Start.North = positionState.North;
 334         pathDesired.Start.East  = positionState.East;
 335         pathDesired.Start.Down  = positionState.Down;
 336         pathDesired.StartingVelocity = pathDesired.EndingVelocity;
 337     } else {
 338         // Get previous waypoint as start point
 339         WaypointData waypointPrev;
 340         WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
 341
 342         pathDesired.Start.North = waypointPrev.Position.North;
 343         pathDesired.Start.East  = waypointPrev.Position.East;
 344         pathDesired.Start.Down  = waypointPrev.Position.Down;
 345         pathDesired.StartingVelocity = waypointPrev.Velocity;
 346     }
 347
 348     PathDesiredSet(&pathDesired);
 349 }
 350
 351
 352 // safety checks for path plan integrity
 353 static uint8_t checkPathPlan()
 354 {
 355     uint16_t i;
 356     uint16_t waypointCount;
 357     uint16_t actionCount;
 358     uint8_t pathCrc;
 359     PathPlanData pathPlan;
 360
 361     // WaypointData waypoint; // using global instead (?)
 362     // PathActionData action; // using global instead (?)
 363
 364     PathPlanGet(&pathPlan);
 365
 366     waypointCount = pathPlan.WaypointCount;
 367     if (waypointCount == 0) {
 368         // an empty path plan is invalid
 369         return false;
 370     }
 371     actionCount = pathPlan.PathActionCount;
 372
 373     // check count consistency
 374     if (waypointCount > UAVObjGetNumInstances(WaypointHandle())) {
 375         // PIOS_DEBUGLOG_Printf("PathPlan : waypoint count error!");
 376         return false;
 377     }
 378     if (actionCount > UAVObjGetNumInstances(PathActionHandle())) {
 379         // PIOS_DEBUGLOG_Printf("PathPlan : path action count error!");
 380         return false;
 381     }
 382
 383     // check CRC
 384     pathCrc = 0;
 385     for (i = 0; i < waypointCount; i++) {
 386         pathCrc = UAVObjUpdateCRC(WaypointHandle(), i, pathCrc);
 387     }
 388     for (i = 0; i < actionCount; i++) {
 389         pathCrc = UAVObjUpdateCRC(PathActionHandle(), i, pathCrc);
 390     }
 391     if (pathCrc != pathPlan.Crc) {
 392         // failed crc check
 393         // PIOS_DEBUGLOG_Printf("PathPlan : bad CRC (%d / %d)!", pathCrc, pathPlan.Crc);
 394         return false;
 395     }
 396
 397     // waypoint consistency
 398     for (i = 0; i < waypointCount; i++) {
 399         WaypointInstGet(i, &waypoint);
 400         if (waypoint.Action >= actionCount) {
 401             // path action id is out of range
 402             return false;
 403         }
 404     }
 405
 406     // path action consistency
 407     for (i = 0; i < actionCount; i++) {
 408         PathActionInstGet(i, &pathAction);
 409         if (pathAction.ErrorDestination >= waypointCount) {
 410             // waypoint id is out of range
 411             return false;
 412         }
 413         if (pathAction.JumpDestination >= waypointCount) {
 414             // waypoint id is out of range
 415             return false;
 416         }
 417     }
 418
 419     // path plan passed checks
 420
 421     return true;
 422 }
 423
 424 // callback function when status changed, issue execution of state machine
 425 void commandUpdated(__attribute__((unused)) UAVObjEvent *ev)
 426 {
 427     PIOS_CALLBACKSCHEDULER_Dispatch(pathDesiredUpdaterHandle);
 428 }
 429
 430 // callback function when waypoints changed in any way, update pathDesired
 431 void statusUpdated(__attribute__((unused)) UAVObjEvent *ev)
 432 {
 433     PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
 434 }
 435
 436
 437 // helper function to go to a specific waypoint
 438 static void setWaypoint(uint16_t num)
 439 {
 440     PathPlanData pathPlan;
 441
 442     PathPlanGet(&pathPlan);
 443
 444     // here it is assumed that the path plan has been validated (waypoint count is consistent)
 445     if (num >= pathPlan.WaypointCount) {
 446         // path plans wrap around
 447         num = 0;
 448     }
 449
 450     waypointActive.Index = num;
 451     WaypointActiveSet(&waypointActive);
 452 }
 453
 454 // execute the appropriate condition and report result
 455 static uint8_t pathConditionCheck()
 456 {
 457     // i thought about a lookup table, but a switch is safer considering there could be invalid EndCondition ID's
 458     switch (pathAction.EndCondition) {
 459     case PATHACTION_ENDCONDITION_NONE:
 460         return conditionNone();
 461
 462         break;
 463     case PATHACTION_ENDCONDITION_TIMEOUT:
 464         return conditionTimeOut();
 465
 466         break;
 467     case PATHACTION_ENDCONDITION_DISTANCETOTARGET:
 468         return conditionDistanceToTarget();
 469
 470         break;
 471     case PATHACTION_ENDCONDITION_LEGREMAINING:
 472         return conditionLegRemaining();
 473
 474         break;
 475     case PATHACTION_ENDCONDITION_BELOWERROR:
 476         return conditionBelowError();
 477
 478         break;
 479     case PATHACTION_ENDCONDITION_ABOVEALTITUDE:
 480         return conditionAboveAltitude();
 481
 482         break;
 483     case PATHACTION_ENDCONDITION_ABOVESPEED:
 484         return conditionAboveSpeed();
 485
 486         break;
 487     case PATHACTION_ENDCONDITION_POINTINGTOWARDSNEXT:
 488         return conditionPointingTowardsNext();
 489
 490         break;
 491     case PATHACTION_ENDCONDITION_PYTHONSCRIPT:
 492         return conditionPythonScript();
 493
 494         break;
 495     case PATHACTION_ENDCONDITION_IMMEDIATE:
 496         return conditionImmediate();
 497
 498         break;
 499     default:
 500         // invalid endconditions are always true to prevent freezes
 501         return true;
 502
 503         break;
 504     }
 505 }
 506
 507 /* the None condition is always false */
 508 static uint8_t conditionNone()
 509 {
 510     return false;
 511 }
 512
 513 /**
 514  * the Timeout condition measures time this waypoint is active
 515  * Parameter 0:  timeout in seconds
 516  */
 517 static uint8_t conditionTimeOut()
 518 {
 519     static uint16_t toWaypoint;
 520     static uint32_t toStarttime;
 521
 522     // reset timer if waypoint changed
 523     if (waypointActive.Index != toWaypoint) {
 524         toWaypoint  = waypointActive.Index;
 525         toStarttime = PIOS_DELAY_GetRaw();
 526     }
 527     if (PIOS_DELAY_DiffuS(toStarttime) >= 1e6f * pathAction.ConditionParameters[0]) {
 528         // make sure we reinitialize even if the same waypoint comes twice
 529         toWaypoint = 0xFFFF;
 530         return true;
 531     }
 532     return false;
 533 }
 534
 535 /**
 536  * the DistanceToTarget measures distance to a waypoint
 537  * returns true if closer
 538  * Parameter 0:  distance in meters
 539  * Parameter 1:  flag: 0=2d 1=3d
 540  */
 541 static uint8_t conditionDistanceToTarget()
 542 {
 543     float distance;
 544     PositionStateData positionState;
 545
 546     PositionStateGet(&positionState);
 547     if (pathAction.ConditionParameters[1] > 0.5f) {
 548         distance = sqrtf(powf(waypoint.Position.North - positionState.North, 2)
 549                          + powf(waypoint.Position.East - positionState.East, 2)
 550                          + powf(waypoint.Position.Down - positionState.Down, 2));
 551     } else {
 552         distance = sqrtf(powf(waypoint.Position.North - positionState.North, 2)
 553                          + powf(waypoint.Position.East - positionState.East, 2));
 554     }
 555
 556     if (distance <= pathAction.ConditionParameters[0]) {
 557         return true;
 558     }
 559     return false;
 560 }
 561
 562
 563 /**
 564  * the LegRemaining measures how far the pathfollower got on a linear path segment
 565  * returns true if closer to destination (path more complete)
 566  * Parameter 0:  relative distance (0= complete, 1= just starting)
 567  */
 568 static uint8_t conditionLegRemaining()
 569 {
 570     PathStatusData pathStatus;
 571
 572     PathStatusGet(&pathStatus);
 573
 574     if (pathStatus.fractional_progress >= (1.0f - pathAction.ConditionParameters[0])) {
 575         return true;
 576     }
 577     return false;
 578 }
 579
 580 /**
 581  * the BelowError measures the error on a path segment
 582  * returns true if error is below margin
 583  * Parameter 0: error margin (in m)
 584  */
 585 static uint8_t conditionBelowError()
 586 {
 587     PathDesiredData pathDesired;
 588     PositionStateData positionState;
 589
 590     PathDesiredGet(&pathDesired);
 591     PositionStateGet(&positionState);
 592
 593     float cur[3] = { positionState.North, positionState.East, positionState.Down };
 594     struct path_status progress;
 595
 596     path_progress(&pathDesired,
 597                   cur, &progress, mode3D);
 598     if (progress.error <= pathAction.ConditionParameters[0]) {
 599         return true;
 600     }
 601     return false;
 602 }
 603
 604 /**
 605  * the AboveAltitude measures the flight altitude relative to home position
 606  * returns true if above critical altitude
 607  * Parameter 0:  altitude in meters
 608  */
 609 static uint8_t conditionAboveAltitude()
 610 {
 611     PositionStateData positionState;
 612
 613     PositionStateGet(&positionState);
 614
 615     if (-positionState.Down >= pathAction.ConditionParameters[0]) {
 616         return true;
 617     }
 618     return false;
 619 }
 620
 621 /**
 622  * the AboveSpeed measures the movement speed (3d)
 623  * returns true if above critical speed
 624  * Parameter 0:  speed in m/s
 625  * Parameter 1:  flag: 0=groundspeed 1=airspeed
 626  */
 627 static uint8_t conditionAboveSpeed()
 628 {
 629     VelocityStateData velocityState;
 630
 631     VelocityStateGet(&velocityState);
 632     float velocity = sqrtf(velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down);
 633
 634     // use airspeed if requested and available
 635     if (pathAction.ConditionParameters[1] > 0.5f) {
 636         AirspeedStateData airspeed;
 637         AirspeedStateGet(&airspeed);
 638         velocity = airspeed.CalibratedAirspeed;
 639     }
 640
 641     if (velocity >= pathAction.ConditionParameters[0]) {
 642         return true;
 643     }
 644     return false;
 645 }
 646
 647
 648 /**
 649  * the PointingTowardsNext measures the horizontal movement vector direction relative to the next waypoint
 650  * regardless whether this waypoint will ever be active (Command could jump to another one on true)
 651  * This is useful for curve segments where the craft should stop circling when facing a certain way (usually the next waypoint)
 652  * returns true if within a certain angular margin
 653  * Parameter 0:  degrees variation allowed
 654  */
 655 static uint8_t conditionPointingTowardsNext()
 656 {
 657     uint16_t nextWaypointId = waypointActive.Index + 1;
 658
 659     if (nextWaypointId >= UAVObjGetNumInstances(WaypointHandle())) {
 660         nextWaypointId = 0;
 661     }
 662     WaypointData nextWaypoint;
 663     WaypointInstGet(nextWaypointId, &nextWaypoint);
 664
 665     float angle1 = atan2f((nextWaypoint.Position.North - waypoint.Position.North), (nextWaypoint.Position.East - waypoint.Position.East));
 666
 667     VelocityStateData velocity;
 668     VelocityStateGet(&velocity);
 669     float angle2 = atan2f(velocity.North, velocity.East);
 670
 671     // calculate the absolute angular difference
 672     angle1 = fabsf(RAD2DEG(angle1 - angle2));
 673     while (angle1 > 360) {
 674         angle1 -= 360;
 675     }
 676
 677     if (angle1 <= pathAction.ConditionParameters[0]) {
 678         return true;
 679     }
 680     return false;
 681 }
 682
 683 /**
 684  * the PythonScript is supposed to read the output of a PyMite program running at the same time
 685  * and return based on its output, likely read out through some to be defined UAVObject
 686  * TODO XXX NOT YET IMPLEMENTED
 687  * returns always true until implemented
 688  * Parameter 0-3: defined by user script
 689  */
 690 static uint8_t conditionPythonScript()
 691 {
 692     return true;
 693 }
 694
 695 /* the immediate condition is always true */
 696 static uint8_t conditionImmediate()
 697 {
 698     return true;
 699 }
 700
 701 /**
 702  * @}
 703  * @}
 704  */