mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-11 19:24:10 +01:00
614 lines
17 KiB
C
614 lines
17 KiB
C
/**
|
|
******************************************************************************
|
|
* @addtogroup OpenPilotModules OpenPilot Modules
|
|
* @{
|
|
* @addtogroup PathPlanner Path Planner Module
|
|
* @brief Executes a series of waypoints
|
|
* @{
|
|
*
|
|
* @file pathplanner.c
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
|
* @brief Executes a series of waypoints
|
|
*
|
|
* @see The GNU Public License (GPL) Version 3
|
|
*
|
|
*****************************************************************************/
|
|
/*
|
|
* This program is free software; you can redistribute it and/or modify
|
|
* it under the terms of the GNU General Public License as published by
|
|
* the Free Software Foundation; either version 3 of the License, or
|
|
* (at your option) any later version.
|
|
*
|
|
* This program is distributed in the hope that it will be useful, but
|
|
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
|
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
|
* for more details.
|
|
*
|
|
* You should have received a copy of the GNU General Public License along
|
|
* with this program; if not, write to the Free Software Foundation, Inc.,
|
|
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
|
*/
|
|
|
|
#include "openpilot.h"
|
|
#include "paths.h"
|
|
|
|
#include "flightstatus.h"
|
|
#include "pathaction.h"
|
|
#include "pathdesired.h"
|
|
#include "pathstatus.h"
|
|
#include "pathplannersettings.h"
|
|
#include "positionactual.h"
|
|
#include "velocityactual.h"
|
|
#include "waypoint.h"
|
|
#include "waypointactive.h"
|
|
|
|
// Private constants
|
|
#define STACK_SIZE_BYTES 1024 // TODO profile and reduce!
|
|
#define TASK_PRIORITY (tskIDLE_PRIORITY+1)
|
|
#define MAX_QUEUE_SIZE 2
|
|
#define F_PI 3.141526535897932f
|
|
#define RAD2DEG (180.0f/F_PI)
|
|
|
|
// Private types
|
|
|
|
// Private functions
|
|
static void pathPlannerTask(void *parameters);
|
|
static void settingsUpdated(UAVObjEvent * ev);
|
|
static void updatePathDesired(UAVObjEvent * ev);
|
|
static void setWaypoint(uint16_t num);
|
|
static void WaypointCreateInstances(uint16_t num);
|
|
static void createPathBox();
|
|
static void createPathLogo();
|
|
|
|
static uint8_t pathConditionCheck();
|
|
static uint8_t conditionNone();
|
|
static uint8_t conditionTimeOut();
|
|
static uint8_t conditionDistanceToTarget();
|
|
static uint8_t conditionLegRemaining();
|
|
static uint8_t conditionAboveAltitude();
|
|
static uint8_t conditionPointingTowardsNext();
|
|
static uint8_t conditionPythonScript();
|
|
static uint8_t conditionImmediate();
|
|
|
|
|
|
// Private variables
|
|
static xTaskHandle taskHandle;
|
|
static xQueueHandle queue;
|
|
static PathPlannerSettingsData pathPlannerSettings;
|
|
static WaypointActiveData waypointActive;
|
|
static WaypointData waypoint;
|
|
static PathActionData pathAction;
|
|
static bool pathplanner_active = false;
|
|
|
|
|
|
/**
|
|
* Module initialization
|
|
*/
|
|
int32_t PathPlannerStart()
|
|
{
|
|
taskHandle = NULL;
|
|
|
|
// Start VM thread
|
|
xTaskCreate(pathPlannerTask, (signed char *)"PathPlanner", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &taskHandle);
|
|
TaskMonitorAdd(TASKINFO_RUNNING_PATHPLANNER, taskHandle);
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Module initialization
|
|
*/
|
|
int32_t PathPlannerInitialize()
|
|
{
|
|
taskHandle = NULL;
|
|
|
|
PathPlannerSettingsInitialize();
|
|
PathActionInitialize();
|
|
PathStatusInitialize();
|
|
PathDesiredInitialize();
|
|
PositionActualInitialize();
|
|
VelocityActualInitialize();
|
|
WaypointInitialize();
|
|
WaypointActiveInitialize();
|
|
|
|
// Create object queue
|
|
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
|
|
|
|
return 0;
|
|
}
|
|
|
|
MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart)
|
|
|
|
/**
|
|
* Module task
|
|
*/
|
|
int32_t bad_inits;
|
|
static void pathPlannerTask(void *parameters)
|
|
{
|
|
// update settings on change
|
|
PathPlannerSettingsConnectCallback(settingsUpdated);
|
|
settingsUpdated(PathPlannerSettingsHandle());
|
|
|
|
// when the active waypoint changes, update pathDesired
|
|
WaypointConnectCallback(updatePathDesired);
|
|
WaypointActiveConnectCallback(updatePathDesired);
|
|
PathActionConnectCallback(updatePathDesired);
|
|
|
|
FlightStatusData flightStatus;
|
|
PathDesiredData pathDesired;
|
|
PathStatusData pathStatus;
|
|
|
|
// Main thread loop
|
|
bool endCondition = false;
|
|
while (1)
|
|
{
|
|
|
|
vTaskDelay(20);
|
|
|
|
FlightStatusGet(&flightStatus);
|
|
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
|
|
pathplanner_active = false;
|
|
continue;
|
|
}
|
|
|
|
WaypointActiveGet(&waypointActive);
|
|
|
|
if(pathplanner_active == false) {
|
|
|
|
pathplanner_active = true;
|
|
|
|
// This triggers callback to update variable
|
|
waypointActive.Index = 0;
|
|
WaypointActiveSet(&waypointActive);
|
|
|
|
continue;
|
|
}
|
|
|
|
WaypointInstGet(waypointActive.Index,&waypoint);
|
|
PathActionInstGet(waypoint.Action, &pathAction);
|
|
PathStatusGet(&pathStatus);
|
|
PathDesiredGet(&pathDesired);
|
|
|
|
// delay next step until path follower has acknowledged the path mode
|
|
if (pathStatus.UID != pathDesired.UID) {
|
|
continue;
|
|
}
|
|
|
|
if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL) {
|
|
setWaypoint(pathAction.ErrorDestination);
|
|
continue;
|
|
}
|
|
|
|
// check if condition has been met
|
|
endCondition = pathConditionCheck();
|
|
|
|
// decide what to do
|
|
switch (pathAction.Command) {
|
|
case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT:
|
|
if (endCondition) setWaypoint(waypointActive.Index+1);
|
|
break;
|
|
case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT:
|
|
if (!endCondition) setWaypoint(waypointActive.Index+1);
|
|
break;
|
|
case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT:
|
|
if (endCondition) setWaypoint(pathAction.JumpDestination);
|
|
break;
|
|
case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT:
|
|
if (!endCondition) setWaypoint(pathAction.JumpDestination);
|
|
break;
|
|
case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT:
|
|
if (endCondition) {
|
|
setWaypoint(pathAction.JumpDestination);
|
|
} else {
|
|
setWaypoint(waypointActive.Index+1);
|
|
}
|
|
break;
|
|
}
|
|
}
|
|
}
|
|
|
|
// standard settings updated callback
|
|
void settingsUpdated(UAVObjEvent * ev) {
|
|
uint8_t preprogrammedPath = pathPlannerSettings.PreprogrammedPath;
|
|
PathPlannerSettingsGet(&pathPlannerSettings);
|
|
if (pathPlannerSettings.PreprogrammedPath != preprogrammedPath) {
|
|
switch(pathPlannerSettings.PreprogrammedPath) {
|
|
case PATHPLANNERSETTINGS_PREPROGRAMMEDPATH_NONE:
|
|
break;
|
|
case PATHPLANNERSETTINGS_PREPROGRAMMEDPATH_10M_BOX:
|
|
createPathBox();
|
|
break;
|
|
case PATHPLANNERSETTINGS_PREPROGRAMMEDPATH_LOGO:
|
|
createPathLogo();
|
|
break;
|
|
|
|
}
|
|
}
|
|
}
|
|
|
|
// callback function when waypoints changed in any way, update pathDesired
|
|
void updatePathDesired(UAVObjEvent * ev) {
|
|
|
|
// only ever touch pathDesired if pathplanner is enabled
|
|
if (!pathplanner_active) return;
|
|
|
|
// use local variables, dont use stack since this is huge and a callback,
|
|
// dont use the globals because we cant use mutexes here
|
|
static WaypointActiveData waypointActive;
|
|
static PathActionData pathAction;
|
|
static WaypointData waypoint;
|
|
static PathDesiredData pathDesired;
|
|
|
|
// find out current waypoint
|
|
WaypointActiveGet(&waypointActive);
|
|
|
|
WaypointInstGet(waypointActive.Index,&waypoint);
|
|
PathActionInstGet(waypoint.Action, &pathAction);
|
|
|
|
pathDesired.End[PATHDESIRED_END_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
|
|
pathDesired.End[PATHDESIRED_END_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
|
|
pathDesired.End[PATHDESIRED_END_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];
|
|
pathDesired.EndingVelocity = waypoint.Velocity;
|
|
pathDesired.Mode = pathAction.Mode;
|
|
pathDesired.ModeParameters[0] = pathAction.ModeParameters[0];
|
|
pathDesired.ModeParameters[1] = pathAction.ModeParameters[1];
|
|
pathDesired.ModeParameters[2] = pathAction.ModeParameters[2];
|
|
pathDesired.ModeParameters[3] = pathAction.ModeParameters[3];
|
|
pathDesired.UID = waypointActive.Index;
|
|
|
|
if(waypointActive.Index == 0) {
|
|
// Get home position as start point
|
|
|
|
pathDesired.Start[PATHDESIRED_START_NORTH] = 0;
|
|
pathDesired.Start[PATHDESIRED_START_EAST] = 0;
|
|
pathDesired.Start[PATHDESIRED_START_DOWN] = -1;
|
|
pathDesired.StartingVelocity = pathDesired.EndingVelocity;
|
|
} else {
|
|
// Get previous waypoint as start point
|
|
WaypointData waypointPrev;
|
|
WaypointInstGet(waypointActive.Index - 1, &waypointPrev);
|
|
|
|
pathDesired.Start[PATHDESIRED_END_NORTH] = waypointPrev.Position[WAYPOINT_POSITION_NORTH];
|
|
pathDesired.Start[PATHDESIRED_END_EAST] = waypointPrev.Position[WAYPOINT_POSITION_EAST];
|
|
pathDesired.Start[PATHDESIRED_END_DOWN] = waypointPrev.Position[WAYPOINT_POSITION_DOWN];
|
|
pathDesired.StartingVelocity = waypointPrev.Velocity;
|
|
}
|
|
PathDesiredSet(&pathDesired);
|
|
|
|
}
|
|
|
|
// helper function to go to a specific waypoint
|
|
static void setWaypoint(uint16_t num) {
|
|
// path plans wrap around
|
|
if (num>=UAVObjGetNumInstances(WaypointHandle())) {
|
|
num = 0;
|
|
}
|
|
|
|
waypointActive.Index = num;
|
|
WaypointActiveSet(&waypointActive);
|
|
|
|
}
|
|
|
|
// helper function to make sure there are enough waypoint instances
|
|
static void WaypointCreateInstances(uint16_t num) {
|
|
|
|
uint16_t t;
|
|
for (t=UAVObjGetNumInstances(WaypointHandle());t<num;t++) {
|
|
WaypointCreateInstance();
|
|
}
|
|
|
|
|
|
}
|
|
|
|
// execute the apropriate condition and report result
|
|
static uint8_t pathConditionCheck() {
|
|
// i thought about a lookup table, but a switch is saver considering there could be invalid EndCondition ID's
|
|
switch (pathAction.EndCondition) {
|
|
case PATHACTION_ENDCONDITION_NONE:
|
|
return conditionNone();
|
|
break;
|
|
case PATHACTION_ENDCONDITION_TIMEOUT:
|
|
return conditionTimeOut();
|
|
break;
|
|
case PATHACTION_ENDCONDITION_DISTANCETOTARGET:
|
|
return conditionDistanceToTarget();
|
|
break;
|
|
case PATHACTION_ENDCONDITION_LEGREMAINING:
|
|
return conditionLegRemaining();
|
|
break;
|
|
case PATHACTION_ENDCONDITION_ABOVEALTITUDE:
|
|
return conditionAboveAltitude();
|
|
break;
|
|
case PATHACTION_ENDCONDITION_POINTINGTOWARDSNEXT:
|
|
return conditionPointingTowardsNext();
|
|
break;
|
|
case PATHACTION_ENDCONDITION_PYTHONSCRIPT:
|
|
return conditionPythonScript();
|
|
break;
|
|
case PATHACTION_ENDCONDITION_IMMEDIATE:
|
|
return conditionImmediate();
|
|
break;
|
|
default:
|
|
// invalid endconditions are always true to prevent freezes
|
|
return true;
|
|
break;
|
|
}
|
|
}
|
|
|
|
/* the None condition is always false */
|
|
static uint8_t conditionNone() {
|
|
return false;
|
|
}
|
|
|
|
/**
|
|
* the Timeout condition measures time this waypoint is active
|
|
* Parameter 0: timeout in seconds
|
|
*/
|
|
static uint8_t conditionTimeOut() {
|
|
static uint16_t toWaypoint;
|
|
static uint32_t toStarttime;
|
|
|
|
// reset timer if waypoint changed
|
|
if (waypointActive.Index!=toWaypoint) {
|
|
toWaypoint = waypointActive.Index;
|
|
toStarttime = PIOS_DELAY_GetRaw();
|
|
}
|
|
if (PIOS_DELAY_DiffuS(toStarttime) >= 1e6 * pathAction.ConditionParameters[0]) {
|
|
// make sure we reinitialize even if the same waypoint comes twice
|
|
toWaypoint = 0xFFFF;
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
/**
|
|
* the DistanceToTarget measures distance to a waypoint
|
|
* returns true if closer
|
|
* Parameter 0: distance in meters
|
|
* Parameter 1: flag: 0=2d 1=3d
|
|
*/
|
|
static uint8_t conditionDistanceToTarget() {
|
|
float distance;
|
|
PositionActualData positionActual;
|
|
|
|
PositionActualGet(&positionActual);
|
|
if (pathAction.ConditionParameters[1]>0.5f) {
|
|
distance=sqrtf(powf( waypoint.Position[0]-positionActual.North ,2)
|
|
+powf( waypoint.Position[1]-positionActual.East ,2)
|
|
+powf( waypoint.Position[1]-positionActual.Down ,2));
|
|
} else {
|
|
distance=sqrtf(powf( waypoint.Position[0]-positionActual.North ,2)
|
|
+powf( waypoint.Position[1]-positionActual.East ,2));
|
|
}
|
|
|
|
if (distance>=pathAction.ConditionParameters[0]) {
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
|
|
/**
|
|
* the LegRemaining measures how far the pathfollower got on a linear path segment
|
|
* returns true if closer to destination (path more complete)
|
|
* Parameter 0: relative distance (0= complete, 1= just starting)
|
|
*/
|
|
static uint8_t conditionLegRemaining() {
|
|
|
|
PathDesiredData pathDesired;
|
|
PositionActualData positionActual;
|
|
PathDesiredGet(&pathDesired);
|
|
PositionActualGet(&positionActual);
|
|
|
|
float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
|
|
struct path_status progress;
|
|
|
|
path_progress(pathDesired.Start, pathDesired.End, cur, &progress);
|
|
if ( progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0] ) {
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
/**
|
|
* the AboveAltitude measures the flight altitude relative to home position
|
|
* returns true if above critical altitude
|
|
* WARNING! Altitudes are always negative (down coordinate)
|
|
* Parameter 0: altitude in meters (negative!)
|
|
*/
|
|
static uint8_t conditionAboveAltitude() {
|
|
PositionActualData positionActual;
|
|
PositionActualGet(&positionActual);
|
|
|
|
if (positionActual.Down <= pathAction.ConditionParameters[0]) {
|
|
return true;
|
|
}
|
|
return false;
|
|
}
|
|
|
|
/**
|
|
* the PointingTowardsNext measures the horizontal movement vector direction relative to the next waypoint
|
|
* regardless whether this waypoint will ever be active (Command could jump to another one on true)
|
|
* This is useful for curve segments where the craft should stop circling when facing a certain way (usually the next waypoint)
|
|
* returns true if within a certain angular margin
|
|
* Parameter 0: degrees variation allowed
|
|
*/
|
|
static uint8_t conditionPointingTowardsNext() {
|
|
uint16_t nextWaypointId = waypointActive.Index+1;
|
|
if (nextWaypointId>=UAVObjGetNumInstances(WaypointHandle())) {
|
|
nextWaypointId = 0;
|
|
}
|
|
WaypointData nextWaypoint;
|
|
WaypointInstGet(nextWaypointId,&nextWaypoint);
|
|
|
|
float angle1 = atan2f((nextWaypoint.Position[0]-waypoint.Position[0]),(nextWaypoint.Position[1]-waypoint.Position[1]));
|
|
|
|
VelocityActualData velocity;
|
|
VelocityActualGet (&velocity);
|
|
float angle2 = atan2f(velocity.North,velocity.East);
|
|
|
|
// calculate the absolute angular difference
|
|
angle1 = fabsf(RAD2DEG * (angle1 - angle2));
|
|
while (angle1>360) angle1-=360;
|
|
|
|
if (angle1 <= pathAction.ConditionParameters[0]) {
|
|
return true;
|
|
}
|
|
return false;
|
|
|
|
}
|
|
|
|
/**
|
|
* the PythonScript is supposed to read the output of a PyMite program running at the same time
|
|
* and return based on its output, likely read out through some to be defined UAVObject
|
|
* TODO XXX NOT YET IMPLEMENTED
|
|
* returns always true until implemented
|
|
* Parameter 0-3: defined by user script
|
|
*/
|
|
static uint8_t conditionPythonScript() {
|
|
return true;
|
|
}
|
|
|
|
/* the immediate condition is always true */
|
|
static uint8_t conditionImmediate() {
|
|
return true;
|
|
}
|
|
|
|
// demo path - box
|
|
static void createPathBox()
|
|
{
|
|
|
|
PathActionData action;
|
|
PathActionInstGet(0,&action);
|
|
action.Mode = PATHACTION_MODE_FLYVECTOR;
|
|
action.ModeParameters[0]=0;
|
|
action.ModeParameters[1]=0;
|
|
action.ModeParameters[2]=0;
|
|
action.ModeParameters[3]=0;
|
|
action.EndCondition = PATHACTION_ENDCONDITION_LEGREMAINING;
|
|
action.ConditionParameters[0] = 0;
|
|
action.ConditionParameters[1] = 0;
|
|
action.ConditionParameters[2] = 0;
|
|
action.ConditionParameters[3] = 0;
|
|
action.Command = PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT;
|
|
action.JumpDestination = 0;
|
|
action.ErrorDestination = 0;
|
|
PathActionInstSet(0,&action);
|
|
|
|
WaypointCreateInstances(6);
|
|
|
|
// Draw O
|
|
WaypointData waypoint;
|
|
waypoint.Action = 0;
|
|
waypoint.Velocity = 2;
|
|
|
|
waypoint.Position[0] = 5;
|
|
waypoint.Position[1] = 5;
|
|
waypoint.Position[2] = -10;
|
|
WaypointInstSet(0, &waypoint);
|
|
|
|
waypoint.Position[0] = -5;
|
|
waypoint.Position[1] = 5;
|
|
WaypointInstSet(1, &waypoint);
|
|
|
|
waypoint.Position[0] = -5;
|
|
waypoint.Position[1] = -5;
|
|
WaypointInstSet(2, &waypoint);
|
|
|
|
waypoint.Position[0] = 5;
|
|
waypoint.Position[1] = -5;
|
|
WaypointInstSet(3, &waypoint);
|
|
|
|
waypoint.Position[0] = 5;
|
|
waypoint.Position[1] = 5;
|
|
WaypointInstSet(4, &waypoint);
|
|
|
|
waypoint.Position[0] = 0;
|
|
waypoint.Position[1] = 0;
|
|
WaypointInstSet(5, &waypoint);
|
|
}
|
|
|
|
// demo path - logo
|
|
static void createPathLogo()
|
|
{
|
|
PathActionData action;
|
|
PathActionInstGet(0,&action);
|
|
action.Mode = PATHACTION_MODE_FLYVECTOR;
|
|
action.ModeParameters[0]=0;
|
|
action.ModeParameters[1]=0;
|
|
action.ModeParameters[2]=0;
|
|
action.ModeParameters[3]=0;
|
|
action.EndCondition = PATHACTION_ENDCONDITION_LEGREMAINING;
|
|
action.ConditionParameters[0] = 0;
|
|
action.ConditionParameters[1] = 0;
|
|
action.ConditionParameters[2] = 0;
|
|
action.ConditionParameters[3] = 0;
|
|
action.Command = PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT;
|
|
action.JumpDestination = 0;
|
|
action.ErrorDestination = 0;
|
|
PathActionInstSet(0,&action);
|
|
|
|
WaypointCreateInstances(42);
|
|
|
|
// Draw O
|
|
WaypointData waypoint;
|
|
waypoint.Velocity = 2; // Since for now this isn't directional just set a mag
|
|
waypoint.Action = 0;
|
|
for(uint32_t i = 0; i < 20; i++) {
|
|
waypoint.Position[1] = 30 * cos(i / 19.0 * 2 * M_PI);
|
|
waypoint.Position[0] = 50 * sin(i / 19.0 * 2 * M_PI);
|
|
waypoint.Position[2] = -50;
|
|
bad_inits += (WaypointInstSet(i, &waypoint) != 0);
|
|
}
|
|
|
|
// Draw P
|
|
for(uint32_t i = 20; i < 35; i++) {
|
|
waypoint.Position[1] = 55 + 20 * cos(i / 10.0 * M_PI - M_PI / 2);
|
|
waypoint.Position[0] = 25 + 25 * sin(i / 10.0 * M_PI - M_PI / 2);
|
|
waypoint.Position[2] = -50;
|
|
bad_inits += (WaypointInstSet(i, &waypoint) != 0);
|
|
}
|
|
|
|
waypoint.Position[1] = 35;
|
|
waypoint.Position[0] = -50;
|
|
waypoint.Position[2] = -50;
|
|
WaypointInstSet(35, &waypoint);
|
|
|
|
// Draw Box
|
|
waypoint.Position[1] = 35;
|
|
waypoint.Position[0] = -60;
|
|
waypoint.Position[2] = -30;
|
|
WaypointInstSet(36, &waypoint);
|
|
|
|
waypoint.Position[1] = 85;
|
|
waypoint.Position[0] = -60;
|
|
waypoint.Position[2] = -30;
|
|
WaypointInstSet(37, &waypoint);
|
|
|
|
waypoint.Position[1] = 85;
|
|
waypoint.Position[0] = 60;
|
|
waypoint.Position[2] = -30;
|
|
WaypointInstSet(38, &waypoint);
|
|
|
|
waypoint.Position[1] = -40;
|
|
waypoint.Position[0] = 60;
|
|
waypoint.Position[2] = -30;
|
|
WaypointInstSet(39, &waypoint);
|
|
|
|
waypoint.Position[1] = -40;
|
|
waypoint.Position[0] = -60;
|
|
waypoint.Position[2] = -30;
|
|
WaypointInstSet(40, &waypoint);
|
|
|
|
waypoint.Position[1] = 35;
|
|
waypoint.Position[0] = -60;
|
|
waypoint.Position[2] = -30;
|
|
WaypointInstSet(41, &waypoint);
|
|
|
|
}
|
|
|
|
/**
|
|
* @}
|
|
* @}
|
|
*/
|