1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-04 12:24:11 +01:00
LibrePilot/flight/modules/PathPlanner/pathplanner.c

504 lines
15 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 <pios_math.h>
#include "paths.h"
#include "flightstatus.h"
#include "airspeedactual.h"
#include "pathaction.h"
#include "pathdesired.h"
#include "pathstatus.h"
#include "positionactual.h"
#include "velocityactual.h"
#include "waypoint.h"
#include "waypointactive.h"
// Private constants
#define STACK_SIZE_BYTES 1024
#define TASK_PRIORITY (tskIDLE_PRIORITY+1)
#define MAX_QUEUE_SIZE 2
#define PATH_PLANNER_UPDATE_RATE_MS 20
// Private types
// Private functions
static void pathPlannerTask(void *parameters);
static void updatePathDesired(UAVObjEvent * ev);
static void setWaypoint(uint16_t num);
static uint8_t pathConditionCheck();
static uint8_t conditionNone();
static uint8_t conditionTimeOut();
static uint8_t conditionDistanceToTarget();
static uint8_t conditionLegRemaining();
static uint8_t conditionBelowError();
static uint8_t conditionAboveAltitude();
static uint8_t conditionAboveSpeed();
static uint8_t conditionPointingTowardsNext();
static uint8_t conditionPythonScript();
static uint8_t conditionImmediate();
// Private variables
static xTaskHandle taskHandle;
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;
PathActionInitialize();
PathStatusInitialize();
PathDesiredInitialize();
PositionActualInitialize();
AirspeedActualInitialize();
VelocityActualInitialize();
WaypointInitialize();
WaypointActiveInitialize();
return 0;
}
MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart)
/**
* Module task
*/
static void pathPlannerTask(void *parameters)
{
// 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(PATH_PLANNER_UPDATE_RATE_MS);
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;
}
// negative destinations DISABLE this feature
if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) {
setWaypoint(pathAction.ErrorDestination);
continue;
}
// check if condition has been met
endCondition = pathConditionCheck();
// decide what to do
switch (pathAction.Command) {
case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT:
endCondition = !endCondition;
case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT:
if (endCondition) setWaypoint(waypointActive.Index+1);
break;
case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT:
endCondition = !endCondition;
case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT:
if (endCondition) {
if (pathAction.JumpDestination<0) {
// waypoint ids <0 code relative jumps
setWaypoint(waypointActive.Index - pathAction.JumpDestination );
} else {
setWaypoint(pathAction.JumpDestination);
}
}
break;
case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT:
if (endCondition) {
if (pathAction.JumpDestination<0) {
// waypoint ids <0 code relative jumps
setWaypoint(waypointActive.Index - pathAction.JumpDestination );
} else {
setWaypoint(pathAction.JumpDestination);
}
} else {
setWaypoint(waypointActive.Index+1);
}
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) {
// First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];
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);
}
// 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_BELOWERROR:
return conditionBelowError();
break;
case PATHACTION_ENDCONDITION_ABOVEALTITUDE:
return conditionAboveAltitude();
break;
case PATHACTION_ENDCONDITION_ABOVESPEED:
return conditionAboveSpeed();
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) >= 1e6f * 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, pathDesired.Mode);
if ( progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0] ) {
return true;
}
return false;
}
/**
* the BelowError measures the error on a path segment
* returns true if error is below margin
* Parameter 0: error margin (in m)
*/
static uint8_t conditionBelowError() {
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, pathDesired.Mode);
if ( progress.error <= 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 AboveSpeed measures the movement speed (3d)
* returns true if above critical speed
* Parameter 0: speed in m/s
* Parameter 1: flag: 0=groundspeed 1=airspeed
*/
static uint8_t conditionAboveSpeed() {
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
float velocity = sqrtf( velocityActual.North*velocityActual.North + velocityActual.East*velocityActual.East + velocityActual.Down*velocityActual.Down );
// use airspeed if requested and available
if (pathAction.ConditionParameters[1]>0.5f) {
AirspeedActualData airspeed;
AirspeedActualGet (&airspeed);
velocity = airspeed.CalibratedAirspeed;
}
if ( velocity >= 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;
}
/**
* @}
* @}
*/