static void airspeedActualUpdatedCb(UAVObjEvent * ev)
{
AirspeedActualData airspeedActual;
VelocityActualData velocityActual;
AirspeedActualGet(&airspeedActual);
VelocityActualGet(&velocityActual);
float groundspeed = sqrtf(velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North );
indicatedAirspeedActualBias = airspeedActual.CalibratedAirspeed - groundspeed;
// 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.
}
/**
* 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);
}
/**
* 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 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 uint8_t updateFixedDesiredAttitude()
{
uint8_t result = 1;
float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f; //Convert from [ms] to [s]
VelocityDesiredData velocityDesired;
VelocityActualData velocityActual;
StabilizationDesiredData stabDesired;
AttitudeActualData attitudeActual;
FixedWingPathFollowerStatusData fixedwingpathfollowerStatus;
AirspeedActualData airspeedActual;
float groundspeedActual;
float groundspeedDesired;
float indicatedAirspeedActual;
float indicatedAirspeedDesired;
float airspeedError;
float pitchCommand;
float descentspeedDesired;
float descentspeedError;
float powerError;
float powerCommand;
float bearingError;
float bearingCommand;
FixedWingPathFollowerStatusGet(&fixedwingpathfollowerStatus);
VelocityActualGet(&velocityActual);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeActualGet(&attitudeActual);
AirspeedActualGet(&airspeedActual);
/**
* Compute speed error (required for throttle and pitch)
*/
// Current ground speed
groundspeedActual = sqrtf( velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North );
// note that airspeedActualBias is ( calibratedAirspeed - groundSpeed ) at the time of measurement,
// but thanks to accelerometers, groundspeed reacts faster to changes in direction
// than airspeed and gps sensors alone
indicatedAirspeedActual = groundspeedActual + indicatedAirspeedActualBias;
// Desired ground speed
groundspeedDesired = sqrtf(velocityDesired.North*velocityDesired.North + velocityDesired.East*velocityDesired.East);
indicatedAirspeedDesired = bound_min_max( groundspeedDesired + indicatedAirspeedActualBias,
fixedWingAirspeeds.BestClimbRateSpeed,
fixedWingAirspeeds.CruiseSpeed);
// Airspeed error (positive means not enough airspeed)
airspeedError = indicatedAirspeedDesired - indicatedAirspeedActual;
// Vertical speed error
descentspeedDesired = bound_min_max (
velocityDesired.Down,
-fixedWingAirspeeds.VerticalVelMax,
fixedWingAirspeeds.VerticalVelMax);
descentspeedError = descentspeedDesired - velocityActual.Down;
// Error condition: wind speed is higher than maximum allowed speed. We are forced backwards!
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 0;
if (groundspeedDesired - indicatedAirspeedActualBias <= 0 ) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_WIND] = 1;
result = 0;
}
// Error condition: plane too slow or too fast
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 0;
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 0;
if ( indicatedAirspeedActual > fixedWingAirspeeds.AirSpeedMax) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_OVERSPEED] = 1;
result = 0;
}
if ( indicatedAirspeedActual > fixedWingAirspeeds.CruiseSpeed * 1.2f) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHSPEED] = 1;
result = 0;
}
if (indicatedAirspeedActual < fixedWingAirspeeds.BestClimbRateSpeed * 0.8f && 1) { //The next three && 1 are placeholders for UAVOs representing LANDING and TAKEOFF
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
result = 0;
}
if (indicatedAirspeedActual < fixedWingAirspeeds.StallSpeedClean && 1 && 1) { //Where the && 1 represents the UAVO that will control whether the airplane is prepped for landing or not
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1;
result = 0;
}
if (indicatedAirspeedActual < fixedWingAirspeeds.StallSpeedDirty && 1) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_STALLSPEED] = 1;
result = 0;
}
if (indicatedAirspeedActual<1e-6f) {
// prevent division by zero, abort without controlling anything. This guidance mode is not suited for takeoff or touchdown, or handling stationary planes
// also we cannot handle planes flying backwards, lets just wait until the nose drops
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWSPEED] = 1;
return 0;
}
/**
* Compute desired throttle command
* positive airspeed error means not enough airspeed
* positive decent_speed_error means decending too fast
*/
// compute proportional throttle response
powerError = -descentspeedError +
bound_min_max(
(airspeedError / fixedWingAirspeeds.BestClimbRateSpeed)* fixedwingpathfollowerSettings.AirspeedToVerticalCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOVERTICALCROSSFEED_KP] ,
-fixedwingpathfollowerSettings.AirspeedToVerticalCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOVERTICALCROSSFEED_MAX],
fixedwingpathfollowerSettings.AirspeedToVerticalCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_AIRSPEEDTOVERTICALCROSSFEED_MAX]
);
// compute saturated integral error throttle response. Make integral leaky for better performance. Approximately 30s time constant.
if (fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI] >0) {
powerIntegral = bound_min_max(powerIntegral + -descentspeedError * dT,
-fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI],
fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_ILIMIT]/fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI]
)*(1.0f-1.0f/(1.0f+30.0f/dT));
} else
powerIntegral = 0;
// Compute final throttle response
powerCommand = bound_min_max(
(powerError * fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KP] +
powerIntegral* fixedwingpathfollowerSettings.PowerPI[FIXEDWINGPATHFOLLOWERSETTINGS_POWERPI_KI]) + fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_NEUTRAL],
fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN],
fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX]);
//Output internal state to telemetry
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_POWER] = powerError;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_POWER] = powerIntegral;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_POWER] = powerCommand;
// set throttle
stabDesired.Throttle = powerCommand;
// Error condition: plane cannot hold altitude at current speed.
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 0;
if (
powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MAX] // throttle at maximum
&& velocityActual.Down > 0 // we ARE going down
&& descentspeedDesired < 0 // we WANT to go up
&& airspeedError > 0 // we are too slow already
)
{
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_LOWPOWER] = 1;
result = 0;
}
// Error condition: plane keeps climbing despite minimum throttle (opposite of above)
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 0;
if (
powerCommand == fixedwingpathfollowerSettings.ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGS_THROTTLELIMIT_MIN] // throttle at minimum
&& velocityActual.Down < 0 // we ARE going up
&& descentspeedDesired > 0 // we WANT to go down
&& airspeedError < 0 // we are too fast already
)
{
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_HIGHPOWER] = 1;
result = 0;
}
/**
* Compute desired pitch command
*/
if (fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI] > 0){
//Integrate with saturation
airspeedErrorInt=bound_min_max(airspeedErrorInt + airspeedError * dT,
-fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT]/fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI],
fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_ILIMIT]/fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]);
} else
airspeedErrorInt = 0;
//Compute the cross feed from vertical speed to pitch, with saturation
float verticalSpeedToPitchCommandComponent=bound_min_max (-descentspeedError * fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_KP],
-fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX],
fixedwingpathfollowerSettings.VerticalToPitchCrossFeed[FIXEDWINGPATHFOLLOWERSETTINGS_VERTICALTOPITCHCROSSFEED_MAX]
);
//Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand= -(airspeedError*fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KP]
+ airspeedErrorInt*fixedwingpathfollowerSettings.SpeedPI[FIXEDWINGPATHFOLLOWERSETTINGS_SPEEDPI_KI]
) + verticalSpeedToPitchCommandComponent;
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_SPEED] = airspeedError;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_SPEED] = airspeedErrorInt;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_SPEED] = pitchCommand;
stabDesired.Pitch = bound_min_max(fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_NEUTRAL] +
pitchCommand,
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MIN],
fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX]);
// Error condition: high speed dive
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 0;
if (
pitchCommand == fixedwingpathfollowerSettings.PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGS_PITCHLIMIT_MAX] // pitch demand is full up
&& velocityActual.Down > 0 // we ARE going down
&& descentspeedDesired < 0 // we WANT to go up
&& airspeedError < 0 // we are too fast already
) {
fixedwingpathfollowerStatus.Errors[FIXEDWINGPATHFOLLOWERSTATUS_ERRORS_PITCHCONTROL] = 1;
result = 0;
}
/**
* Compute desired roll command
*/
if (groundspeedDesired> 1e-6f) {
bearingError = RAD2DEG * (atan2f(velocityDesired.East,velocityDesired.North) - atan2f(velocityActual.East,velocityActual.North));
} else {
// if we are not supposed to move, keep going wherever we are now. Don't make things worse by changing direction.
bearingError = 0;
}
if (bearingError<-180.0f) bearingError+=360.0f;
if (bearingError>180.0f) bearingError-=360.0f;
bearingIntegral = bound_min_max(bearingIntegral + bearingError * dT * fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_KI],
-fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_ILIMIT],
fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_ILIMIT]);
bearingCommand = (bearingError * fixedwingpathfollowerSettings.BearingPI[FIXEDWINGPATHFOLLOWERSETTINGS_BEARINGPI_KP] +
bearingIntegral);
fixedwingpathfollowerStatus.Error[FIXEDWINGPATHFOLLOWERSTATUS_ERROR_BEARING] = bearingError;
fixedwingpathfollowerStatus.ErrorInt[FIXEDWINGPATHFOLLOWERSTATUS_ERRORINT_BEARING] = bearingIntegral;
fixedwingpathfollowerStatus.Command[FIXEDWINGPATHFOLLOWERSTATUS_COMMAND_BEARING] = bearingCommand;
stabDesired.Roll = bound_min_max( fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_NEUTRAL] +
bearingCommand,
fixedwingpathfollowerSettings.RollLimit[FIXEDWINGPATHFOLLOWERSETTINGS_ROLLLIMIT_MIN],
fixedwingpathfollowerSettings.RollLimit[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;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
if (isnan(stabDesired.Roll) || isnan(stabDesired.Pitch) || isnan(stabDesired.Yaw) || isnan(stabDesired.Throttle)) {
northVelIntegral = 0;
eastVelIntegral = 0;
downVelIntegral = 0;
bearingIntegral = 0;
speedIntegral = 0;
accelIntegral = 0;
powerIntegral = 0;
airspeedErrorInt = 0;
result = 0;
} else {
StabilizationDesiredSet(&stabDesired);
}
FixedWingPathFollowerStatusSet(&fixedwingpathfollowerStatus);
return result;
}
/**
* 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()
{
static portTickType lastSysTime;
portTickType thisSysTime = xTaskGetTickCount();;
float dT;
VelocityDesiredData velocityDesired;
VelocityActualData velocityActual;
AttitudeDesiredData attitudeDesired;
AttitudeActualData attitudeActual;
NedAccelData nedAccel;
GuidanceSettingsData guidanceSettings;
StabilizationSettingsData stabSettings;
SystemSettingsData systemSettings;
float northError;
float northCommand;
float eastError;
float eastCommand;
float downError;
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;
SystemSettingsGet(&systemSettings);
GuidanceSettingsGet(&guidanceSettings);
VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired);
AttitudeDesiredGet(&attitudeDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeActualGet(&attitudeActual);
StabilizationSettingsGet(&stabSettings);
NedAccelGet(&nedAccel);
attitudeDesired.Yaw = 0; // try and face north
// Compute desired north command
northError = velocityDesired.North - velocityActual.North;
northIntegral = bound(northIntegral + northError * dT,
-guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
northCommand = (northError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] +
northIntegral * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI] -
nedAccel.North * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD]);
// Compute desired east command
eastError = velocityDesired.East - velocityActual.East;
eastIntegral = bound(eastIntegral + eastError * dT,
-guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT],
guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_ILIMIT]);
eastCommand = (eastError * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KP] +
eastIntegral * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KI] -
nedAccel.East * guidanceSettings.HorizontalVelPID[GUIDANCESETTINGS_HORIZONTALVELPID_KD]);
// Compute desired down command
downError = velocityDesired.Down - velocityActual.Down;
downIntegral = bound(downIntegral + downError * dT,
-guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT],
guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_ILIMIT]);
downCommand = (downError * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KP] +
downIntegral * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KI] -
nedAccel.Down * guidanceSettings.VerticalVelPID[GUIDANCESETTINGS_VERTICALVELPID_KD]);
attitudeDesired.Throttle = bound(downCommand, 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
attitudeDesired.Pitch = bound(-northCommand * cosf(attitudeActual.Yaw * M_PI / 180) +
-eastCommand * sinf(attitudeActual.Yaw * M_PI / 180),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
attitudeDesired.Roll = bound(-northCommand * sinf(attitudeActual.Yaw * M_PI / 180) +
eastCommand * cosf(attitudeActual.Yaw * M_PI / 180),
-guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch);
if(guidanceSettings.ThrottleControl == GUIDANCESETTINGS_THROTTLECONTROL_FALSE) {
// For now override throttle with manual control. Disable at your risk, quad goes to China.
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
attitudeDesired.Throttle = manualControl.Throttle;
}
AttitudeDesiredSet(&attitudeDesired);
}
/**
* 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 updateGroundDesiredAttitude()
{
float dT = guidanceSettings.UpdatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityActualData velocityActual;
StabilizationDesiredData stabDesired;
AttitudeActualData attitudeActual;
NedAccelData nedAccel;
GroundPathFollowerSettingsData guidanceSettings;
StabilizationSettingsData stabSettings;
SystemSettingsData systemSettings;
SystemSettingsGet(&systemSettings);
GroundPathFollowerSettingsGet(&guidanceSettings);
VelocityActualGet(&velocityActual);
VelocityDesiredGet(&velocityDesired);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeActualGet(&attitudeActual);
StabilizationSettingsGet(&stabSettings);
NedAccelGet(&nedAccel);
float northVel = velocityActual.North;
float eastVel = velocityActual.East;
// Calculate direction from velocityDesired and set stabDesired.Yaw
stabDesired.Yaw = atan2f( velocityDesired.East, velocityDesired.North ) * RAD2DEG;
// Calculate throttle and set stabDesired.Throttle
float velDesired = sqrtf(powf(velocityDesired.East,2) + powf(velocityDesired.North,2));
float velActual = sqrtf(powf(eastVel,2) + powf(northVel,2));
ManualControlCommandData manualControlData;
ManualControlCommandGet(&manualControlData);
switch (guidanceSettings.ThrottleControl) {
case GROUNDPATHFOLLOWERSETTINGS_THROTTLECONTROL_MANUAL:
{
stabDesired.Throttle = manualControlData.Throttle;
break;
}
case GROUNDPATHFOLLOWERSETTINGS_THROTTLECONTROL_PROPORTIONAL:
{
float velRatio = velDesired / guidanceSettings.HorizontalVelMax;
stabDesired.Throttle = guidanceSettings.MaxThrottle * velRatio;
if (guidanceSettings.ManualOverride == GROUNDPATHFOLLOWERSETTINGS_MANUALOVERRIDE_TRUE) {
stabDesired.Throttle = stabDesired.Throttle * manualControlData.Throttle;
}
break;
}
case GROUNDPATHFOLLOWERSETTINGS_THROTTLECONTROL_AUTO:
{
float velError = velDesired - velActual;
stabDesired.Throttle = pid_apply(&ground_pids[VELOCITY], velError, dT) + velDesired * guidanceSettings.VelocityFeedforward;
if (guidanceSettings.ManualOverride == GROUNDPATHFOLLOWERSETTINGS_MANUALOVERRIDE_TRUE) {
stabDesired.Throttle = stabDesired.Throttle * manualControlData.Throttle;
}
break;
}
default:
{
PIOS_Assert(0);
break;
}
}
// Limit throttle as per settings
stabDesired.Throttle = bound_min_max(stabDesired.Throttle, 0, guidanceSettings.MaxThrottle);
// Set StabilizationDesired object
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
StabilizationDesiredSet(&stabDesired);
}
/**
* 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 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;
}
/**
* 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;
}
/**
* 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
*/
uint8_t waypointFollowing(uint8_t flightMode, FixedWingPathFollowerSettingsCCData fixedwingpathfollowerSettings)
{
float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f; //Convert from [ms] to [s]
VelocityActualData velocityActual;
StabilizationDesiredData stabDesired;
float trueAirspeed;
float calibratedAirspeedActual;
float airspeedDesired;
float airspeedError;
float pitchCommand;
float powerCommand;
float headingError_R;
float rollCommand;
//TODO: Move these out of the loop
FixedWingAirspeedsData fixedwingAirspeeds;
FixedWingAirspeedsGet(&fixedwingAirspeeds);
VelocityActualGet(&velocityActual);
StabilizationDesiredGet(&stabDesired);
// TODO: Create UAVO that merges airspeed together
AirspeedActualTrueAirspeedGet(&trueAirspeed);
PositionActualData positionActual;
PositionActualGet(&positionActual);
PathDesiredData pathDesired;
PathDesiredGet(&pathDesired);
if (flightStatusUpdate) {
//Reset integrals
integral->totalEnergyError = 0;
integral->airspeedError = 0;
integral->lineError = 0;
integral->circleError = 0;
if (flightMode == FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME) {
// Simple Return To Home mode: climb 10 meters and fly to home position
pathDesired.Start[PATHDESIRED_START_NORTH] =
positionActual.North;
pathDesired.Start[PATHDESIRED_START_EAST] =
positionActual.East;
pathDesired.Start[PATHDESIRED_START_DOWN] =
positionActual.Down;
pathDesired.End[PATHDESIRED_END_NORTH] = 0;
pathDesired.End[PATHDESIRED_END_EAST] = 0;
pathDesired.End[PATHDESIRED_END_DOWN] =
positionActual.Down - 10;
pathDesired.StartingVelocity =
fixedwingAirspeeds.BestClimbRateSpeed;
pathDesired.EndingVelocity =
fixedwingAirspeeds.BestClimbRateSpeed;
pathDesired.Mode = PATHDESIRED_MODE_FLYVECTOR;
homeOrbit = false;
} else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) {
// Simple position hold: stay at present altitude and position
//Offset by one so that the start and end points don't perfectly coincide
pathDesired.Start[PATHDESIRED_START_NORTH] =
positionActual.North - 1;
pathDesired.Start[PATHDESIRED_START_EAST] =
positionActual.East;
pathDesired.Start[PATHDESIRED_START_DOWN] =
positionActual.Down;
pathDesired.End[PATHDESIRED_END_NORTH] =
positionActual.North;
pathDesired.End[PATHDESIRED_END_EAST] =
positionActual.East;
pathDesired.End[PATHDESIRED_END_DOWN] =
positionActual.Down;
pathDesired.StartingVelocity =
fixedwingAirspeeds.BestClimbRateSpeed;
pathDesired.EndingVelocity =
fixedwingAirspeeds.BestClimbRateSpeed;
pathDesired.Mode = PATHDESIRED_MODE_FLYVECTOR;
}
PathDesiredSet(&pathDesired);
flightStatusUpdate = false;
}
/**
* Compute speed error (required for throttle and pitch)
*/
// Current airspeed
calibratedAirspeedActual = trueAirspeed; //BOOOOOOOOOO!!! Where's the conversion from TAS to CAS?
// Current heading
float headingActual_R =
atan2f(velocityActual.East, velocityActual.North);
// Desired airspeed
airspeedDesired = pathDesired.EndingVelocity;
// Airspeed error
airspeedError = airspeedDesired - calibratedAirspeedActual;
/**
* Compute desired throttle command
*/
//Proxy because instead of m*(1/2*v^2+g*h), it's v^2+2*gh. This saves processing power
float totalEnergyProxySetpoint = powf(pathDesired.EndingVelocity,
2.0f) -
2.0f * GRAVITY * pathDesired.End[2];
float totalEnergyProxyActual =
powf(trueAirspeed, 2.0f) - 2.0f * GRAVITY * positionActual.Down;
float errorTotalEnergy =
totalEnergyProxySetpoint - totalEnergyProxyActual;
float throttle_kp =
fixedwingpathfollowerSettings.
ThrottlePI[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLEPI_KP];
float throttle_ki = fixedwingpathfollowerSettings.ThrottlePI[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLEPI_KI];
float throttle_ilimit = fixedwingpathfollowerSettings.ThrottlePI[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLEPI_ILIMIT];
//Integrate with bound. Make integral leaky for better performance. Approximately 30s time constant.
if (throttle_ilimit > 0.0f) {
integral->totalEnergyError =
bound(integral->totalEnergyError + errorTotalEnergy * dT,
-throttle_ilimit / throttle_ki,
throttle_ilimit / throttle_ki) * (1.0f -
1.0f / (1.0f +
30.0f /
dT));
}
powerCommand = errorTotalEnergy * throttle_kp
+ integral->totalEnergyError * throttle_ki;
float throttlelimit_neutral =
fixedwingpathfollowerSettings.
ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLELIMIT_NEUTRAL];
float throttlelimit_min =
fixedwingpathfollowerSettings.
ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLELIMIT_MIN];
float throttlelimit_max =
fixedwingpathfollowerSettings.
ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLELIMIT_MAX];
// set throttle
stabDesired.Throttle = bound(powerCommand + throttlelimit_neutral,
throttlelimit_min, throttlelimit_max);
/**
* Compute desired pitch command
*/
float airspeed_kp =
fixedwingpathfollowerSettings.
AirspeedPI[FIXEDWINGPATHFOLLOWERSETTINGSCC_AIRSPEEDPI_KP];
float airspeed_ki =
fixedwingpathfollowerSettings.
AirspeedPI[FIXEDWINGPATHFOLLOWERSETTINGSCC_AIRSPEEDPI_KI];
float airspeed_ilimit =
fixedwingpathfollowerSettings.
AirspeedPI[FIXEDWINGPATHFOLLOWERSETTINGSCC_AIRSPEEDPI_ILIMIT];
if (airspeed_ki > 0.0f) {
//Integrate with saturation
integral->airspeedError =
bound(integral->airspeedError + airspeedError * dT,
-airspeed_ilimit / airspeed_ki,
airspeed_ilimit / airspeed_ki);
}
//Compute the cross feed from altitude to pitch, with saturation
float pitchcrossfeed_kp =
fixedwingpathfollowerSettings.
VerticalToPitchCrossFeed
[FIXEDWINGPATHFOLLOWERSETTINGSCC_VERTICALTOPITCHCROSSFEED_KP];
float pitchcrossfeed_min =
fixedwingpathfollowerSettings.
VerticalToPitchCrossFeed
[FIXEDWINGPATHFOLLOWERSETTINGSCC_VERTICALTOPITCHCROSSFEED_MAX];
float pitchcrossfeed_max =
fixedwingpathfollowerSettings.
VerticalToPitchCrossFeed
[FIXEDWINGPATHFOLLOWERSETTINGSCC_VERTICALTOPITCHCROSSFEED_MAX];
float alitudeError = -(pathDesired.End[2] - positionActual.Down); //Negative to convert from Down to altitude
float altitudeToPitchCommandComponent =
bound(alitudeError * pitchcrossfeed_kp,
-pitchcrossfeed_min,
pitchcrossfeed_max);
//Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand = -(airspeedError * airspeed_kp
+ integral->airspeedError * airspeed_ki) +
altitudeToPitchCommandComponent;
//Saturate pitch command
float pitchlimit_neutral =
fixedwingpathfollowerSettings.
PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_PITCHLIMIT_NEUTRAL];
float pitchlimit_min =
fixedwingpathfollowerSettings.
PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_PITCHLIMIT_MIN];
float pitchlimit_max =
fixedwingpathfollowerSettings.
PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_PITCHLIMIT_MAX];
stabDesired.Pitch = bound(pitchlimit_neutral +
pitchCommand, pitchlimit_min, pitchlimit_max);
/**
* Compute desired roll command
*/
float p[3] =
{ positionActual.North, positionActual.East, positionActual.Down };
float *c = pathDesired.End;
float *r = pathDesired.Start;
float q[3] = { pathDesired.End[0] - pathDesired.Start[0],
pathDesired.End[1] - pathDesired.Start[1],
pathDesired.End[2] - pathDesired.Start[2]
};
float k_path = fixedwingpathfollowerSettings.VectorFollowingGain / pathDesired.EndingVelocity; //Divide gain by airspeed so that the turn rate is independent of airspeed
float k_orbit = fixedwingpathfollowerSettings.OrbitFollowingGain / pathDesired.EndingVelocity; //Divide gain by airspeed so that the turn rate is independent of airspeed
float k_psi_int = fixedwingpathfollowerSettings.FollowerIntegralGain;
//========================================
//SHOULD NOT BE HARD CODED
bool direction;
float chi_inf = PI / 4.0f; //THIS NEEDS TO BE A FUNCTION OF HOW LONG OUR PATH IS.
//Saturate chi_inf. I.e., never approach the path at a steeper angle than 45 degrees
chi_inf = chi_inf < PI / 4.0f ? PI / 4.0f : chi_inf;
//========================================
float rho;
float headingCommand_R;
float pncn = p[0] - c[0];
float pece = p[1] - c[1];
float d = sqrtf(pncn * pncn + pece * pece);
//Assume that we want a 15 degree bank angle. This should yield a nice, non-agressive turn
#define ROLL_FOR_HOLDING_CIRCLE 15.0f
//Calculate radius, rho, using r*omega=v and omega = g/V_g * tan(phi)
//THIS SHOULD ONLY BE CALCULATED ONCE, INSTEAD OF EVERY TIME
rho = powf(pathDesired.EndingVelocity,
2) / (GRAVITY * tanf(fabs(ROLL_FOR_HOLDING_CIRCLE * DEG2RAD)));
typedef enum {
LINE,
ORBIT
} pathTypes_t;
pathTypes_t pathType;
//Determine if we should fly on a line or orbit path.
switch (flightMode) {
case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME:
if (d < rho + 5.0f * pathDesired.EndingVelocity || homeOrbit == true) { //When approx five seconds from the circle, start integrating into it
pathType = ORBIT;
homeOrbit = true;
} else {
pathType = LINE;
}
break;
case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD:
pathType = ORBIT;
break;
default:
pathType = LINE;
break;
}
//Check to see if we've gone past our destination. Since the path follower
//is simply following a vector, it has no concept of where the vector ends.
//It will simply keep following it to infinity if we don't stop it. So while
//we don't know why the commutation to the next point failed, we don't know
//we don't want the plane flying off.
if (pathType == LINE) {
//Compute the norm squared of the horizontal path length
//IT WOULD BE NICE TO ONLY DO THIS ONCE PER WAYPOINT UPDATE, INSTEAD OF
//EVERY LOOP
float pathLength2 = q[0] * q[0] + q[1] * q[1];
//Perform a quick vector math operation, |a| < a.b/|a| = |b|cos(theta),
//to test if we've gone past the waypoint. Add in a distance equal to 5s
//of flight time, for good measure to make sure we don't add jitter.
if (((p[0] - r[0]) * q[0] + (p[1] - r[1]) * q[1]) >
pathLength2 + 5.0f * pathDesired.EndingVelocity) {
//Whoops, we've really overflown our destination point, and haven't
//received any instructions. Start circling
//flightMode will reset the next time a waypoint changes, so there's
//no danger of it getting stuck in orbit mode.
flightMode = FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD;
pathType = ORBIT;
}
}
switch (pathType) {
case ORBIT:
if (pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLELEFT) {
direction = false;
} else {
//In the case where the direction is undefined, always fly in a
//clockwise fashion
direction = true;
}
headingCommand_R =
followOrbit(c, rho, direction, p, headingActual_R, k_orbit,
k_psi_int, dT);
break;
case LINE:
headingCommand_R =
followStraightLine(r, q, p, headingActual_R, chi_inf,
k_path, k_psi_int, dT);
break;
}
//Calculate heading error
headingError_R = headingCommand_R - headingActual_R;
//Wrap heading error around circle
if (headingError_R < -PI)
headingError_R += 2.0f * PI;
if (headingError_R > PI)
headingError_R -= 2.0f * PI;
//GET RID OF THE RAD2DEG. IT CAN BE FACTORED INTO HeadingPI
float rolllimit_neutral =
fixedwingpathfollowerSettings.
RollLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_ROLLLIMIT_NEUTRAL];
float rolllimit_min =
fixedwingpathfollowerSettings.
RollLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_ROLLLIMIT_MIN];
float rolllimit_max =
fixedwingpathfollowerSettings.
RollLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_ROLLLIMIT_MAX];
float headingpi_kp =
fixedwingpathfollowerSettings.
HeadingPI[FIXEDWINGPATHFOLLOWERSETTINGSCC_HEADINGPI_KP];
rollCommand = (headingError_R * headingpi_kp) * RAD2DEG;
//Turn heading
stabDesired.Roll = bound(rolllimit_neutral +
rollCommand, rolllimit_min, rolllimit_max);
#ifdef SIM_OSX
fprintf(stderr, " headingError_R: %f, rollCommand: %f\n",
headingError_R, rollCommand);
#endif
/**
* Compute desired yaw command
*/
// Coordinated flight, so we reset the desired yaw.
stabDesired.Yaw = 0;
stabDesired.
StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] =
STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.
StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] =
STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.
StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] =
STABILIZATIONDESIRED_STABILIZATIONMODE_NONE;
StabilizationDesiredSet(&stabDesired);
//Stuff some debug variables into PathDesired, because right now these
//fields aren't being used.
pathDesired.ModeParameters[0] = pitchCommand;
pathDesired.ModeParameters[1] = airspeedError;
pathDesired.ModeParameters[2] = integral->airspeedError;
pathDesired.ModeParameters[3] = alitudeError;
pathDesired.UID = errorTotalEnergy;
PathDesiredSet(&pathDesired);
return 0;
}
/**
* @brief Use the INSGPS fusion algorithm in either indoor or outdoor mode (use GPS)
* @params[in] first_run This is the first run so trigger reinitialization
* @params[in] outdoor_mode If true use the GPS for position, if false weakly pull to (0,0)
* @return 0 for success, -1 for failure
*/
static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode)
{
UAVObjEvent ev;
GyrosData gyrosData;
AccelsData accelsData;
MagnetometerData magData;
BaroAltitudeData baroData;
GPSPositionData gpsData;
GPSVelocityData gpsVelData;
GyrosBiasData gyrosBias;
static bool mag_updated = false;
static bool baro_updated;
static bool gps_updated;
static bool gps_vel_updated;
static float baroOffset = 0;
static uint32_t ins_last_time = 0;
static bool inited;
float NED[3] = {0.0f, 0.0f, 0.0f};
float vel[3] = {0.0f, 0.0f, 0.0f};
float zeros[3] = {0.0f, 0.0f, 0.0f};
// Perform the update
uint16_t sensors = 0;
float dT;
// Wait until the gyro and accel object is updated, if a timeout then go to failsafe
if ( (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) ||
(xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE) )
{
// Do not set attitude timeout warnings in simulation mode
if (!AttitudeActualReadOnly()){
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
return -1;
}
}
if (inited) {
mag_updated = 0;
baro_updated = 0;
gps_updated = 0;
gps_vel_updated = 0;
}
if (first_run) {
inited = false;
init_stage = 0;
mag_updated = 0;
baro_updated = 0;
gps_updated = 0;
gps_vel_updated = 0;
ins_last_time = PIOS_DELAY_GetRaw();
return 0;
}
mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE);
baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE;
gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode;
// Get most recent data
GyrosGet(&gyrosData);
AccelsGet(&accelsData);
MagnetometerGet(&magData);
BaroAltitudeGet(&baroData);
GPSPositionGet(&gpsData);
GPSVelocityGet(&gpsVelData);
GyrosBiasGet(&gyrosBias);
// Discard mag if it has NAN (normally from bad calibration)
mag_updated &= (magData.x == magData.x && magData.y == magData.y && magData.z == magData.z);
// Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily
// switching between indoor and outdoor mode with Set = false)
mag_updated &= (homeLocation.Be[0] != 0 || homeLocation.Be[1] != 0 || homeLocation.Be[2]);
// Have a minimum requirement for gps usage
gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP <= 4.0f) && (homeLocation.Set == HOMELOCATION_SET_TRUE);
if (!inited)
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
else if (outdoor_mode && gpsData.Satellites < 7)
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR);
else
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode)) {
// Don't initialize until all sensors are read
if (init_stage == 0 && !outdoor_mode) {
float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
float q[4];
float pos[3] = {0.0f, 0.0f, 0.0f};
// Initialize barometric offset to homelocation altitude
baroOffset = -baroData.Altitude;
pos[2] = -(baroData.Altitude + baroOffset);
// Reset the INS algorithm
INSGPSInit();
INSSetMagVar(revoCalibration.mag_var);
INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var);
INSSetBaroVar(revoCalibration.baro_var);
// Initialize the gyro bias from the settings
float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f};
INSSetGyroBias(gyro_bias);
xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
MagnetometerGet(&magData);
// Set initial attitude
AttitudeActualData attitudeActual;
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual);
q[0] = attitudeActual.q1;
q[1] = attitudeActual.q2;
q[2] = attitudeActual.q3;
q[3] = attitudeActual.q4;
INSSetState(pos, zeros, q, zeros, zeros);
INSResetP(Pdiag);
} else if (init_stage == 0 && outdoor_mode) {
float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f};
float q[4];
float NED[3];
// Reset the INS algorithm
INSGPSInit();
INSSetMagVar(revoCalibration.mag_var);
INSSetAccelVar(revoCalibration.accel_var);
INSSetGyroVar(revoCalibration.gyro_var);
INSSetBaroVar(revoCalibration.baro_var);
INSSetMagNorth(homeLocation.Be);
// Initialize the gyro bias from the settings
float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f};
INSSetGyroBias(gyro_bias);
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
// Transform the GPS position into NED coordinates
getNED(&gpsPosition, NED);
// Initialize barometric offset to cirrent GPS NED coordinate
baroOffset = -NED[2] - baroData.Altitude;
xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS);
MagnetometerGet(&magData);
// Set initial attitude
AttitudeActualData attitudeActual;
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual);
q[0] = attitudeActual.q1;
q[1] = attitudeActual.q2;
q[2] = attitudeActual.q3;
q[3] = attitudeActual.q4;
INSSetState(NED, zeros, q, zeros, zeros);
INSResetP(Pdiag);
} else if (init_stage > 0) {
// Run prediction a bit before any corrections
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
GyrosBiasGet(&gyrosBias);
float gyros[3] = {(gyrosData.x + gyrosBias.x) * F_PI / 180.0f,
(gyrosData.y + gyrosBias.y) * F_PI / 180.0f,
(gyrosData.z + gyrosBias.z) * F_PI / 180.0f};
INSStatePrediction(gyros, &accelsData.x, dT);
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
attitude.q1 = Nav.q[0];
attitude.q2 = Nav.q[1];
attitude.q3 = Nav.q[2];
attitude.q4 = Nav.q[3];
Quaternion2RPY(&attitude.q1,&attitude.Roll);
AttitudeActualSet(&attitude);
}
init_stage++;
if(init_stage > 10)
inited = true;
ins_last_time = PIOS_DELAY_GetRaw();
return 0;
}
if (!inited)
return 0;
dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f;
ins_last_time = PIOS_DELAY_GetRaw();
// This should only happen at start up or at mode switches
if(dT > 0.01f)
dT = 0.01f;
else if(dT <= 0.001f)
dT = 0.001f;
// If the gyro bias setting was updated we should reset
// the state estimate of the EKF
if(gyroBiasSettingsUpdated) {
float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f};
INSSetGyroBias(gyro_bias);
gyroBiasSettingsUpdated = false;
}
// Because the sensor module remove the bias we need to add it
// back in here so that the INS algorithm can track it correctly
float gyros[3] = {gyrosData.x * F_PI / 180.0f, gyrosData.y * F_PI / 180.0f, gyrosData.z * F_PI / 180.0f};
if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) {
gyros[0] += gyrosBias.x * F_PI / 180.0f;
gyros[1] += gyrosBias.y * F_PI / 180.0f;
gyros[2] += gyrosBias.z * F_PI / 180.0f;
}
// Advance the state estimate
INSStatePrediction(gyros, &accelsData.x, dT);
// Copy the attitude into the UAVO
AttitudeActualData attitude;
AttitudeActualGet(&attitude);
attitude.q1 = Nav.q[0];
attitude.q2 = Nav.q[1];
attitude.q3 = Nav.q[2];
attitude.q4 = Nav.q[3];
Quaternion2RPY(&attitude.q1,&attitude.Roll);
AttitudeActualSet(&attitude);
// Advance the covariance estimate
INSCovariancePrediction(dT);
if(mag_updated)
sensors |= MAG_SENSORS;
if(baro_updated)
sensors |= BARO_SENSOR;
INSSetMagNorth(homeLocation.Be);
if (gps_updated && outdoor_mode)
{
INSSetPosVelVar(revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_POS], revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_VEL]);
sensors |= POS_SENSORS;
if (0) { // Old code to take horizontal velocity from GPS Position update
sensors |= HORIZ_SENSORS;
vel[0] = gpsData.Groundspeed * cosf(gpsData.Heading * F_PI / 180.0f);
vel[1] = gpsData.Groundspeed * sinf(gpsData.Heading * F_PI / 180.0f);
vel[2] = 0;
}
// Transform the GPS position into NED coordinates
getNED(&gpsData, NED);
// Track barometric altitude offset with a low pass filter
baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset +
(1.0f - BARO_OFFSET_LOWPASS_ALPHA )
* ( -NED[2] - baroData.Altitude );
} else if (!outdoor_mode) {
INSSetPosVelVar(1e2f, 1e2f);
vel[0] = vel[1] = vel[2] = 0;
NED[0] = NED[1] = 0;
NED[2] = -(baroData.Altitude + baroOffset);
sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS;
sensors |= POS_SENSORS |VERT_SENSORS;
}
if (gps_vel_updated && outdoor_mode) {
sensors |= HORIZ_SENSORS | VERT_SENSORS;
vel[0] = gpsVelData.North;
vel[1] = gpsVelData.East;
vel[2] = gpsVelData.Down;
}
/*
* TODO: Need to add a general sanity check for all the inputs to make sure their kosher
* although probably should occur within INS itself
*/
if (sensors)
INSCorrection(&magData.x, NED, vel, ( baroData.Altitude + baroOffset ), sensors);
// Copy the position and velocity into the UAVO
PositionActualData positionActual;
PositionActualGet(&positionActual);
positionActual.North = Nav.Pos[0];
positionActual.East = Nav.Pos[1];
positionActual.Down = Nav.Pos[2];
PositionActualSet(&positionActual);
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
velocityActual.North = Nav.Vel[0];
velocityActual.East = Nav.Vel[1];
velocityActual.Down = Nav.Vel[2];
VelocityActualSet(&velocityActual);
if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE && !gyroBiasSettingsUpdated) {
// Copy the gyro bias into the UAVO except when it was updated
// from the settings during the calculation, then consume it
// next cycle
gyrosBias.x = Nav.gyro_bias[0] * 180.0f / F_PI;
gyrosBias.y = Nav.gyro_bias[1] * 180.0f / F_PI;
gyrosBias.z = Nav.gyro_bias[2] * 180.0f / F_PI;
GyrosBiasSet(&gyrosBias);
}
return 0;
}
static int32_t updateAttitudeComplementary(bool first_run)
{
UAVObjEvent ev;
GyrosData gyrosData;
AccelsData accelsData;
static int32_t timeval;
float dT;
static uint8_t init = 0;
// Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe
if ( xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE ||
xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE )
{
// When one of these is updated so should the other
// Do not set attitude timeout warnings in simulation mode
if (!AttitudeActualReadOnly()){
AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING);
return -1;
}
}
AccelsGet(&accelsData);
// During initialization and
FlightStatusData flightStatus;
FlightStatusGet(&flightStatus);
if(first_run) {
#if defined(PIOS_INCLUDE_HMC5883)
// To initialize we need a valid mag reading
if ( xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE )
return -1;
MagnetometerData magData;
MagnetometerGet(&magData);
#else
MagnetometerData magData;
magData.x = 100;
magData.y = 0;
magData.z = 0;
#endif
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
init = 0;
attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI;
attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI;
attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI;
RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1);
AttitudeActualSet(&attitudeActual);
timeval = PIOS_DELAY_GetRaw();
return 0;
}
if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) {
// For first 7 seconds use accels to get gyro bias
attitudeSettings.AccelKp = 1;
attitudeSettings.AccelKi = 0.9;
attitudeSettings.YawBiasRate = 0.23;
magKp = 1;
} else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) {
attitudeSettings.AccelKp = 1;
attitudeSettings.AccelKi = 0.9;
attitudeSettings.YawBiasRate = 0.23;
magKp = 1;
init = 0;
} else if (init == 0) {
// Reload settings (all the rates)
AttitudeSettingsGet(&attitudeSettings);
magKp = 0.01f;
init = 1;
}
GyrosGet(&gyrosData);
// Compute the dT using the cpu clock
dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f;
timeval = PIOS_DELAY_GetRaw();
float q[4];
AttitudeActualData attitudeActual;
AttitudeActualGet(&attitudeActual);
float grot[3];
float accel_err[3];
// Get the current attitude estimate
quat_copy(&attitudeActual.q1, q);
// Rotate gravity to body frame and cross with accels
grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2]));
grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1]));
grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]);
CrossProduct((const float *) &accelsData.x, (const float *) grot, accel_err);
// Account for accel magnitude
accel_mag = accelsData.x*accelsData.x + accelsData.y*accelsData.y + accelsData.z*accelsData.z;
accel_mag = sqrtf(accel_mag);
accel_err[0] /= accel_mag;
accel_err[1] /= accel_mag;
accel_err[2] /= accel_mag;
if ( xQueueReceive(magQueue, &ev, 0) != pdTRUE )
{
// Rotate gravity to body frame and cross with accels
float brot[3];
float Rbe[3][3];
MagnetometerData mag;
Quaternion2R(q, Rbe);
MagnetometerGet(&mag);
// If the mag is producing bad data don't use it (normally bad calibration)
if (mag.x == mag.x && mag.y == mag.y && mag.z == mag.z) {
rot_mult(Rbe, homeLocation.Be, brot);
float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z);
mag.x /= mag_len;
mag.y /= mag_len;
mag.z /= mag_len;
float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]);
brot[0] /= bmag;
brot[1] /= bmag;
brot[2] /= bmag;
// Only compute if neither vector is null
if (bmag < 1 || mag_len < 1)
mag_err[0] = mag_err[1] = mag_err[2] = 0;
else
CrossProduct((const float *) &mag.x, (const float *) brot, mag_err);
}
} else {
mag_err[0] = mag_err[1] = mag_err[2] = 0;
}
// Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s
GyrosBiasData gyrosBias;
GyrosBiasGet(&gyrosBias);
gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi;
gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi;
gyrosBias.z -= mag_err[2] * magKi;
GyrosBiasSet(&gyrosBias);
// Correct rates based on error, integral component dealt with in updateSensors
gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT;
gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT;
gyrosData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT;
// Work out time derivative from INSAlgo writeup
// Also accounts for the fact that gyros are in deg/s
float qdot[4];
qdot[0] = (-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT * F_PI / 180 / 2;
qdot[1] = (q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT * F_PI / 180 / 2;
qdot[2] = (q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT * F_PI / 180 / 2;
qdot[3] = (-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT * F_PI / 180 / 2;
// Take a time step
q[0] = q[0] + qdot[0];
q[1] = q[1] + qdot[1];
q[2] = q[2] + qdot[2];
q[3] = q[3] + qdot[3];
if(q[0] < 0) {
q[0] = -q[0];
q[1] = -q[1];
q[2] = -q[2];
q[3] = -q[3];
}
// Renomalize
qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]);
q[0] = q[0] / qmag;
q[1] = q[1] / qmag;
q[2] = q[2] / qmag;
q[3] = q[3] / qmag;
// If quaternion has become inappropriately short or is nan reinit.
// THIS SHOULD NEVER ACTUALLY HAPPEN
if((fabs(qmag) < 1.0e-3f) || (qmag != qmag)) {
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
}
quat_copy(q, &attitudeActual.q1);
// Convert into eueler degrees (makes assumptions about RPY order)
Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll);
AttitudeActualSet(&attitudeActual);
// Flush these queues for avoid errors
xQueueReceive(baroQueue, &ev, 0);
if ( xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE ) {
float NED[3];
// Transform the GPS position into NED coordinates
GPSPositionData gpsPosition;
GPSPositionGet(&gpsPosition);
getNED(&gpsPosition, NED);
PositionActualData positionActual;
PositionActualGet(&positionActual);
positionActual.North = NED[0];
positionActual.East = NED[1];
positionActual.Down = NED[2];
PositionActualSet(&positionActual);
}
if ( xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE ) {
// Transform the GPS position into NED coordinates
GPSVelocityData gpsVelocity;
GPSVelocityGet(&gpsVelocity);
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
velocityActual.North = gpsVelocity.North;
velocityActual.East = gpsVelocity.East;
velocityActual.Down = gpsVelocity.Down;
VelocityActualSet(&velocityActual);
}
AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE);
return 0;
}
bool vtol_follower_control_loiter(float dT, float *hold_pos, float *att_adj,
float *alt_adj) {
LoiterCommandData cmd;
LoiterCommandGet(&cmd);
// XXX TODO reproject when we're not issuing body-centric commands
float commands_rp[2] = {
cmd.Roll,
cmd.Pitch
};
const float CMD_THRESHOLD = 0.2f;
float command_mag = vectorn_magnitude(commands_rp, 2);
float deadband_mag = loiter_deadband(command_mag, CMD_THRESHOLD, 40);
float down_cmd = 0;
if (guidanceSettings.ThrottleControl &&
guidanceSettings.LoiterAllowAltControl) {
// Inverted because we want units in "Down" frame
// Doubled to recenter to 1 to -1 scale from 0-1.
// loiter_deadband clips appropriately.
down_cmd = loiter_deadband(1 - (cmd.Throttle * 2),
altitudeHoldSettings.Deadband / 100.0f,
altitudeHoldSettings.Expo);
}
// Peak detect and decay of the past command magnitude
static float historic_mag = 0.0f;
// Initialize altitude command
*alt_adj = 0;
// First reduce by decay constant
historic_mag *= loiter_brakealpha;
// If our current magnitude is greater than the result, increase it.
if (deadband_mag > historic_mag) {
historic_mag = deadband_mag;
}
// And if we haven't had any significant command lately, bug out and
// do nothing.
if ((historic_mag < 0.001f) && (fabsf(down_cmd) < 0.001f)) {
att_adj[0] = 0; att_adj[1] = 0;
return false;
}
// Normalize our command magnitude. Command vectors from this
// point are normalized.
if (command_mag >= 0.001f) {
commands_rp[0] /= command_mag;
commands_rp[1] /= command_mag;
} else {
// Just pick a direction
commands_rp[0] = 0.0f;
commands_rp[1] = -1.0f;
}
// Find our current position error
PositionActualData positionActual;
PositionActualGet(&positionActual);
float cur_pos_ned[3] = { positionActual.North,
positionActual.East, positionActual.Down };
float total_poserr_ned[3];
vector3_distances(cur_pos_ned, hold_pos, total_poserr_ned, false);
if (deadband_mag >= 0.001f) {
float yaw;
AttitudeActualYawGet(&yaw);
float commands_ne[2];
// 90 degrees here compensates for the above being in roll-pitch
// order vs. north-east (and where yaw is defined).
vector2_rotate(commands_rp, commands_ne, 90 + yaw);
VelocityActualData velocityActual;
VelocityActualGet(&velocityActual);
// Come up with a target velocity for us to fly the command
// at, considering our current momentum in that direction.
float target_vel = guidanceSettings.LoiterInitialMaxVel *
deadband_mag;
// Plus whatever current velocity we're making good in
// that direction..
// find the portion of our current velocity vector parallel to
// cmd.
float parallel_sign =
velocityActual.North * commands_ne[0] +
velocityActual.East * commands_ne[1];
if (parallel_sign > 0) {
float parallel_mag = sqrtf(
powf(velocityActual.North * commands_ne[0], 2) +
powf(velocityActual.East * commands_ne[1], 2));
target_vel += deadband_mag * parallel_mag;
}
// Feed the target velocity forward for our new desired position
hold_pos[0] = cur_pos_ned[0] +
commands_ne[0] * target_vel *
guidanceSettings.LoiterLookaheadTimeConstant;
hold_pos[1] = cur_pos_ned[1] +
commands_ne[1] * target_vel *
guidanceSettings.LoiterLookaheadTimeConstant;
}
// Now put a portion of the error back in. At full stick
// deflection, decay error at specified time constant
float scaled_error_alpha = 1 - historic_mag * (1 - loiter_errordecayalpha);
hold_pos[0] -= scaled_error_alpha * total_poserr_ned[0];
hold_pos[1] -= scaled_error_alpha * total_poserr_ned[1];
// Compute attitude feedforward
att_adj[0] = deadband_mag * commands_rp[0] *
guidanceSettings.LoiterAttitudeFeedthrough;
att_adj[1] = deadband_mag * commands_rp[1] *
guidanceSettings.LoiterAttitudeFeedthrough;
// If we are being commanded to climb or descend...
if (fabsf(down_cmd) >= 0.001f) {
// Forgive all altitude error so when position controller comes
// back we do something sane
hold_pos[2] = cur_pos_ned[2];
// and output an adjustment for velocity control use */
*alt_adj = down_cmd * guidanceSettings.VerticalVelMax;
}
return true;
}
