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

Only update position desired when you change waypoints active

Browse Source
This commit is contained in:
James Cotton 2012-03-15 04:24:39 -05:00
parent d93ee95a4a
commit 32f76db8ae
1 changed files with 29 additions and 8 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -125,18 +125,39 @@ static void pathPlannerTask(void *parameters)
const float MIN_RADIUS = 2.0f; // Radius to consider at waypoint (m) const float MIN_RADIUS = 2.0f; // Radius to consider at waypoint (m)
// Main thread loop // Main thread loop
bool pathplanner_active = false;
while (1) while (1)
{ {
FlightStatusGet(&flightStatus); FlightStatusGet(&flightStatus);
vTaskDelay(100); vTaskDelay(100);
if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) if (flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_PATHPLANNER) {
pathplanner_active = false;
continue; continue;
}
if(pathplanner_active == false) {
WaypointActiveSet(&waypointActive);
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);
pathplanner_active = true;
continue;
}
PositionActualGet(&positionActual); PositionActualGet(&positionActual);
WaypointActiveGet(&waypointActive); WaypointActiveGet(&waypointActive);
bad_reads += (WaypointInstGet(waypointActive.Index, &waypoint) != 0); WaypointInstGet(waypointActive.Index, &waypoint);
float r2 = powf(positionActual.North - waypoint.Position[WAYPOINT_POSITION_NORTH], 2) + float r2 = powf(positionActual.North - waypoint.Position[WAYPOINT_POSITION_NORTH], 2) +
powf(positionActual.East - waypoint.Position[WAYPOINT_POSITION_EAST], 2) + powf(positionActual.East - waypoint.Position[WAYPOINT_POSITION_EAST], 2) +
@ -153,6 +174,12 @@ static void pathPlannerTask(void *parameters)
// Oh shit, tried to go to non-existant waypoint // Oh shit, tried to go to non-existant waypoint
continue; continue;
} }
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; 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
@ -166,12 +193,6 @@ static void pathPlannerTask(void *parameters)
PIOS_DEBUG_Assert(0); PIOS_DEBUG_Assert(0);
} }
} }
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);
} }
} }