rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Treat the waypoint action as something to do once you hit that location. (e.g.

Browse Source 
PathToNext) and add a Stop value to indicate not to advance to the next
waypoint.  This is essentially a perament loiter to the pathfollower (I think)
but it's really just to have the ability to "delete" waypoints.
This commit is contained in:
James Cotton 2012-06-02 15:07:40 -05:00
parent e686ac0b1d
commit 4ad23864c8
3 changed files with 18 additions and 14 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -146,7 +146,7 @@ static void pathPlannerTask(void *parameters)
// We hit this waypoint // We hit this waypoint
if (r2 < (MIN_RADIUS * MIN_RADIUS)) { if (r2 < (MIN_RADIUS * MIN_RADIUS)) {
switch(waypoint.Action) { switch(waypoint.Action) {
case WAYPOINT_ACTION_NEXT: case WAYPOINT_ACTION_PATHTONEXT:
waypointActive.Index++; waypointActive.Index++;
WaypointActiveSet(&waypointActive); WaypointActiveSet(&waypointActive);
@ -160,6 +160,8 @@ static void pathPlannerTask(void *parameters)
pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT; pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
PathDesiredSet(&pathDesired); PathDesiredSet(&pathDesired);
break; break;
case WAYPOINT_ACTION_STOP:
break;
default: default:
PIOS_DEBUG_Assert(0); PIOS_DEBUG_Assert(0);
} }
@ -179,7 +181,7 @@ static void pathPlannerTask(void *parameters)
if (progress.fractional_progress >= 1) { if (progress.fractional_progress >= 1) {
switch(waypoint.Action) { switch(waypoint.Action) {
case WAYPOINT_ACTION_NEXT: case WAYPOINT_ACTION_PATHTONEXT:
waypointActive.Index++; waypointActive.Index++;
WaypointActiveSet(&waypointActive); WaypointActiveSet(&waypointActive);
@ -193,6 +195,8 @@ static void pathPlannerTask(void *parameters)
pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT; pathDesired.Mode = PATHDESIRED_MODE_ENDPOINT;
PathDesiredSet(&pathDesired); PathDesiredSet(&pathDesired);
break; break;
case WAYPOINT_ACTION_STOP:
break;
default: default:
PIOS_DEBUG_Assert(0); PIOS_DEBUG_Assert(0);
} }
@ -300,7 +304,7 @@ static void createPathBox()
// Draw O // Draw O
WaypointData waypoint; WaypointData waypoint;
waypoint.Velocity[0] = 2; // Since for now this isn't directional just set a mag waypoint.Velocity[0] = 2; // Since for now this isn't directional just set a mag
waypoint.Action = WAYPOINT_ACTION_NEXT; waypoint.Action = WAYPOINT_ACTION_PATHTONEXT;
waypoint.Position[0] = 5; waypoint.Position[0] = 5;
waypoint.Position[1] = 5; waypoint.Position[1] = 5;
@ -337,7 +341,7 @@ static void createPathLogo()
waypoint.Position[1] = 30 * cos(i / 19.0 * 2 * M_PI); waypoint.Position[1] = 30 * cos(i / 19.0 * 2 * M_PI);
waypoint.Position[0] = 50 * sin(i / 19.0 * 2 * M_PI); waypoint.Position[0] = 50 * sin(i / 19.0 * 2 * M_PI);
waypoint.Position[2] = -50; waypoint.Position[2] = -50;
waypoint.Action = WAYPOINT_ACTION_NEXT; waypoint.Action = WAYPOINT_ACTION_PATHTONEXT;
WaypointCreateInstance(); WaypointCreateInstance();
bad_inits += (WaypointInstSet(i, &waypoint) != 0); bad_inits += (WaypointInstSet(i, &waypoint) != 0);
} }
@ -347,7 +351,7 @@ static void createPathLogo()
waypoint.Position[1] = 55 + 20 * cos(i / 10.0 * M_PI - M_PI / 2); 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[0] = 25 + 25 * sin(i / 10.0 * M_PI - M_PI / 2);
waypoint.Position[2] = -50; waypoint.Position[2] = -50;
waypoint.Action = WAYPOINT_ACTION_NEXT; waypoint.Action = WAYPOINT_ACTION_PATHTONEXT;
WaypointCreateInstance(); WaypointCreateInstance();
bad_inits += (WaypointInstSet(i, &waypoint) != 0); bad_inits += (WaypointInstSet(i, &waypoint) != 0);
} }
@ -355,7 +359,7 @@ static void createPathLogo()
waypoint.Position[1] = 35; waypoint.Position[1] = 35;
waypoint.Position[0] = -50; waypoint.Position[0] = -50;
waypoint.Position[2] = -50; waypoint.Position[2] = -50;
waypoint.Action = WAYPOINT_ACTION_NEXT; waypoint.Action = WAYPOINT_ACTION_PATHTONEXT;
WaypointCreateInstance(); WaypointCreateInstance();
WaypointInstSet(35, &waypoint); WaypointInstSet(35, &waypoint);
@ -363,42 +367,42 @@ static void createPathLogo()
waypoint.Position[1] = 35; waypoint.Position[1] = 35;
waypoint.Position[0] = -60; waypoint.Position[0] = -60;
waypoint.Position[2] = -30; waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT; waypoint.Action = WAYPOINT_ACTION_PATHTONEXT;
WaypointCreateInstance(); WaypointCreateInstance();
WaypointInstSet(36, &waypoint); WaypointInstSet(36, &waypoint);
waypoint.Position[1] = 85; waypoint.Position[1] = 85;
waypoint.Position[0] = -60; waypoint.Position[0] = -60;
waypoint.Position[2] = -30; waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT; waypoint.Action = WAYPOINT_ACTION_PATHTONEXT;
WaypointCreateInstance(); WaypointCreateInstance();
WaypointInstSet(37, &waypoint); WaypointInstSet(37, &waypoint);
waypoint.Position[1] = 85; waypoint.Position[1] = 85;
waypoint.Position[0] = 60; waypoint.Position[0] = 60;
waypoint.Position[2] = -30; waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT; waypoint.Action = WAYPOINT_ACTION_PATHTONEXT;
WaypointCreateInstance(); WaypointCreateInstance();
WaypointInstSet(38, &waypoint); WaypointInstSet(38, &waypoint);
waypoint.Position[1] = -40; waypoint.Position[1] = -40;
waypoint.Position[0] = 60; waypoint.Position[0] = 60;
waypoint.Position[2] = -30; waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT; waypoint.Action = WAYPOINT_ACTION_PATHTONEXT;
WaypointCreateInstance(); WaypointCreateInstance();
WaypointInstSet(39, &waypoint); WaypointInstSet(39, &waypoint);
waypoint.Position[1] = -40; waypoint.Position[1] = -40;
waypoint.Position[0] = -60; waypoint.Position[0] = -60;
waypoint.Position[2] = -30; waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT; waypoint.Action = WAYPOINT_ACTION_PATHTONEXT;
WaypointCreateInstance(); WaypointCreateInstance();
WaypointInstSet(40, &waypoint); WaypointInstSet(40, &waypoint);
waypoint.Position[1] = 35; waypoint.Position[1] = 35;
waypoint.Position[0] = -60; waypoint.Position[0] = -60;
waypoint.Position[2] = -30; waypoint.Position[2] = -30;
waypoint.Action = WAYPOINT_ACTION_NEXT; waypoint.Action = WAYPOINT_ACTION_PATHTONEXT;
WaypointCreateInstance(); WaypointCreateInstance();
WaypointInstSet(41, &waypoint); WaypointInstSet(41, &waypoint);

2
shared/uavobjectdefinition/pathdesired.xml
View File

@ -7,7 +7,7 @@
<field name="StartingVelocity" units="m/s" type="float" elements="1" default="0"/> <field name="StartingVelocity" units="m/s" type="float" elements="1" default="0"/>
<field name="EndingVelocity" units="m/s" type="float" elements="1" default="0"/> <field name="EndingVelocity" units="m/s" type="float" elements="1" default="0"/>
<field name="Mode" units="" type="enum" elements="1" options="Endpoint,Path" default="0"/> <field name="Mode" units="" type="enum" elements="1" options="Endpoint,Path,Land" default="0"/>
<!-- Endpoint Mode - Fly to end location and attempt to stay there --> <!-- Endpoint Mode - Fly to end location and attempt to stay there -->
<!-- Path Mode - Attempt to fly a line from Start to End using specifed velocity --> <!-- Path Mode - Attempt to fly a line from Start to End using specifed velocity -->

2
shared/uavobjectdefinition/waypoint.xml
View File

@ -4,7 +4,7 @@
<field name="Position" units="m" type="float" elementnames="North, East, Down"/> <field name="Position" units="m" type="float" elementnames="North, East, Down"/>
<field name="Velocity" units="m/s" type="float" elementnames="North, East, Down"/> <field name="Velocity" units="m/s" type="float" elementnames="North, East, Down"/>
<field name="YawDesired" units="deg" type="float" elements="1"/> <field name="YawDesired" units="deg" type="float" elements="1"/>
<field name="Action" units="" type="enum" elements="1" options="Next,RTH,Loiter10s,Land"/> <field name="Action" units="" type="enum" elements="1" options="PathToNext,EndpointToNext,RTH,Loiter10s,Land,Stop"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/> <telemetrygcs acked="false" updatemode="manual" period="0"/>
<telemetryflight acked="false" updatemode="periodic" period="4000"/> <telemetryflight acked="false" updatemode="periodic" period="4000"/>