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

path library: updated API and usage from pathfollowers, added new conditions and additional user feedback in pathstatus

Browse Source
This commit is contained in:
Corvus Corax 2012-05-28 01:51:17 +02:00
parent 634a190d4f
commit e6f4a40d3e
8 changed files with 200 additions and 98 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
flight/Libraries/inc/paths.h
View File

@ -34,7 +34,6 @@ struct path_status {
float path_direction[2];
};
void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status);
void circle_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise);
void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, uint8_t mode);
#endif

89
flight/Libraries/paths.c
View File

@ -27,6 +27,89 @@
#include "pios.h"
#include "paths.h"
#include "uavobjectmanager.h" // <--.
#include "pathdesired.h" //<-- needed only for correct ENUM macro usage with path modes (PATHDESIRED_MODE_xxx,
// no direct UAVObject usage allowed in this file
// private functions
static void path_endpoint( float * start_point, float * end_point, float * cur_point, struct path_status * status);
static void path_vector( float * start_point, float * end_point, float * cur_point, struct path_status * status);
static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise);
/**
* @brief Compute progress along path and deviation from it
* @param[in] start_point Starting point
* @param[in] end_point Ending point
* @param[in] cur_point Current location
* @param[in] mode Path following mode
* @param[out] status Structure containing progress along path and deviation
*/
void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, uint8_t mode)
{
switch(mode) {
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_DRIVEVECTOR:
return path_vector(start_point, end_point, cur_point, status);
break;
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
return path_circle(start_point, end_point, cur_point, status, 1);
break;
case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
return path_circle(start_point, end_point, cur_point, status, 0);
break;
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT:
default:
// use the endpoint as default failsafe if called in unknown modes
return path_endpoint(start_point, end_point, cur_point, status);
break;
}
}
/**
* @brief Compute progress towards endpoint. Deviation equals distance
* @param[in] start_point Starting point
* @param[in] end_point Ending point
* @param[in] cur_point Current location
* @param[out] status Structure containing progress along path and deviation
*/
static void path_endpoint( float * start_point, float * end_point, float * cur_point, struct path_status * status)
{
float path_north, path_east, diff_north, diff_east;
float dist_path, dist_diff;
// we do not correct in this mode
status->correction_direction[0] = status->correction_direction[1] = 0;
// Distance to go
path_north = end_point[0] - start_point[0];
path_east = end_point[1] - start_point[1];
// Current progress location relative to end
diff_north = end_point[0] - cur_point[0];
diff_east = end_point[1] - cur_point[1];
dist_diff = sqrtf( diff_north * diff_north + diff_east * diff_east );
dist_path = sqrtf( path_north * path_north + path_east * path_east );
if(dist_path < 1e-6 || dist_diff < 1e-6 ) {
status->fractional_progress = 1;
status->error = 0;
status->path_direction[0] = status->path_direction[1] = 0;
return;
}
status->fractional_progress = 1 - dist_diff / dist_path;
status->error = dist_diff;
// Compute direction to travel
status->path_direction[0] = diff_north / dist_diff;
status->path_direction[1] = diff_east / dist_diff;
}
/**
* @brief Compute progress along path and deviation from it
* @param[in] start_point Starting point
@ -34,7 +117,7 @@
* @param[in] cur_point Current location
* @param[out] status Structure containing progress along path and deviation
*/
void path_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status)
static void path_vector( float * start_point, float * end_point, float * cur_point, struct path_status * status)
{
float path_north, path_east, diff_north, diff_east;
float dist_path;
@ -56,7 +139,7 @@ void path_progress(float * start_point, float * end_point, float * cur_point, st
status->fractional_progress = 1;
status->error = 0;
status->correction_direction[0] = status->correction_direction[1] = 0;
status->path_direction[0] = status->path_direction[1] = 1e-6;
status->path_direction[0] = status->path_direction[1] = 0;
return;
}
@ -85,7 +168,7 @@ void path_progress(float * start_point, float * end_point, float * cur_point, st
* @param[in] cur_point Current location
* @param[out] status Structure containing progress along path and deviation
*/
void circle_progress(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise)
static void path_circle(float * start_point, float * end_point, float * cur_point, struct path_status * status, bool clockwise)
{
float radius_north, radius_east, diff_north, diff_east;
float radius,cradius;

118
flight/Modules/FixedWingPathFollower/fixedwingpathfollower.c
View File

@ -82,13 +82,13 @@
static bool followerEnabled = false;
static xTaskHandle pathfollowerTaskHandle;
static PathDesiredData pathDesired;
static PathStatusData pathStatus;
static FixedWingPathFollowerSettingsData fixedwingpathfollowerSettings;
// Private functions
static void pathfollowerTask(void *parameters);
static void SettingsUpdatedCb(UAVObjEvent * ev);
static void updatePathVelocity(int8_t circledirection);
static void updateEndpointVelocity();
static void updatePathVelocity();
static uint8_t updateFixedDesiredAttitude();
static void updateFixedAttitude();
static void baroAirspeedUpdatedCb(UAVObjEvent * ev);
@ -152,7 +152,6 @@ static void pathfollowerTask(void *parameters)
{
SystemSettingsData systemSettings;
FlightStatusData flightStatus;
PathStatusData pathStatus;
portTickType lastUpdateTime;
@ -195,7 +194,7 @@ static void pathfollowerTask(void *parameters)
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOBASE:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYENDPOINT) {
updateEndpointVelocity();
updatePathVelocity();
result = updateFixedDesiredAttitude();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
@ -210,39 +209,11 @@ static void pathfollowerTask(void *parameters)
pathStatus.UID = pathDesired.UID;
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
switch(pathDesired.Mode) {
// TODO: Make updateFixedDesiredAttitude and velocity report success and update PATHSTATUS_STATUS accordingly
case PATHDESIRED_MODE_FLYENDPOINT:
updateEndpointVelocity();
result = updateFixedDesiredAttitude();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
} else {
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
}
break;
case PATHDESIRED_MODE_FLYVECTOR:
updatePathVelocity(0);
result = updateFixedDesiredAttitude();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
} else {
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
}
break;
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
updatePathVelocity(1);
result = updateFixedDesiredAttitude();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
} else {
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_WARNING);
}
break;
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity(-1);
updatePathVelocity();
result = updateFixedDesiredAttitude();
if (result) {
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
@ -286,7 +257,7 @@ static void pathfollowerTask(void *parameters)
* Takes in @ref PositionActual and compares it to @ref PathDesired
* and computes @ref VelocityDesired
*/
static void updatePathVelocity(int8_t circledirection)
static void updatePathVelocity()
{
PositionActualData positionActual;
PositionActualGet(&positionActual);
@ -300,76 +271,51 @@ static void updatePathVelocity(int8_t circledirection)
};
struct path_status progress;
if (!circledirection) {
path_progress(pathDesired.Start, pathDesired.End, cur, &progress);
} else {
circle_progress(pathDesired.Start, pathDesired.End, cur, &progress, circledirection>0 );
}
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
float groundspeed;
if (!circledirection && progress.fractional_progress<1) {
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
bound(progress.fractional_progress,0,1);
} else {
groundspeed = pathDesired.EndingVelocity;
float altitudeSetpoint;
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
groundspeed = pathDesired.EndingVelocity;
altitudeSetpoint = pathDesired.End[2];
break;
case PATHDESIRED_MODE_FLYENDPOINT:
case PATHDESIRED_MODE_DRIVEENDPOINT:
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_DRIVEVECTOR:
default:
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
bound(progress.fractional_progress,0,1);
altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
bound(progress.fractional_progress,0,1);
break;
}
// calculate velocity - can be zero if waypoints are too close
VelocityDesiredData velocityDesired;
velocityDesired.North = progress.path_direction[0] * groundspeed;
velocityDesired.East = progress.path_direction[1] * groundspeed;
velocityDesired.North = progress.path_direction[0] * bound(groundspeed,1e-6,groundspeed);
velocityDesired.East = progress.path_direction[1] * bound(groundspeed,1e-6,groundspeed);
float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP;
// calculate correction - can also be zero if correction vector is 0 or no error present
velocityDesired.North += progress.correction_direction[0] * error_speed;
velocityDesired.East += progress.correction_direction[1] * error_speed;
float altitudeSetpoint;
if (!circledirection) {
altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
bound(progress.fractional_progress,0,1);
} else {
altitudeSetpoint = pathDesired.End[2];
}
float downError = altitudeSetpoint - positionActual.Down;
velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP;
// update pathstatus
pathStatus.error = progress.error;
pathStatus.fractional_progress = progress.fractional_progress;
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired velocity from the current position
*
* Takes in @ref PositionActual and compares it to @ref PositionDesired
* and computes @ref VelocityDesired
*/
void updateEndpointVelocity()
{
PositionActualData positionActual;
VelocityActualData velocityActual;
VelocityDesiredData velocityDesired;
PositionActualGet(&positionActual);
VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired);
float northError;
float eastError;
float downError;
// Compute commands - look ahead fixedwingpathfollowerSettings.CourseFeedForward seconds
northError = pathDesired.End[PATHDESIRED_END_NORTH] - ( positionActual.North + (velocityActual.North * fixedwingpathfollowerSettings.CourseFeedForward));
velocityDesired.North = northError * fixedwingpathfollowerSettings.HorizontalPosP;
eastError = pathDesired.End[PATHDESIRED_END_EAST] - ( positionActual.East + (velocityActual.East * fixedwingpathfollowerSettings.CourseFeedForward));
velocityDesired.East = eastError * fixedwingpathfollowerSettings.HorizontalPosP;
downError = pathDesired.End[PATHDESIRED_END_DOWN] - ( positionActual.Down + (velocityActual.Down * fixedwingpathfollowerSettings.CourseFeedForward));
velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP;
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired attitude from a fixed preset

14
flight/Modules/ManualControl/manualcontrol.c
View File

@ -639,9 +639,14 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.Start[PATHDESIRED_START_NORTH] = 0;
pathDesired.Start[PATHDESIRED_START_EAST] = 0;
pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down - 10;
pathDesired.End[PATHDESIRED_END_NORTH] = 0;
pathDesired.End[PATHDESIRED_END_EAST] = 0;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 1;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
pathDesired.StartingVelocity=1;
pathDesired.EndingVelocity=0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
} else if(changed) {
@ -651,9 +656,14 @@ static void updatePathDesired(ManualControlCommandData * cmd, bool changed,bool
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
pathDesired.Start[PATHDESIRED_END_NORTH] = positionActual.North;
pathDesired.Start[PATHDESIRED_END_EAST] = positionActual.East;
pathDesired.Start[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North;
pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 1;
pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10;
pathDesired.StartingVelocity=1;
pathDesired.EndingVelocity=0;
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
PathDesiredSet(&pathDesired);
} else {

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

@ -33,6 +33,7 @@
#include "paths.h"
#include "flightstatus.h"
#include "baroairspeed.h" // TODO: make baroairspeed optional
#include "pathaction.h"
#include "pathdesired.h"
#include "pathstatus.h"
@ -65,7 +66,9 @@ static uint8_t conditionNone();
static uint8_t conditionTimeOut();
static uint8_t conditionDistanceToTarget();
static uint8_t conditionLegRemaining();
static uint8_t conditionBelowError();
static uint8_t conditionAboveAltitude();
static uint8_t conditionAboveSpeed();
static uint8_t conditionPointingTowardsNext();
static uint8_t conditionPythonScript();
static uint8_t conditionImmediate();
@ -107,6 +110,7 @@ int32_t PathPlannerInitialize()
PathStatusInitialize();
PathDesiredInitialize();
PositionActualInitialize();
BaroAirspeedInitialize();
VelocityActualInitialize();
WaypointInitialize();
WaypointActiveInitialize();
@ -316,9 +320,15 @@ static uint8_t pathConditionCheck() {
case PATHACTION_ENDCONDITION_LEGREMAINING:
return conditionLegRemaining();
break;
case PATHACTION_ENDCONDITION_BELOWERROR:
return conditionBelowError();
break;
case PATHACTION_ENDCONDITION_ABOVEALTITUDE:
return conditionAboveAltitude();
break;
case PATHACTION_ENDCONDITION_ABOVESPEED:
return conditionAboveSpeed();
break;
case PATHACTION_ENDCONDITION_POINTINGTOWARDSNEXT:
return conditionPointingTowardsNext();
break;
@ -403,13 +413,35 @@ static uint8_t conditionLegRemaining() {
float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress);
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
if ( progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0] ) {
return true;
}
return false;
}
/**
* the BelowError measures the error on a path segment
* returns true if error is below margin
* Parameter 0: error margin (in m)
*/
static uint8_t conditionBelowError() {
PathDesiredData pathDesired;
PositionActualData positionActual;
PathDesiredGet(&pathDesired);
PositionActualGet(&positionActual);
float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
if ( progress.error <= pathAction.ConditionParameters[0] ) {
return true;
}
return false;
}
/**
* the AboveAltitude measures the flight altitude relative to home position
* returns true if above critical altitude
@ -426,6 +458,34 @@ static uint8_t conditionAboveAltitude() {
return false;
}
/**
* the AboveSpeed measures the movement speed (3d)
* returns true if above critical speed
* Parameter 0: speed in m/s
* Parameter 1: flag: 0=groundspeed 1=airspeed
*/
static uint8_t conditionAboveSpeed() {
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
float velocity = sqrtf( velocityActual.North*velocityActual.North + velocityActual.East*velocityActual.East + velocityActual.Down*velocityActual.Down );
// use airspeed if requested and available
if (pathAction.ConditionParameters[1]>0.5f) {
BaroAirspeedData baroAirspeed;
BaroAirspeedGet (&baroAirspeed);
if (baroAirspeed.Connected == BAROAIRSPEED_CONNECTED_TRUE) {
velocity = baroAirspeed.Airspeed;
}
}
if ( velocity >= pathAction.ConditionParameters[0]) {
return true;
}
return false;
}
/**
* the PointingTowardsNext measures the horizontal movement vector direction relative to the next waypoint
* regardless whether this waypoint will ever be active (Command could jump to another one on true)

6
flight/Modules/VtolPathFollower/vtolpathfollower.c
View File

@ -216,6 +216,8 @@ static void vtolPathFollowerTask(void *parameters)
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
break;
case PATHDESIRED_MODE_FLYVECTOR:
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
case PATHDESIRED_MODE_FLYCIRCLELEFT:
updatePathVelocity();
updateVtolDesiredAttitude();
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE,SYSTEMALARMS_ALARM_OK);
@ -269,10 +271,10 @@ static void updatePathVelocity()
float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
struct path_status progress;
path_progress(pathDesired.Start, pathDesired.End, cur, &progress);
path_progress(pathDesired.Start, pathDesired.End, cur, &progress, pathDesired.Mode);
float groundspeed = pathDesired.StartingVelocity +
(pathDesired.EndingVelocity - pathDesired.StartingVelocity) * progress.fractional_progress;
(pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound ( progress.fractional_progress,0,1);
if(progress.fractional_progress > 1)
groundspeed = 0;

4
shared/uavobjectdefinition/pathaction.xml
View File

@ -9,8 +9,8 @@
<field name="ModeParameters" units="" type="float" elements="4" default="0"/>
<field name="EndCondition" units="" type="enum" elements="1" options="None,TimeOut,
DistanceToTarget,LegRemaining,
AboveAltitude,
DistanceToTarget,LegRemaining,BelowError,
AboveAltitude,AboveSpeed,
PointingTowardsNext,
PythonScript,
Immediate" default="None" />

2
shared/uavobjectdefinition/pathstatus.xml
View File

@ -5,6 +5,8 @@
<field name="UID" units="" type="uint8" elements="1" default="0"/>
<!-- 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="fractional_progress" units="" type="float" elements="1"/>
<field name="error" units="m" type="float" elements="1"/>
<access gcs="readwrite" flight="readwrite"/>
<telemetrygcs acked="false" updatemode="manual" period="0"/>