rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-01-29 14:52:12 +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 "guidancesettings.h"
#include "pathdesired.h" #include "pathdesired.h"
#include "positionactual.h" #include "positionactual.h"
#include "positiondesired.h"
#include "waypoint.h" #include "waypoint.h"
#include "waypointactive.h" #include "waypointactive.h"
@ -103,7 +102,6 @@ static void pathPlannerTask(void *parameters)
FlightStatusData flightStatus; FlightStatusData flightStatus;
PathDesiredData pathDesired; PathDesiredData pathDesired;
PositionActualData positionActual; PositionActualData positionActual;
PositionDesiredData positionDesired;
createPath(); createPath();
@ -152,11 +150,12 @@ static void pathPlannerTask(void *parameters)
break; break;
case WAYPOINT_ACTION_RTH: case WAYPOINT_ACTION_RTH:
// Fly back to the home location but 20 m above it // Fly back to the home location but 20 m above it
PositionDesiredGet(&positionDesired); PathDesiredGet(&pathDesired);
positionDesired.North = 0; pathDesired.End[PATHDESIRED_END_NORTH] = 0;
positionDesired.East = 0; pathDesired.End[PATHDESIRED_END_EAST] = 0;
positionDesired.Down = -20; pathDesired.End[PATHDESIRED_END_DOWN] = -20;
PositionDesiredSet(&positionDesired); pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
PathDesiredSet(&pathDesired);
break; break;
default: default:
PIOS_DEBUG_Assert(0); PIOS_DEBUG_Assert(0);
@ -184,11 +183,12 @@ static void pathPlannerTask(void *parameters)
break; break;
case WAYPOINT_ACTION_RTH: case WAYPOINT_ACTION_RTH:
// Fly back to the home location but 20 m above it // Fly back to the home location but 20 m above it
PositionDesiredGet(&positionDesired); PathDesiredGet(&pathDesired);
positionDesired.North = 0; pathDesired.End[PATHDESIRED_END_NORTH] = 0;
positionDesired.East = 0; pathDesired.End[PATHDESIRED_END_EAST] = 0;
positionDesired.Down = -20; pathDesired.End[PATHDESIRED_END_DOWN] = -20;
PositionDesiredSet(&positionDesired); pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
PathDesiredSet(&pathDesired);
break; break;
default: default:
PIOS_DEBUG_Assert(0); PIOS_DEBUG_Assert(0);
@ -216,15 +216,18 @@ static void waypointsUpdated(UAVObjEvent * ev)
GuidanceSettingsGet(&guidanceSettings); GuidanceSettingsGet(&guidanceSettings);
PathDesiredData pathDesired;
switch(guidanceSettings.PathMode) { switch(guidanceSettings.PathMode) {
case GUIDANCESETTINGS_PATHMODE_ENDPOINT: case GUIDANCESETTINGS_PATHMODE_ENDPOINT:
{ {
PositionDesiredData positionDesired; PathDesiredGet(&pathDesired);
PositionDesiredGet(&positionDesired); pathDesired.End[PATHDESIRED_END_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
positionDesired.North = waypoint.Position[WAYPOINT_POSITION_NORTH]; pathDesired.End[PATHDESIRED_END_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
positionDesired.East = waypoint.Position[WAYPOINT_POSITION_EAST]; pathDesired.End[PATHDESIRED_END_DOWN] = -waypoint.Position[WAYPOINT_POSITION_DOWN];
positionDesired.Down = waypoint.Position[WAYPOINT_POSITION_DOWN]; pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
PositionDesiredSet(&positionDesired); PathDesiredSet(&pathDesired);
} }
break; break;
@ -235,6 +238,7 @@ static void waypointsUpdated(UAVObjEvent * ev)
pathDesired.End[PATHDESIRED_END_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH]; pathDesired.End[PATHDESIRED_END_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
pathDesired.End[PATHDESIRED_END_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST]; pathDesired.End[PATHDESIRED_END_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
pathDesired.End[PATHDESIRED_END_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN]; 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) + pathDesired.EndingVelocity = sqrtf(powf(waypoint.Velocity[WAYPOINT_VELOCITY_NORTH],2) +
powf(waypoint.Velocity[WAYPOINT_VELOCITY_EAST],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; SystemSettingsData systemSettings;
FlightStatusData flightStatus; FlightStatusData flightStatus;
PathDesiredData pathDesired;
portTickType lastUpdateTime; portTickType lastUpdateTime;

2
shared/uavobjectdefinition/positionactual.xml
View File

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