/*
* Initialize function loads first data sets, and allocates memory for structure.
*/
void gps_airspeedInitialize()
{
// This method saves memory in case we don't use the GPS module.
gps = (struct GPSGlobals *)pvPortMalloc(sizeof(struct GPSGlobals));
// GPS airspeed calculation variables
VelocityStateInitialize();
VelocityStateData gpsVelData;
VelocityStateGet(&gpsVelData);
gps->gpsVelOld_N = gpsVelData.North;
gps->gpsVelOld_E = gpsVelData.East;
gps->gpsVelOld_D = gpsVelData.Down;
gps->oldAirspeed = 0.0f;
AttitudeStateData attData;
AttitudeStateGet(&attData);
float Rbe[3][3];
float q[4] = { attData.q1, attData.q2, attData.q3, attData.q4 };
// Calculate rotation matrix
Quaternion2R(q, Rbe);
gps->RbeCol1_old[0] = Rbe[0][0];
gps->RbeCol1_old[1] = Rbe[0][1];
gps->RbeCol1_old[2] = Rbe[0][2];
}
/*
* Calculate airspeed as a function of GPS groundspeed and vehicle attitude.
* From "IMU Wind Estimation (Theory)", by William Premerlani.
* The idea is that V_gps=V_air+V_wind. If we assume wind constant, =>
* V_gps_2-V_gps_1 = (V_air_2+V_wind_2) -(V_air_1+V_wind_1) = V_air_2 - V_air_1.
* If we assume airspeed constant, => V_gps_2-V_gps_1 = |V|*(f_2 - f1),
* where "f" is the fuselage vector in earth coordinates.
* We then solve for |V| = |V_gps_2-V_gps_1|/ |f_2 - f1|.
*/
void gps_airspeedGet(AirspeedSensorData *airspeedData, AirspeedSettingsData *airspeedSettings)
{
float Rbe[3][3];
{ // Scoping to save memory. We really just need Rbe.
AttitudeStateData attData;
AttitudeStateGet(&attData);
float q[4] = { attData.q1, attData.q2, attData.q3, attData.q4 };
// Calculate rotation matrix
Quaternion2R(q, Rbe);
}
// Calculate the cos(angle) between the two fuselage basis vectors
float cosDiff = (Rbe[0][0] * gps->RbeCol1_old[0]) + (Rbe[0][1] * gps->RbeCol1_old[1]) + (Rbe[0][2] * gps->RbeCol1_old[2]);
// If there's more than a 5 degree difference between two fuselage measurements, then we have sufficient delta to continue.
if (fabsf(cosDiff) < cosf(DEG2RAD(5.0f))) {
VelocityStateData gpsVelData;
VelocityStateGet(&gpsVelData);
if (gpsVelData.North * gpsVelData.North + gpsVelData.East * gpsVelData.East + gpsVelData.Down * gpsVelData.Down < 1.0f) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
return; // do not calculate if gps velocity is insufficient...
}
// Calculate the norm^2 of the difference between the two GPS vectors
float normDiffGPS2 = powf(gpsVelData.North - gps->gpsVelOld_N, 2.0f) + powf(gpsVelData.East - gps->gpsVelOld_E, 2.0f) + powf(gpsVelData.Down - gps->gpsVelOld_D, 2.0f);
// Calculate the norm^2 of the difference between the two fuselage vectors
float normDiffAttitude2 = powf(Rbe[0][0] - gps->RbeCol1_old[0], 2.0f) + powf(Rbe[0][1] - gps->RbeCol1_old[1], 2.0f) + powf(Rbe[0][2] - gps->RbeCol1_old[2], 2.0f);
// Airspeed magnitude is the ratio between the two difference norms
float airspeed = sqrtf(normDiffGPS2 / normDiffAttitude2);
if (!IS_REAL(airspeedData->CalibratedAirspeed)) {
airspeedData->CalibratedAirspeed = 0;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
} else {
// need a low pass filter to filter out spikes in non coordinated maneuvers
airspeedData->CalibratedAirspeed = (1.0f - airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha) * gps->oldAirspeed + airspeedSettings->GroundSpeedBasedEstimationLowPassAlpha * airspeed;
gps->oldAirspeed = airspeedData->CalibratedAirspeed;
airspeedData->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
}
// Save old variables for next pass
gps->gpsVelOld_N = gpsVelData.North;
gps->gpsVelOld_E = gpsVelData.East;
gps->gpsVelOld_D = gpsVelData.Down;
gps->RbeCol1_old[0] = Rbe[0][0];
gps->RbeCol1_old[1] = Rbe[0][1];
gps->RbeCol1_old[2] = Rbe[0][2];
}
}
/**
* Initialise the module, called on startup
* \returns 0 on success or -1 if initialisation failed
*/
int32_t AttitudeInitialize(void)
{
AttitudeStateInitialize();
AttitudeSettingsInitialize();
AccelGyroSettingsInitialize();
AccelStateInitialize();
GyroStateInitialize();
// Initialize quaternion
AttitudeStateData attitude;
AttitudeStateGet(&attitude);
attitude.q1 = 1;
attitude.q2 = 0;
attitude.q3 = 0;
attitude.q4 = 0;
AttitudeStateSet(&attitude);
// Cannot trust the values to init right above if BL runs
gyro_correct_int[0] = 0;
gyro_correct_int[1] = 0;
gyro_correct_int[2] = 0;
q[0] = 1;
q[1] = 0;
q[2] = 0;
q[3] = 0;
for (uint8_t i = 0; i < 3; i++) {
for (uint8_t j = 0; j < 3; j++) {
R[i][j] = 0;
}
}
trim_requested = false;
AttitudeSettingsConnectCallback(&settingsUpdatedCb);
AccelGyroSettingsConnectCallback(&settingsUpdatedCb);
return 0;
}
int8_t VtolFlyController::UpdateStabilizationDesired(bool yaw_attitude, float yaw_direction)
{
uint8_t result = 1;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
StabilizationBankData stabSettings;
float northCommand;
float eastCommand;
StabilizationDesiredGet(&stabDesired);
AttitudeStateGet(&attitudeState);
StabilizationBankGet(&stabSettings);
controlNE.GetNECommand(&northCommand, &eastCommand);
float angle_radians = DEG2RAD(attitudeState.Yaw);
float cos_angle = cosf(angle_radians);
float sine_angle = sinf(angle_radians);
float maxPitch = vtolPathFollowerSettings->MaxRollPitch;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Pitch = boundf(-northCommand * cos_angle - eastCommand * sine_angle, -maxPitch, maxPitch); // this should be in the controller
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Roll = boundf(-northCommand * sine_angle + eastCommand * cos_angle, -maxPitch, maxPitch);
ManualControlCommandData manualControl;
ManualControlCommandGet(&manualControl);
// TODO The below need to be rewritten because the PID implementation has changed.
#if 0
// DEBUG HACK: allow user to skew compass on purpose to see if emergency failsafe kicks in
if (vtolPathFollowerSettings->FlyawayEmergencyFallback == VTOLPATHFOLLOWERSETTINGS_FLYAWAYEMERGENCYFALLBACK_DEBUGTEST) {
attitudeState.Yaw += 120.0f;
if (attitudeState.Yaw > 180.0f) {
attitudeState.Yaw -= 360.0f;
}
}
if ( // emergency flyaway detection
( // integral already at its limit
vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[0].iAccumulator) < 1e-6f ||
vtolPathFollowerSettings.HorizontalVelPID.ILimit - fabsf(global.PIDvel[1].iAccumulator) < 1e-6f
) &&
// angle between desired and actual velocity >90 degrees (by dot product)
(velocityDesired.North * velocityState.North + velocityDesired.East * velocityState.East < 0.0f) &&
// quad is moving at significant speed (during flyaway it would keep speeding up)
squaref(velocityState.North) + squaref(velocityState.East) > 1.0f
) {
vtolEmergencyFallback += dT;
if (vtolEmergencyFallback >= vtolPathFollowerSettings->FlyawayEmergencyFallbackTriggerTime) {
// after emergency timeout, trigger alarm - everything else is handled by callers
// (switch to emergency algorithm, switch to emergency waypoint in pathplanner, alarms, ...)
result = 0;
}
} else {
vtolEmergencyFallback = 0.0f;
}
#endif // if 0
if (yaw_attitude) {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.Yaw = yaw_direction;
} else {
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK;
stabDesired.Yaw = stabSettings.MaximumRate.Yaw * manualControl.Yaw;
}
// default thrust mode to cruise control
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL;
if (mManualThrust) {
stabDesired.Thrust = manualControl.Thrust;
} else {
stabDesired.Thrust = controlDown.GetDownCommand();
}
StabilizationDesiredSet(&stabDesired);
return result;
}
static filterResult filter(stateFilter *self, stateEstimation *state)
{
struct data *this = (struct data *)self->localdata;
if (reloadSettings) {
reloadSettings = false;
AltitudeFilterSettingsGet(&this->settings);
}
if (this->first_run) {
// Initialize to current altitude reading at initial location
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
this->first_run = 0;
this->initTimer = xTaskGetTickCount();
}
} else {
// save existing position and velocity updates so GPS will still work
if (IS_SET(state->updated, SENSORUPDATES_pos)) {
this->pos[0] = state->pos[0];
this->pos[1] = state->pos[1];
this->pos[2] = state->pos[2];
state->pos[2] = -this->altitudeState;
}
if (IS_SET(state->updated, SENSORUPDATES_vel)) {
this->vel[0] = state->vel[0];
this->vel[1] = state->vel[1];
this->vel[2] = state->vel[2];
state->vel[2] = -this->velocityState;
}
if (IS_SET(state->updated, SENSORUPDATES_accel)) {
// rotate accels into global coordinate frame
AttitudeStateData att;
AttitudeStateGet(&att);
float Rbe[3][3];
Quaternion2R(&att.q1, Rbe);
float current = -(Rbe[0][2] * state->accel[0] + Rbe[1][2] * state->accel[1] + Rbe[2][2] * state->accel[2] + this->gravity);
// low pass filter accelerometers
this->accelState = (1.0f - this->settings.AccelLowPassKp) * this->accelState + this->settings.AccelLowPassKp * current;
if (((xTaskGetTickCount() - this->initTimer) / portTICK_RATE_MS) < INITIALIZATION_DURATION_MS) {
// allow the offset to reach quickly the target value in case of small AccelDriftKi
this->accelBiasState = (1.0f - this->settings.InitializationAccelDriftKi) * this->accelBiasState + this->settings.InitializationAccelDriftKi * this->accelState;
} else {
// correct accel offset (low pass zeroing)
this->accelBiasState = (1.0f - this->settings.AccelDriftKi) * this->accelBiasState + this->settings.AccelDriftKi * this->accelState;
}
// correct velocity and position state (integration)
// low pass for average dT, compensate timing jitter from scheduler
//
float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt1config);
float speedLast = this->velocityState;
this->velocityState += 0.5f * (this->accelLast + (this->accelState - this->accelBiasState)) * dT;
this->accelLast = this->accelState - this->accelBiasState;
this->altitudeState += 0.5f * (speedLast + this->velocityState) * dT;
state->pos[0] = this->pos[0];
state->pos[1] = this->pos[1];
state->pos[2] = -this->altitudeState;
state->updated |= SENSORUPDATES_pos;
state->vel[0] = this->vel[0];
state->vel[1] = this->vel[1];
state->vel[2] = -this->velocityState;
state->updated |= SENSORUPDATES_vel;
}
if (IS_SET(state->updated, SENSORUPDATES_baro)) {
// correct the altitude state (simple low pass)
this->altitudeState = (1.0f - this->settings.BaroKp) * this->altitudeState + this->settings.BaroKp * state->baro[0];
// correct the velocity state (low pass differentiation)
// low pass for average dT, compensate timing jitter from scheduler
float dT = PIOS_DELTATIME_GetAverageSeconds(&this->dt2config);
this->velocityState = (1.0f - (this->settings.BaroKp * this->settings.BaroKp)) * this->velocityState + (this->settings.BaroKp * this->settings.BaroKp) * (state->baro[0] - this->baroLast) / dT;
this->baroLast = state->baro[0];
state->pos[0] = this->pos[0];
state->pos[1] = this->pos[1];
state->pos[2] = -this->altitudeState;
state->updated |= SENSORUPDATES_pos;
state->vel[0] = this->vel[0];
state->vel[1] = this->vel[1];
state->vel[2] = -this->velocityState;
state->updated |= SENSORUPDATES_vel;
}
}
return FILTERRESULT_OK;
}
/**
* Compute desired attitude from the desired velocity for fixed wing craft
*/
uint8_t FixedWingFlyController::updateFixedDesiredAttitude()
{
uint8_t result = 1;
const float dT = fixedWingSettings->UpdatePeriod / 1000.0f;
VelocityDesiredData velocityDesired;
VelocityStateData velocityState;
StabilizationDesiredData stabDesired;
AttitudeStateData attitudeState;
FixedWingPathFollowerStatusData fixedWingPathFollowerStatus;
AirspeedStateData airspeedState;
SystemSettingsData systemSettings;
float groundspeedProjection;
float indicatedAirspeedState;
float indicatedAirspeedDesired;
float airspeedError;
float pitchCommand;
float descentspeedDesired;
float descentspeedError;
float powerCommand;
float airspeedVector[2];
float fluidMovement[2];
float courseComponent[2];
float courseError;
float courseCommand;
FixedWingPathFollowerStatusGet(&fixedWingPathFollowerStatus);
VelocityStateGet(&velocityState);
StabilizationDesiredGet(&stabDesired);
VelocityDesiredGet(&velocityDesired);
AttitudeStateGet(&attitudeState);
AirspeedStateGet(&airspeedState);
SystemSettingsGet(&systemSettings);
/**
* Compute speed error and course
*/
// missing sensors for airspeed-direction we have to assume within
// reasonable error that measured airspeed is actually the airspeed
// component in forward pointing direction
// airspeedVector is normalized
airspeedVector[0] = cos_lookup_deg(attitudeState.Yaw);
airspeedVector[1] = sin_lookup_deg(attitudeState.Yaw);
// current ground speed projected in forward direction
groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1];
// note that airspeedStateBias is ( calibratedAirspeed - groundspeedProjection ) at the time of measurement,
// but thanks to accelerometers, groundspeedProjection reacts faster to changes in direction
// than airspeed and gps sensors alone
indicatedAirspeedState = groundspeedProjection + indicatedAirspeedStateBias;
// fluidMovement is a vector describing the aproximate movement vector of
// the surrounding fluid in 2d space (aka wind vector)
fluidMovement[0] = velocityState.North - (indicatedAirspeedState * airspeedVector[0]);
fluidMovement[1] = velocityState.East - (indicatedAirspeedState * airspeedVector[1]);
// calculate the movement vector we need to fly to reach velocityDesired -
// taking fluidMovement into account
courseComponent[0] = velocityDesired.North - fluidMovement[0];
courseComponent[1] = velocityDesired.East - fluidMovement[1];
indicatedAirspeedDesired = boundf(sqrtf(courseComponent[0] * courseComponent[0] + courseComponent[1] * courseComponent[1]),
fixedWingSettings->HorizontalVelMin,
fixedWingSettings->HorizontalVelMax);
// if we could fly at arbitrary speeds, we'd just have to move towards the
// courseComponent vector as previously calculated and we'd be fine
// unfortunately however we are bound by min and max air speed limits, so
// we need to recalculate the correct course to meet at least the
// velocityDesired vector direction at our current speed
// this overwrites courseComponent
bool valid = correctCourse(courseComponent, (float *)&velocityDesired.North, fluidMovement, indicatedAirspeedDesired);
// Error condition: wind speed too high, we can't go where we want anymore
fixedWingPathFollowerStatus.Errors.Wind = 0;
if ((!valid) &&
fixedWingSettings->Safetymargins.Wind > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Wind = 1;
result = 0;
}
// Airspeed error
airspeedError = indicatedAirspeedDesired - indicatedAirspeedState;
// Vertical speed error
descentspeedDesired = boundf(
velocityDesired.Down,
-fixedWingSettings->VerticalVelMax,
fixedWingSettings->VerticalVelMax);
descentspeedError = descentspeedDesired - velocityState.Down;
// Error condition: plane too slow or too fast
fixedWingPathFollowerStatus.Errors.Highspeed = 0;
fixedWingPathFollowerStatus.Errors.Lowspeed = 0;
if (indicatedAirspeedState > systemSettings.AirSpeedMax * fixedWingSettings->Safetymargins.Overspeed) {
fixedWingPathFollowerStatus.Errors.Overspeed = 1;
result = 0;
}
if (indicatedAirspeedState > fixedWingSettings->HorizontalVelMax * fixedWingSettings->Safetymargins.Highspeed) {
fixedWingPathFollowerStatus.Errors.Highspeed = 1;
result = 0;
}
if (indicatedAirspeedState < fixedWingSettings->HorizontalVelMin * fixedWingSettings->Safetymargins.Lowspeed) {
fixedWingPathFollowerStatus.Errors.Lowspeed = 1;
result = 0;
}
if (indicatedAirspeedState < systemSettings.AirSpeedMin * fixedWingSettings->Safetymargins.Stallspeed) {
fixedWingPathFollowerStatus.Errors.Stallspeed = 1;
result = 0;
}
/**
* Compute desired thrust command
*/
// Compute the cross feed from vertical speed to pitch, with saturation
float speedErrorToPowerCommandComponent = boundf(
(airspeedError / fixedWingSettings->HorizontalVelMin) * fixedWingSettings->AirspeedToPowerCrossFeed.Kp,
-fixedWingSettings->AirspeedToPowerCrossFeed.Max,
fixedWingSettings->AirspeedToPowerCrossFeed.Max
);
// Compute final thrust response
powerCommand = pid_apply(&PIDpower, -descentspeedError, dT) +
speedErrorToPowerCommandComponent;
// Output internal state to telemetry
fixedWingPathFollowerStatus.Error.Power = descentspeedError;
fixedWingPathFollowerStatus.ErrorInt.Power = PIDpower.iAccumulator;
fixedWingPathFollowerStatus.Command.Power = powerCommand;
// set thrust
stabDesired.Thrust = boundf(fixedWingSettings->ThrustLimit.Neutral + powerCommand,
fixedWingSettings->ThrustLimit.Min,
fixedWingSettings->ThrustLimit.Max);
// Error condition: plane cannot hold altitude at current speed.
fixedWingPathFollowerStatus.Errors.Lowpower = 0;
if (fixedWingSettings->ThrustLimit.Neutral + powerCommand >= fixedWingSettings->ThrustLimit.Max && // thrust at maximum
velocityState.Down > 0.0f && // we ARE going down
descentspeedDesired < 0.0f && // we WANT to go up
airspeedError > 0.0f && // we are too slow already
fixedWingSettings->Safetymargins.Lowpower > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Lowpower = 1;
result = 0;
}
// Error condition: plane keeps climbing despite minimum thrust (opposite of above)
fixedWingPathFollowerStatus.Errors.Highpower = 0;
if (fixedWingSettings->ThrustLimit.Neutral + powerCommand <= fixedWingSettings->ThrustLimit.Min && // thrust at minimum
velocityState.Down < 0.0f && // we ARE going up
descentspeedDesired > 0.0f && // we WANT to go down
airspeedError < 0.0f && // we are too fast already
fixedWingSettings->Safetymargins.Highpower > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Highpower = 1;
result = 0;
}
/**
* Compute desired pitch command
*/
// Compute the cross feed from vertical speed to pitch, with saturation
float verticalSpeedToPitchCommandComponent = boundf(-descentspeedError * fixedWingSettings->VerticalToPitchCrossFeed.Kp,
-fixedWingSettings->VerticalToPitchCrossFeed.Max,
fixedWingSettings->VerticalToPitchCrossFeed.Max
);
// Compute the pitch command as err*Kp + errInt*Ki + X_feed.
pitchCommand = -pid_apply(&PIDspeed, airspeedError, dT) + verticalSpeedToPitchCommandComponent;
fixedWingPathFollowerStatus.Error.Speed = airspeedError;
fixedWingPathFollowerStatus.ErrorInt.Speed = PIDspeed.iAccumulator;
fixedWingPathFollowerStatus.Command.Speed = pitchCommand;
stabDesired.Pitch = boundf(fixedWingSettings->PitchLimit.Neutral + pitchCommand,
fixedWingSettings->PitchLimit.Min,
fixedWingSettings->PitchLimit.Max);
// Error condition: high speed dive
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 0;
if (fixedWingSettings->PitchLimit.Neutral + pitchCommand >= fixedWingSettings->PitchLimit.Max && // pitch demand is full up
velocityState.Down > 0.0f && // we ARE going down
descentspeedDesired < 0.0f && // we WANT to go up
airspeedError < 0.0f && // we are too fast already
fixedWingSettings->Safetymargins.Pitchcontrol > 0.5f) { // alarm switched on
fixedWingPathFollowerStatus.Errors.Pitchcontrol = 1;
result = 0;
}
/**
* Compute desired roll command
*/
courseError = RAD2DEG(atan2f(courseComponent[1], courseComponent[0])) - attitudeState.Yaw;
if (courseError < -180.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f) {
courseError -= 360.0f;
}
// overlap calculation. Theres a dead zone behind the craft where the
// counter-yawing of some craft while rolling could render a desired right
// turn into a desired left turn. Making the turn direction based on
// current roll angle keeps the plane committed to a direction once chosen
if (courseError < -180.0f + (fixedWingSettings->ReverseCourseOverlap * 0.5f)
&& attitudeState.Roll > 0.0f) {
courseError += 360.0f;
}
if (courseError > 180.0f - (fixedWingSettings->ReverseCourseOverlap * 0.5f)
&& attitudeState.Roll < 0.0f) {
courseError -= 360.0f;
}
courseCommand = pid_apply(&PIDcourse, courseError, dT);
fixedWingPathFollowerStatus.Error.Course = courseError;
fixedWingPathFollowerStatus.ErrorInt.Course = PIDcourse.iAccumulator;
fixedWingPathFollowerStatus.Command.Course = courseCommand;
stabDesired.Roll = boundf(fixedWingSettings->RollLimit.Neutral +
courseCommand,
fixedWingSettings->RollLimit.Min,
fixedWingSettings->RollLimit.Max);
// TODO: find a check to determine loss of directional control. Likely needs some check of derivative
/**
* Compute desired yaw command
*/
// TODO implement raw control mode for yaw and base on Accels.Y
stabDesired.Yaw = 0.0f;
stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE;
stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_MANUAL;
StabilizationDesiredSet(&stabDesired);
FixedWingPathFollowerStatusSet(&fixedWingPathFollowerStatus);
return result;
}
