rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-17 02:52:12 +01:00
Code Issues Releases Activity

Implemented new programmable PathPlanner

Browse Source
This commit is contained in:
Corvus Corax 2012-05-21 21:42:28 +02:00
parent 992e189bcd
commit 68fe938016
4 changed files with 387 additions and 182 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

4
flight/Modules/ManualControl/manualcontrol.c
View File

@ -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 {

12
flight/Modules/PathPlanner/inc/pathplanner.h
View File

@ -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

545
flight/Modules/PathPlanner/pathplanner.c
View File

@ -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<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()
{
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);
}

8
shared/uavobjectdefinition/pathaction.xml
View File

@ -9,14 +9,16 @@
<field name="ModeParameters" units="" type="float" elements="4" default="0"/>
<field name="EndCondition" units="" type="enum" elements="1" options="None,TimeOut,
DistanceToTarget,LegRemaining,PointingTowardsNext,
AboveAltitude,BelowAltitude,
DistanceToTarget,LegRemaining,
AboveAltitude,
PointingTowardsNext,
PythonScript,
Immediate" default="None" />
<field name="ConditionParameters" units="" type="float" elements="4" default="0"/>
<field name="Command" units="" type="enum" elements="1" options="OnConditionNextWaypoint,OnConditionJumpWaypoint,IfConditionJumpWaypointElseNextWaypoint" default="OnConditionNextWaypoint" />
<field name="Command" units="" type="enum" elements="1" options="OnConditionNextWaypoint,OnNotConditionNextWaypoint,
OnConditionJumpWaypoint,OnNotConditionJumpWaypoint,
IfConditionJumpWaypointElseNextWaypoint" default="OnConditionNextWaypoint" />
<field name="JumpDestination" units="waypoint" type="uint8" elements="1" default="0"/>
<field name="ErrorDestination" units="waypoint" type="uint8" elements="1" default="0"/>