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