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

Add box around logo path

Browse Source
This commit is contained in:
James Cotton 2012-03-15 12:28:21 -05:00
parent bfb0c6935e
commit 5827e3f565
1 changed files with 81 additions and 32 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -52,6 +52,7 @@ static WaypointData waypoint;
// Private functions // Private functions
static void pathPlannerTask(void *parameters); static void pathPlannerTask(void *parameters);
static void waypointsUpdated(UAVObjEvent * ev); static void waypointsUpdated(UAVObjEvent * ev);
static void createPath();
/** /**
* Module initialization * Module initialization
*/ */
@ -98,36 +99,9 @@ static void pathPlannerTask(void *parameters)
PositionActualData positionActual; PositionActualData positionActual;
PositionDesiredData positionDesired; PositionDesiredData positionDesired;
WaypointActiveData waypointActive; createPath();
WaypointData waypoint;
for(uint32_t i = 0; i < 20; i++) {
waypoint.Position[1] = 30 * cos(i / 10.0 * M_PI);
waypoint.Position[0] = 50 * sin(i / 10.0 * M_PI);
waypoint.Position[2] = -50;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
bad_inits += (WaypointInstSet(i, &waypoint) != 0);
}
for(uint32_t i = 20; i < 35; i++) { const float MIN_RADIUS = 4.0f; // Radius to consider at waypoint (m)
waypoint.Position[1] = 55 + 20 * cos(i / 10.0 * M_PI - M_PI / 2);
waypoint.Position[0] = 25 + 25 * sin(i / 10.0 * M_PI - M_PI / 2);
waypoint.Position[2] = -50;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
bad_inits += (WaypointInstSet(i, &waypoint) != 0);
}
waypoint.Position[1] = 35;
waypoint.Position[0] = -50;
waypoint.Position[2] = -50;
waypoint.Action = WAYPOINT_ACTION_RTH;
WaypointCreateInstance();
WaypointInstSet(35, &waypoint);
const float MIN_RADIUS = 2.0f; // Radius to consider at waypoint (m)
// Main thread loop // Main thread loop
bool pathplanner_active = false; bool pathplanner_active = false;
@ -203,6 +177,81 @@ static void waypointsUpdated(UAVObjEvent * ev)
PositionDesiredSet(&positionDesired); PositionDesiredSet(&positionDesired);
} }
static void createPath()
{
// Draw O
WaypointData waypoint;
for(uint32_t i = 0; i < 20; i++) {
waypoint.Position[1] = 30 * cos(i / 19.0 * 2 * M_PI);
waypoint.Position[0] = 50 * sin(i / 19.0 * 2 * M_PI);
waypoint.Position[2] = -50;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
bad_inits += (WaypointInstSet(i, &waypoint) != 0);
}
// Draw P
for(uint32_t i = 20; i < 35; i++) {
waypoint.Position[1] = 55 + 20 * cos(i / 10.0 * M_PI - M_PI / 2);
waypoint.Position[0] = 25 + 25 * sin(i / 10.0 * M_PI - M_PI / 2);
waypoint.Position[2] = -50;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
bad_inits += (WaypointInstSet(i, &waypoint) != 0);
}
waypoint.Position[1] = 35;
waypoint.Position[0] = -50;
waypoint.Position[2] = -50;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
WaypointInstSet(35, &waypoint);
// Draw Box
waypoint.Position[1] = 35;
waypoint.Position[0] = -60;
waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
WaypointInstSet(36, &waypoint);
waypoint.Position[1] = 85;
waypoint.Position[0] = -60;
waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
WaypointInstSet(37, &waypoint);
waypoint.Position[1] = 85;
waypoint.Position[0] = 60;
waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
WaypointInstSet(38, &waypoint);
waypoint.Position[1] = -40;
waypoint.Position[0] = 60;
waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
WaypointInstSet(39, &waypoint);
waypoint.Position[1] = -40;
waypoint.Position[0] = -60;
waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
WaypointInstSet(40, &waypoint);
waypoint.Position[1] = 35;
waypoint.Position[0] = -60;
waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT;
WaypointCreateInstance();
WaypointInstSet(41, &waypoint);
}
/** /**
* @} * @}
* @} * @}