flight/modules/PathPlanner/pathplanner.c

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