mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-26 15:54:15 +01:00
Adds a 3D mode to path_vector. This allows vertical path segments without
position changes. PathStatus now also contains correction_direction and path_direction to make path following behaviour more transparent.
This commit is contained in:
Werner Backes
parent
7da0034775
commit
065ba1a0c8
@ -26,6 +26,7 @@
|
||||
|
||||
#include <pios.h>
|
||||
#include <pios_math.h>
|
||||
#include <mathmisc.h>
|
||||
|
||||
#include "paths.h"
|
||||
|
||||
@ -35,7 +36,7 @@
|
||||
|
||||
// private functions
|
||||
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);
|
||||
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:
|
||||
@ -113,7 +117,11 @@ static void path_endpoint(float *start_point, float *end_point, float *cur_point
|
||||
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
|
||||
@ -128,53 +136,70 @@ 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 (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 {
|
||||
// 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, false);
|
||||
// Assume progress=1 and zero-length path.
|
||||
status->path_direction[0] = 0;
|
||||
status->path_direction[1] = 0;
|
||||
status->path_direction[2] = 0;
|
||||
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.
|
||||
// Limiting fractional_progress makes sure the resulting point is also
|
||||
// limited to be between start and endpoint.
|
||||
status->fractional_progress = boundf(status->fractional_progress, 0, 1);
|
||||
|
||||
status->fractional_progress = dot / (dist_path * dist_path);
|
||||
status->error = normal[0] * diff_north + normal[1] * diff_east;
|
||||
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];
|
||||
|
||||
// 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->correction_direction[2] = 0;
|
||||
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];
|
||||
|
||||
// Now just want magnitude of error
|
||||
status->error = fabs(status->error);
|
||||
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]);
|
||||
|
||||
// Compute direction to travel
|
||||
status->path_direction[0] = path_north / dist_path;
|
||||
status->path_direction[1] = path_east / dist_path;
|
||||
status->path_direction[2] = 0;
|
||||
// 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;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
|
||||
@ -370,22 +370,19 @@ 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 speed;
|
||||
switch (pathDesired.Mode) {
|
||||
@ -414,36 +411,45 @@ static void updatePathVelocity()
|
||||
}
|
||||
|
||||
VelocityDesiredData velocityDesired;
|
||||
velocityDesired.North = progress.path_direction[0] * speed;
|
||||
velocityDesired.East = progress.path_direction[1] * speed;
|
||||
velocityDesired.Down = progress.path_direction[2] * speed;
|
||||
|
||||
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 = velocityDesired.Down + (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);
|
||||
}
|
||||
|
||||
|
||||
@ -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