mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-03-15 07:29:15 +01:00
path library: updated API and usage from pathfollowers, added new conditions and additional user feedback in pathstatus
This commit is contained in:
parent
634a190d4f
commit
e6f4a40d3e
@ -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
|
||||
|
@ -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;
|
||||
|
@ -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
|
||||
|
@ -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 {
|
||||
|
@ -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)
|
||||
|
@ -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;
|
||||
|
||||
|
@ -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" />
|
||||
|
@ -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"/>
|
||||
|
Loading…
x
Reference in New Issue
Block a user