/**
* 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);
}
/**
* 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;
}
