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

PathPlanner: fixed a few failsafes, changed waypointID to int16 - negative jump ids have special meaning

Browse Source
This commit is contained in:
Corvus Corax 2012-05-28 20:50:09 +02:00
parent 51a4e82642
commit 1ad2b19d28
6 changed files with 36 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
flight/Libraries/paths.c
View File

@ -101,11 +101,7 @@ static void path_endpoint( float * start_point, float * end_point, float * cur_p
return; return;
} }
if(dist_path < 1e-6) { status->fractional_progress = 1 - dist_diff / (1 + dist_path);
status->fractional_progress = 0;
} else {
status->fractional_progress = 1 - dist_diff / dist_path;
}
status->error = dist_diff; status->error = dist_diff;
// Compute direction to travel // Compute direction to travel
@ -140,10 +136,11 @@ static void path_vector( float * start_point, float * end_point, float * cur_poi
dist_path = sqrtf( path_north * path_north + path_east * path_east ); dist_path = sqrtf( path_north * path_north + path_east * path_east );
if(dist_path < 1e-6) { if(dist_path < 1e-6) {
// if the path is too short, we cannot determine vector direction.
// Fly towards the endpoint to prevent flying away,
// but assume progress=1 either way.
path_endpoint( start_point, end_point, cur_point, status );
status->fractional_progress = 1; status->fractional_progress = 1;
status->error = 0;
status->correction_direction[0] = status->correction_direction[1] = 0;
status->path_direction[0] = status->path_direction[1] = 0;
return; return;
} }

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

@ -126,7 +126,6 @@ MODULE_INITCALL(PathPlannerInitialize, PathPlannerStart)
/** /**
* Module task * Module task
*/ */
int32_t bad_inits;
static void pathPlannerTask(void *parameters) static void pathPlannerTask(void *parameters)
{ {
// update settings on change // update settings on change
@ -178,7 +177,8 @@ static void pathPlannerTask(void *parameters)
continue; continue;
} }
if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL) { // negative destinations DISABLE this feature
if (pathStatus.Status == PATHSTATUS_STATUS_CRITICAL && waypointActive.Index != pathAction.ErrorDestination && pathAction.ErrorDestination >= 0) {
setWaypoint(pathAction.ErrorDestination); setWaypoint(pathAction.ErrorDestination);
continue; continue;
} }
@ -188,21 +188,31 @@ static void pathPlannerTask(void *parameters)
// decide what to do // decide what to do
switch (pathAction.Command) { switch (pathAction.Command) {
case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT:
endCondition = !endCondition;
case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT: case PATHACTION_COMMAND_ONCONDITIONNEXTWAYPOINT:
if (endCondition) setWaypoint(waypointActive.Index+1); if (endCondition) setWaypoint(waypointActive.Index+1);
break; break;
case PATHACTION_COMMAND_ONNOTCONDITIONNEXTWAYPOINT:
if (!endCondition) setWaypoint(waypointActive.Index+1);
break;
case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT:
if (endCondition) setWaypoint(pathAction.JumpDestination);
break;
case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT: case PATHACTION_COMMAND_ONNOTCONDITIONJUMPWAYPOINT:
if (!endCondition) setWaypoint(pathAction.JumpDestination); endCondition = !endCondition;
case PATHACTION_COMMAND_ONCONDITIONJUMPWAYPOINT:
if (endCondition) {
if (pathAction.JumpDestination<0) {
// waypoint ids <0 code relative jumps
setWaypoint(waypointActive.Index - pathAction.JumpDestination );
} else {
setWaypoint(pathAction.JumpDestination);
}
}
break; break;
case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT: case PATHACTION_COMMAND_IFCONDITIONJUMPWAYPOINTELSENEXTWAYPOINT:
if (endCondition) { if (endCondition) {
setWaypoint(pathAction.JumpDestination); if (pathAction.JumpDestination<0) {
// waypoint ids <0 code relative jumps
setWaypoint(waypointActive.Index - pathAction.JumpDestination );
} else {
setWaypoint(pathAction.JumpDestination);
}
} else { } else {
setWaypoint(waypointActive.Index+1); setWaypoint(waypointActive.Index+1);
} }
@ -261,11 +271,11 @@ void updatePathDesired(UAVObjEvent * ev) {
pathDesired.UID = waypointActive.Index; pathDesired.UID = waypointActive.Index;
if(waypointActive.Index == 0) { if(waypointActive.Index == 0) {
// Get home position as start point // First waypoint has itself as start point (used to be home position but that proved dangerous when looping)
pathDesired.Start[PATHDESIRED_START_NORTH] = 0; pathDesired.Start[PATHDESIRED_START_NORTH] = waypoint.Position[WAYPOINT_POSITION_NORTH];
pathDesired.Start[PATHDESIRED_START_EAST] = 0; pathDesired.Start[PATHDESIRED_START_EAST] = waypoint.Position[WAYPOINT_POSITION_EAST];
pathDesired.Start[PATHDESIRED_START_DOWN] = -1; pathDesired.Start[PATHDESIRED_START_DOWN] = waypoint.Position[WAYPOINT_POSITION_DOWN];
pathDesired.StartingVelocity = pathDesired.EndingVelocity; pathDesired.StartingVelocity = pathDesired.EndingVelocity;
} else { } else {
// Get previous waypoint as start point // Get previous waypoint as start point
@ -629,7 +639,7 @@ static void createPathLogo()
waypoint.Position[1] = SIZE * 30 * cos(i / 19.0 * 2 * M_PI); waypoint.Position[1] = SIZE * 30 * cos(i / 19.0 * 2 * M_PI);
waypoint.Position[0] = SIZE * 50 * sin(i / 19.0 * 2 * M_PI); waypoint.Position[0] = SIZE * 50 * sin(i / 19.0 * 2 * M_PI);
waypoint.Position[2] = -50; waypoint.Position[2] = -50;
bad_inits += (WaypointInstSet(i, &waypoint) != 0); WaypointInstSet(i, &waypoint);
} }
// Draw P // Draw P
@ -637,7 +647,7 @@ static void createPathLogo()
waypoint.Position[1] = SIZE * (55 + 20 * cos(i / 10.0 * M_PI - M_PI / 2)); waypoint.Position[1] = SIZE * (55 + 20 * cos(i / 10.0 * M_PI - M_PI / 2));
waypoint.Position[0] = SIZE * (25 + 25 * sin(i / 10.0 * M_PI - M_PI / 2)); waypoint.Position[0] = SIZE * (25 + 25 * sin(i / 10.0 * M_PI - M_PI / 2));
waypoint.Position[2] = -50; waypoint.Position[2] = -50;
bad_inits += (WaypointInstSet(i, &waypoint) != 0); WaypointInstSet(i, &waypoint);
} }
waypoint.Position[1] = SIZE * 35; waypoint.Position[1] = SIZE * 35;

4
shared/uavobjectdefinition/pathaction.xml
View File

@ -19,8 +19,8 @@
<field name="Command" units="" type="enum" elements="1" options="OnConditionNextWaypoint,OnNotConditionNextWaypoint, <field name="Command" units="" type="enum" elements="1" options="OnConditionNextWaypoint,OnNotConditionNextWaypoint,
OnConditionJumpWaypoint,OnNotConditionJumpWaypoint, OnConditionJumpWaypoint,OnNotConditionJumpWaypoint,
IfConditionJumpWaypointElseNextWaypoint" default="OnConditionNextWaypoint" /> IfConditionJumpWaypointElseNextWaypoint" default="OnConditionNextWaypoint" />
<field name="JumpDestination" units="waypoint" type="uint8" elements="1" default="0"/> <field name="JumpDestination" units="waypoint" type="int16" elements="1" default="0"/>
<field name="ErrorDestination" units="waypoint" type="uint8" elements="1" default="0"/> <field name="ErrorDestination" units="waypoint" type="int16" elements="1" default="0"/>
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/> <telemetrygcs acked="false" updatemode="manual" period="0"/>

2
shared/uavobjectdefinition/pathdesired.xml
View File

@ -17,7 +17,7 @@
<!-- Circle Mode - move a circular pattern around End with radius End-Start (straight line in the vertical)--> <!-- Circle Mode - move a circular pattern around End with radius End-Start (straight line in the vertical)-->
<field name="ModeParameters" units="" type="float" elements="4" default="0"/> <field name="ModeParameters" units="" type="float" elements="4" default="0"/>
<field name="UID" units="" type="uint8" elements="1" default="0"/> <field name="UID" units="" type="int16" elements="1" default="0"/>
<!-- unique ID confirmed with pathfollower in pathstatus to acknowledge a change in PathDesired --> <!-- unique ID confirmed with pathfollower in pathstatus to acknowledge a change in PathDesired -->
<access gcs="readwrite" flight="readwrite"/> <access gcs="readwrite" flight="readwrite"/>

2
shared/uavobjectdefinition/pathstatus.xml
View File

@ -2,7 +2,7 @@
<object name="PathStatus" singleinstance="true" settings="false"> <object name="PathStatus" singleinstance="true" settings="false">
<description>Status of the current path mode Can come from any @ref PathFollower module</description> <description>Status of the current path mode Can come from any @ref PathFollower module</description>
<field name="UID" units="" type="uint8" elements="1" default="0"/> <field name="UID" units="" type="int16" elements="1" default="0"/>
<!-- unique ID confirmed with pathfollower in pathstatus to acknowledge a change in PathDesired --> <!-- unique ID confirmed with pathfollower in pathstatus to acknowledge a change in PathDesired -->
<field name="Status" units="" type="enum" elements="1" options="InProgress,Completed,Warning,Critical"/> <field name="Status" units="" type="enum" elements="1" options="InProgress,Completed,Warning,Critical"/>
<field name="fractional_progress" units="" type="float" elements="1"/> <field name="fractional_progress" units="" type="float" elements="1"/>

2
shared/uavobjectdefinition/waypointactive.xml
View File

@ -1,7 +1,7 @@
<xml> <xml>
<object name="WaypointActive" singleinstance="true" settings="false"> <object name="WaypointActive" singleinstance="true" settings="false">
<description>Indicates the currently active waypoint</description> <description>Indicates the currently active waypoint</description>
<field name="Index" units="" type="uint8" elements="1"/> <field name="Index" units="" type="int16" elements="1"/>
<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="onchange" period="0"/> <telemetryflight acked="false" updatemode="onchange" period="0"/>