mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-18 03:52:11 +01:00
Merge remote-tracking branch 'origin/D-Lite/PathFollowerImprovements' into corvuscorax/OP-1156_pathfollower-unification
This commit is contained in:
Corvus Corax
commit
a3d18bb578
@ -30,8 +30,8 @@
|
||||
struct path_status {
|
||||
float fractional_progress;
|
||||
float error;
|
||||
float correction_direction[2];
|
||||
float path_direction[2];
|
||||
float correction_direction[3];
|
||||
float path_direction[3];
|
||||
};
|
||||
|
||||
void path_progress(float *start_point, float *end_point, float *cur_point, struct path_status *status, uint8_t mode);
|
||||
|
||||
@ -26,6 +26,7 @@
|
||||
|
||||
#include <pios.h>
|
||||
#include <pios_math.h>
|
||||
#include <mathmisc.h>
|
||||
|
||||
#include "paths.h"
|
||||
|
||||
@ -34,8 +35,8 @@
|
||||
// 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_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode);
|
||||
static void path_vector(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode);
|
||||
static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise);
|
||||
|
||||
/**
|
||||
@ -50,8 +51,11 @@ void path_progress(float *start_point, float *end_point, float *cur_point, struc
|
||||
{
|
||||
switch (mode) {
|
||||
case PATHDESIRED_MODE_FLYVECTOR:
|
||||
return path_vector(start_point, end_point, cur_point, status, true);
|
||||
|
||||
break;
|
||||
case PATHDESIRED_MODE_DRIVEVECTOR:
|
||||
return path_vector(start_point, end_point, cur_point, status);
|
||||
return path_vector(start_point, end_point, cur_point, status, false);
|
||||
|
||||
break;
|
||||
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
|
||||
@ -65,10 +69,13 @@ void path_progress(float *start_point, float *end_point, float *cur_point, struc
|
||||
|
||||
break;
|
||||
case PATHDESIRED_MODE_FLYENDPOINT:
|
||||
return path_endpoint(start_point, end_point, cur_point, status, true);
|
||||
|
||||
break;
|
||||
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);
|
||||
return path_endpoint(start_point, end_point, cur_point, status, false);
|
||||
|
||||
break;
|
||||
}
|
||||
@ -80,39 +87,48 @@ void path_progress(float *start_point, float *end_point, float *cur_point, struc
|
||||
* @param[in] end_point Ending point
|
||||
* @param[in] cur_point Current location
|
||||
* @param[out] status Structure containing progress along path and deviation
|
||||
* @param[in] mode3D set true to include altitude in distance and progress calculation
|
||||
*/
|
||||
static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status)
|
||||
static void path_endpoint(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool mode3D)
|
||||
{
|
||||
float path_north, path_east, diff_north, diff_east;
|
||||
float path[3], diff[3];
|
||||
float dist_path, dist_diff;
|
||||
|
||||
// we do not correct in this mode
|
||||
status->correction_direction[0] = status->correction_direction[1] = 0;
|
||||
status->correction_direction[0] = status->correction_direction[1] = status->correction_direction[2] = 0;
|
||||
|
||||
// Distance to go
|
||||
path_north = end_point[0] - start_point[0];
|
||||
path_east = end_point[1] - start_point[1];
|
||||
path[0] = end_point[0] - start_point[0];
|
||||
path[1] = end_point[1] - start_point[1];
|
||||
path[2] = mode3D ? end_point[2] - start_point[2] : 0;
|
||||
|
||||
// Current progress location relative to end
|
||||
diff_north = end_point[0] - cur_point[0];
|
||||
diff_east = end_point[1] - cur_point[1];
|
||||
diff[0] = end_point[0] - cur_point[0];
|
||||
diff[1] = end_point[1] - cur_point[1];
|
||||
diff[2] = mode3D ? end_point[2] - cur_point[2] : 0;
|
||||
|
||||
dist_diff = sqrtf(diff_north * diff_north + diff_east * diff_east);
|
||||
dist_path = sqrtf(path_north * path_north + path_east * path_east);
|
||||
dist_diff = sqrtf(diff[0]*diff[0] + diff[1]*diff[1] + diff[2]*diff[2]);
|
||||
dist_path = sqrtf(path[0]*path[0] + path[1]*path[1] + path[2]*path[2]);
|
||||
|
||||
if (dist_diff < 1e-6f) {
|
||||
status->fractional_progress = 1;
|
||||
status->error = 0;
|
||||
status->path_direction[0] = status->path_direction[1] = 0;
|
||||
status->path_direction[0] = status->path_direction[1] = 0;
|
||||
status->path_direction[2] = 1;
|
||||
return;
|
||||
}
|
||||
|
||||
status->fractional_progress = 1 - dist_diff / (1 + dist_path);
|
||||
if (dist_path + 1 > dist_diff) {
|
||||
status->fractional_progress = 1 - dist_diff / (1 + dist_path);
|
||||
} else {
|
||||
status->fractional_progress = 0; // we don't want fractional_progress to become negative
|
||||
}
|
||||
status->error = dist_diff;
|
||||
|
||||
// Compute direction to travel
|
||||
status->path_direction[0] = diff_north / dist_diff;
|
||||
status->path_direction[1] = diff_east / dist_diff;
|
||||
status->path_direction[0] = diff[0] / dist_diff;
|
||||
status->path_direction[1] = diff[1] / dist_diff;
|
||||
status->path_direction[2] = diff[2] / dist_diff;
|
||||
}
|
||||
|
||||
/**
|
||||
@ -121,51 +137,65 @@ static void path_endpoint(float *start_point, float *end_point, float *cur_point
|
||||
* @param[in] end_point Ending point
|
||||
* @param[in] cur_point Current location
|
||||
* @param[out] status Structure containing progress along path and deviation
|
||||
* @param[in] mode3D set true to include altitude in distance and progress calculation
|
||||
*/
|
||||
static void path_vector(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, bool mode3D)
|
||||
{
|
||||
float path_north, path_east, diff_north, diff_east;
|
||||
float path[3], diff[3];
|
||||
float dist_path;
|
||||
float dot;
|
||||
float normal[2];
|
||||
float track_point[3];
|
||||
|
||||
// Distance to go
|
||||
path_north = end_point[0] - start_point[0];
|
||||
path_east = end_point[1] - start_point[1];
|
||||
path[0] = end_point[0] - start_point[0];
|
||||
path[1] = end_point[1] - start_point[1];
|
||||
path[2] = mode3D ? end_point[2] - start_point[2] : 0;
|
||||
|
||||
// Current progress location relative to start
|
||||
diff_north = cur_point[0] - start_point[0];
|
||||
diff_east = cur_point[1] - start_point[1];
|
||||
diff[0] = cur_point[0] - start_point[0];
|
||||
diff[1] = cur_point[1] - start_point[1];
|
||||
diff[2] = mode3D ? cur_point[2] - start_point[2]: 0;
|
||||
|
||||
dot = path_north * diff_north + path_east * diff_east;
|
||||
dist_path = sqrtf(path_north * path_north + path_east * path_east);
|
||||
dot = path[0] * diff[0] + path[1] * diff[1] + path[2] * diff[2];
|
||||
dist_path = sqrtf(path[0] * path[0] + path[1] * path[1] + path[2] * path[2]);
|
||||
|
||||
if (dist_path < 1e-6f) {
|
||||
// if the path is too short, we cannot determine vector direction.
|
||||
if (dist_path > 1e-6f) {
|
||||
// Compute direction to travel & progress
|
||||
status->path_direction[0] = path[0] / dist_path;
|
||||
status->path_direction[1] = path[1] / dist_path;
|
||||
status->path_direction[2] = path[2] / dist_path;
|
||||
status->fractional_progress = dot / (dist_path * dist_path);
|
||||
} else {
|
||||
// Fly towards the endpoint to prevent flying away,
|
||||
// but assume progress=1 either way.
|
||||
path_endpoint(start_point, end_point, cur_point, status);
|
||||
path_endpoint(start_point, end_point, cur_point, status, mode3D);
|
||||
status->fractional_progress = 1;
|
||||
return;
|
||||
}
|
||||
|
||||
// Compute the normal to the path
|
||||
normal[0] = -path_east / dist_path;
|
||||
normal[1] = path_north / dist_path;
|
||||
// Compute point on track that is closest to our current position.
|
||||
track_point[0] = status->fractional_progress * path[0] + start_point[0];
|
||||
track_point[1] = status->fractional_progress * path[1] + start_point[1];
|
||||
track_point[2] = status->fractional_progress * path[2] + start_point[2];
|
||||
|
||||
status->fractional_progress = dot / (dist_path * dist_path);
|
||||
status->error = normal[0] * diff_north + normal[1] * diff_east;
|
||||
status->correction_direction[0] = track_point[0] - cur_point[0];
|
||||
status->correction_direction[1] = track_point[1] - cur_point[1];
|
||||
status->correction_direction[2] = track_point[2] - cur_point[2];
|
||||
|
||||
// Compute direction to correct error
|
||||
status->correction_direction[0] = (status->error > 0) ? -normal[0] : normal[0];
|
||||
status->correction_direction[1] = (status->error > 0) ? -normal[1] : normal[1];
|
||||
status->error = sqrt(status->correction_direction[0] * status->correction_direction[0] +
|
||||
status->correction_direction[1] * status->correction_direction[1] +
|
||||
status->correction_direction[2] * status->correction_direction[2]);
|
||||
|
||||
// Now just want magnitude of error
|
||||
status->error = fabs(status->error);
|
||||
|
||||
// Compute direction to travel
|
||||
status->path_direction[0] = path_north / dist_path;
|
||||
status->path_direction[1] = path_east / dist_path;
|
||||
// Normalize correction_direction but avoid division by zero
|
||||
if (status->error > 1e-6f) {
|
||||
status->correction_direction[0] /= status->error;
|
||||
status->correction_direction[1] /= status->error;
|
||||
status->correction_direction[2] /= status->error;
|
||||
} else {
|
||||
status->correction_direction[0] = 0;
|
||||
status->correction_direction[1] = 0;
|
||||
status->correction_direction[2] = 1;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
@ -200,8 +230,10 @@ static void path_circle(float *start_point, float *end_point, float *cur_point,
|
||||
status->error = radius;
|
||||
status->correction_direction[0] = 0;
|
||||
status->correction_direction[1] = 1;
|
||||
status->correction_direction[2] = 0;
|
||||
status->path_direction[0] = 1;
|
||||
status->path_direction[1] = 0;
|
||||
status->path_direction[2] = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
@ -246,10 +278,12 @@ static void path_circle(float *start_point, float *end_point, float *cur_point,
|
||||
// Compute direction to correct error
|
||||
status->correction_direction[0] = (status->error > 0 ? 1 : -1) * diff_north / cradius;
|
||||
status->correction_direction[1] = (status->error > 0 ? 1 : -1) * diff_east / cradius;
|
||||
status->correction_direction[2] = 0;
|
||||
|
||||
// Compute direction to travel
|
||||
status->path_direction[0] = normal[0];
|
||||
status->path_direction[1] = normal[1];
|
||||
status->path_direction[2] = 0;
|
||||
|
||||
status->error = fabs(status->error);
|
||||
}
|
||||
|
||||
@ -128,9 +128,19 @@ void plan_setup_returnToBase()
|
||||
PathDesiredSet(&pathDesired);
|
||||
}
|
||||
|
||||
static PiOSDeltatimeConfig landdT;
|
||||
void plan_setup_land()
|
||||
{
|
||||
float descendspeed;
|
||||
plan_setup_positionHold();
|
||||
|
||||
FlightModeSettingsLandingVelocityGet(&descendspeed);
|
||||
PathDesiredData pathDesired;
|
||||
PathDesiredGet(&pathDesired);
|
||||
pathDesired.StartingVelocity = descendspeed;
|
||||
pathDesired.EndingVelocity = descendspeed;
|
||||
PathDesiredSet(&pathDesired);
|
||||
PIOS_DELTATIME_Init(&landdT, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -138,12 +148,18 @@ void plan_setup_land()
|
||||
*/
|
||||
void plan_run_land()
|
||||
{
|
||||
float downPos, descendspeed;
|
||||
PathDesiredEndData pathDesiredEnd;
|
||||
|
||||
PathDesiredEndGet(&pathDesiredEnd);
|
||||
PositionStateDownGet(&downPos); // current down position
|
||||
PathDesiredEndGet(&pathDesiredEnd); // desired position
|
||||
PathDesiredEndingVelocityGet(&descendspeed);
|
||||
|
||||
PositionStateDownGet(&pathDesiredEnd.Down);
|
||||
pathDesiredEnd.Down += 5;
|
||||
// desired position is updated to match the desired descend speed but don't run ahead
|
||||
// too far if the current position can't keep up. This normaly means we have landed.
|
||||
if (pathDesiredEnd.Down - downPos < 10) {
|
||||
pathDesiredEnd.Down += descendspeed * PIOS_DELTATIME_GetAverageSeconds(&landdT);
|
||||
}
|
||||
|
||||
PathDesiredEndSet(&pathDesiredEnd);
|
||||
}
|
||||
|
||||
@ -370,79 +370,86 @@ static void updatePOIBearing()
|
||||
static void updatePathVelocity()
|
||||
{
|
||||
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
|
||||
float downCommand;
|
||||
|
||||
PathDesiredData pathDesired;
|
||||
|
||||
PathDesiredGet(&pathDesired);
|
||||
PositionStateData positionState;
|
||||
PositionStateGet(&positionState);
|
||||
|
||||
float cur[3] =
|
||||
{ positionState.North, positionState.East, positionState.Down };
|
||||
float current_position[3] = { positionState.North, positionState.East, positionState.Down };
|
||||
struct path_status progress;
|
||||
|
||||
path_progress(
|
||||
cast_struct_to_array(pathDesired.Start, pathDesired.Start.North),
|
||||
cast_struct_to_array(pathDesired.End, pathDesired.End.North),
|
||||
cur, &progress, pathDesired.Mode);
|
||||
current_position, &progress, pathDesired.Mode);
|
||||
|
||||
float groundspeed;
|
||||
float speed;
|
||||
switch (pathDesired.Mode) {
|
||||
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
|
||||
case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
|
||||
case PATHDESIRED_MODE_FLYCIRCLELEFT:
|
||||
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
|
||||
groundspeed = pathDesired.EndingVelocity;
|
||||
speed = pathDesired.EndingVelocity;
|
||||
break;
|
||||
case PATHDESIRED_MODE_FLYENDPOINT:
|
||||
case PATHDESIRED_MODE_DRIVEENDPOINT:
|
||||
groundspeed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf(progress.fractional_progress, 0, 1);
|
||||
speed = pathDesired.EndingVelocity - pathDesired.EndingVelocity * boundf(progress.fractional_progress, 0, 1);
|
||||
if (progress.fractional_progress > 1) {
|
||||
groundspeed = 0;
|
||||
speed = 0;
|
||||
}
|
||||
break;
|
||||
case PATHDESIRED_MODE_FLYVECTOR:
|
||||
case PATHDESIRED_MODE_DRIVEVECTOR:
|
||||
default:
|
||||
groundspeed = pathDesired.StartingVelocity
|
||||
speed = pathDesired.StartingVelocity
|
||||
+ (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1);
|
||||
if (progress.fractional_progress > 1) {
|
||||
groundspeed = 0;
|
||||
speed = 0;
|
||||
}
|
||||
break;
|
||||
}
|
||||
|
||||
VelocityDesiredData velocityDesired;
|
||||
velocityDesired.North = progress.path_direction[0] * groundspeed;
|
||||
velocityDesired.East = progress.path_direction[1] * groundspeed;
|
||||
|
||||
float error_speed = progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp;
|
||||
float correction_velocity[2] =
|
||||
{ progress.correction_direction[0] * error_speed, progress.correction_direction[1] * error_speed };
|
||||
northPosIntegral += progress.correction_direction[0] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Ki * dT;
|
||||
eastPosIntegral += progress.correction_direction[1] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Ki * dT;
|
||||
downPosIntegral += progress.correction_direction[2] * progress.error * vtolpathfollowerSettings.VerticalPosPI.Ki * dT;
|
||||
|
||||
float total_vel = sqrtf(powf(correction_velocity[0], 2) + powf(correction_velocity[1], 2));
|
||||
float scale = 1;
|
||||
if (total_vel > vtolpathfollowerSettings.HorizontalVelMax) {
|
||||
scale = vtolpathfollowerSettings.HorizontalVelMax / total_vel;
|
||||
northPosIntegral = boundf(northPosIntegral, -vtolpathfollowerSettings.HorizontalPosPI.ILimit,
|
||||
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
|
||||
eastPosIntegral = boundf(eastPosIntegral, -vtolpathfollowerSettings.HorizontalPosPI.ILimit,
|
||||
vtolpathfollowerSettings.HorizontalPosPI.ILimit);
|
||||
downPosIntegral = boundf(downPosIntegral, -vtolpathfollowerSettings.VerticalPosPI.ILimit,
|
||||
vtolpathfollowerSettings.VerticalPosPI.ILimit);
|
||||
|
||||
velocityDesired.North = progress.path_direction[0] * speed + northPosIntegral +
|
||||
progress.correction_direction[0] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp;
|
||||
velocityDesired.East = progress.path_direction[1] * speed + eastPosIntegral +
|
||||
progress.correction_direction[1] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Kp;
|
||||
velocityDesired.Down = progress.path_direction[2] * speed + downPosIntegral +
|
||||
progress.correction_direction[2] * progress.error * vtolpathfollowerSettings.VerticalPosPI.Kp;
|
||||
|
||||
// Make sure the desired velocities don't exceed PathFollower limits.
|
||||
float groundspeedDesired = sqrtf(powf(velocityDesired.North, 2) + powf(velocityDesired.East, 2));
|
||||
|
||||
if (groundspeedDesired > vtolpathfollowerSettings.HorizontalVelMax) {
|
||||
velocityDesired.North *= vtolpathfollowerSettings.HorizontalVelMax / groundspeedDesired;
|
||||
velocityDesired.East *= vtolpathfollowerSettings.HorizontalVelMax / groundspeedDesired;
|
||||
}
|
||||
|
||||
velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
|
||||
velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
|
||||
|
||||
float altitudeSetpoint = pathDesired.Start.Down + (pathDesired.End.Down - pathDesired.Start.Down) * boundf(progress.fractional_progress, 0, 1);
|
||||
|
||||
float downError = altitudeSetpoint - positionState.Down;
|
||||
downPosIntegral = boundf(downPosIntegral + downError * dT * vtolpathfollowerSettings.VerticalPosPI.Ki,
|
||||
-vtolpathfollowerSettings.VerticalPosPI.ILimit,
|
||||
vtolpathfollowerSettings.VerticalPosPI.ILimit);
|
||||
downCommand = (downError * vtolpathfollowerSettings.VerticalPosPI.Kp + downPosIntegral);
|
||||
velocityDesired.Down = boundf(downCommand, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
|
||||
velocityDesired.Down = boundf(velocityDesired.Down, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
|
||||
|
||||
// update pathstatus
|
||||
pathStatus.error = progress.error;
|
||||
pathStatus.fractional_progress = progress.fractional_progress;
|
||||
pathStatus.error = progress.error;
|
||||
pathStatus.fractional_progress = progress.fractional_progress;
|
||||
pathStatus.path_direction_north = progress.path_direction[0];
|
||||
pathStatus.path_direction_east = progress.path_direction[1];
|
||||
pathStatus.path_direction_down = progress.path_direction[2];
|
||||
|
||||
pathStatus.correction_direction_north = progress.correction_direction[0];
|
||||
pathStatus.correction_direction_east = progress.correction_direction[1];
|
||||
pathStatus.correction_direction_down = progress.correction_direction[2];
|
||||
VelocityDesiredSet(&velocityDesired);
|
||||
}
|
||||
|
||||
|
||||
@ -110,6 +110,7 @@
|
||||
<field name="DisarmingSequenceTime" units="ms" type="uint16" elements="1" defaultvalue="1000"/>
|
||||
<field name="DisableSanityChecks" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
|
||||
<field name="ReturnToBaseAltitudeOffset" units="m" type="float" elements="1" defaultvalue="10"/>
|
||||
<field name="LandingVelocity" units="m" type="float" elements="1" defaultvalue="0.4"/>
|
||||
<field name="PositionHoldOffset" units="m" type="float" elementnames="Horizontal,Vertical" defaultvalue="10,2"/>
|
||||
<!-- optimized for current vtolpathfollower,
|
||||
for fixed wing pathfollower set to Horizontal=500,Vertical=5 -->
|
||||
|
||||
@ -7,7 +7,12 @@
|
||||
<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"/>
|
||||
|
||||
<field name="path_direction_north" units="m" type="float" elements="1"/>
|
||||
<field name="path_direction_east" units="m" type="float" elements="1"/>
|
||||
<field name="path_direction_down" units="m" type="float" elements="1"/>
|
||||
<field name="correction_direction_north" units="m" type="float" elements="1"/>
|
||||
<field name="correction_direction_east" units="m" type="float" elements="1"/>
|
||||
<field name="correction_direction_down" units="m" type="float" elements="1"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="false" updatemode="manual" period="0"/>
|
||||
<telemetryflight acked="false" updatemode="periodic" period="1000"/>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user