rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-18 08:54:15 +01:00
Code Issues Releases Activity

Add path navigation mode to the path planner module. I don't like how this is

Browse Source 
done because the mode is in the settings so can't be command from path planner.
I.e. it would be nice to be able to say do these paths then go to position hold
mode.
This commit is contained in:
James Cotton 2012-04-10 02:51:50 -05:00
parent dfa18eaef1
commit a42d756b3e
1 changed files with 114 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -30,7 +30,11 @@
*/
#include "openpilot.h"
#include "paths.h"
#include "flightstatus.h"
#include "guidancesettings.h"
#include "pathdesired.h"
#include "positionactual.h"
#include "positiondesired.h"
#include "waypoint.h"
@ -48,6 +52,7 @@ static xTaskHandle taskHandle;
static xQueueHandle queue;
static WaypointActiveData waypointActive;
static WaypointData waypoint;
static GuidanceSettingsData guidanceSettings;
// Private functions
static void pathPlannerTask(void *parameters);
@ -96,6 +101,7 @@ static void pathPlannerTask(void *parameters)
WaypointActiveConnectCallback(waypointsUpdated);
FlightStatusData flightStatus;
PathDesiredData pathDesired;
PositionActualData positionActual;
PositionDesiredData positionDesired;
@ -126,31 +132,70 @@ static void pathPlannerTask(void *parameters)
continue;
}
PositionActualGet(&positionActual);
GuidanceSettingsGet(&guidanceSettings);
switch(guidanceSettings.PathMode) {
case GUIDANCESETTINGS_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);
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);
break;
case WAYPOINT_ACTION_RTH:
// Fly back to the home location but 20 m above it
PositionDesiredGet(&positionDesired);
positionDesired.North = 0;
positionDesired.East = 0;
positionDesired.Down = -20;
PositionDesiredSet(&positionDesired);
break;
default:
PIOS_DEBUG_Assert(0);
}
}
// 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
PositionDesiredGet(&positionDesired);
positionDesired.North = 0;
positionDesired.East = 0;
positionDesired.Down = -20;
PositionDesiredSet(&positionDesired);
break;
default:
PIOS_DEBUG_Assert(0);
}
break;
case GUIDANCESETTINGS_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
PositionDesiredGet(&positionDesired);
positionDesired.North = 0;
positionDesired.East = 0;
positionDesired.Down = -20;
PositionDesiredSet(&positionDesired);
break;
default:
PIOS_DEBUG_Assert(0);
}
}
break;
}
}
}
@ -168,13 +213,53 @@ static void waypointsUpdated(UAVObjEvent * ev)
WaypointActiveGet(&waypointActive);
WaypointInstGet(waypointActive.Index, &waypoint);
GuidanceSettingsGet(&guidanceSettings);
PositionDesiredData positionDesired;
PositionDesiredGet(&positionDesired);
positionDesired.North = waypoint.Position[WAYPOINT_POSITION_NORTH];
positionDesired.East = waypoint.Position[WAYPOINT_POSITION_EAST];
positionDesired.Down = waypoint.Position[WAYPOINT_POSITION_DOWN];
PositionDesiredSet(&positionDesired);
switch(guidanceSettings.PathMode) {
case GUIDANCESETTINGS_PATHMODE_ENDPOINT:
{
PositionDesiredData positionDesired;
PositionDesiredGet(&positionDesired);
positionDesired.North = waypoint.Position[WAYPOINT_POSITION_NORTH];
positionDesired.East = waypoint.Position[WAYPOINT_POSITION_EAST];
positionDesired.Down = waypoint.Position[WAYPOINT_POSITION_DOWN];
PositionDesiredSet(&positionDesired);
}
break;
case GUIDANCESETTINGS_PATHMODE_PATH:
{
PathDesiredData pathDesired;
pathDesired.StartingVelocity = 2;
pathDesired.EndingVelocity = 2;
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];
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;
} 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];
}
PathDesiredSet(&pathDesired);
}
break;
}
}
static void createPath()