mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-18 03:52:11 +01:00
OP-1156 fix path logic to not deviate from correct altitude too much
This commit is contained in:
Corvus Corax
parent
09e8b38407
commit
b562fcb02e
@ -30,10 +30,10 @@
|
||||
struct path_status {
|
||||
float fractional_progress;
|
||||
float error;
|
||||
float correction_direction[3];
|
||||
float path_direction[3];
|
||||
float path_vector[3];
|
||||
float correction_vector[3];
|
||||
};
|
||||
|
||||
void path_progress(float *start_point, float *end_point, float *cur_point, struct path_status *status, uint8_t mode);
|
||||
void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status);
|
||||
|
||||
#endif
|
||||
|
||||
@ -28,54 +28,51 @@
|
||||
#include <pios_math.h>
|
||||
#include <mathmisc.h>
|
||||
|
||||
#include "paths.h"
|
||||
|
||||
#include "uavobjectmanager.h" // <--.
|
||||
#include "pathdesired.h" // <-- needed only for correct ENUM macro usage with path modes (PATHDESIRED_MODE_xxx,
|
||||
#include "paths.h"
|
||||
// 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, 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);
|
||||
static void path_endpoint(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode);
|
||||
static void path_vector(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode);
|
||||
static void path_circle(PathDesiredData *path, 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] path PathDesired structure
|
||||
* @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)
|
||||
void path_progress(PathDesiredData *path, float *cur_point, struct path_status *status)
|
||||
{
|
||||
switch (mode) {
|
||||
switch (path->Mode) {
|
||||
case PATHDESIRED_MODE_FLYVECTOR:
|
||||
return path_vector(start_point, end_point, cur_point, status, true);
|
||||
return path_vector(path, cur_point, status, true);
|
||||
|
||||
break;
|
||||
case PATHDESIRED_MODE_DRIVEVECTOR:
|
||||
return path_vector(start_point, end_point, cur_point, status, false);
|
||||
return path_vector(path, cur_point, status, false);
|
||||
|
||||
break;
|
||||
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
|
||||
case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
|
||||
return path_circle(start_point, end_point, cur_point, status, 1);
|
||||
return path_circle(path, cur_point, status, 1);
|
||||
|
||||
break;
|
||||
case PATHDESIRED_MODE_FLYCIRCLELEFT:
|
||||
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
|
||||
return path_circle(start_point, end_point, cur_point, status, 0);
|
||||
return path_circle(path, cur_point, status, 0);
|
||||
|
||||
break;
|
||||
case PATHDESIRED_MODE_FLYENDPOINT:
|
||||
return path_endpoint(start_point, end_point, cur_point, status, true);
|
||||
return path_endpoint(path, 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, false);
|
||||
return path_endpoint(path, cur_point, status, false);
|
||||
|
||||
break;
|
||||
}
|
||||
@ -83,127 +80,120 @@ void path_progress(float *start_point, float *end_point, float *cur_point, struc
|
||||
|
||||
/**
|
||||
* @brief Compute progress towards endpoint. Deviation equals distance
|
||||
* @param[in] start_point Starting point
|
||||
* @param[in] end_point Ending point
|
||||
* @param[in] path PathDesired
|
||||
* @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, bool mode3D)
|
||||
static void path_endpoint(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D)
|
||||
{
|
||||
float path[3], diff[3];
|
||||
float diff[3];
|
||||
float dist_path, dist_diff;
|
||||
|
||||
// we do not correct in this mode
|
||||
status->correction_direction[0] = status->correction_direction[1] = status->correction_direction[2] = 0;
|
||||
|
||||
// Distance to go
|
||||
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;
|
||||
status->path_vector[0] = path->End.North - path->Start.North;
|
||||
status->path_vector[1] = path->End.East - path->Start.East;
|
||||
status->path_vector[2] = mode3D ? path->End.Down - path->Start.Down : 0.0f;
|
||||
|
||||
// Current progress location relative to end
|
||||
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;
|
||||
diff[0] = path->End.North - cur_point[0];
|
||||
diff[1] = path->End.East - cur_point[1];
|
||||
diff[2] = mode3D ? path->End.Down - cur_point[2] : 0.0f;
|
||||
|
||||
dist_diff = vector_lengthf(diff, 3);
|
||||
dist_path = vector_lengthf(path, 3);
|
||||
dist_path = vector_lengthf(status->path_vector, 3);
|
||||
|
||||
if (dist_diff < 1e-6f) {
|
||||
status->fractional_progress = 1;
|
||||
status->error = 0;
|
||||
status->path_direction[0] = status->path_direction[1] = 0;
|
||||
status->path_direction[2] = 0;
|
||||
status->fractional_progress = 1;
|
||||
status->error = 0.0f;
|
||||
status->correction_vector[0] = status->correction_vector[1] = status->correction_vector[2] = 0.0f;
|
||||
// we have no base movement direction in this mode
|
||||
status->path_vector[0] = status->path_vector[1] = status->path_vector[2] = 0.0f;
|
||||
|
||||
return;
|
||||
}
|
||||
|
||||
if (dist_path + 1 > dist_diff) {
|
||||
status->fractional_progress = 1 - dist_diff / (1 + dist_path);
|
||||
if (fmaxf(dist_path, 1.0f) > dist_diff) {
|
||||
status->fractional_progress = 1 - dist_diff / fmaxf(dist_path, 1.0f);
|
||||
} 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[0] / dist_diff;
|
||||
status->path_direction[1] = diff[1] / dist_diff;
|
||||
status->path_direction[2] = diff[2] / dist_diff;
|
||||
// Compute correction vector
|
||||
status->correction_vector[0] = diff[0];
|
||||
status->correction_vector[1] = diff[1];
|
||||
status->correction_vector[2] = diff[2];
|
||||
|
||||
// base movement direction in this mode is a constant velocity offset on top of correction in the same direction
|
||||
status->path_vector[0] = path->EndingVelocity * status->correction_vector[0] / dist_diff;
|
||||
status->path_vector[1] = path->EndingVelocity * status->correction_vector[1] / dist_diff;
|
||||
status->path_vector[2] = path->EndingVelocity * status->correction_vector[2] / dist_diff;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Compute progress along path and deviation from it
|
||||
* @param[in] start_point Starting point
|
||||
* @param[in] end_point Ending point
|
||||
* @param[in] path PathDesired
|
||||
* @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, bool mode3D)
|
||||
static void path_vector(PathDesiredData *path, float *cur_point, struct path_status *status, bool mode3D)
|
||||
{
|
||||
float path[3], diff[3];
|
||||
float diff[3];
|
||||
float dist_path;
|
||||
float dot;
|
||||
float velocity;
|
||||
float track_point[3];
|
||||
|
||||
// Distance to go
|
||||
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;
|
||||
status->path_vector[0] = path->End.North - path->Start.North;
|
||||
status->path_vector[1] = path->End.East - path->Start.East;
|
||||
status->path_vector[2] = mode3D ? path->End.Down - path->Start.Down : 0.0f;
|
||||
|
||||
// Current progress location relative to start
|
||||
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;
|
||||
diff[0] = cur_point[0] - path->Start.North;
|
||||
diff[1] = cur_point[1] - path->Start.East;
|
||||
diff[2] = mode3D ? cur_point[2] - path->Start.Down : 0.0f;
|
||||
|
||||
dot = path[0] * diff[0] + path[1] * diff[1] + path[2] * diff[2];
|
||||
dist_path = vector_lengthf(path, 3);
|
||||
dot = status->path_vector[0] * diff[0] + status->path_vector[1] * diff[1] + status->path_vector[2] * diff[2];
|
||||
dist_path = vector_lengthf(status->path_vector, 3);
|
||||
|
||||
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, mode3D);
|
||||
path_endpoint(path, cur_point, status, mode3D);
|
||||
status->fractional_progress = 1;
|
||||
return;
|
||||
}
|
||||
|
||||
// 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];
|
||||
track_point[0] = status->fractional_progress * status->path_vector[0] + path->Start.North;
|
||||
track_point[1] = status->fractional_progress * status->path_vector[1] + path->Start.East;
|
||||
track_point[2] = status->fractional_progress * status->path_vector[2] + path->Start.Down;
|
||||
|
||||
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];
|
||||
status->correction_vector[0] = track_point[0] - cur_point[0];
|
||||
status->correction_vector[1] = track_point[1] - cur_point[1];
|
||||
status->correction_vector[2] = track_point[2] - cur_point[2];
|
||||
|
||||
status->error = vector_lengthf(status->correction_direction, 3);
|
||||
status->error = vector_lengthf(status->correction_vector, 3);
|
||||
|
||||
// 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] = 0;
|
||||
}
|
||||
// correct movement vector to current velocity
|
||||
velocity = path->StartingVelocity + boundf(status->fractional_progress, 0.0f, 1.0f) * (path->EndingVelocity - path->StartingVelocity);
|
||||
status->path_vector[0] = velocity * status->path_vector[0] / dist_path;
|
||||
status->path_vector[1] = velocity * status->path_vector[1] / dist_path;
|
||||
status->path_vector[2] = velocity * status->path_vector[2] / dist_path;
|
||||
}
|
||||
|
||||
/**
|
||||
* @brief Compute progress along circular path and deviation from it
|
||||
* @param[in] start_point Starting point
|
||||
* @param[in] end_point Center point
|
||||
* @param[in] path PathDesired
|
||||
* @param[in] cur_point Current location
|
||||
* @param[out] status Structure containing progress along path and deviation
|
||||
*/
|
||||
static void path_circle(float *start_point, float *end_point, float *cur_point, struct path_status *status, bool clockwise)
|
||||
static void path_circle(PathDesiredData *path, float *cur_point, struct path_status *status, bool clockwise)
|
||||
{
|
||||
float radius_north, radius_east, diff_north, diff_east, diff_down;
|
||||
float radius, cradius;
|
||||
@ -212,30 +202,30 @@ static void path_circle(float *start_point, float *end_point, float *cur_point,
|
||||
float a_diff, a_radius;
|
||||
|
||||
// Radius
|
||||
radius_north = end_point[0] - start_point[0];
|
||||
radius_east = end_point[1] - start_point[1];
|
||||
radius_north = path->End.North - path->Start.North;
|
||||
radius_east = path->End.East - path->Start.East;
|
||||
|
||||
// Current location relative to center
|
||||
diff_north = cur_point[0] - end_point[0];
|
||||
diff_east = cur_point[1] - end_point[1];
|
||||
diff_down = cur_point[2] - end_point[2];
|
||||
diff_north = cur_point[0] - path->End.North;
|
||||
diff_east = cur_point[1] - path->End.East;
|
||||
diff_down = cur_point[2] - path->End.Down;
|
||||
|
||||
radius = sqrtf(squaref(radius_north) + squaref(radius_east));
|
||||
cradius = sqrtf(squaref(diff_north) + squaref(diff_east));
|
||||
|
||||
// circles are always horizontal (for now - TODO: allow 3d circles - problem: clockwise/counterclockwise does no longer apply)
|
||||
status->path_direction[2] = 0.0f;
|
||||
status->path_vector[2] = 0.0f;
|
||||
|
||||
// error is current radius minus wanted radius - positive if too close
|
||||
status->error = radius - cradius;
|
||||
|
||||
if (cradius < 1e-6f) {
|
||||
// cradius is zero, just fly somewhere and make sure correction is still a normal
|
||||
status->fractional_progress = 1;
|
||||
status->correction_direction[0] = 0;
|
||||
status->correction_direction[1] = 1;
|
||||
status->path_direction[0] = 1;
|
||||
status->path_direction[1] = 0;
|
||||
// cradius is zero, just fly somewhere
|
||||
status->fractional_progress = 1;
|
||||
status->correction_vector[0] = 0;
|
||||
status->correction_vector[1] = 0;
|
||||
status->path_vector[0] = path->EndingVelocity;
|
||||
status->path_vector[1] = 0;
|
||||
} else {
|
||||
if (clockwise) {
|
||||
// Compute the normal to the radius clockwise
|
||||
@ -270,20 +260,18 @@ static void path_circle(float *start_point, float *end_point, float *cur_point,
|
||||
progress = 1.0f - progress;
|
||||
}
|
||||
|
||||
status->fractional_progress = progress;
|
||||
status->fractional_progress = progress;
|
||||
|
||||
// Compute direction to travel
|
||||
status->path_direction[0] = normal[0];
|
||||
status->path_direction[1] = normal[1];
|
||||
status->path_vector[0] = normal[0] * path->EndingVelocity;
|
||||
status->path_vector[1] = normal[1] * path->EndingVelocity;
|
||||
|
||||
// Compute direction to correct error
|
||||
status->correction_direction[0] = status->error * diff_north / cradius;
|
||||
status->correction_direction[1] = status->error * diff_east / cradius;
|
||||
status->correction_vector[0] = status->error * diff_north / cradius;
|
||||
status->correction_vector[1] = status->error * diff_east / cradius;
|
||||
}
|
||||
|
||||
status->correction_direction[2] = -diff_down;
|
||||
|
||||
vector_normalizef(status->correction_direction, 3);
|
||||
status->correction_vector[2] = -diff_down;
|
||||
|
||||
status->error = fabs(status->error);
|
||||
}
|
||||
|
||||
@ -70,8 +70,6 @@ void plan_setup_positionHold()
|
||||
|
||||
FlightModeSettingsPositionHoldOffsetData offset;
|
||||
FlightModeSettingsPositionHoldOffsetGet(&offset);
|
||||
float startingVelocity;
|
||||
FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity);
|
||||
|
||||
pathDesired.End.North = positionState.North;
|
||||
pathDesired.End.East = positionState.East;
|
||||
@ -79,7 +77,7 @@ void plan_setup_positionHold()
|
||||
pathDesired.Start.North = positionState.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
|
||||
pathDesired.Start.East = positionState.East;
|
||||
pathDesired.Start.Down = positionState.Down;
|
||||
pathDesired.StartingVelocity = startingVelocity;
|
||||
pathDesired.StartingVelocity = 0.0f;
|
||||
pathDesired.EndingVelocity = 0.0f;
|
||||
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
|
||||
|
||||
@ -110,8 +108,6 @@ void plan_setup_returnToBase()
|
||||
destDown = MIN(positionStateDown, takeoffLocation.Down) - destDown;
|
||||
FlightModeSettingsPositionHoldOffsetData offset;
|
||||
FlightModeSettingsPositionHoldOffsetGet(&offset);
|
||||
float startingVelocity;
|
||||
FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity);
|
||||
|
||||
pathDesired.End.North = takeoffLocation.North;
|
||||
pathDesired.End.East = takeoffLocation.East;
|
||||
@ -121,7 +117,7 @@ void plan_setup_returnToBase()
|
||||
pathDesired.Start.East = takeoffLocation.East;
|
||||
pathDesired.Start.Down = destDown;
|
||||
|
||||
pathDesired.StartingVelocity = startingVelocity;
|
||||
pathDesired.StartingVelocity = 0.0f;
|
||||
pathDesired.EndingVelocity = 0.0f;
|
||||
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
|
||||
|
||||
@ -131,15 +127,12 @@ void plan_setup_returnToBase()
|
||||
static PiOSDeltatimeConfig landdT;
|
||||
void plan_setup_land()
|
||||
{
|
||||
float descendspeed;
|
||||
|
||||
plan_setup_positionHold();
|
||||
|
||||
FlightModeSettingsLandingVelocityGet(&descendspeed);
|
||||
PathDesiredData pathDesired;
|
||||
PathDesiredGet(&pathDesired);
|
||||
pathDesired.StartingVelocity = descendspeed;
|
||||
pathDesired.EndingVelocity = descendspeed;
|
||||
pathDesired.StartingVelocity = 0.0f;
|
||||
pathDesired.EndingVelocity = 0.0f;
|
||||
PathDesiredSet(&pathDesired);
|
||||
PIOS_DELTATIME_Init(&landdT, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA);
|
||||
}
|
||||
@ -383,8 +376,6 @@ void plan_setup_AutoCruise()
|
||||
|
||||
FlightModeSettingsPositionHoldOffsetData offset;
|
||||
FlightModeSettingsPositionHoldOffsetGet(&offset);
|
||||
float startingVelocity;
|
||||
FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity);
|
||||
|
||||
// initialization is flight in direction of the nose.
|
||||
// the velocity is not relevant, as it will be reset by the run function even during first call
|
||||
@ -404,7 +395,7 @@ void plan_setup_AutoCruise()
|
||||
pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter
|
||||
pathDesired.Start.East = pathDesired.End.East;
|
||||
pathDesired.Start.Down = pathDesired.End.Down;
|
||||
pathDesired.StartingVelocity = startingVelocity;
|
||||
pathDesired.StartingVelocity = 0.0f;
|
||||
pathDesired.EndingVelocity = 0.0f;
|
||||
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
|
||||
|
||||
|
||||
@ -53,6 +53,7 @@
|
||||
#include <CoordinateConversions.h>
|
||||
#include <pios_struct_helper.h>
|
||||
#include <sin_lookup.h>
|
||||
#include <pathdesired.h>
|
||||
#include <paths.h>
|
||||
#include <sanitycheck.h>
|
||||
|
||||
@ -62,7 +63,6 @@
|
||||
#include <vtolpathfollowersettings.h>
|
||||
#include <flightstatus.h>
|
||||
#include <pathstatus.h>
|
||||
#include <pathdesired.h>
|
||||
#include <positionstate.h>
|
||||
#include <velocitystate.h>
|
||||
#include <velocitydesired.h>
|
||||
@ -432,12 +432,10 @@ static float updatePathBearing()
|
||||
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);
|
||||
path_progress(&pathDesired, cur, &progress);
|
||||
|
||||
// atan2f always returns in between + and - 180 degrees
|
||||
float yaw = RAD2DEG(atan2f(progress.path_direction[1], progress.path_direction[0]));
|
||||
float yaw = RAD2DEG(atan2f(progress.path_vector[1], progress.path_vector[0]));
|
||||
// result is in between 0 and 360 degrees
|
||||
if (yaw < 0.0f) {
|
||||
yaw += 360.0f;
|
||||
@ -581,40 +579,13 @@ static void updatePathVelocity(float kFF, float kH, float kV, bool limited)
|
||||
positionState.Down + (velocityState.Down * kFF) };
|
||||
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);
|
||||
|
||||
float groundspeed;
|
||||
switch (pathDesired.Mode) {
|
||||
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
|
||||
case PATHDESIRED_MODE_DRIVECIRCLERIGHT:
|
||||
case PATHDESIRED_MODE_FLYCIRCLELEFT:
|
||||
case PATHDESIRED_MODE_DRIVECIRCLELEFT:
|
||||
groundspeed = pathDesired.EndingVelocity;
|
||||
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) *
|
||||
boundf(progress.fractional_progress, 0.0f, 1.0f);
|
||||
break;
|
||||
}
|
||||
// make sure groundspeed is not zero
|
||||
if (limited && groundspeed < 1e-6f) {
|
||||
groundspeed = 1e-6f;
|
||||
}
|
||||
path_progress(&pathDesired, cur, &progress);
|
||||
|
||||
// calculate velocity - can be zero if waypoints are too close
|
||||
VelocityDesiredData velocityDesired;
|
||||
velocityDesired.North = progress.path_direction[0];
|
||||
velocityDesired.East = progress.path_direction[1];
|
||||
velocityDesired.Down = progress.path_direction[2];
|
||||
|
||||
float error_speed_horizontal = progress.error * kH;
|
||||
float error_speed_vertical = progress.error * kV;
|
||||
velocityDesired.North = progress.path_vector[0];
|
||||
velocityDesired.East = progress.path_vector[1];
|
||||
velocityDesired.Down = progress.path_vector[2];
|
||||
|
||||
if (limited &&
|
||||
// if a plane is crossing its desired flightpath facing the wrong way (away from flight direction)
|
||||
@ -629,39 +600,27 @@ static void updatePathVelocity(float kFF, float kH, float kV, bool limited)
|
||||
// difference between path_direction and velocitystate >90 degrees ( 4th sector, facing away from everything )
|
||||
// fix: ignore correction, steer in path direction until the situation has become better (condition doesn't apply anymore)
|
||||
// calculating angles < 90 degrees through dot products
|
||||
((progress.path_direction[0] * velocityState.North + progress.path_direction[1] * velocityState.East + progress.path_direction[2] * velocityState.Down) < 0.0f) &&
|
||||
((progress.correction_direction[0] * velocityState.North + progress.correction_direction[1] * velocityState.East + progress.correction_direction[2] * velocityState.Down) < 0.0f)) {
|
||||
error_speed_horizontal = 0.0f;
|
||||
error_speed_vertical = 0.0f;
|
||||
}
|
||||
|
||||
// calculate correction - can also be zero if correction vector is 0 or no error present
|
||||
velocityDesired.North += progress.correction_direction[0] * error_speed_horizontal;
|
||||
velocityDesired.East += progress.correction_direction[1] * error_speed_horizontal;
|
||||
velocityDesired.Down += progress.correction_direction[2] * error_speed_vertical;
|
||||
|
||||
// scale to correct length
|
||||
float l = sqrtf(velocityDesired.North * velocityDesired.North + velocityDesired.East * velocityDesired.East + velocityDesired.Down * velocityDesired.Down);
|
||||
if (l > 1e-9f) {
|
||||
velocityDesired.North *= groundspeed / l;
|
||||
velocityDesired.East *= groundspeed / l;
|
||||
velocityDesired.Down *= groundspeed / l;
|
||||
(vector_lengthf(progress.path_vector, 2) > 1e-6f) &&
|
||||
((progress.path_vector[0] * velocityState.North + progress.path_vector[1] * velocityState.East) < 0.0f) &&
|
||||
((progress.correction_vector[0] * velocityState.North + progress.correction_vector[1] * velocityState.East) < 0.0f)) {
|
||||
;
|
||||
} else {
|
||||
velocityDesired.North = 0.0f;
|
||||
velocityDesired.East = 0.0f;
|
||||
velocityDesired.Down = 0.0f;
|
||||
// calculate correction
|
||||
velocityDesired.North += progress.correction_vector[0] * kH;
|
||||
velocityDesired.East += progress.correction_vector[1] * kH;
|
||||
}
|
||||
velocityDesired.Down += progress.correction_vector[2] * kV;
|
||||
|
||||
// update pathstatus
|
||||
pathStatus.error = progress.error;
|
||||
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.path_direction_north = progress.path_vector[0];
|
||||
pathStatus.path_direction_east = progress.path_vector[1];
|
||||
pathStatus.path_direction_down = progress.path_vector[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];
|
||||
pathStatus.correction_direction_north = progress.correction_vector[0];
|
||||
pathStatus.correction_direction_east = progress.correction_vector[1];
|
||||
pathStatus.correction_direction_down = progress.correction_vector[2];
|
||||
|
||||
VelocityDesiredSet(&velocityDesired);
|
||||
}
|
||||
|
||||
@ -525,9 +525,8 @@ static uint8_t conditionLegRemaining()
|
||||
float cur[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);
|
||||
path_progress(&pathDesired,
|
||||
cur, &progress);
|
||||
if (progress.fractional_progress >= 1.0f - pathAction.ConditionParameters[0]) {
|
||||
return true;
|
||||
}
|
||||
@ -550,9 +549,8 @@ static uint8_t conditionBelowError()
|
||||
float cur[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);
|
||||
path_progress(&pathDesired,
|
||||
cur, &progress);
|
||||
if (progress.error <= pathAction.ConditionParameters[0]) {
|
||||
return true;
|
||||
}
|
||||
|
||||
@ -114,8 +114,6 @@
|
||||
<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 -->
|
||||
<field name="PositionHoldStartingVelocity" units="m/s" type="float" elements="1" defaultvalue="20"/>
|
||||
<!-- currently ignored by vtolpathfollower -->
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
<telemetryflight acked="true" updatemode="onchange" period="0"/>
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user