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 to 0 index in the waypoint list
 226     if (pathplanner_active == false) {
 227         pathplanner_active = true;
 228
 229         FlightModeSettingsFlightModeChangeRestartsPathPlanOptions restart;
 230         FlightModeSettingsFlightModeChangeRestartsPathPlanGet(&restart);
 231         if (restart == FLIGHTMODESETTINGS_FLIGHTMODECHANGERESTARTSPATHPLAN_TRUE) {
 232             setWaypoint(0);
 233             return;
 234         }
 235     }
 236
 237     WaypointInstGet(waypointActive.Index, &waypoint);
 238     PathActionInstGet(waypoint.Action, &pathAction);
 239     PathStatusData pathStatus;
 240     PathStatusGet(&pathStatus);
 241
 242     // delay next step until path follower has acknowledged the path mode
 243     if (pathStatus.UID != pathDesired.UID) {
 244         return;
 245     }
 246
 247     // negative destinations DISABLE this feature
 248     if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) {
 249         setWaypoint(pathAction.ErrorDestination);
 250         return;
 251     }
 252
 253
 254     // check if condition has been met
 255     endCondition = pathConditionCheck();
 256     // decide what to do
 257     switch (pathAction.Command) {
 258     case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT:
 259         endCondition = !endCondition;
 260     case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT:
 261         if (endCondition) {
 262             setWaypoint(waypointActive.Index + 1);
 263         }
 264         break;
 265     case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT:
 266         endCondition = !endCondition;
 267     case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT:
 268         if (endCondition) {
 269             if (pathAction.JumpDestination < 0) {
 270                 // waypoint ids <0 code relative jumps
 271                 setWaypoint(waypointActive.Index - pathAction.JumpDestination);
 272             } else {
 273                 setWaypoint(pathAction.JumpDestination);
 274             }
 275         }
 276         break;
 277     case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT:
 278         if (endCondition) {
 279             if (pathAction.JumpDestination < 0) {
 280                 // waypoint ids <0 code relative jumps
 281                 setWaypoint(waypointActive.Index - pathAction.JumpDestination);
 282             } else {
 283                 setWaypoint(pathAction.JumpDestination);
 284             }
 285         } else {
 286             setWaypoint(waypointActive.Index + 1);
 287         }
 288         break;
 289     }
 290 }
 291
 292 // callback function when waypoints changed in any way, update pathDesired
 293 void updatePathDesired()
 294 {
 295     // only ever touch pathDesired if pathplanner is enabled
 296     if (!pathplanner_active) {
 297         return;
 298     }
 299
 300     // find out current waypoint
 301     WaypointActiveGet(&waypointActive);
 302     WaypointInstGet(waypointActive.Index, &waypoint);
 303
 304     PathActionInstGet(waypoint.Action, &pathAction);
 305
 306     PathDesiredData pathDesired;
 307
 308     pathDesired.End.North = waypoint.Position.North;
 309     pathDesired.End.East  = waypoint.Position.East;
 310     pathDesired.End.Down  = waypoint.Position.Down;
 311     pathDesired.EndingVelocity    = waypoint.Velocity;
 312     pathDesired.Mode = pathAction.Mode;
 313     pathDesired.ModeParameters[0] = pathAction.ModeParameters[0];
 314     pathDesired.ModeParameters[1] = pathAction.ModeParameters[1];
 315     pathDesired.ModeParameters[2] = pathAction.ModeParameters[2];
 316     pathDesired.ModeParameters[3] = pathAction.ModeParameters[3];
 317     pathDesired.UID = waypointActive.Index;
 318
 319
 320     if (waypointActive.Index == 0) {
 321         PositionStateData positionState;
 322         PositionStateGet(&positionState);
 323         // First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
 324
 325         /*pathDesired.Start[PATHDESIRED_START_NORTH] =  waypoint.Position[WAYPOINT_POSITION_NORTH];
 326            pathDesired.Start[PATHDESIRED_START_EAST] =  waypoint.Position[WAYPOINT_POSITION_EAST];
 327            pathDesired.Start[PATHDESIRED_START_DOWN] =  waypoint.Position[WAYPOINT_POSITION_DOWN];*/
 328         // note: if certain flightmodes need to override Start, End or mode parameters, that should happen within
 329         // the pathfollower as needed. This also holds if Start is replaced by current position for takeoff and landing
 330         pathDesired.Start.North = positionState.North;
 331         pathDesired.Start.East  = positionState.East;
 332         pathDesired.Start.Down  = positionState.Down;
 333         pathDesired.StartingVelocity = pathDesired.EndingVelocity;
 334     } else {
 335         // Get previous waypoint as start point
 336         WaypointData waypointPrev;
 337         WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
 338
 339         pathDesired.Start.North = waypointPrev.Position.North;
 340         pathDesired.Start.East  = waypointPrev.Position.East;
 341         pathDesired.Start.Down  = waypointPrev.Position.Down;
 342         pathDesired.StartingVelocity = waypointPrev.Velocity;
 343     }
 344
 345     PathDesiredSet(&pathDesired);
 346 }
 347
 348
 349 // safety checks for path plan integrity
 350 static uint8_t checkPathPlan()
 351 {
 352     uint16_t i;
 353     uint16_t waypointCount;
 354     uint16_t actionCount;
 355     uint8_t pathCrc;
 356     PathPlanData pathPlan;
 357
 358     // WaypointData waypoint; // using global instead (?)
 359     // PathActionData action; // using global instead (?)
 360
 361     PathPlanGet(&pathPlan);
 362
 363     waypointCount = pathPlan.WaypointCount;
 364     if (waypointCount == 0) {
 365         // an empty path plan is invalid
 366         return false;
 367     }
 368     actionCount = pathPlan.PathActionCount;
 369
 370     // check count consistency
 371     if (waypointCount > UAVObjGetNumInstances(WaypointHandle())) {
 372         // PIOS_DEBUGLOG_Printf("PathPlan : waypoint count error!");
 373         return false;
 374     }
 375     if (actionCount > UAVObjGetNumInstances(PathActionHandle())) {
 376         // PIOS_DEBUGLOG_Printf("PathPlan : path action count error!");
 377         return false;
 378     }
 379
 380     // check CRC
 381     pathCrc = 0;
 382     for (i = 0; i < waypointCount; i++) {
 383         pathCrc = UAVObjUpdateCRC(WaypointHandle(), i, pathCrc);
 384     }
 385     for (i = 0; i < actionCount; i++) {
 386         pathCrc = UAVObjUpdateCRC(PathActionHandle(), i, pathCrc);
 387     }
 388     if (pathCrc != pathPlan.Crc) {
 389         // failed crc check
 390         // PIOS_DEBUGLOG_Printf("PathPlan : bad CRC (%d / %d)!", pathCrc, pathPlan.Crc);
 391         return false;
 392     }
 393
 394     // waypoint consistency
 395     for (i = 0; i < waypointCount; i++) {
 396         WaypointInstGet(i, &waypoint);
 397         if (waypoint.Action >= actionCount) {
 398             // path action id is out of range
 399             return false;
 400         }
 401     }
 402
 403     // path action consistency
 404     for (i = 0; i < actionCount; i++) {
 405         PathActionInstGet(i, &pathAction);
 406         if (pathAction.ErrorDestination >= waypointCount) {
 407             // waypoint id is out of range
 408             return false;
 409         }
 410         if (pathAction.JumpDestination >= waypointCount) {
 411             // waypoint id is out of range
 412             return false;
 413         }
 414     }
 415
 416     // path plan passed checks
 417
 418     return true;
 419 }
 420
 421 // callback function when status changed, issue execution of state machine
 422 void commandUpdated(__attribute__((unused)) UAVObjEvent *ev)
 423 {
 424     PIOS_CALLBACKSCHEDULER_Dispatch(pathDesiredUpdaterHandle);
 425 }
 426
 427 // callback function when waypoints changed in any way, update pathDesired
 428 void statusUpdated(__attribute__((unused)) UAVObjEvent *ev)
 429 {
 430     PIOS_CALLBACKSCHEDULER_Dispatch(pathPlannerHandle);
 431 }
 432
 433
 434 // helper function to go to a specific waypoint
 435 static void setWaypoint(uint16_t num)
 436 {
 437     PathPlanData pathPlan;
 438
 439     PathPlanGet(&pathPlan);
 440
 441     // here it is assumed that the path plan has been validated (waypoint count is consistent)
 442     if (num >= pathPlan.WaypointCount) {
 443         // path plans wrap around
 444         num = 0;
 445     }
 446
 447     waypointActive.Index = num;
 448     WaypointActiveSet(&waypointActive);
 449 }
 450
 451 // execute the appropriate condition and report result
 452 static uint8_t pathConditionCheck()
 453 {
 454     // i thought about a lookup table, but a switch is safer considering there could be invalid EndCondition ID's
 455     switch (pathAction.EndCondition) {
 456     case PATHACTION_ENDCONDITION_NONE:
 457         return conditionNone();
 458
 459         break;
 460     case PATHACTION_ENDCONDITION_TIMEOUT:
 461         return conditionTimeOut();
 462
 463         break;
 464     case PATHACTION_ENDCONDITION_DISTANCETOTARGET:
 465         return conditionDistanceToTarget();
 466
 467         break;
 468     case PATHACTION_ENDCONDITION_LEGREMAINING:
 469         return conditionLegRemaining();
 470
 471         break;
 472     case PATHACTION_ENDCONDITION_BELOWERROR:
 473         return conditionBelowError();
 474
 475         break;
 476     case PATHACTION_ENDCONDITION_ABOVEALTITUDE:
 477         return conditionAboveAltitude();
 478
 479         break;
 480     case PATHACTION_ENDCONDITION_ABOVESPEED:
 481         return conditionAboveSpeed();
 482
 483         break;
 484     case PATHACTION_ENDCONDITION_POINTINGTOWARDSNEXT:
 485         return conditionPointingTowardsNext();
 486
 487         break;
 488     case PATHACTION_ENDCONDITION_PYTHONSCRIPT:
 489         return conditionPythonScript();
 490
 491         break;
 492     case PATHACTION_ENDCONDITION_IMMEDIATE:
 493         return conditionImmediate();
 494
 495         break;
 496     default:
 497         // invalid endconditions are always true to prevent freezes
 498         return true;
 499
 500         break;
 501     }
 502 }
 503
 504 /* the None condition is always false */
 505 static uint8_t conditionNone()
 506 {
 507     return false;
 508 }
 509
 510 /**
 511  * the Timeout condition measures time this waypoint is active
 512  * Parameter 0:  timeout in seconds
 513  */
 514 static uint8_t conditionTimeOut()
 515 {
 516     static uint16_t toWaypoint;
 517     static uint32_t toStarttime;
 518
 519     // reset timer if waypoint changed
 520     if (waypointActive.Index != toWaypoint) {
 521         toWaypoint  = waypointActive.Index;
 522         toStarttime = PIOS_DELAY_GetRaw();
 523     }
 524     if (PIOS_DELAY_DiffuS(toStarttime) >= 1e6f * pathAction.ConditionParameters[0]) {
 525         // make sure we reinitialize even if the same waypoint comes twice
 526         toWaypoint = 0xFFFF;
 527         return true;
 528     }
 529     return false;
 530 }
 531
 532 /**
 533  * the DistanceToTarget measures distance to a waypoint
 534  * returns true if closer
 535  * Parameter 0:  distance in meters
 536  * Parameter 1:  flag: 0=2d 1=3d
 537  */
 538 static uint8_t conditionDistanceToTarget()
 539 {
 540     float distance;
 541     PositionStateData positionState;
 542
 543     PositionStateGet(&positionState);
 544     if (pathAction.ConditionParameters[1] > 0.5f) {
 545         distance = sqrtf(powf(waypoint.Position.North - positionState.North, 2)
 546                          + powf(waypoint.Position.East - positionState.East, 2)
 547                          + powf(waypoint.Position.Down - positionState.Down, 2));
 548     } else {
 549         distance = sqrtf(powf(waypoint.Position.North - positionState.North, 2)
 550                          + powf(waypoint.Position.East - positionState.East, 2));
 551     }
 552
 553     if (distance <= pathAction.ConditionParameters[0]) {
 554         return true;
 555     }
 556     return false;
 557 }
 558
 559
 560 /**
 561  * the LegRemaining measures how far the pathfollower got on a linear path segment
 562  * returns true if closer to destination (path more complete)
 563  * Parameter 0:  relative distance (0= complete, 1= just starting)
 564  */
 565 static uint8_t conditionLegRemaining()
 566 {
 567     PathStatusData pathStatus;
 568
 569     PathStatusGet(&pathStatus);
 570
 571     if (pathStatus.fractional_progress >= (1.0f - pathAction.ConditionParameters[0])) {
 572         return true;
 573     }
 574     return false;
 575 }
 576
 577 /**
 578  * the BelowError measures the error on a path segment
 579  * returns true if error is below margin
 580  * Parameter 0: error margin (in m)
 581  */
 582 static uint8_t conditionBelowError()
 583 {
 584     PathDesiredData pathDesired;
 585     PositionStateData positionState;
 586
 587     PathDesiredGet(&pathDesired);
 588     PositionStateGet(&positionState);
 589
 590     float cur[3] = { positionState.North, positionState.East, positionState.Down };
 591     struct path_status progress;
 592
 593     path_progress(&pathDesired,
 594                   cur, &progress, mode3D);
 595     if (progress.error <= pathAction.ConditionParameters[0]) {
 596         return true;
 597     }
 598     return false;
 599 }
 600
 601 /**
 602  * the AboveAltitude measures the flight altitude relative to home position
 603  * returns true if above critical altitude
 604  * WARNING! Altitudes are always negative (down coordinate)
 605  * Parameter 0:  altitude in meters (negative!)
 606  */
 607 static uint8_t conditionAboveAltitude()
 608 {
 609     PositionStateData positionState;
 610
 611     PositionStateGet(&positionState);
 612
 613     if (positionState.Down <= pathAction.ConditionParameters[0]) {
 614         return true;
 615     }
 616     return false;
 617 }
 618
 619 /**
 620  * the AboveSpeed measures the movement speed (3d)
 621  * returns true if above critical speed
 622  * Parameter 0:  speed in m/s
 623  * Parameter 1:  flag: 0=groundspeed 1=airspeed
 624  */
 625 static uint8_t conditionAboveSpeed()
 626 {
 627     VelocityStateData velocityState;
 628
 629     VelocityStateGet(&velocityState);
 630     float velocity = sqrtf(velocityState.North * velocityState.North + velocityState.East * velocityState.East + velocityState.Down * velocityState.Down);
 631
 632     // use airspeed if requested and available
 633     if (pathAction.ConditionParameters[1] > 0.5f) {
 634         AirspeedStateData airspeed;
 635         AirspeedStateGet(&airspeed);
 636         velocity = airspeed.CalibratedAirspeed;
 637     }
 638
 639     if (velocity >= pathAction.ConditionParameters[0]) {
 640         return true;
 641     }
 642     return false;
 643 }
 644
 645
 646 /**
 647  * the PointingTowardsNext measures the horizontal movement vector direction relative to the next waypoint
 648  * regardless whether this waypoint will ever be active (Command could jump to another one on true)
 649  * This is useful for curve segments where the craft should stop circling when facing a certain way (usually the next waypoint)
 650  * returns true if within a certain angular margin
 651  * Parameter 0:  degrees variation allowed
 652  */
 653 static uint8_t conditionPointingTowardsNext()
 654 {
 655     uint16_t nextWaypointId = waypointActive.Index + 1;
 656
 657     if (nextWaypointId >= UAVObjGetNumInstances(WaypointHandle())) {
 658         nextWaypointId = 0;
 659     }
 660     WaypointData nextWaypoint;
 661     WaypointInstGet(nextWaypointId, &nextWaypoint);
 662
 663     float angle1 = atan2f((nextWaypoint.Position.North - waypoint.Position.North), (nextWaypoint.Position.East - waypoint.Position.East));
 664
 665     VelocityStateData velocity;
 666     VelocityStateGet(&velocity);
 667     float angle2 = atan2f(velocity.North, velocity.East);
 668
 669     // calculate the absolute angular difference
 670     angle1 = fabsf(RAD2DEG(angle1 - angle2));
 671     while (angle1 > 360) {
 672         angle1 -= 360;
 673     }
 674
 675     if (angle1 <= pathAction.ConditionParameters[0]) {
 676         return true;
 677     }
 678     return false;
 679 }
 680
 681 /**
 682  * the PythonScript is supposed to read the output of a PyMite program running at the same time
 683  * and return based on its output, likely read out through some to be defined UAVObject
 684  * TODO XXX NOT YET IMPLEMENTED
 685  * returns always true until implemented
 686  * Parameter 0-3: defined by user script
 687  */
 688 static uint8_t conditionPythonScript()
 689 {
 690     return true;
 691 }
 692
 693 /* the immediate condition is always true */
 694 static uint8_t conditionImmediate()
 695 {
 696     return true;
 697 }
 698
 699 /**
 700  * @}
 701  * @}
 702  */