static void getVector(float controlVector[4], vario_type type) { FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); // scale controlVector[3] (thrust) by vertical/horizontal to have vertical plane less sensitive controlVector[3] *= offset.Vertical / offset.Horizontal; float length = sqrtf(controlVector[0] * controlVector[0] + controlVector[1] * controlVector[1] + controlVector[3] * controlVector[3]); if (length <= 1e-9f) { length = 1.0f; // should never happen as getVector is not called if control within deadband } { float direction[3] = { controlVector[1] / length, // pitch is north controlVector[0] / length, // roll is east controlVector[3] / length // thrust is down }; controlVector[0] = direction[0]; controlVector[1] = direction[1]; controlVector[2] = direction[2]; } controlVector[3] = length * offset.Horizontal; // rotate north and east - rotation angle based on type float angle; switch (type) { case NSEW: angle = 0.0f; // NSEW no rotation takes place break; case FPV: // local rotation, using current yaw AttitudeStateYawGet(&angle); break; case LOS: // determine location based on vector from takeoff to current location { PositionStateData positionState; PositionStateGet(&positionState); TakeOffLocationData takeoffLocation; TakeOffLocationGet(&takeoffLocation); angle = RAD2DEG(atan2f(positionState.East - takeoffLocation.East, positionState.North - takeoffLocation.North)); } break; } // rotate horizontally by angle { float rotated[2] = { controlVector[0] * cos_lookup_deg(angle) - controlVector[1] * sin_lookup_deg(angle), controlVector[0] * sin_lookup_deg(angle) + controlVector[1] * cos_lookup_deg(angle) }; controlVector[0] = rotated[0]; controlVector[1] = rotated[1]; } }
void plan_setup_AutoCruise() { PositionStateData positionState; PositionStateGet(&positionState); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); FlightModeSettingsPositionHoldOffsetData offset; FlightModeSettingsPositionHoldOffsetGet(&offset); float startingVelocity; FlightModeSettingsPositionHoldStartingVelocityGet(&startingVelocity); // initialization is flight in direction of the nose. // the velocity is not relevant, as it will be reset by the run function even during first call float angle; AttitudeStateYawGet(&angle); float vector[2] = { cos_lookup_deg(angle), sin_lookup_deg(angle) }; hold_position[0] = positionState.North; hold_position[1] = positionState.East; hold_position[2] = positionState.Down; pathDesired.End.North = hold_position[0] + vector[0]; pathDesired.End.East = hold_position[1] + vector[1]; pathDesired.End.Down = hold_position[2]; // start position has the same offset as in position hold pathDesired.Start.North = pathDesired.End.North + offset.Horizontal; // in FlyEndPoint the direction of this vector does not matter pathDesired.Start.East = pathDesired.End.East; pathDesired.Start.Down = pathDesired.End.Down; pathDesired.StartingVelocity = startingVelocity; pathDesired.EndingVelocity = 0.0f; pathDesired.Mode = PATHDESIRED_MODE_FLYENDPOINT; PathDesiredSet(&pathDesired); // re-iniztializing deltatime is valid and also good practice here since // getAverageSeconds() has not been called/updated in a long time if we were in a different flightmode. PIOS_DELTATIME_Init(&actimeval, UPDATE_EXPECTED, UPDATE_MIN, UPDATE_MAX, UPDATE_ALPHA); }
/** * Use the lookup table to return sine(angle) where angle is in radians * @param[in] angle Angle in radians * @returns sin(angle) */ float sin_lookup_rad(float angle) { int degrees = angle * RAD2DEG; return sin_lookup_deg(degrees); }
/** * Get cos(angle) using the sine lookup table * @param[in] angle Angle in degrees * @returns cos(angle) */ float cos_lookup_deg(float angle) { return sin_lookup_deg(angle + 90); }
/** * Apply a step function for the stabilization controller and monitor the * result * * Used to Replace the rate PID with a relay to measure the critical properties of this axis * i.e. period and gain */ int stabilization_relay_rate(float error, float *output, int axis, bool reinit) { RelayTuningData relay; RelayTuningGet(&relay); static portTickType lastHighTime; static portTickType lastLowTime; static float accum_sin, accum_cos; static uint32_t accumulated = 0; const uint16_t DEGLITCH_TIME = 20; // ms const float AMPLITUDE_ALPHA = 0.95f; const float PERIOD_ALPHA = 0.95f; portTickType thisTime = xTaskGetTickCount(); static bool rateRelayRunning[MAX_AXES]; // This indicates the current estimate of the smoothed error. So when it is high // we are waiting for it to go low. static bool high = false; // On first run initialize estimates to something reasonable if (reinit) { rateRelayRunning[axis] = false; cast_struct_to_array(relay.Period, relay.Period.Roll)[axis] = 200; cast_struct_to_array(relay.Gain, relay.Gain.Roll)[axis] = 0; accum_sin = 0; accum_cos = 0; accumulated = 0; // These should get reinitialized anyway high = true; lastHighTime = thisTime; lastLowTime = thisTime; RelayTuningSet(&relay); } RelayTuningSettingsData relaySettings; RelayTuningSettingsGet(&relaySettings); // Compute output, simple threshold on error *output = high ? relaySettings.Amplitude : -relaySettings.Amplitude; /**** The code below here is to estimate the properties of the oscillation ****/ // Make sure the period can't go below limit if (cast_struct_to_array(relay.Period, relay.Period.Roll)[axis] < DEGLITCH_TIME) { cast_struct_to_array(relay.Period, relay.Period.Roll)[axis] = DEGLITCH_TIME; } // Project the error onto a sine and cosine of the same frequency // to accumulate the average amplitude int32_t dT = thisTime - lastHighTime; float phase = ((float)360 * (float)dT) / cast_struct_to_array(relay.Period, relay.Period.Roll)[axis]; if (phase >= 360) { phase = 0; } accum_sin += sin_lookup_deg(phase) * error; accum_cos += cos_lookup_deg(phase) * error; accumulated++; // Make sure we've had enough time since last transition then check for a change in the output bool time_hysteresis = (high ? (thisTime - lastHighTime) : (thisTime - lastLowTime)) > DEGLITCH_TIME; if (!high && time_hysteresis && error > relaySettings.HysteresisThresh) { /* POSITIVE CROSSING DETECTED */ float this_amplitude = 2 * sqrtf(accum_sin * accum_sin + accum_cos * accum_cos) / accumulated; float this_gain = this_amplitude / relaySettings.Amplitude; accumulated = 0; accum_sin = 0; accum_cos = 0; if (rateRelayRunning[axis] == false) { rateRelayRunning[axis] = true; cast_struct_to_array(relay.Period, relay.Period.Roll)[axis] = 200; cast_struct_to_array(relay.Gain, relay.Gain.Roll)[axis] = 0; } else { // Low pass filter each amplitude and period cast_struct_to_array(relay.Gain, relay.Gain.Roll)[axis] = cast_struct_to_array(relay.Gain, relay.Gain.Roll)[axis] * AMPLITUDE_ALPHA + this_gain * (1 - AMPLITUDE_ALPHA); cast_struct_to_array(relay.Period, relay.Period.Roll)[axis] = cast_struct_to_array(relay.Period, relay.Period.Roll)[axis] * PERIOD_ALPHA + dT * (1 - PERIOD_ALPHA); } lastHighTime = thisTime; high = true; RelayTuningSet(&relay); } else if (high && time_hysteresis && error < -relaySettings.HysteresisThresh) { /* FALLING CROSSING DETECTED */ lastLowTime = thisTime; high = false; } return 0; }
/** * 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; }
// with both solutions for C // this might be reached in headwind stronger than maximum allowed // airspeed. return false; } } void FixedWingFlyController::AirspeedStateUpdatedCb(__attribute__((unused)) UAVObjEvent *ev) { AirspeedStateData airspeedState; VelocityStateData velocityState; AirspeedStateGet(&airspeedState); VelocityStateGet(&velocityState); float airspeedVector[2]; float yaw; AttitudeStateYawGet(&yaw); airspeedVector[0] = cos_lookup_deg(yaw); airspeedVector[1] = sin_lookup_deg(yaw); // vector projection of groundspeed on airspeed vector to handle both forward and backwards movement float groundspeedProjection = velocityState.North * airspeedVector[0] + velocityState.East * airspeedVector[1]; indicatedAirspeedStateBias = airspeedState.CalibratedAirspeed - groundspeedProjection; // 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. // This has a side effect that in the absence of any airspeed updates, the // pathfollower will fly using groundspeed. }