From 68fe9380166a180f39072995955f2494d7c26894 Mon Sep 17 00:00:00 2001 From: Corvus Corax Date: Mon, 21 May 2012 21:42:28 +0200 Subject: [PATCH] Implemented new programmable PathPlanner --- flight/Modules/ManualControl/manualcontrol.c | 4 +- flight/Modules/PathPlanner/inc/pathplanner.h | 12 +- flight/Modules/PathPlanner/pathplanner.c | 545 +++++++++++++------ shared/uavobjectdefinition/pathaction.xml | 8 +- 4 files changed, 387 insertions(+), 182 deletions(-) diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 5de382418..39f066f17 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -641,7 +641,7 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool PathDesiredGet(&pathDesired); pathDesired.End[PATHDESIRED_END_NORTH] = 0; pathDesired.End[PATHDESIRED_END_EAST] = 0; - pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; + pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 1; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); } else if(changed) { @@ -653,7 +653,7 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool PathDesiredGet(&pathDesired); pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; - pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; + pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 1; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); } else { diff --git a/flight/Modules/PathPlanner/inc/pathplanner.h b/flight/Modules/PathPlanner/inc/pathplanner.h index dc51cca7f..67fe7acc1 100644 --- a/flight/Modules/PathPlanner/inc/pathplanner.h +++ b/flight/Modules/PathPlanner/inc/pathplanner.h @@ -2,11 +2,11 @@ ****************************************************************************** * @addtogroup OpenPilotModules OpenPilot Modules * @{ - * @addtogroup FlightPlan Flight Plan Module + * @addtogroup PathPlanner Flight Plan Module * @brief Executes flight plan scripts in Python * @{ * - * @file flightplan.c + * @file pathplanner.c * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. * @brief Executes flight plan scripts in Python * @@ -28,9 +28,9 @@ * with this program; if not, write to the Free Software Foundation, Inc., * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ -#ifndef FLIGHTPLAN_H -#define FLIGHTPLAN_H +#ifndef PATHPLANNER_H +#define PATHPLANNER_H -int32_t FlightPlanInitialize(); +// public initialisations go here -#endif // FLIGHTPLAN_H +#endif // PATHPLANNER_H diff --git a/flight/Modules/PathPlanner/pathplanner.c b/flight/Modules/PathPlanner/pathplanner.c index 57f617560..76e6123d0 100644 --- a/flight/Modules/PathPlanner/pathplanner.c +++ b/flight/Modules/PathPlanner/pathplanner.c @@ -33,32 +33,53 @@ #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 +#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; -// Private functions -static void pathPlannerTask(void *parameters); -static void settingsUpdated(UAVObjEvent * ev); -static void waypointsUpdated(UAVObjEvent * ev); -static void createPathBox(); -static void createPathLogo(); /** * Module initialization @@ -82,6 +103,11 @@ int32_t PathPlannerInitialize() taskHandle = NULL; PathPlannerSettingsInitialize(); + PathActionInitialize(); + PathStatusInitialize(); + PathDesiredInitialize(); + PositionActualInitialize(); + VelocityActualInitialize(); WaypointInitialize(); WaypointActiveInitialize(); @@ -97,23 +123,23 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart) * Module task */ int32_t bad_inits; -int32_t bad_reads; static void pathPlannerTask(void *parameters) { + // update settings on change PathPlannerSettingsConnectCallback(settingsUpdated); settingsUpdated(PathPlannerSettingsHandle()); - WaypointConnectCallback(waypointsUpdated); - WaypointActiveConnectCallback(waypointsUpdated); + // when the active waypoint changes, update pathDesired + WaypointConnectCallback(updatePathDesired); + WaypointActiveConnectCallback(updatePathDesired); + PathActionConnectCallback(updatePathDesired); FlightStatusData flightStatus; PathDesiredData pathDesired; - PositionActualData positionActual; + PathStatusData pathStatus; - const float MIN_RADIUS = 4.0f; // Radius to consider at waypoint (m) - // Main thread loop - bool pathplanner_active = false; + bool endCondition = false; while (1) { @@ -125,152 +151,63 @@ static void pathPlannerTask(void *parameters) continue; } + WaypointActiveGet(&waypointActive); + if(pathplanner_active == false) { + + pathplanner_active = true; + // This triggers callback to update variable - WaypointActiveGet(&waypointActive); waypointActive.Index = 0; WaypointActiveSet(&waypointActive); - pathplanner_active = true; continue; } - - switch(pathPlannerSettings.PathMode) { - case PATHPLANNERSETTINGS_PATHMODE_ENDPOINT: - PositionActualGet(&positionActual); - - float r2 = powf(positionActual.North - waypoint.Position[WAYPOINT_POSITION_NORTH], 2) + - powf(positionActual.East - waypoint.Position[WAYPOINT_POSITION_EAST], 2) + - powf(positionActual.Down - waypoint.Position[WAYPOINT_POSITION_DOWN], 2); - - // We hit this waypoint - if (r2 < (MIN_RADIUS * MIN_RADIUS)) { - switch(waypoint.Action) { - case WAYPOINT_ACTION_NEXT: - waypointActive.Index++; - WaypointActiveSet(&waypointActive); - break; - case WAYPOINT_ACTION_RTH: - // Fly back to the home location but 20 m above it - PathDesiredGet(&pathDesired); - pathDesired.End[PATHDESIRED_END_NORTH] = 0; - pathDesired.End[PATHDESIRED_END_EAST] = 0; - pathDesired.End[PATHDESIRED_END_DOWN] = -20; - pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT; - PathDesiredSet(&pathDesired); - break; - default: - PIOS_DEBUG_Assert(0); - } - } + 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 PATHPLANNERSETTINGS_PATHMODE_PATH: - - 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) { - switch(waypoint.Action) { - case WAYPOINT_ACTION_NEXT: - waypointActive.Index++; - WaypointActiveSet(&waypointActive); - - break; - case WAYPOINT_ACTION_RTH: - // Fly back to the home location but 20 m above it - PathDesiredGet(&pathDesired); - pathDesired.End[PATHDESIRED_END_NORTH] = 0; - pathDesired.End[PATHDESIRED_END_EAST] = 0; - pathDesired.End[PATHDESIRED_END_DOWN] = -20; - pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT; - PathDesiredSet(&pathDesired); - break; - default: - PIOS_DEBUG_Assert(0); - } + 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; } } } -/** - * On changed waypoints or active waypoint update position desired - * if we are in charge - */ -static void waypointsUpdated(UAVObjEvent * ev) -{ - FlightStatusData flightStatus; - FlightStatusGet(&flightStatus); - if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) - return; - - WaypointActiveGet(&waypointActive); - WaypointInstGet(waypointActive.Index, &waypoint); - - PathDesiredData pathDesired; - - switch(pathPlannerSettings.PathMode) { - case PATHPLANNERSETTINGS_PATHMODE_ENDPOINT: - { - PathDesiredGet(&pathDesired); - 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.Mode = PATHDESIRED_MODE_ENDPOINT; - PathDesiredSet(&pathDesired); - - } - break; - - case PATHPLANNERSETTINGS_PATHMODE_PATH: - { - PathDesiredData pathDesired; - - 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.Mode = PATHDESIRED_MODE_PATH; - pathDesired.EndingVelocity = sqrtf(powf(waypoint.Velocity[WAYPOINT_VELOCITY_NORTH],2) + - powf(waypoint.Velocity[WAYPOINT_VELOCITY_EAST],2)); - - if(waypointActive.Index == 0) { - // Get current position as start point - PositionActualData positionActual; - PositionActualGet(&positionActual); - - pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; - pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; - pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.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 = sqrtf(powf(waypointPrev.Velocity[WAYPOINT_VELOCITY_NORTH],2) + - powf(waypointPrev.Velocity[WAYPOINT_VELOCITY_EAST],2)); - - } - - PathDesiredSet(&pathDesired); - } - break; - } -} - +// standard settings updated callback void settingsUpdated(UAVObjEvent * ev) { uint8_t preprogrammedPath = pathPlannerSettings.PreprogrammedPath; PathPlannerSettingsGet(&pathPlannerSettings); @@ -289,18 +226,281 @@ void settingsUpdated(UAVObjEvent * ev) { } } +// 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= 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() { - WaypointCreateInstance(); - WaypointCreateInstance(); - WaypointCreateInstance(); - WaypointCreateInstance(); - WaypointCreateInstance(); - + + 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.Velocity[0] = 2; // Since for now this isn't directional just set a mag - waypoint.Action = WAYPOINT_ACTION_NEXT; + waypoint.Action = 0; + waypoint.Velocity = 2; waypoint.Position[0] = 5; waypoint.Position[1] = 5; @@ -328,17 +528,36 @@ static void createPathBox() 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[0] = 2; // Since for now this isn't directional just set a mag + 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; - waypoint.Action = WAYPOINT_ACTION_NEXT; - WaypointCreateInstance(); bad_inits += (WaypointInstSet(i, &waypoint) != 0); } @@ -347,59 +566,43 @@ static void createPathLogo() 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; - waypoint.Action = WAYPOINT_ACTION_NEXT; - WaypointCreateInstance(); bad_inits += (WaypointInstSet(i, &waypoint) != 0); } waypoint.Position[1] = 35; waypoint.Position[0] = -50; waypoint.Position[2] = -50; - waypoint.Action = WAYPOINT_ACTION_NEXT; - WaypointCreateInstance(); WaypointInstSet(35, &waypoint); // Draw Box waypoint.Position[1] = 35; waypoint.Position[0] = -60; waypoint.Position[2] = -30; - waypoint.Action = WAYPOINT_ACTION_NEXT; - WaypointCreateInstance(); WaypointInstSet(36, &waypoint); waypoint.Position[1] = 85; waypoint.Position[0] = -60; waypoint.Position[2] = -30; - waypoint.Action = WAYPOINT_ACTION_NEXT; - WaypointCreateInstance(); WaypointInstSet(37, &waypoint); waypoint.Position[1] = 85; waypoint.Position[0] = 60; waypoint.Position[2] = -30; - waypoint.Action = WAYPOINT_ACTION_NEXT; - WaypointCreateInstance(); WaypointInstSet(38, &waypoint); waypoint.Position[1] = -40; waypoint.Position[0] = 60; waypoint.Position[2] = -30; - waypoint.Action = WAYPOINT_ACTION_NEXT; - WaypointCreateInstance(); WaypointInstSet(39, &waypoint); waypoint.Position[1] = -40; waypoint.Position[0] = -60; waypoint.Position[2] = -30; - waypoint.Action = WAYPOINT_ACTION_NEXT; - WaypointCreateInstance(); WaypointInstSet(40, &waypoint); waypoint.Position[1] = 35; waypoint.Position[0] = -60; waypoint.Position[2] = -30; - waypoint.Action = WAYPOINT_ACTION_NEXT; - WaypointCreateInstance(); WaypointInstSet(41, &waypoint); } diff --git a/shared/uavobjectdefinition/pathaction.xml b/shared/uavobjectdefinition/pathaction.xml index abb95abcc..8261c170c 100644 --- a/shared/uavobjectdefinition/pathaction.xml +++ b/shared/uavobjectdefinition/pathaction.xml @@ -9,14 +9,16 @@ - +