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

If the waypoint index is changed then update teh position desired. This should

Browse Source 
probably be done with change callbacks because currently changing the waypoints
themselves doesn't update position desired.
This commit is contained in:
James Cotton 2012-03-15 04:33:32 -05:00
parent 6d1c36680f
commit c26cceb47b
1 changed files with 24 additions and 16 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -49,6 +49,7 @@ static xQueueHandle queue;
// Private functions
static void pathPlannerTask(void *parameters);
static void updatePositionDesired();
/**
* Module initialization
@ -126,6 +127,7 @@ static void pathPlannerTask(void *parameters)
// Main thread loop
bool pathplanner_active = false;
uint32_t last_index=0;
while (1)
{
FlightStatusGet(&flightStatus);
@ -142,15 +144,10 @@ static void pathPlannerTask(void *parameters)
waypointActive.Index = 0;
WaypointActiveSet(&waypointActive);
WaypointInstGet(waypointActive.Index, &waypoint);
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);
updatePositionDesired();
pathplanner_active = true;
last_index = waypointActive.Index;
continue;
}
@ -159,6 +156,9 @@ static void pathPlannerTask(void *parameters)
WaypointActiveGet(&waypointActive);
WaypointInstGet(waypointActive.Index, &waypoint);
if(last_index != waypointActive.Index)
updatePositionDesired();
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);
@ -170,16 +170,9 @@ static void pathPlannerTask(void *parameters)
waypointActive.Index++;
WaypointActiveSet(&waypointActive);
if(WaypointInstGet(waypointActive.Index, &waypoint) != 0) {
// Oh shit, tried to go to non-existant waypoint
continue;
}
updatePositionDesired();
last_index = waypointActive.Index;
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 WAYPOINT_ACTION_RTH:
// Fly back to the home location but 20 m above it
@ -196,6 +189,21 @@ static void pathPlannerTask(void *parameters)
}
}
static void updatePositionDesired()
{
PositionDesiredData positionDesired;
WaypointActiveData waypointActive;
WaypointData waypoint;
WaypointInstGet(waypointActive.Index, &waypoint);
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);
}
/**
* @}
* @}