/**
* 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 attitude from the desired velocity
*
* Takes in @ref NedActual which has the acceleration in the
* NED frame as the feedback term and then compares the
* @ref VelocityActual against the @ref VelocityDesired
*/
static void updateVtolDesiredAttitude()
{
float dT = guidanceSettings.UpdatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityActualData velocityActual;
StabilizationDesiredData stabDesired;
AttitudeActualData attitudeActual;
NedAccelData nedAccel;
StabilizationSettingsData stabSettings;
float northError;
float northCommand;
float eastError;
float eastCommand;
float downError;
float downCommand;
VtolPathFollowerSettingsGet(&guidanceSettings);
VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeActualGet(&attitudeActual);
StabilizationSettingsGet(&stabSettings);
NedAccelGet(&nedAccel);
float northVel = velocityActual.North;
float eastVel = velocityActual.East;
float downVel = velocityActual.Down;
// Compute desired north command from velocity error
northError = velocityDesired.North - northVel;
northCommand = pid_apply_antiwindup(&vtol_pids[NORTH_VELOCITY], northError,
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch, dT) + velocityDesired.North * guidanceSettings.VelocityFeedforward;
// Compute desired east command from velocity error
eastError = velocityDesired.East - eastVel;
eastCommand = pid_apply_antiwindup(&vtol_pids[NORTH_VELOCITY], eastError,
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch, dT) + velocityDesired.East * guidanceSettings.VelocityFeedforward;
// Compute desired down command. Using NED accel as the damping term
downError = velocityDesired.Down - downVel;
// Negative is critical here since throttle is negative with down
downCommand = -pid_apply_antiwindup(&vtol_pids[DOWN_VELOCITY], downError, -1, 1, dT) +
nedAccel.Down * guidanceSettings.VerticalVelPID[VTOLPATHFOLLOWERSETTINGS_VERTICALVELPID_KD];
// If this setting is zero then the throttle level available when enabled is used for hover:wq
float used_throttle_offset = (guidanceSettings.HoverThrottle == 0) ? throttleOffset : guidanceSettings.HoverThrottle;
stabDesired.Throttle = bound_min_max(downCommand + used_throttle_offset, 0, 1);
// 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.
// Notice the inputs are crudely bounded by the anti-winded but if both N and E were
// saturated and the craft were at 45 degrees that would result in a value greater than
// the limit, so apply limit again here.
stabDesired.Pitch = bound_min_max(-northCommand * cosf(attitudeActual.Yaw * DEG2RAD) +
-eastCommand * sinf(attitudeActual.Yaw * DEG2RAD),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
stabDesired.Roll = bound_min_max(-northCommand * sinf(attitudeActual.Yaw * DEG2RAD) +
eastCommand * cosf(attitudeActual.Yaw * DEG2RAD),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
if(guidanceSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) {
// For now override throttle with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
stabDesired.Throttle = manualControl.Throttle;
}
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDEPLUS;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDEPLUS;
float yaw;
switch(guidanceSettings.YawMode) {
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_RATE:
/* This is awkward. This allows the transmitter to control the yaw while flying navigation */
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_AXISLOCK:
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSettings.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_ATTITUDE:
ManualControlCommandYawGet(&yaw);
stabDesired.Yaw = stabSettings.YawMax * yaw;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
break;
case VTOLPATHFOLLOWERSETTINGS_YAWMODE_POI:
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI;
break;
}
StabilizationDesiredSet(&stabDesired);
}
/**
* Module thread, should not return.
*/
static void altitudeHoldTask(void *parameters)
{
bool engaged = false;
AltitudeHoldDesiredData altitudeHoldDesired;
StabilizationDesiredData stabilizationDesired;
AltitudeHoldSettingsData altitudeHoldSettings;
UAVObjEvent ev;
struct pid velocity_pid;
// Listen for object updates.
AltitudeHoldDesiredConnectQueue(queue);
AltitudeHoldSettingsConnectQueue(queue);
FlightStatusConnectQueue(queue);
AltitudeHoldSettingsGet(&altitudeHoldSettings);
pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp,
altitudeHoldSettings.VelocityKi, 0.0f, 1.0f);
AlarmsSet(SYSTEMALARMS_ALARM_ALTITUDEHOLD, SYSTEMALARMS_ALARM_OK);
// Main task loop
const uint32_t dt_ms = 5;
const float dt_s = dt_ms * 0.001f;
uint32_t timeout = dt_ms;
while (1) {
if (PIOS_Queue_Receive(queue, &ev, timeout) != true) {
} else if (ev.obj == FlightStatusHandle()) {
uint8_t flight_mode;
FlightStatusFlightModeGet(&flight_mode);
if (flight_mode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD && !engaged) {
// Copy the current throttle as a starting point for integral
StabilizationDesiredThrottleGet(&velocity_pid.iAccumulator);
velocity_pid.iAccumulator *= 1000.0f; // pid library scales up accumulator by 1000
engaged = true;
} else if (flight_mode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD)
engaged = false;
// Run loop at 20 Hz when engaged otherwise just slowly wait for it to be engaged
timeout = engaged ? dt_ms : 100;
} else if (ev.obj == AltitudeHoldDesiredHandle()) {
AltitudeHoldDesiredGet(&altitudeHoldDesired);
} else if (ev.obj == AltitudeHoldSettingsHandle()) {
AltitudeHoldSettingsGet(&altitudeHoldSettings);
pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp,
altitudeHoldSettings.VelocityKi, 0.0f, 1.0f);
}
bool landing = altitudeHoldDesired.Land == ALTITUDEHOLDDESIRED_LAND_TRUE;
// For landing mode allow throttle to go negative to allow the integrals
// to stop winding up
const float min_throttle = landing ? -0.1f : 0.0f;
// When engaged compute altitude controller output
if (engaged) {
float position_z, velocity_z, altitude_error;
PositionActualDownGet(&position_z);
VelocityActualDownGet(&velocity_z);
position_z = -position_z; // Use positive up convention
velocity_z = -velocity_z; // Use positive up convention
// Compute the altitude error
altitude_error = altitudeHoldDesired.Altitude - position_z;
// Velocity desired is from the outer controller plus the set point
float velocity_desired = altitude_error * altitudeHoldSettings.PositionKp + altitudeHoldDesired.ClimbRate;
float throttle_desired = pid_apply_antiwindup(&velocity_pid,
velocity_desired - velocity_z,
min_throttle, 1.0f, // positive limits since this is throttle
dt_s);
if (altitudeHoldSettings.AttitudeComp > 0) {
// Throttle desired is at this point the mount desired in the up direction, we can
// account for the attitude if desired
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
// Project a unit vector pointing up into the body frame and
// get the z component
float fraction = attitudeActual.q1 * attitudeActual.q1 -
attitudeActual.q2 * attitudeActual.q2 -
attitudeActual.q3 * attitudeActual.q3 +
attitudeActual.q4 * attitudeActual.q4;
// Add ability to scale up the amount of compensation to achieve
// level forward flight
fraction = powf(fraction, (float) altitudeHoldSettings.AttitudeComp / 100.0f);
// Dividing by the fraction remaining in the vertical projection will
// attempt to compensate for tilt. This acts like the thrust is linear
// with the output which isn't really true. If the fraction is starting
// to go negative we are inverted and should shut off throttle
throttle_desired = (fraction > 0.1f) ? (throttle_desired / fraction) : 0.0f;
}
StabilizationDesiredGet(&stabilizationDesired);
stabilizationDesired.Throttle = bound_min_max(throttle_desired, min_throttle, 1.0f);
if (landing) {
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabilizationDesired.Roll = 0;
stabilizationDesired.Pitch = 0;
stabilizationDesired.Yaw = 0;
} else {
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabilizationDesired.Roll = altitudeHoldDesired.Roll;
stabilizationDesired.Pitch = altitudeHoldDesired.Pitch;
stabilizationDesired.Yaw = altitudeHoldDesired.Yaw;
}
StabilizationDesiredSet(&stabilizationDesired);
}
}
}
/**
* Compute the desired acceleration based on the desired
* velocity and actual velocity
*/
static int32_t vtol_follower_control_accel(float dT)
{
VelocityDesiredData velocityDesired;
VelocityActualData velocityActual;
AccelDesiredData accelDesired;
NedAccelData nedAccel;
float north_error, north_acceleration;
float east_error, east_acceleration;
float down_error;
static float last_north_velocity;
static float last_east_velocity;
NedAccelGet(&nedAccel);
VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired);
// Optionally compute the acceleration required component from a changing velocity desired
if (guidanceSettings.VelocityChangePrediction == VTOLPATHFOLLOWERSETTINGS_VELOCITYCHANGEPREDICTION_TRUE && dT > 0) {
north_acceleration = (velocityDesired.North - last_north_velocity) / dT;
east_acceleration = (velocityDesired.East - last_east_velocity) / dT;
last_north_velocity = velocityDesired.North;
last_east_velocity = velocityDesired.East;
} else {
north_acceleration = 0;
east_acceleration = 0;
}
// Convert the max angles into the maximum angle that would be requested
const float MAX_ACCELERATION = GRAVITY * sinf(guidanceSettings.MaxRollPitch * DEG2RAD);
// Compute desired north command from velocity error
north_error = velocityDesired.North - velocityActual.North;
north_acceleration += pid_apply_antiwindup(&vtol_pids[NORTH_VELOCITY], north_error,
-MAX_ACCELERATION, MAX_ACCELERATION, dT) +
velocityDesired.North * guidanceSettings.VelocityFeedforward +
-nedAccel.North * guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD];
// Compute desired east command from velocity error
east_error = velocityDesired.East - velocityActual.East;
east_acceleration += pid_apply_antiwindup(&vtol_pids[EAST_VELOCITY], east_error,
-MAX_ACCELERATION, MAX_ACCELERATION, dT) +
velocityDesired.East * guidanceSettings.VelocityFeedforward +
-nedAccel.East * guidanceSettings.HorizontalVelPID[VTOLPATHFOLLOWERSETTINGS_HORIZONTALVELPID_KD];
accelDesired.North = north_acceleration;
accelDesired.East = east_acceleration;
// Note: vertical controller really isn't currently in units of acceleration and the anti-windup may
// not be appropriate. However, it is fine for now since it this output is just directly used on the
// output. To use this appropriately we need a model of throttle to acceleration.
// Compute desired down command. Using NED accel as the damping term
down_error = velocityDesired.Down - velocityActual.Down;
// Negative is critical here since throttle is negative with down
accelDesired.Down = -pid_apply_antiwindup(&vtol_pids[DOWN_VELOCITY], down_error, -1, 0, dT);
// Store the desired acceleration
AccelDesiredSet(&accelDesired);
return 0;
}
/**
* 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;
}