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

Migrate path planner from position desired to path desired

Browse Source
This commit is contained in:
James Cotton 2012-05-08 03:04:44 -05:00
parent 37bb2cfb77
commit 2857b58f8e
3 changed files with 23 additions and 20 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -36,7 +36,6 @@
#include "guidancesettings.h"
#include "pathdesired.h"
#include "positionactual.h"
#include "positiondesired.h"
#include "waypoint.h"
#include "waypointactive.h"
@ -103,7 +102,6 @@ static void pathPlannerTask(void *parameters)
FlightStatusData flightStatus;
PathDesiredData pathDesired;
PositionActualData positionActual;
PositionDesiredData positionDesired;
createPath();
@ -152,11 +150,12 @@ static void pathPlannerTask(void *parameters)
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);
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);
@ -184,11 +183,12 @@ static void pathPlannerTask(void *parameters)
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);
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);
@ -216,15 +216,18 @@ static void waypointsUpdated(UAVObjEvent * ev)
GuidanceSettingsGet(&guidanceSettings);
PathDesiredData pathDesired;
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);
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;
@ -235,6 +238,7 @@ static void waypointsUpdated(UAVObjEvent * ev)
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));

1
flight/Modules/VtolPathFollower/vtolpathfollower.c
View File

@ -132,7 +132,6 @@ static void vtolPathFollowerTask(void *parameters)
{
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
PathDesiredData pathDesired;
portTickType lastUpdateTime;

2
shared/uavobjectdefinition/positionactual.xml
View File

@ -5,7 +5,7 @@
<field name="East" units="m" type="float" elements="1"/>
<field name="Down" units="m" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetrygcs acked="false" updatemode="periodic" period="1000"/>
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
<logging updatemode="periodic" period="1000"/>
</object>