mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
OP-1156 refactored pathfollower - new unified module as delayed callback - compiles but untested
This commit is contained in:
Corvus Corax
parent
a3d18bb578
commit
8944419b0f
@ -52,4 +52,31 @@ static inline float boundf(float val, float boundary1, float boundary2)
|
||||
return val;
|
||||
}
|
||||
|
||||
static inline float squaref(float x)
|
||||
{
|
||||
return x * x;
|
||||
}
|
||||
|
||||
static inline float vector_lengthf(float *vector, const uint8_t dim)
|
||||
{
|
||||
float length = 0.0f;
|
||||
|
||||
for (int t = 0; t < dim; t++) {
|
||||
length += squaref(vector[t]);
|
||||
}
|
||||
return sqrtf(length);
|
||||
}
|
||||
|
||||
static inline void vector_normalizef(float *vector, const uint8_t dim)
|
||||
{
|
||||
float length = vector_lengthf(vector, dim);
|
||||
|
||||
if (length <= 0.0f || isnan(length)) {
|
||||
return;
|
||||
}
|
||||
for (int t = 0; t < dim; t++) {
|
||||
vector[t] /= length;
|
||||
}
|
||||
}
|
||||
|
||||
#endif /* MATHMISC_H */
|
||||
|
||||
@ -53,7 +53,7 @@ void path_progress(float *start_point, float *end_point, float *cur_point, struc
|
||||
case PATHDESIRED_MODE_FLYVECTOR:
|
||||
return path_vector(start_point, end_point, cur_point, status, true);
|
||||
|
||||
break;
|
||||
break;
|
||||
case PATHDESIRED_MODE_DRIVEVECTOR:
|
||||
return path_vector(start_point, end_point, cur_point, status, false);
|
||||
|
||||
@ -98,23 +98,23 @@ static void path_endpoint(float *start_point, float *end_point, float *cur_point
|
||||
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;
|
||||
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[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] = 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[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]);
|
||||
dist_diff = vector_lengthf(diff, 3);
|
||||
dist_path = vector_lengthf(path, 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] = 1;
|
||||
status->path_direction[0] = status->path_direction[1] = 0;
|
||||
status->path_direction[2] = 0;
|
||||
return;
|
||||
}
|
||||
|
||||
@ -147,23 +147,23 @@ static void path_vector(float *start_point, float *end_point, float *cur_point,
|
||||
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;
|
||||
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[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] - start_point[0];
|
||||
diff[1] = cur_point[1] - start_point[1];
|
||||
diff[2] = mode3D ? cur_point[2] - start_point[2] : 0;
|
||||
|
||||
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]);
|
||||
dot = path[0] * diff[0] + path[1] * diff[1] + path[2] * diff[2];
|
||||
dist_path = vector_lengthf(path, 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->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,
|
||||
@ -182,9 +182,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point,
|
||||
status->correction_direction[1] = track_point[1] - cur_point[1];
|
||||
status->correction_direction[2] = track_point[2] - cur_point[2];
|
||||
|
||||
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]);
|
||||
status->error = vector_lengthf(status->correction_direction, 3);
|
||||
|
||||
// Normalize correction_direction but avoid division by zero
|
||||
if (status->error > 1e-6f) {
|
||||
@ -194,7 +192,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point,
|
||||
} else {
|
||||
status->correction_direction[0] = 0;
|
||||
status->correction_direction[1] = 0;
|
||||
status->correction_direction[2] = 1;
|
||||
status->correction_direction[2] = 0;
|
||||
}
|
||||
}
|
||||
|
||||
@ -207,7 +205,7 @@ static void path_vector(float *start_point, float *end_point, float *cur_point,
|
||||
*/
|
||||
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_north, radius_east, diff_north, diff_east, diff_down;
|
||||
float radius, cradius;
|
||||
float normal[2];
|
||||
float progress;
|
||||
@ -220,70 +218,72 @@ static void path_circle(float *start_point, float *end_point, float *cur_point,
|
||||
// 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];
|
||||
|
||||
radius = sqrtf(powf(radius_north, 2) + powf(radius_east, 2));
|
||||
cradius = sqrtf(powf(diff_north, 2) + powf(diff_east, 2));
|
||||
radius = sqrtf(squaref(radius_north) + squaref(radius_east));
|
||||
cradius = sqrtf(squaref(diff_north) + squaref(diff_east));
|
||||
|
||||
if (cradius < 1e-6f) {
|
||||
// cradius is zero, just fly somewhere and make sure correction is still a normal
|
||||
status->fractional_progress = 1;
|
||||
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;
|
||||
}
|
||||
|
||||
if (clockwise) {
|
||||
// Compute the normal to the radius clockwise
|
||||
normal[0] = -diff_east / cradius;
|
||||
normal[1] = diff_north / cradius;
|
||||
} else {
|
||||
// Compute the normal to the radius counter clockwise
|
||||
normal[0] = diff_east / cradius;
|
||||
normal[1] = -diff_north / cradius;
|
||||
}
|
||||
|
||||
// normalize progress to 0..1
|
||||
a_diff = atan2f(diff_north, diff_east);
|
||||
a_radius = atan2f(radius_north, radius_east);
|
||||
|
||||
if (a_diff < 0) {
|
||||
a_diff += 2.0f * M_PI_F;
|
||||
}
|
||||
if (a_radius < 0) {
|
||||
a_radius += 2.0f * M_PI_F;
|
||||
}
|
||||
|
||||
progress = (a_diff - a_radius + M_PI_F) / (2.0f * M_PI_F);
|
||||
|
||||
if (progress < 0) {
|
||||
progress += 1.0f;
|
||||
} else if (progress >= 1.0f) {
|
||||
progress -= 1.0f;
|
||||
}
|
||||
|
||||
if (clockwise) {
|
||||
progress = 1 - progress;
|
||||
}
|
||||
|
||||
status->fractional_progress = progress;
|
||||
// circles are always horizontal (for now - TODO: allow 3d circles - problem: clockwise/counterclockwise does no longer apply)
|
||||
status->path_direction[2] = 0.0f;
|
||||
|
||||
// error is current radius minus wanted radius - positive if too close
|
||||
status->error = radius - cradius;
|
||||
|
||||
// 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;
|
||||
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;
|
||||
} else {
|
||||
if (clockwise) {
|
||||
// Compute the normal to the radius clockwise
|
||||
normal[0] = -diff_east / cradius;
|
||||
normal[1] = diff_north / cradius;
|
||||
} else {
|
||||
// Compute the normal to the radius counter clockwise
|
||||
normal[0] = diff_east / cradius;
|
||||
normal[1] = -diff_north / cradius;
|
||||
}
|
||||
|
||||
// Compute direction to travel
|
||||
status->path_direction[0] = normal[0];
|
||||
status->path_direction[1] = normal[1];
|
||||
status->path_direction[2] = 0;
|
||||
// normalize progress to 0..1
|
||||
a_diff = atan2f(diff_north, diff_east);
|
||||
a_radius = atan2f(radius_north, radius_east);
|
||||
|
||||
if (a_diff < 0) {
|
||||
a_diff += 2.0f * M_PI_F;
|
||||
}
|
||||
if (a_radius < 0) {
|
||||
a_radius += 2.0f * M_PI_F;
|
||||
}
|
||||
|
||||
progress = (a_diff - a_radius + M_PI_F) / (2.0f * M_PI_F);
|
||||
|
||||
if (progress < 0.0f) {
|
||||
progress += 1.0f;
|
||||
} else if (progress >= 1.0f) {
|
||||
progress -= 1.0f;
|
||||
}
|
||||
|
||||
if (clockwise) {
|
||||
progress = 1.0f - progress;
|
||||
}
|
||||
|
||||
status->fractional_progress = progress;
|
||||
|
||||
// Compute direction to travel
|
||||
status->path_direction[0] = normal[0];
|
||||
status->path_direction[1] = normal[1];
|
||||
|
||||
// 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_direction[2] = -diff_down;
|
||||
|
||||
vector_normalizef(status->correction_direction, 3);
|
||||
|
||||
status->error = fabs(status->error);
|
||||
}
|
||||
|
||||
@ -132,6 +132,7 @@ static PiOSDeltatimeConfig landdT;
|
||||
void plan_setup_land()
|
||||
{
|
||||
float descendspeed;
|
||||
|
||||
plan_setup_positionHold();
|
||||
|
||||
FlightModeSettingsLandingVelocityGet(&descendspeed);
|
||||
|
||||
@ -51,12 +51,29 @@
|
||||
#include <math.h>
|
||||
#include <pid.h>
|
||||
#include <CoordinateConversions.h>
|
||||
#include <pios_struct_helper.h>
|
||||
#include <sin_lookup.h>
|
||||
#include <paths.h>
|
||||
#include <sanitycheck.h>
|
||||
|
||||
|
||||
#include <fixedwingpathfollowersettings.h>
|
||||
#include <fixedwingpathfollowerstatus.h>
|
||||
#include <vtolpathfollowersettings.h>
|
||||
#include <flightstatus.h>
|
||||
#include <pathstatus.h>
|
||||
#include <pathdesired.h>
|
||||
#include <positionstate.h>
|
||||
#include <velocitystate.h>
|
||||
#include <velocitydesired.h>
|
||||
#include <stabilizationdesired.h>
|
||||
#include <airspeedstate.h>
|
||||
#include <attitudestate.h>
|
||||
#include <takeofflocation.h>
|
||||
#include <poilocation.h>
|
||||
#include <manualcontrolcommand.h>
|
||||
#include <systemsettings.h>
|
||||
#include <stabilizationbank.h>
|
||||
|
||||
|
||||
// Private constants
|
||||
@ -67,16 +84,53 @@
|
||||
#define PF_IDLE_UPDATE_RATE_MS 100
|
||||
|
||||
#define STACK_SIZE_BYTES 2048
|
||||
|
||||
#define DEADBAND_HIGH 0.10f
|
||||
#define DEADBAND_LOW -0.10f
|
||||
// Private types
|
||||
|
||||
struct Integrals {
|
||||
float vel[3];
|
||||
float course;
|
||||
float speed;
|
||||
float power;
|
||||
float airspeed;
|
||||
float poiRadius;
|
||||
bool vtolEmergencyFallback;
|
||||
};
|
||||
|
||||
|
||||
// Private variables
|
||||
static DelayedCallbackInfo *pathFollowerCBInfo;
|
||||
|
||||
static uint32_t updatePeriod = PF_IDLE_UPDATE_RATE_MS;
|
||||
static struct Integrals i;
|
||||
static PathStatusData pathStatus;
|
||||
static PathDesiredData pathDesired;
|
||||
static FixedWingPathFollowerSettingsData fixedWingPathFollowerSettings;
|
||||
static VtolPathFollowerSettingsData vtolPathFollowerSettings;
|
||||
|
||||
// correct speed by measured airspeed
|
||||
static float indicatedAirspeedStateBias = 0.0f;
|
||||
|
||||
|
||||
// Private functions
|
||||
static void pathFollowerTask(void);
|
||||
static void resetIntegrals();
|
||||
static void SettingsUpdatedCb(UAVObjEvent *ev);
|
||||
static uint8_t updateAutoPilotByFrameType();
|
||||
static uint8_t updateAutoPilotFixedWing();
|
||||
static uint8_t updateAutoPilotVtol();
|
||||
static float updateTailInBearing();
|
||||
static float updateCourseBearing();
|
||||
static float updatePOIBearing();
|
||||
static void processPOI();
|
||||
static void updatePathVelocity(float kFF, float kH, float kV, bool limited);
|
||||
static uint8_t updateFixedDesiredAttitude();
|
||||
static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction);
|
||||
static void updateFixedAttitude();
|
||||
static void updateVtolDesiredAttitudeEmergencyFallback();
|
||||
static void airspeedStateUpdatedCb(UAVObjEvent *ev);
|
||||
static bool correctCourse(float *C, float *V, float *F, float s);
|
||||
|
||||
/**
|
||||
* Initialise the module, called on startup
|
||||
@ -85,6 +139,7 @@ static void SettingsUpdatedCb(UAVObjEvent *ev);
|
||||
int32_t PathFollowerStart()
|
||||
{
|
||||
// Start main task
|
||||
PathStatusGet(&pathStatus);
|
||||
SettingsUpdatedCb(NULL);
|
||||
PIOS_CALLBACKSCHEDULER_Dispatch(pathFollowerCBInfo);
|
||||
|
||||
@ -99,15 +154,32 @@ int32_t PathFollowerInitialize()
|
||||
{
|
||||
// initialize objects
|
||||
FixedWingPathFollowerSettingsInitialize();
|
||||
FixedWingPathFollowerStatusInitialize();
|
||||
VtolPathFollowerSettingsInitialize();
|
||||
FlightStatusInitialize();
|
||||
PathStatusInitialize();
|
||||
PathDesiredInitialize();
|
||||
PositionStateInitialize();
|
||||
VelocityStateInitialize();
|
||||
VelocityDesiredInitialize();
|
||||
StabilizationDesiredInitialize();
|
||||
AirspeedStateInitialize();
|
||||
AttitudeStateInitialize();
|
||||
TakeOffLocationInitialize();
|
||||
PoiLocationInitialize();
|
||||
ManualControlCommandInitialize();
|
||||
SystemSettingsInitialize();
|
||||
StabilizationBankInitialize();
|
||||
|
||||
// reset integrals
|
||||
resetIntegrals();
|
||||
|
||||
// Create object queue
|
||||
|
||||
pathFollowerCBInfo = PIOS_CALLBACKSCHEDULER_Create(&pathFollowerTask, CALLBACK_PRIORITY, CBTASK_PRIORITY, CALLBACKINFO_RUNNING_ALTITUDEHOLD, STACK_SIZE_BYTES);
|
||||
FixedWingPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
|
||||
VtolPathFollowerSettingsConnectCallback(&SettingsUpdatedCb);
|
||||
PathDesiredConnectCallback(SettingsUpdatedCb);
|
||||
AirspeedStateConnectCallback(airspeedStateUpdatedCb);
|
||||
|
||||
return 0;
|
||||
}
|
||||
@ -122,17 +194,1021 @@ static void pathFollowerTask(void)
|
||||
FlightStatusData flightStatus;
|
||||
|
||||
FlightStatusGet(&flightStatus);
|
||||
|
||||
if (flightStatus.ControlChain.PathFollower != FLIGHTSTATUS_CONTROLCHAIN_TRUE) {
|
||||
resetIntegrals();
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_UNINITIALISED);
|
||||
PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, PF_IDLE_UPDATE_RATE_MS, CALLBACK_UPDATEMODE_SOONER);
|
||||
return;
|
||||
}
|
||||
|
||||
// do pathfollower things here
|
||||
if (flightStatus.FlightMode == FLIGHTSTATUS_FLIGHTMODE_POI) { // TODO Hack from vtolpathfollower, move that shit into manualcontrol!
|
||||
processPOI();
|
||||
}
|
||||
|
||||
pathStatus.UID = pathDesired.UID;
|
||||
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
|
||||
switch (pathDesired.Mode) {
|
||||
case PATHDESIRED_MODE_FLYENDPOINT:
|
||||
case PATHDESIRED_MODE_FLYVECTOR:
|
||||
case PATHDESIRED_MODE_FLYCIRCLERIGHT:
|
||||
case PATHDESIRED_MODE_FLYCIRCLELEFT:
|
||||
{
|
||||
uint8_t result = updateAutoPilotByFrameType();
|
||||
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_FIXEDATTITUDE:
|
||||
updateFixedAttitude(pathDesired.ModeParameters);
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_OK);
|
||||
break;
|
||||
case PATHDESIRED_MODE_DISARMALARM:
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_CRITICAL);
|
||||
break;
|
||||
default:
|
||||
pathStatus.Status = PATHSTATUS_STATUS_CRITICAL;
|
||||
AlarmsSet(SYSTEMALARMS_ALARM_GUIDANCE, SYSTEMALARMS_ALARM_ERROR);
|
||||
break;
|
||||
}
|
||||
PathStatusSet(&pathStatus);
|
||||
|
||||
PIOS_CALLBACKSCHEDULER_Schedule(pathFollowerCBInfo, updatePeriod, CALLBACK_UPDATEMODE_SOONER);
|
||||
}
|
||||
|
||||
|
||||
static void SettingsUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
{
|
||||
// read in settings (TODO)
|
||||
FixedWingPathFollowerSettingsGet(&fixedWingPathFollowerSettings);
|
||||
VtolPathFollowerSettingsGet(&vtolPathFollowerSettings);
|
||||
PathDesiredGet(&pathDesired);
|
||||
}
|
||||
|
||||
|
||||
static void airspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev)
|
||||
{
|
||||
AirspeedStateData airspeedState;
|
||||
VelocityStateData velocityState;
|
||||
|
||||
AirspeedStateGet(&airspeedState);
|
||||
VelocityStateGet(&velocityState);
|
||||
float airspeedVector[2];
|
||||
float yaw;
|
||||
AttitudeStateYawGet(&yaw);
|
||||
airspeedVector[0] = cos_lookup_deg(yaw);
|
||||
airspeedVector[1] = sin_lookup_deg(yaw);
|
||||
// vector projection of groundspeed on airspeed vector to handle both forward and backwards movement
|
||||
float groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
|
||||
|
||||
indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeedProjection;
|
||||
// note - we do fly by Indicated Airspeed (== calibrated airspeed) however
|
||||
// since airspeed is updated less often than groundspeed, we use sudden
|
||||
// changes to groundspeed to offset the airspeed by the same measurement.
|
||||
// This has a side effect that in the absence of any airspeed updates, the
|
||||
// pathfollower will fly using groundspeed.
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* reset integrals
|
||||
*/
|
||||
static void resetIntegrals()
|
||||
{
|
||||
i.vel[0] = 0.0f;
|
||||
i.vel[1] = 0.0f;
|
||||
i.vel[2] = 0.0f;
|
||||
i.course = 0.0f;
|
||||
i.speed = 0.0f;
|
||||
i.power = 0.0f;
|
||||
i.airspeed = 0.0f;
|
||||
i.poiRadius = 0.0f;
|
||||
i.vtolEmergencyFallback = 0;
|
||||
}
|
||||
|
||||
static uint8_t updateAutoPilotByFrameType()
|
||||
{
|
||||
FrameType_t frameType = GetCurrentFrameType();
|
||||
|
||||
if (frameType == FRAME_TYPE_CUSTOM || frameType == FRAME_TYPE_GROUND) {
|
||||
switch (vtolPathFollowerSettings.TreatCustomCraftAs) {
|
||||
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_FIXEDWING:
|
||||
frameType = FRAME_TYPE_FIXED_WING;
|
||||
break;
|
||||
case VTOLPATHFOLLOWERSETTINGS_TREATCUSTOMCRAFTAS_VTOL:
|
||||
frameType = FRAME_TYPE_MULTIROTOR;
|
||||
break;
|
||||
}
|
||||
}
|
||||
switch (frameType) {
|
||||
case FRAME_TYPE_MULTIROTOR:
|
||||
case FRAME_TYPE_HELI:
|
||||
updatePeriod = vtolPathFollowerSettings.UpdatePeriod;
|
||||
return updateAutoPilotVtol();
|
||||
|
||||
break;
|
||||
case FRAME_TYPE_FIXED_WING:
|
||||
default:
|
||||
updatePeriod = fixedWingPathFollowerSettings.UpdatePeriod;
|
||||
return updateAutoPilotFixedWing();
|
||||
|
||||
break;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* fixed wing autopilot:
|
||||
* straight forward:
|
||||
* 1. update path velocity for limited motion crafts
|
||||
* 2. update attitude according to default fixed wing pathfollower algorithm
|
||||
*/
|
||||
static uint8_t updateAutoPilotFixedWing()
|
||||
{
|
||||
updatePathVelocity(fixedWingPathFollowerSettings.CourseFeedForward, fixedWingPathFollowerSettings.HorizontalPosP, fixedWingPathFollowerSettings.VerticalPosP, true);
|
||||
return updateFixedDesiredAttitude();
|
||||
}
|
||||
|
||||
/**
|
||||
* vtol autopilot
|
||||
* use hover capable algorithm with unlimeted movement calculation. if that fails (flyaway situation due to compass failure)
|
||||
* fall back to emergency fallback autopilot to keep minimum amount of flight control
|
||||
*/
|
||||
static uint8_t updateAutoPilotVtol()
|
||||
{
|
||||
if (!i.vtolEmergencyFallback) {
|
||||
if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ALWAYS) {
|
||||
updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, true);
|
||||
updateVtolDesiredAttitudeEmergencyFallback();
|
||||
return 1;
|
||||
} else {
|
||||
updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, false);
|
||||
uint8_t result = 1;
|
||||
bool yaw_attitude = true;
|
||||
float yaw = 0.0f;
|
||||
switch (vtolPathFollowerSettings.YawControl) {
|
||||
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_MANUAL:
|
||||
yaw_attitude = false;
|
||||
break;
|
||||
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_TAILIN:
|
||||
yaw = updateTailInBearing();
|
||||
break;
|
||||
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_COURSE:
|
||||
yaw = updateCourseBearing();
|
||||
break;
|
||||
case VTOLPATHFOLLOWERSETTINGS_YAWCONTROL_POI:
|
||||
yaw = updatePOIBearing();
|
||||
break;
|
||||
}
|
||||
updateVtolDesiredAttitude(yaw_attitude, yaw);
|
||||
if (!result && (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_ENABLED || vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST)) {
|
||||
i.vtolEmergencyFallback = true;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
} else {
|
||||
updatePathVelocity(vtolPathFollowerSettings.CourseFeedForward, vtolPathFollowerSettings.HorizontalPosP, vtolPathFollowerSettings.VerticalPosP, true);
|
||||
updateVtolDesiredAttitudeEmergencyFallback();
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Compute bearing of current takeoff location
|
||||
*/
|
||||
static float updateTailInBearing()
|
||||
{
|
||||
PositionStateData p;
|
||||
|
||||
PositionStateGet(&p);
|
||||
TakeOffLocationData t;
|
||||
TakeOffLocationGet(&t);
|
||||
// atan2f always returns in between + and - 180 degrees
|
||||
float yaw = RAD2DEG(atan2f(p.East - t.East, p.North - t.North));
|
||||
// result is in between 0 and 360 degrees
|
||||
if (yaw < 0.0f) {
|
||||
yaw += 360.0f;
|
||||
}
|
||||
return yaw;
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute bearing of current movement direction
|
||||
*/
|
||||
static float updateCourseBearing()
|
||||
{
|
||||
VelocityStateData v;
|
||||
|
||||
VelocityStateGet(&v);
|
||||
// atan2f always returns in between + and - 180 degrees
|
||||
float yaw = RAD2DEG(atan2f(v.East, v.North));
|
||||
// result is in between 0 and 360 degrees
|
||||
if (yaw < 0.0f) {
|
||||
yaw += 360.0f;
|
||||
}
|
||||
return yaw;
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute bearing between current position and POI
|
||||
*/
|
||||
static float updatePOIBearing()
|
||||
{
|
||||
PoiLocationData poi;
|
||||
|
||||
PoiLocationGet(&poi);
|
||||
PositionStateData positionState;
|
||||
PositionStateGet(&positionState);
|
||||
|
||||
float dT = updatePeriod / 1000.0f;
|
||||
float dLoc[3];
|
||||
float yaw = 0;
|
||||
/*float elevation = 0;*/
|
||||
|
||||
dLoc[0] = positionState.North - poi.North;
|
||||
dLoc[1] = positionState.East - poi.East;
|
||||
dLoc[2] = positionState.Down - poi.Down;
|
||||
|
||||
if (dLoc[1] < 0) {
|
||||
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
|
||||
} else {
|
||||
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f;
|
||||
}
|
||||
ManualControlCommandData manualControlData;
|
||||
ManualControlCommandGet(&manualControlData);
|
||||
|
||||
float pathAngle = 0;
|
||||
if (manualControlData.Roll > DEADBAND_HIGH) {
|
||||
pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
|
||||
} else if (manualControlData.Roll < DEADBAND_LOW) {
|
||||
pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
|
||||
}
|
||||
|
||||
return yaw + (pathAngle / 2.0f);
|
||||
}
|
||||
|
||||
/**
|
||||
* process POI control logic TODO: this should most likely go into manualcontrol!!!!
|
||||
* TODO: the whole process of POI handling likely needs cleanup and rethinking, might be broken since manualcontrol was refactored currently
|
||||
**/
|
||||
static void processPOI()
|
||||
{
|
||||
float dT = updatePeriod / 1000.0f;
|
||||
|
||||
|
||||
PositionStateData positionState;
|
||||
|
||||
PositionStateGet(&positionState);
|
||||
// CameraDesiredData cameraDesired;
|
||||
// CameraDesiredGet(&cameraDesired);
|
||||
StabilizationDesiredData stabDesired;
|
||||
StabilizationDesiredGet(&stabDesired);
|
||||
PoiLocationData poi;
|
||||
PoiLocationGet(&poi);
|
||||
|
||||
float dLoc[3];
|
||||
float yaw = 0;
|
||||
/*float elevation = 0;*/
|
||||
|
||||
dLoc[0] = positionState.North - poi.North;
|
||||
dLoc[1] = positionState.East - poi.East;
|
||||
dLoc[2] = positionState.Down - poi.Down;
|
||||
|
||||
if (dLoc[1] < 0) {
|
||||
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) + 180.0f;
|
||||
} else {
|
||||
yaw = RAD2DEG(atan2f(dLoc[1], dLoc[0])) - 180.0f;
|
||||
}
|
||||
|
||||
// distance
|
||||
float distance = sqrtf(powf(dLoc[0], 2.0f) + powf(dLoc[1], 2.0f));
|
||||
|
||||
ManualControlCommandData manualControlData;
|
||||
ManualControlCommandGet(&manualControlData);
|
||||
|
||||
float changeRadius = 0;
|
||||
// Move closer or further, radially
|
||||
if (manualControlData.Pitch > DEADBAND_HIGH) {
|
||||
changeRadius = (manualControlData.Pitch - DEADBAND_HIGH) * dT * 100.0f;
|
||||
} else if (manualControlData.Pitch < DEADBAND_LOW) {
|
||||
changeRadius = (manualControlData.Pitch - DEADBAND_LOW) * dT * 100.0f;
|
||||
}
|
||||
|
||||
// move along circular path
|
||||
float pathAngle = 0;
|
||||
if (manualControlData.Roll > DEADBAND_HIGH) {
|
||||
pathAngle = -(manualControlData.Roll - DEADBAND_HIGH) * dT * 300.0f;
|
||||
} else if (manualControlData.Roll < DEADBAND_LOW) {
|
||||
pathAngle = -(manualControlData.Roll - DEADBAND_LOW) * dT * 300.0f;
|
||||
} else if (manualControlData.Roll >= DEADBAND_LOW && manualControlData.Roll <= DEADBAND_HIGH) {
|
||||
// change radius only when not circling
|
||||
i.poiRadius = distance + changeRadius;
|
||||
}
|
||||
|
||||
// don't try to move any closer
|
||||
if (i.poiRadius >= 3.0f || changeRadius > 0) {
|
||||
if (fabsf(pathAngle) > 0.0f || fabsf(changeRadius) > 0.0f) {
|
||||
pathDesired.End.North = poi.North + (i.poiRadius * cosf(DEG2RAD(pathAngle + yaw - 180.0f)));
|
||||
pathDesired.End.East = poi.East + (i.poiRadius * sinf(DEG2RAD(pathAngle + yaw - 180.0f)));
|
||||
pathDesired.StartingVelocity = 1.0f;
|
||||
pathDesired.EndingVelocity = 0.0f;
|
||||
pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT;
|
||||
PathDesiredSet(&pathDesired);
|
||||
}
|
||||
}
|
||||
// not above
|
||||
if (distance >= 3.0f) {
|
||||
// You can feed this into camerastabilization
|
||||
/*elevation = RAD2DEG(atan2f(dLoc[2],distance));*/
|
||||
|
||||
// cameraDesired.Yaw=yaw;
|
||||
// cameraDesired.PitchOrServo2=elevation;
|
||||
|
||||
// CameraDesiredSet(&cameraDesired);
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute desired velocity from the current position and path
|
||||
*/
|
||||
static void updatePathVelocity(float kFF, float kH, float kV, bool limited)
|
||||
{
|
||||
PositionStateData positionState;
|
||||
|
||||
PositionStateGet(&positionState);
|
||||
VelocityStateData velocityState;
|
||||
VelocityStateGet(&velocityState);
|
||||
|
||||
// look ahead kFF seconds
|
||||
float cur[3] = { positionState.North + (velocityState.North * kFF),
|
||||
positionState.East + (velocityState.East * kFF),
|
||||
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;
|
||||
}
|
||||
|
||||
// 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;
|
||||
|
||||
if (limited &&
|
||||
// if a plane is crossing its desired flightpath facing the wrong way (away from flight direction)
|
||||
// it would turn towards the flightpath to get on its desired course. This however would reverse the correction vector
|
||||
// once it crosses the flightpath again, which would make it again turn towards the flightpath (but away from its desired heading)
|
||||
// leading to an S-shape snake course the wrong way
|
||||
// this only happens especially if HorizontalPosP is too high, as otherwise the angle between velocity desired and path_direction won't
|
||||
// turn steep unless there is enough space complete the turn before crossing the flightpath
|
||||
// in this case the plane effectively needs to be turned around
|
||||
// indicators:
|
||||
// difference between correction_direction and velocitystate >90 degrees and
|
||||
// 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;
|
||||
} else {
|
||||
velocityDesired.North = 0.0f;
|
||||
velocityDesired.East = 0.0f;
|
||||
velocityDesired.Down = 0.0f;
|
||||
}
|
||||
|
||||
// update pathstatus
|
||||
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);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Compute desired attitude from the desired velocity for fixed wing craft
|
||||
*/
|
||||
static uint8_t updateFixedDesiredAttitude()
|
||||
{
|
||||
uint8_t result = 1;
|
||||
|
||||
const float dT = updatePeriod / 1000.0f; // Convert from [ms] to [s]
|
||||
|
||||
VelocityDesiredData velocityDesired;
|
||||
VelocityStateData velocityState;
|
||||
StabilizationDesiredData stabDesired;
|
||||
AttitudeStateData attitudeState;
|
||||
FixedWingPathFollowerStatusData fixedWingPathFollowerStatus;
|
||||
AirspeedStateData airspeedState;
|
||||
SystemSettingsData systemSettings;
|
||||
|
||||
float groundspeedProjection;
|
||||
float indicatedAirspeedState;
|
||||
float indicatedAirspeedDesired;
|
||||
float airspeedError;
|
||||
|
||||
float pitchCommand;
|
||||
|
||||
float descentspeedDesired;
|
||||
float descentspeedError;
|
||||
float powerCommand;
|
||||
|
||||
float airspeedVector[2];
|
||||
float fluidMovement[2];
|
||||
float courseComponent[2];
|
||||
float courseError;
|
||||
float courseCommand;
|
||||
|
||||
FixedWingPathFollowerStatusGet(&fixedWingPathFollowerStatus);
|
||||
|
||||
VelocityStateGet(&velocityState);
|
||||
StabilizationDesiredGet(&stabDesired);
|
||||
VelocityDesiredGet(&velocityDesired);
|
||||
AttitudeStateGet(&attitudeState);
|
||||
AirspeedStateGet(&airspeedState);
|
||||
SystemSettingsGet(&systemSettings);
|
||||
|
||||
/**
|
||||
* Compute speed error and course
|
||||
*/
|
||||
// missing sensors for airspeed-direction we have to assume within
|
||||
// reasonable error that measured airspeed is actually the airspeed
|
||||
// component in forward pointing direction
|
||||
// airspeedVector is normalized
|
||||
airspeedVector[0] = cos_lookup_deg(attitudeState.Yaw);
|
||||
airspeedVector[1] = sin_lookup_deg(attitudeState.Yaw);
|
||||
|
||||
// current ground speed projected in forward direction
|
||||
groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
|
||||
|
||||
// note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
|
||||
// but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction
|
||||
// than airspeed and gps sensors alone
|
||||
indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
|
||||
|
||||
// fluidMovement is a vector describing the aproximate movement vector of
|
||||
// the surrounding fluid in 2d space (aka wind vector)
|
||||
fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]);
|
||||
fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]);
|
||||
|
||||
// calculate the movement vector we need to fly to reach velocityDesired -
|
||||
// taking fluidMovement into account
|
||||
courseComponent[0] = velocityDesired.North - fluidMovement[0];
|
||||
courseComponent[1] = velocityDesired.East - fluidMovement[1];
|
||||
|
||||
indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]),
|
||||
fixedWingPathFollowerSettings.HorizontalVelMin,
|
||||
fixedWingPathFollowerSettings.HorizontalVelMax);
|
||||
|
||||
// if we could fly at arbitrary speeds, we'd just have to move towards the
|
||||
// courseComponent vector as previously calculated and we'd be fine
|
||||
// unfortunately however we are bound by min and max air speed limits, so
|
||||
// we need to recalculate the correct course to meet at least the
|
||||
// velocityDesired vector direction at our current speed
|
||||
// this overwrites courseComponent
|
||||
bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired);
|
||||
|
||||
// Error condition: wind speed too high, we can't go where we want anymore
|
||||
fixedWingPathFollowerStatus.Errors.Wind = 0;
|
||||
if ((!valid) &&
|
||||
fixedWingPathFollowerSettings.Safetymargins.Wind > 0.5f) { // alarm switched on
|
||||
fixedWingPathFollowerStatus.Errors.Wind = 1;
|
||||
result = 0;
|
||||
}
|
||||
|
||||
// Airspeed error
|
||||
airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
|
||||
|
||||
// Vertical speed error
|
||||
descentspeedDesired = boundf(
|
||||
velocityDesired.Down,
|
||||
-fixedWingPathFollowerSettings.VerticalVelMax,
|
||||
fixedWingPathFollowerSettings.VerticalVelMax);
|
||||
descentspeedError = descentspeedDesired - velocityState.Down;
|
||||
|
||||
// Error condition: plane too slow or too fast
|
||||
fixedWingPathFollowerStatus.Errors.Highspeed = 0;
|
||||
fixedWingPathFollowerStatus.Errors.Lowspeed = 0;
|
||||
if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingPathFollowerSettings.Safetymargins.Overspeed) {
|
||||
fixedWingPathFollowerStatus.Errors.Overspeed = 1;
|
||||
result = 0;
|
||||
}
|
||||
if (indicatedAirspeedState > fixedWingPathFollowerSettings.HorizontalVelMax * fixedWingPathFollowerSettings.Safetymargins.Highspeed) {
|
||||
fixedWingPathFollowerStatus.Errors.Highspeed = 1;
|
||||
result = 0;
|
||||
}
|
||||
if (indicatedAirspeedState < fixedWingPathFollowerSettings.HorizontalVelMin * fixedWingPathFollowerSettings.Safetymargins.Lowspeed) {
|
||||
fixedWingPathFollowerStatus.Errors.Lowspeed = 1;
|
||||
result = 0;
|
||||
}
|
||||
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingPathFollowerSettings.Safetymargins.Stallspeed) {
|
||||
fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
|
||||
result = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute desired thrust command
|
||||
*/
|
||||
// compute saturated integral error thrust response. Make integral leaky for better performance. Approximately 30s time constant.
|
||||
if (fixedWingPathFollowerSettings.PowerPI.Ki > 0.0f) {
|
||||
i.power = boundf(i.power + -descentspeedError * dT,
|
||||
-fixedWingPathFollowerSettings.PowerPI.ILimit / fixedWingPathFollowerSettings.PowerPI.Ki,
|
||||
fixedWingPathFollowerSettings.PowerPI.ILimit / fixedWingPathFollowerSettings.PowerPI.Ki
|
||||
) * (1.0f - 1.0f / (1.0f + 30.0f / dT));
|
||||
} else {
|
||||
i.power = 0.0f;
|
||||
}
|
||||
|
||||
// Compute the cross feed from vertical speed to pitch, with saturation
|
||||
float speedErrorToPowerCommandComponent = boundf(
|
||||
(airspeedError / fixedWingPathFollowerSettings.HorizontalVelMin) * fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Kp,
|
||||
-fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max,
|
||||
fixedWingPathFollowerSettings.AirspeedToPowerCrossFeed.Max
|
||||
);
|
||||
|
||||
// Compute final thrust response
|
||||
powerCommand = -descentspeedError * fixedWingPathFollowerSettings.PowerPI.Kp +
|
||||
i.power * fixedWingPathFollowerSettings.PowerPI.Ki +
|
||||
speedErrorToPowerCommandComponent;
|
||||
|
||||
// Output internal state to telemetry
|
||||
fixedWingPathFollowerStatus.Error.Power = descentspeedError;
|
||||
fixedWingPathFollowerStatus.ErrorInt.Power = i.power;
|
||||
fixedWingPathFollowerStatus.Command.Power = powerCommand;
|
||||
|
||||
// set thrust
|
||||
stabDesired.Thrust = boundf(fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand,
|
||||
fixedWingPathFollowerSettings.ThrustLimit.Min,
|
||||
fixedWingPathFollowerSettings.ThrustLimit.Max);
|
||||
|
||||
// Error condition: plane cannot hold altitude at current speed.
|
||||
fixedWingPathFollowerStatus.Errors.Lowpower = 0;
|
||||
if (fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand >= fixedWingPathFollowerSettings.ThrustLimit.Max && // thrust at maximum
|
||||
velocityState.Down > 0.0f && // we ARE going down
|
||||
descentspeedDesired < 0.0f && // we WANT to go up
|
||||
airspeedError > 0.0f && // we are too slow already
|
||||
fixedWingPathFollowerSettings.Safetymargins.Lowpower > 0.5f) { // alarm switched on
|
||||
fixedWingPathFollowerStatus.Errors.Lowpower = 1;
|
||||
result = 0;
|
||||
}
|
||||
// Error condition: plane keeps climbing despite minimum thrust (opposite of above)
|
||||
fixedWingPathFollowerStatus.Errors.Highpower = 0;
|
||||
if (fixedWingPathFollowerSettings.ThrustLimit.Neutral + powerCommand <= fixedWingPathFollowerSettings.ThrustLimit.Min && // thrust at minimum
|
||||
velocityState.Down < 0.0f && // we ARE going up
|
||||
descentspeedDesired > 0.0f && // we WANT to go down
|
||||
airspeedError < 0.0f && // we are too fast already
|
||||
fixedWingPathFollowerSettings.Safetymargins.Highpower > 0.5f) { // alarm switched on
|
||||
fixedWingPathFollowerStatus.Errors.Highpower = 1;
|
||||
result = 0;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Compute desired pitch command
|
||||
*/
|
||||
if (fixedWingPathFollowerSettings.SpeedPI.Ki > 0) {
|
||||
// Integrate with saturation
|
||||
i.airspeed = boundf(i.airspeed + airspeedError * dT,
|
||||
-fixedWingPathFollowerSettings.SpeedPI.ILimit / fixedWingPathFollowerSettings.SpeedPI.Ki,
|
||||
fixedWingPathFollowerSettings.SpeedPI.ILimit / fixedWingPathFollowerSettings.SpeedPI.Ki);
|
||||
}
|
||||
|
||||
// Compute the cross feed from vertical speed to pitch, with saturation
|
||||
float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Kp,
|
||||
-fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max,
|
||||
fixedWingPathFollowerSettings.VerticalToPitchCrossFeed.Max
|
||||
);
|
||||
|
||||
// Compute the pitch command as err*Kp + errInt*Ki + X_feed.
|
||||
pitchCommand = -(airspeedError * fixedWingPathFollowerSettings.SpeedPI.Kp
|
||||
+ i.airspeed * fixedWingPathFollowerSettings.SpeedPI.Ki
|
||||
) + verticalSpeedToPitchCommandComponent;
|
||||
|
||||
fixedWingPathFollowerStatus.Error.Speed = airspeedError;
|
||||
fixedWingPathFollowerStatus.ErrorInt.Speed = i.airspeed;
|
||||
fixedWingPathFollowerStatus.Command.Speed = pitchCommand;
|
||||
|
||||
stabDesired.Pitch = boundf(fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand,
|
||||
fixedWingPathFollowerSettings.PitchLimit.Min,
|
||||
fixedWingPathFollowerSettings.PitchLimit.Max);
|
||||
|
||||
// Error condition: high speed dive
|
||||
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0;
|
||||
if (fixedWingPathFollowerSettings.PitchLimit.Neutral + pitchCommand >= fixedWingPathFollowerSettings.PitchLimit.Max && // pitch demand is full up
|
||||
velocityState.Down > 0.0f && // we ARE going down
|
||||
descentspeedDesired < 0.0f && // we WANT to go up
|
||||
airspeedError < 0.0f && // we are too fast already
|
||||
fixedWingPathFollowerSettings.Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
|
||||
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1;
|
||||
result = 0;
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute desired roll command
|
||||
*/
|
||||
courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
|
||||
|
||||
if (courseError < -180.0f) {
|
||||
courseError += 360.0f;
|
||||
}
|
||||
if (courseError > 180.0f) {
|
||||
courseError -= 360.0f;
|
||||
}
|
||||
|
||||
// overlap calculation. Theres a dead zone behind the craft where the
|
||||
// counter-yawing of some craft while rolling could render a desired right
|
||||
// turn into a desired left turn. Making the turn direction based on
|
||||
// current roll angle keeps the plane committed to a direction once chosen
|
||||
if (courseError < -180.0f + (fixedWingPathFollowerSettings.ReverseCourseOverlap * 0.5f)
|
||||
&& attitudeState.Roll > 0.0f) {
|
||||
courseError += 360.0f;
|
||||
}
|
||||
if (courseError > 180.0f - (fixedWingPathFollowerSettings.ReverseCourseOverlap * 0.5f)
|
||||
&& attitudeState.Roll < 0.0f) {
|
||||
courseError -= 360.0f;
|
||||
}
|
||||
|
||||
i.course = boundf(i.course + courseError * dT * fixedWingPathFollowerSettings.CoursePI.Ki,
|
||||
-fixedWingPathFollowerSettings.CoursePI.ILimit,
|
||||
fixedWingPathFollowerSettings.CoursePI.ILimit);
|
||||
courseCommand = (courseError * fixedWingPathFollowerSettings.CoursePI.Kp +
|
||||
i.course);
|
||||
|
||||
fixedWingPathFollowerStatus.Error.Course = courseError;
|
||||
fixedWingPathFollowerStatus.ErrorInt.Course = i.course;
|
||||
fixedWingPathFollowerStatus.Command.Course = courseCommand;
|
||||
|
||||
stabDesired.Roll = boundf(fixedWingPathFollowerSettings.RollLimit.Neutral +
|
||||
courseCommand,
|
||||
fixedWingPathFollowerSettings.RollLimit.Min,
|
||||
fixedWingPathFollowerSettings.RollLimit.Max);
|
||||
|
||||
// TODO: find a check to determine loss of directional control. Likely needs some check of derivative
|
||||
|
||||
|
||||
/**
|
||||
* Compute desired yaw command
|
||||
*/
|
||||
// TODO implement raw control mode for yaw and base on Accels.Y
|
||||
stabDesired.Yaw = 0.0f;
|
||||
|
||||
|
||||
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
|
||||
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
|
||||
|
||||
StabilizationDesiredSet(&stabDesired);
|
||||
|
||||
FixedWingPathFollowerStatusSet(&fixedWingPathFollowerStatus);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Function to calculate course vector C based on airspeed s, fluid movement F
|
||||
* and desired movement vector V
|
||||
* parameters in: V,F,s
|
||||
* parameters out: C
|
||||
* returns true if a valid solution could be found for V,F,s, false if not
|
||||
* C will be set to a best effort attempt either way
|
||||
*/
|
||||
static bool correctCourse(float *C, float *V, float *F, float s)
|
||||
{
|
||||
// Approach:
|
||||
// Let Sc be a circle around origin marking possible movement vectors
|
||||
// of the craft with airspeed s (all possible options for C)
|
||||
// Let Vl be a line through the origin along movement vector V where fr any
|
||||
// point v on line Vl v = k * (V / |V|) = k' * V
|
||||
// Let Wl be a line parallel to Vl where for any point v on line Vl exists
|
||||
// a point w on WL with w = v - F
|
||||
// Then any intersection between circle Sc and line Wl represents course
|
||||
// vector which would result in a movement vector
|
||||
// V' = k * ( V / |V|) = k' * V
|
||||
// If there is no intersection point, S is insufficient to compensate
|
||||
// for F and we can only try to fly in direction of V (thus having wind drift
|
||||
// but at least making progress orthogonal to wind)
|
||||
|
||||
s = fabsf(s);
|
||||
float f = vector_lengthf(F, 2);
|
||||
|
||||
// normalize Cn=V/|V|, |V| must be >0
|
||||
float v = vector_lengthf(V, 2);
|
||||
if (v < 1e-6f) {
|
||||
// if |V|=0, we aren't supposed to move, turn into the wind
|
||||
// (this allows hovering)
|
||||
C[0] = -F[0];
|
||||
C[1] = -F[1];
|
||||
// if desired airspeed matches fluidmovement a hover is actually
|
||||
// intended so return true
|
||||
return fabsf(f - s) < 1e-3f;
|
||||
}
|
||||
float Vn[2] = { V[0] / v, V[1] / v };
|
||||
|
||||
// project F on V
|
||||
float fp = F[0] * Vn[0] + F[1] * Vn[1];
|
||||
|
||||
// find component Fo of F that is orthogonal to V
|
||||
// (which is exactly the distance between Vl and Wl)
|
||||
float Fo[2] = { F[0] - (fp * Vn[0]), F[1] - (fp * Vn[1]) };
|
||||
float fo2 = Fo[0] * Fo[0] + Fo[1] * Fo[1];
|
||||
|
||||
// find k where k * Vn = C - Fo
|
||||
// |C|=s is the hypothenuse in any rectangular triangle formed by k * Vn and Fo
|
||||
// so k^2 + fo^2 = s^2 (since |Vn|=1)
|
||||
float k2 = s * s - fo2;
|
||||
if (k2 <= -1e-3f) {
|
||||
// there is no solution, we will be drifted off either way
|
||||
// fallback: fly stupidly in direction of V and hope for the best
|
||||
C[0] = V[0];
|
||||
C[1] = V[1];
|
||||
return false;
|
||||
} else if (k2 <= 1e-3f) {
|
||||
// there is exactly one solution: -Fo
|
||||
C[0] = -Fo[0];
|
||||
C[1] = -Fo[1];
|
||||
return true;
|
||||
}
|
||||
// we have two possible solutions k positive and k negative as there are
|
||||
// two intersection points between Wl and Sc
|
||||
// which one is better? two criteria:
|
||||
// 1. we MUST move in the right direction, if any k leads to -v its invalid
|
||||
// 2. we should minimize the speed error
|
||||
float k = sqrt(k2);
|
||||
float C1[2] = { -k * Vn[0] - Fo[0], -k * Vn[1] - Fo[1] };
|
||||
float C2[2] = { k *Vn[0] - Fo[0], k * Vn[1] - Fo[1] };
|
||||
// project C+F on Vn to find signed resulting movement vector length
|
||||
float vp1 = (C1[0] + F[0]) * Vn[0] + (C1[1] + F[1]) * Vn[1];
|
||||
float vp2 = (C2[0] + F[0]) * Vn[0] + (C2[1] + F[1]) * Vn[1];
|
||||
if (vp1 >= 0.0f && fabsf(v - vp1) < fabsf(v - vp2)) {
|
||||
// in this case the angle between course and resulting movement vector
|
||||
// is greater than 90 degrees - so we actually fly backwards
|
||||
C[0] = C1[0];
|
||||
C[1] = C1[1];
|
||||
return true;
|
||||
}
|
||||
C[0] = C2[0];
|
||||
C[1] = C2[1];
|
||||
if (vp2 >= 0.0f) {
|
||||
// in this case the angle between course and movement vector is less than
|
||||
// 90 degrees, but we do move in the right direction
|
||||
return true;
|
||||
} else {
|
||||
// in this case we actually get driven in the opposite direction of V
|
||||
// with both solutions for C
|
||||
// this might be reached in headwind stronger than maximum allowed
|
||||
// airspeed.
|
||||
return false;
|
||||
}
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute desired attitude from the desired velocity
|
||||
*
|
||||
* Takes in @ref NedState which has the acceleration in the
|
||||
* NED frame as the feedback term and then compares the
|
||||
* @ref VelocityState against the @ref VelocityDesired
|
||||
*/
|
||||
static int8_t updateVtolDesiredAttitude(bool yaw_attitude, float yaw_direction)
|
||||
{
|
||||
float dT = updatePeriod / 1000.0f;
|
||||
uint8_t result = 1;
|
||||
|
||||
VelocityDesiredData velocityDesired;
|
||||
VelocityStateData velocityState;
|
||||
StabilizationDesiredData stabDesired;
|
||||
AttitudeStateData attitudeState;
|
||||
StabilizationBankData stabSettings;
|
||||
SystemSettingsData systemSettings;
|
||||
|
||||
float northError;
|
||||
float northCommand;
|
||||
|
||||
float eastError;
|
||||
float eastCommand;
|
||||
|
||||
float downError;
|
||||
float downCommand;
|
||||
|
||||
SystemSettingsGet(&systemSettings);
|
||||
VelocityStateGet(&velocityState);
|
||||
VelocityDesiredGet(&velocityDesired);
|
||||
StabilizationDesiredGet(&stabDesired);
|
||||
VelocityDesiredGet(&velocityDesired);
|
||||
AttitudeStateGet(&attitudeState);
|
||||
StabilizationBankGet(&stabSettings);
|
||||
|
||||
// Testing code - refactor into manual control command
|
||||
ManualControlCommandData manualControlData;
|
||||
ManualControlCommandGet(&manualControlData);
|
||||
|
||||
// Compute desired north command
|
||||
northError = velocityDesired.North - velocityState.North;
|
||||
i.vel[0] = boundf(i.vel[0] + northError * dT * vtolPathFollowerSettings.HorizontalVelPI.Ki,
|
||||
-vtolPathFollowerSettings.HorizontalVelPI.ILimit,
|
||||
vtolPathFollowerSettings.HorizontalVelPI.ILimit);
|
||||
northCommand = (northError * vtolPathFollowerSettings.HorizontalVelPI.Kp + i.vel[0]
|
||||
+ velocityDesired.North * vtolPathFollowerSettings.VelocityFeedforward);
|
||||
|
||||
// Compute desired east command
|
||||
eastError = velocityDesired.East - velocityState.East;
|
||||
i.vel[1] = boundf(i.vel[1] + eastError * dT * vtolPathFollowerSettings.HorizontalVelPI.Ki,
|
||||
-vtolPathFollowerSettings.HorizontalVelPI.ILimit,
|
||||
vtolPathFollowerSettings.HorizontalVelPI.ILimit);
|
||||
eastCommand = (eastError * vtolPathFollowerSettings.HorizontalVelPI.Kp + i.vel[1]
|
||||
+ velocityDesired.East * vtolPathFollowerSettings.VelocityFeedforward);
|
||||
|
||||
// Compute desired down command
|
||||
downError = velocityDesired.Down - velocityState.Down;
|
||||
// Must flip this sign
|
||||
downError = -downError;
|
||||
i.vel[2] = boundf(i.vel[2] + downError * dT * vtolPathFollowerSettings.VerticalVelPI.Ki,
|
||||
-vtolPathFollowerSettings.VerticalVelPI.ILimit,
|
||||
vtolPathFollowerSettings.VerticalVelPI.ILimit);
|
||||
downCommand = (downError * vtolPathFollowerSettings.VerticalVelPI.Kp + i.vel[2]);
|
||||
|
||||
stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max);
|
||||
|
||||
|
||||
// DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in
|
||||
if (vtolPathFollowerSettings.FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST) {
|
||||
attitudeState.Yaw += 120.0f;
|
||||
if (attitudeState.Yaw > 180.0f) {
|
||||
attitudeState.Yaw -= 360.0f;
|
||||
}
|
||||
}
|
||||
|
||||
if ( // emergency flyaway detection
|
||||
(fabsf(i.vel[0]) - vtolPathFollowerSettings.HorizontalVelPI.ILimit < 1e-6f || fabsf(i.vel[1]) - vtolPathFollowerSettings.HorizontalVelPI.ILimit < 1e-6f) && // integral at its limit
|
||||
velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f // angle between desired and actual velocity >90 degrees
|
||||
) {
|
||||
result = 0; // trigger alarm - everything else is handled by callers (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...)
|
||||
}
|
||||
|
||||
// Project the north and east command signals into the pitch and roll based on yaw. For this to behave well the
|
||||
// craft should move similarly for 5 deg roll versus 5 deg pitch
|
||||
stabDesired.Pitch = boundf(-northCommand * cosf(DEG2RAD(attitudeState.Yaw)) +
|
||||
-eastCommand * sinf(DEG2RAD(attitudeState.Yaw)),
|
||||
-vtolPathFollowerSettings.MaxRollPitch, vtolPathFollowerSettings.MaxRollPitch);
|
||||
stabDesired.Roll = boundf(-northCommand * sinf(DEG2RAD(attitudeState.Yaw)) +
|
||||
eastCommand * cosf(DEG2RAD(attitudeState.Yaw)),
|
||||
-vtolPathFollowerSettings.MaxRollPitch, vtolPathFollowerSettings.MaxRollPitch);
|
||||
|
||||
if (vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
|
||||
// For now override thrust with manual control. Disable at your risk, quad goes to China.
|
||||
ManualControlCommandData manualControl;
|
||||
ManualControlCommandGet(&manualControl);
|
||||
stabDesired.Thrust = manualControl.Thrust;
|
||||
}
|
||||
|
||||
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
if (yaw_attitude) {
|
||||
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.Yaw = yaw_direction;
|
||||
} else {
|
||||
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
|
||||
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControlData.Yaw;
|
||||
}
|
||||
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
|
||||
StabilizationDesiredSet(&stabDesired);
|
||||
|
||||
return result;
|
||||
}
|
||||
|
||||
/**
|
||||
* Compute desired attitude for vtols - emergency fallback
|
||||
*/
|
||||
static void updateVtolDesiredAttitudeEmergencyFallback()
|
||||
{
|
||||
float dT = updatePeriod / 1000.0f;
|
||||
|
||||
VelocityDesiredData velocityDesired;
|
||||
VelocityStateData velocityState;
|
||||
StabilizationDesiredData stabDesired;
|
||||
|
||||
float courseError;
|
||||
float courseCommand;
|
||||
|
||||
float downError;
|
||||
float downCommand;
|
||||
|
||||
VelocityStateGet(&velocityState);
|
||||
VelocityDesiredGet(&velocityDesired);
|
||||
|
||||
ManualControlCommandData manualControlData;
|
||||
ManualControlCommandGet(&manualControlData);
|
||||
|
||||
courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North));
|
||||
|
||||
if (courseError < -180.0f) {
|
||||
courseError += 360.0f;
|
||||
}
|
||||
if (courseError > 180.0f) {
|
||||
courseError -= 360.0f;
|
||||
}
|
||||
|
||||
|
||||
courseCommand = (courseError * vtolPathFollowerSettings.EmergencyFallbackYawRate.kP);
|
||||
|
||||
stabDesired.Yaw = boundf(courseCommand, -vtolPathFollowerSettings.EmergencyFallbackYawRate.Max, vtolPathFollowerSettings.EmergencyFallbackYawRate.Max);
|
||||
|
||||
// Compute desired down command
|
||||
downError = velocityDesired.Down - velocityState.Down;
|
||||
// Must flip this sign
|
||||
downError = -downError;
|
||||
i.vel[2] = boundf(i.vel[2] + downError * dT * vtolPathFollowerSettings.VerticalVelPI.Ki,
|
||||
-vtolPathFollowerSettings.VerticalVelPI.ILimit,
|
||||
vtolPathFollowerSettings.VerticalVelPI.ILimit);
|
||||
downCommand = (downError * vtolPathFollowerSettings.VerticalVelPI.Kp + i.vel[2]);
|
||||
|
||||
stabDesired.Thrust = boundf(downCommand + vtolPathFollowerSettings.ThrustLimits.Neutral, vtolPathFollowerSettings.ThrustLimits.Min, vtolPathFollowerSettings.ThrustLimits.Max);
|
||||
|
||||
|
||||
stabDesired.Roll = vtolPathFollowerSettings.EmergencyFallbackAttitude.Roll;
|
||||
stabDesired.Pitch = vtolPathFollowerSettings.EmergencyFallbackAttitude.Pitch;
|
||||
|
||||
if (vtolPathFollowerSettings.ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) {
|
||||
// For now override thrust with manual control. Disable at your risk, quad goes to China.
|
||||
ManualControlCommandData manualControl;
|
||||
ManualControlCommandGet(&manualControl);
|
||||
stabDesired.Thrust = manualControl.Thrust;
|
||||
}
|
||||
|
||||
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
|
||||
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
|
||||
StabilizationDesiredSet(&stabDesired);
|
||||
}
|
||||
|
||||
|
||||
/**
|
||||
* Compute desired attitude from a fixed preset
|
||||
*
|
||||
*/
|
||||
static void updateFixedAttitude(float *attitude)
|
||||
{
|
||||
StabilizationDesiredData stabDesired;
|
||||
|
||||
StabilizationDesiredGet(&stabDesired);
|
||||
stabDesired.Roll = attitude[0];
|
||||
stabDesired.Pitch = attitude[1];
|
||||
stabDesired.Yaw = attitude[2];
|
||||
stabDesired.Thrust = attitude[3];
|
||||
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
|
||||
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
|
||||
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
|
||||
StabilizationDesiredSet(&stabDesired);
|
||||
}
|
||||
|
||||
@ -372,6 +372,7 @@ static void updatePathVelocity()
|
||||
float dT = vtolpathfollowerSettings.UpdatePeriod / 1000.0f;
|
||||
|
||||
PathDesiredData pathDesired;
|
||||
|
||||
PathDesiredGet(&pathDesired);
|
||||
PositionStateData positionState;
|
||||
PositionStateGet(&positionState);
|
||||
@ -403,7 +404,7 @@ static void updatePathVelocity()
|
||||
case PATHDESIRED_MODE_DRIVEVECTOR:
|
||||
default:
|
||||
speed = pathDesired.StartingVelocity
|
||||
+ (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1);
|
||||
+ (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * boundf(progress.fractional_progress, 0, 1);
|
||||
if (progress.fractional_progress > 1) {
|
||||
speed = 0;
|
||||
}
|
||||
@ -416,19 +417,19 @@ static void updatePathVelocity()
|
||||
eastPosIntegral += progress.correction_direction[1] * progress.error * vtolpathfollowerSettings.HorizontalPosPI.Ki * dT;
|
||||
downPosIntegral += progress.correction_direction[2] * progress.error * vtolpathfollowerSettings.VerticalPosPI.Ki * dT;
|
||||
|
||||
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);
|
||||
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;
|
||||
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;
|
||||
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;
|
||||
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));
|
||||
@ -441,7 +442,7 @@ static void updatePathVelocity()
|
||||
velocityDesired.Down = boundf(velocityDesired.Down, -vtolpathfollowerSettings.VerticalVelMax, vtolpathfollowerSettings.VerticalVelMax);
|
||||
|
||||
// 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];
|
||||
|
||||
@ -1,18 +1,23 @@
|
||||
<xml>
|
||||
<object name="VtolPathFollowerSettings" singleinstance="true" settings="true" category="Control">
|
||||
<description>Settings for the @ref VtolPathFollowerModule</description>
|
||||
<field name="GuidanceMode" units="" type="enum" elements="1" options="DUAL_LOOP,VELOCITY_CONTROL" defaultvalue="DUAL_LOOP"/>
|
||||
<field name="HorizontalVelMax" units="m/s" type="uint16" elements="1" defaultvalue="2"/>
|
||||
<field name="VerticalVelMax" units="m/s" type="uint16" elements="1" defaultvalue="1"/>
|
||||
<field name="HorizontalPosPI" units="(m/s)/m" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="0.25,0.02,1"/>
|
||||
<field name="HorizontalVelPID" units="deg/(m/s)" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="8,0.5,0.002,4"/>
|
||||
<field name="VerticalPosPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="0.4,0.02,1"/>
|
||||
<field name="VerticalVelPID" units="" type="float" elementnames="Kp,Ki,Kd,ILimit" defaultvalue="0.1,0.01,0,1"/>
|
||||
<field name="TreatCustomCraftAs" units="switch" type="enum" elements="1" options="FixedWing,VTOL" defaultvalue="FixedWing"/>
|
||||
<field name="HorizontalVelMax" units="m/s" type="float" elements="1" defaultvalue="2.0"/>
|
||||
<field name="VerticalVelMax" units="m/s" type="float" elements="1" defaultvalue="1.0"/>
|
||||
<field name="CourseFeedForward" units="s" type="float" elements="1" defaultvalue="1.0"/>
|
||||
<field name="HorizontalPosP" units="(m/s)/m" type="float" elements="1" defaultvalue="0.25"/>
|
||||
<field name="VerticalPosP" units="" type="float" elements="1" defaultvalue="0.4"/>
|
||||
<field name="HorizontalVelPI" units="deg/(m/s)" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="8.0, 0.5, 10"/>
|
||||
<field name="VerticalVelPI" units="" type="float" elementnames="Kp,Ki,ILimit" defaultvalue="0.1, 0.01, 1.0"/>
|
||||
<field name="ThrustLimits" units="" type="float" elementnames="Min,Neutral,Max" defaultvalue="0.2, 0.5, 0.9"/>
|
||||
<field name="VelocityFeedforward" units="deg/(m/s)" type="float" elements="1" defaultvalue="2"/>
|
||||
<field name="ThrustControl" units="" type="enum" elements="1" options="FALSE,TRUE" defaultvalue="FALSE"/>
|
||||
<field name="VelocitySource" units="" type="enum" elements="1" options="STATE_ESTIMATION,GPS_VELNED,GPS_GROUNDSPEED" defaultvalue="STATE_ESTIMATION"/>
|
||||
<field name="ThrustControl" units="" type="enum" elements="1" options="manual,auto" defaultvalue="manual"/>
|
||||
<field name="YawControl" units="" type="enum" elements="1" options="manual,tailin,course,poi" defaultvalue="manual"/>
|
||||
<field name="FlyawayEmergencyFallback" units="switch" type="enum" elements="1" options="disabled,enabled,always,debugtest" defaultvalue="enabled"/>
|
||||
<field name="EmergencyFallbackAttitude" units="deg" type="float" elementnames="Roll,Pitch" defaultvalue="0,-10.0"/>
|
||||
<field name="EmergencyFallbackYawRate" units="(deg/s)/deg" type="float" elementnames="kP,Max" defaultvalue="2.0, 30.0"/>
|
||||
<field name="MaxRollPitch" units="deg" type="float" elements="1" defaultvalue="20"/>
|
||||
<field name="UpdatePeriod" units="ms" type="int32" elements="1" defaultvalue="50"/>
|
||||
<field name="UpdatePeriod" units="ms" type="uint16" elements="1" defaultvalue="50"/>
|
||||
<access gcs="readwrite" flight="readwrite"/>
|
||||
<telemetrygcs acked="true" updatemode="onchange" period="0"/>
|
||||
<telemetryflight acked="true" updatemode="onchange" period="0"/>
|
||||
|
||||
Loading…
Reference in New Issue
Block a user