/**
* Compute desired velocity from the current position
*
* Takes in @ref PositionActual and compares it to @ref PositionDesired
* and computes @ref VelocityDesired
*/
void updateVtolDesiredVelocity()
{
GuidanceSettingsData guidanceSettings;
PositionActualData positionActual;
PositionDesiredData positionDesired;
VelocityDesiredData velocityDesired;
GuidanceSettingsGet(&guidanceSettings);
PositionActualGet(&positionActual);
PositionDesiredGet(&positionDesired);
VelocityDesiredGet(&velocityDesired);
// Note all distances in cm
float dNorth = positionDesired.North - positionActual.North;
float dEast = positionDesired.East - positionActual.East;
float distance = sqrt(pow(dNorth, 2) + pow(dEast, 2));
float heading = atan2f(dEast, dNorth);
float groundspeed = bound(distance * guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_KP],
0, guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_MAX]);
velocityDesired.North = groundspeed * cosf(heading);
velocityDesired.East = groundspeed * sinf(heading);
float dDown = positionDesired.Down - positionActual.Down;
velocityDesired.Down = bound(dDown * guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_KP],
-guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_MAX],
guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_MAX]);
VelocityDesiredSet(&velocityDesired);
}
void VtolVelocityController::UpdateVelocityDesired()
{
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
VelocityDesiredData velocityDesired;
controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
velocityDesired.Down = 0.0f;
float north, east;
controlNE.GetVelocityDesired(&north, &east);
velocityDesired.North = north;
velocityDesired.East = east;
// update pathstatus
pathStatus->error = 0.0f;
pathStatus->fractional_progress = 0.0f;
pathStatus->path_direction_north = velocityDesired.North;
pathStatus->path_direction_east = velocityDesired.East;
pathStatus->path_direction_down = velocityDesired.Down;
pathStatus->correction_direction_north = velocityDesired.North - velocityState.North;
pathStatus->correction_direction_east = velocityDesired.East - velocityState.East;
pathStatus->correction_direction_down = 0.0f;
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired velocity from the current position
*
* Takes in @ref PositionActual and compares it to @ref PositionDesired
* and computes @ref VelocityDesired
*/
void updateVtolDesiredVelocity()
{
static portTickType lastSysTime;
portTickType thisSysTime = xTaskGetTickCount();;
float dT = 0;
GuidanceSettingsData guidanceSettings;
PositionActualData positionActual;
PositionDesiredData positionDesired;
VelocityDesiredData velocityDesired;
GuidanceSettingsGet(&guidanceSettings);
PositionActualGet(&positionActual);
PositionDesiredGet(&positionDesired);
VelocityDesiredGet(&velocityDesired);
float northError;
float eastError;
float downError;
float northCommand;
float eastCommand;
float downCommand;
// Check how long since last update
if(thisSysTime > lastSysTime) // reuse dt in case of wraparound
dT = (thisSysTime - lastSysTime) / portTICK_RATE_MS / 1000.0f;
lastSysTime = thisSysTime;
// Note all distances in cm
// Compute desired north command
northError = positionDesired.North - positionActual.North;
northPosIntegral = bound(northPosIntegral + northError * dT * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KI],
-guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT],
guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT]);
northCommand = (northError * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP] +
northPosIntegral);
eastError = positionDesired.East - positionActual.East;
eastPosIntegral = bound(eastPosIntegral + eastError * dT * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KI],
-guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT],
guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT]);
eastCommand = (eastError * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP] +
eastPosIntegral);
velocityDesired.North = bound(northCommand,-guidanceSettings.HorizontalVelMax,guidanceSettings.HorizontalVelMax);
velocityDesired.East = bound(eastCommand,-guidanceSettings.HorizontalVelMax,guidanceSettings.HorizontalVelMax);
downError = positionDesired.Down - positionActual.Down;
downPosIntegral = bound(downPosIntegral + downError * dT * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KI],
-guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT],
guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT]);
downCommand = (downError * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KP] + downPosIntegral);
velocityDesired.Down = bound(downCommand,
-guidanceSettings.VerticalVelMax,
guidanceSettings.VerticalVelMax);
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired velocity from the current position
*
* Takes in @ref PositionActual and compares it to @ref PositionDesired
* and computes @ref VelocityDesired
*/
void updateEndpointVelocity()
{
float dT = guidanceSettings.UpdatePeriod / 1000.0f;
PositionActualData positionActual;
VelocityDesiredData velocityDesired;
PositionActualGet(&positionActual);
VelocityDesiredGet(&velocityDesired);
float northError;
float eastError;
float downError;
float northCommand;
float eastCommand;
float northPos = positionActual.North;
float eastPos = positionActual.East;
float downPos = positionActual.Down;
// Compute desired north command velocity from position error
northError = pathDesired.End[PATHDESIRED_END_NORTH] - northPos;
northCommand = pid_apply_antiwindup(&vtol_pids[NORTH_POSITION], northError,
-guidanceSettings.HorizontalVelMax, guidanceSettings.HorizontalVelMax, dT);
// Compute desired east command velocity from position error
eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos;
eastCommand = pid_apply_antiwindup(&vtol_pids[EAST_POSITION], eastError,
-guidanceSettings.HorizontalVelMax, guidanceSettings.HorizontalVelMax, dT);
// Limit the maximum velocity any direction (not north and east separately)
float total_vel = sqrtf(powf(northCommand,2) + powf(eastCommand,2));
float scale = 1;
if(total_vel > guidanceSettings.HorizontalVelMax)
scale = guidanceSettings.HorizontalVelMax / total_vel;
velocityDesired.North = northCommand * scale;
velocityDesired.East = eastCommand * scale;
// Compute the desired velocity from the position difference
downError = pathDesired.End[PATHDESIRED_END_DOWN] - downPos;
velocityDesired.Down = pid_apply_antiwindup(&vtol_pids[DOWN_POSITION], downError,
-guidanceSettings.VerticalVelMax, guidanceSettings.VerticalVelMax, dT);
VelocityDesiredSet(&velocityDesired);
// Indicate whether we are in radius of this endpoint
uint8_t path_status = PATHSTATUS_STATUS_INPROGRESS;
float distance2 = powf(northError, 2) + powf(eastError, 2);
if (distance2 < (guidanceSettings.EndpointRadius * guidanceSettings.EndpointRadius)) {
path_status = PATHSTATUS_STATUS_COMPLETED;
}
PathStatusStatusSet(&path_status);
}
/**
* Compute desired velocity from the current position and path
*
* Takes in @ref PositionActual and compares it to @ref PathDesired
* and computes @ref VelocityDesired
*/
static void updatePathVelocity()
{
float dT = guidanceSettings.UpdatePeriod / 1000.0f;
PositionActualData positionActual;
PositionActualGet(&positionActual);
float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
struct path_status progress;
path_progress(&pathDesired, cur, &progress);
// Update the path status UAVO
PathStatusData pathStatus;
PathStatusGet(&pathStatus);
pathStatus.fractional_progress = progress.fractional_progress;
if (pathStatus.fractional_progress < 1)
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
else
pathStatus.Status = PATHSTATUS_STATUS_COMPLETED;
PathStatusSet(&pathStatus);
float groundspeed = pathDesired.StartingVelocity +
(pathDesired.EndingVelocity - pathDesired.StartingVelocity) * progress.fractional_progress;
if(progress.fractional_progress > 1)
groundspeed = 0;
VelocityDesiredData velocityDesired;
velocityDesired.North = progress.path_direction[0] * groundspeed;
velocityDesired.East = progress.path_direction[1] * groundspeed;
float error_speed = progress.error * guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP];
float correction_velocity[2] = {progress.correction_direction[0] * error_speed,
progress.correction_direction[1] * error_speed};
float total_vel = sqrtf(powf(correction_velocity[0],2) + powf(correction_velocity[1],2));
float scale = 1;
if(total_vel > guidanceSettings.HorizontalVelMax)
scale = guidanceSettings.HorizontalVelMax / total_vel;
// Currently not apply a PID loop for horizontal corrections
velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
// Interpolate desired velocity along the path
float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
bound_min_max(progress.fractional_progress,0,1);
float downError = altitudeSetpoint - positionActual.Down;
velocityDesired.Down = pid_apply_antiwindup(&vtol_pids[DOWN_POSITION], downError,
-guidanceSettings.VerticalVelMax, guidanceSettings.VerticalVelMax, dT);
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired velocity from the current position and path
*/
void VtolFlyController::UpdateVelocityDesired()
{
PositionStateData positionState;
PositionStateGet(&positionState);
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
VelocityDesiredData velocityDesired;
// look ahead kFF seconds
float cur[3] = { positionState.North + (velocityState.North * vtolPathFollowerSettings->CourseFeedForward),
positionState.East + (velocityState.East * vtolPathFollowerSettings->CourseFeedForward),
positionState.Down + (velocityState.Down * vtolPathFollowerSettings->CourseFeedForward) };
struct path_status progress;
path_progress(pathDesired, cur, &progress, true);
controlNE.ControlPositionWithPath(&progress);
if (!mManualThrust) {
controlDown.ControlPositionWithPath(&progress);
}
controlNE.UpdateVelocityState(velocityState.North, velocityState.East);
controlDown.UpdateVelocityState(velocityState.Down);
float north, east;
controlNE.GetVelocityDesired(&north, &east);
velocityDesired.North = north;
velocityDesired.East = east;
if (!mManualThrust) {
velocityDesired.Down = controlDown.GetVelocityDesired();
} else { velocityDesired.Down = 0.0f; }
// update pathstatus
pathStatus->error = progress.error;
pathStatus->fractional_progress = progress.fractional_progress;
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_vector[0];
pathStatus->correction_direction_east = progress.correction_vector[1];
pathStatus->correction_direction_down = progress.correction_vector[2];
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired velocity from the current position and path
*
* Takes in @ref PositionActual and compares it to @ref PathDesired
* and computes @ref VelocityDesired
*/
static void updatePathVelocity()
{
PositionActualData positionActual;
PositionActualGet(&positionActual);
float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
struct path_status progress;
path_progress(&pathDesired, cur, &progress);
// Update the path status UAVO
PathStatusData pathStatus;
PathStatusGet(&pathStatus);
pathStatus.fractional_progress = progress.fractional_progress;
if (pathStatus.fractional_progress < 1)
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
else
pathStatus.Status = PATHSTATUS_STATUS_COMPLETED;
PathStatusSet(&pathStatus);
float groundspeed = pathDesired.StartingVelocity +
(pathDesired.EndingVelocity - pathDesired.StartingVelocity) * progress.fractional_progress;
if(progress.fractional_progress > 1)
groundspeed = 0;
VelocityDesiredData velocityDesired;
velocityDesired.North = progress.path_direction[0] * groundspeed;
velocityDesired.East = progress.path_direction[1] * groundspeed;
float error_speed = progress.error * guidanceSettings.HorizontalPosPI[GROUNDPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP];
float correction_velocity[2] = {progress.correction_direction[0] * error_speed,
progress.correction_direction[1] * error_speed};
float total_vel = sqrtf(powf(correction_velocity[0],2) + powf(correction_velocity[1],2));
float scale = 1;
if(total_vel > guidanceSettings.HorizontalVelMax)
scale = guidanceSettings.HorizontalVelMax / total_vel;
// Currently not apply a PID loop for horizontal corrections
velocityDesired.North += progress.correction_direction[0] * error_speed * scale;
velocityDesired.East += progress.correction_direction[1] * error_speed * scale;
// No altitude control on a ground vehicle
velocityDesired.Down = 0;
VelocityDesiredSet(&velocityDesired);
}
/**
* Set the desired velocity from the input sticks
*/
static void manualSetDesiredVelocity()
{
ManualControlCommandData cmd;
VelocityDesiredData velocityDesired;
ManualControlCommandGet(&cmd);
VelocityDesiredGet(&velocityDesired);
GuidanceSettingsData guidanceSettings;
GuidanceSettingsGet(&guidanceSettings);
velocityDesired.North = -guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_MAX] * cmd.Pitch;
velocityDesired.East = guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_MAX] * cmd.Roll;
velocityDesired.Down = 0;
VelocityDesiredSet(&velocityDesired);
}
/**
* Set the desired velocity from the input sticks
*/
static void manualSetDesiredVelocity()
{
ManualControlCommandData cmd;
VelocityDesiredData velocityDesired;
ManualControlCommandGet(&cmd);
VelocityDesiredGet(&velocityDesired);
GuidanceSettingsData guidanceSettings;
GuidanceSettingsGet(&guidanceSettings);
velocityDesired.North = -guidanceSettings.HorizontalVelMax * cmd.Pitch;
velocityDesired.East = guidanceSettings.HorizontalVelMax * cmd.Roll;
velocityDesired.Down = 0;
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired velocity from the current position and path
*
* Takes in @ref PositionActual and compares it to @ref PathDesired
* and computes @ref VelocityDesired
*/
static void updatePathVelocity()
{
PositionActualData positionActual;
PositionActualGet(&positionActual);
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
float cur[3] = {positionActual.North, positionActual.East, positionActual.Down};
struct path_status progress;
path_progress(&pathDesired, cur, &progress);
float groundspeed = 0;
float altitudeSetpoint = 0;
switch (pathDesired.Mode) {
case PATHDESIRED_MODE_CIRCLERIGHT:
case PATHDESIRED_MODE_CIRCLELEFT:
groundspeed = pathDesired.EndingVelocity;
altitudeSetpoint = pathDesired.End[2];
break;
case PATHDESIRED_MODE_ENDPOINT:
case PATHDESIRED_MODE_VECTOR:
default:
groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) *
bound_min_max(progress.fractional_progress,0,1);
altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) *
bound_min_max(progress.fractional_progress,0,1);
break;
}
// this ensures a significant forward component at least close to the real trajectory
if (groundspeed<fixedWingAirspeeds.BestClimbRateSpeed/10.0f)
groundspeed=fixedWingAirspeeds.BestClimbRateSpeed/10.0f;
// calculate velocity - can be zero if waypoints are too close
VelocityDesiredData velocityDesired;
velocityDesired.North = progress.path_direction[0] * groundspeed;
velocityDesired.East = progress.path_direction[1] * groundspeed;
float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP;
// calculate correction - can also be zero if correction vector is 0 or no error present
velocityDesired.North += progress.correction_direction[0] * error_speed;
velocityDesired.East += progress.correction_direction[1] * error_speed;
float downError = altitudeSetpoint - positionActual.Down;
velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP;
// update pathstatus
pathStatus.error = progress.error;
pathStatus.fractional_progress = progress.fractional_progress;
pathStatus.fractional_progress = progress.fractional_progress;
if (pathStatus.fractional_progress < 1)
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
else
pathStatus.Status = PATHSTATUS_STATUS_COMPLETED;
pathStatus.Waypoint = pathDesired.Waypoint;
VelocityDesiredSet(&velocityDesired);
}
/**
* Compute desired velocity from the current position and path
*/
void FixedWingFlyController::updatePathVelocity(float kFF, bool limited)
{
PositionStateData positionState;
PositionStateGet(&positionState);
VelocityStateData velocityState;
VelocityStateGet(&velocityState);
VelocityDesiredData velocityDesired;
const float dT = fixedWingSettings->UpdatePeriod / 1000.0f;
// 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(pathDesired, cur, &progress, true);
// calculate velocity - can be zero if waypoints are too close
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)
// 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
(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 {
// calculate correction
velocityDesired.North += pid_apply(&PIDposH[0], progress.correction_vector[0], dT);
velocityDesired.East += pid_apply(&PIDposH[1], progress.correction_vector[1], dT);
}
velocityDesired.Down += pid_apply(&PIDposV, progress.correction_vector[2], dT);
// update pathstatus
pathStatus->error = progress.error;
pathStatus->fractional_progress = progress.fractional_progress;
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_vector[0];
pathStatus->correction_direction_east = progress.correction_vector[1];
pathStatus->correction_direction_down = progress.correction_vector[2];
VelocityDesiredSet(&velocityDesired);
}
/**
* Controller to maintain/seek a position and optionally descend.
* @param[in] dT time since last eval
* @param[in] hold_pos_ned a position to hold
* @param[in] landing whether to descend
* @param[in] update_status does this update path_status, or does somoene else?
*/
static int32_t vtol_follower_control_simple(const float dT,
const float *hold_pos_ned, bool landing, bool update_status) {
PositionActualData positionActual;
VelocityDesiredData velocityDesired;
PositionActualGet(&positionActual);
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
/* Where would we be in ___ second at current rates? */
const float cur_pos_ned[3] = {
positionActual.North +
velocityActual.North * guidanceSettings.PositionFeedforward,
positionActual.East +
velocityActual.East * guidanceSettings.PositionFeedforward,
positionActual.Down };
float errors_ned[3];
/* Calculate the difference between where we want to be and the
* above position */
vtol_calculate_difference(cur_pos_ned, hold_pos_ned, errors_ned, false);
float horiz_error_mag = vtol_magnitude(errors_ned, 2);
float scale_horiz_error_mag = 0;
/* Apply a cubic deadband; if we are already really close don't work
* as hard to fix it as if we're far away. Prevents chasing high
* frequency noise in direction of correction.
*
* That is, when we're far, noise in estimated position mostly results
* in noise/dither in how fast we go towards the target. When we're
* close, there's a large directional / hunting component. This
* attenuates that.
*/
if (horiz_error_mag > 0.00001f) {
scale_horiz_error_mag = vtol_deadband(horiz_error_mag,
guidanceSettings.EndpointDeadbandWidth,
guidanceSettings.EndpointDeadbandCenterGain,
vtol_end_m, vtol_end_r) / horiz_error_mag;
}
float damped_ne[2] = { errors_ned[0] * scale_horiz_error_mag,
errors_ned[1] * scale_horiz_error_mag };
float commands_ned[3];
// Compute desired north command velocity from position error
commands_ned[0] = pid_apply_antiwindup(&vtol_pids[NORTH_POSITION], damped_ne[0],
-guidanceSettings.HorizontalVelMax, guidanceSettings.HorizontalVelMax, dT);
// Compute desired east command velocity from position error
commands_ned[1] = pid_apply_antiwindup(&vtol_pids[EAST_POSITION], damped_ne[1],
-guidanceSettings.HorizontalVelMax, guidanceSettings.HorizontalVelMax, dT);
if (!landing) {
// Compute desired down comand velocity from the position difference
commands_ned[2] = pid_apply_antiwindup(&vtol_pids[DOWN_POSITION], errors_ned[2],
-guidanceSettings.VerticalVelMax, guidanceSettings.VerticalVelMax, dT);
} else {
// Just use the landing rate.
commands_ned[2] = guidanceSettings.LandingRate;
}
// Limit the maximum horizontal velocity any direction (not north and east separately)
vtol_limit_velocity(commands_ned, guidanceSettings.HorizontalVelMax);
velocityDesired.North = commands_ned[0];
velocityDesired.East = commands_ned[1];
velocityDesired.Down = commands_ned[2];
VelocityDesiredSet(&velocityDesired);
if (update_status) {
uint8_t path_status = PATHSTATUS_STATUS_INPROGRESS;
if (!landing) {
const bool criterion_altitude =
(errors_ned[2]> -guidanceSettings.WaypointAltitudeTol) || (!guidanceSettings.ThrottleControl);
// Indicate whether we are in radius of this endpoint
// And at/above the altitude requested
if ((vtol_magnitude(errors_ned, 2) < guidanceSettings.EndpointRadius) && criterion_altitude) {
path_status = PATHSTATUS_STATUS_COMPLETED;
}
} // landing never terminates.
PathStatusStatusSet(&path_status);
}
return 0;
}
/**
* Compute desired velocity to follow the desired path from the current location.
* @param[in] dT the time since last evaluation
* @param[in] pathDesired the desired path to follow
* @param[out] progress the current progress information along that path
* @returns 0 if successful, <0 if an error occurred
*
* The calculated velocity to attempt is stored in @ref VelocityDesired
*/
int32_t vtol_follower_control_path(const float dT, const PathDesiredData *pathDesired,
struct path_status *progress)
{
PositionActualData positionActual;
PositionActualGet(&positionActual);
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
PathStatusData pathStatus;
PathStatusGet(&pathStatus);
const float cur_pos_ned[3] = {
positionActual.North +
velocityActual.North * guidanceSettings.PositionFeedforward,
positionActual.East +
velocityActual.East * guidanceSettings.PositionFeedforward,
positionActual.Down };
path_progress(pathDesired, cur_pos_ned, progress);
// Check if we have already completed this leg
bool current_leg_completed =
(pathStatus.Status == PATHSTATUS_STATUS_COMPLETED) &&
(pathStatus.Waypoint == pathDesired->Waypoint);
pathStatus.fractional_progress = progress->fractional_progress;
pathStatus.error = progress->error;
pathStatus.Waypoint = pathDesired->Waypoint;
// Figure out how low (high) we should be and the error
const float altitudeSetpoint = vtol_interpolate(progress->fractional_progress,
pathDesired->Start[2], pathDesired->End[2]);
const float downError = altitudeSetpoint - positionActual.Down;
// If leg is completed signal this
if (current_leg_completed || pathStatus.fractional_progress > 1.0f) {
const bool criterion_altitude =
(downError > -guidanceSettings.WaypointAltitudeTol) ||
(!guidanceSettings.ThrottleControl);
// Once we complete leg and hit altitude criterion signal this
// waypoint is done. Or if we're not controlling throttle,
// ignore height for completion.
// Waypoint heights are thus treated as crossing restrictions-
// cross this point at or above...
if (criterion_altitude || current_leg_completed) {
pathStatus.Status = PATHSTATUS_STATUS_COMPLETED;
PathStatusSet(&pathStatus);
} else {
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
PathStatusSet(&pathStatus);
}
// Wait here for new path segment
return vtol_follower_control_simple(dT, pathDesired->End, false, false);
}
// Interpolate desired velocity and altitude along the path
float groundspeed = vtol_interpolate(progress->fractional_progress,
pathDesired->StartingVelocity, pathDesired->EndingVelocity);
float error_speed = vtol_deadband(progress->error,
guidanceSettings.PathDeadbandWidth,
guidanceSettings.PathDeadbandCenterGain,
vtol_path_m, vtol_path_r) *
guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP];
/* Sum the desired path movement vector with the correction vector */
float commands_ned[3];
commands_ned[0] = progress->path_direction[0] * groundspeed +
progress->correction_direction[0] * error_speed;
commands_ned[1] = progress->path_direction[1] * groundspeed +
progress->correction_direction[1] * error_speed;
/* Limit the total velocity based on the configured value. */
vtol_limit_velocity(commands_ned, guidanceSettings.HorizontalVelMax);
commands_ned[2] = pid_apply_antiwindup(&vtol_pids[DOWN_POSITION], downError,
-guidanceSettings.VerticalVelMax, guidanceSettings.VerticalVelMax, dT);
VelocityDesiredData velocityDesired;
VelocityDesiredGet(&velocityDesired);
velocityDesired.North = commands_ned[0];
velocityDesired.East = commands_ned[1];
velocityDesired.Down = commands_ned[2];
VelocityDesiredSet(&velocityDesired);
pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS;
PathStatusSet(&pathStatus);
return 0;
}