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]; } }
static float CruiseControlAngleToFactor(float angle) { float factor; // avoid singularity if (angle > 89.999f && angle < 90.001f) { factor = stabSettings.settings.CruiseControlMaxPowerFactor; } else { // the simple bank angle boost calculation that Cruise Control revolves around factor = 1.0f / fabsf(cos_lookup_deg(angle)); // factor in the power trim, no effect at 1.0, linear effect increases with factor factor = (factor - 1.0f) * stabSettings.cruiseControl.power_trim + 1.0f; // limit to user specified max power multiplier if (factor > stabSettings.settings.CruiseControlMaxPowerFactor) { factor = stabSettings.settings.CruiseControlMaxPowerFactor; } } return factor; }
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 cos(angle) */ float cos_lookup_rad(float angle) { int degrees = angle * RAD2DEG; return cos_lookup_deg(degrees); }
/** * 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. }
void cruisecontrol_compute_factor(AttitudeStateData *attitude, float thrustDemand) { static float previous_angle; static uint32_t previous_time = 0; static bool previous_time_valid = false; // For multiple, speedy flips this mainly strives to address the // fact that (due to thrust delay) thrust didn't average straight // down, but at an angle. For less speedy flips it acts like it // used to. It can be turned off by setting power delay to 0. // It takes significant time for the motors of a multi-copter to // spin up. It takes significant time for the collective servo of // a CP heli to move from one end to the other. Both of those are // modeled here as linear, i.e. twice as much change takes twice // as long. Given a correctly configured maximum delay time this // code calculates how far in advance to start the control // transition so that half way through the physical transition it // is just crossing the transition angle. // Example: Rotation rate = 360. Full stroke delay = 0.2 // Transition angle 90 degrees. Start the transition 0.1 second // before 90 degrees (36 degrees at 360 deg/sec) and it will be // complete 0.1 seconds after 90 degrees. // Note that this code only handles the transition to/from inverted // thrust. It doesn't handle the case where thrust is changed a // lot in a small angle range when that range is close to 90 degrees. // It doesn't handle the small constant "system delay" caused by the // delay between reading sensors and actuators beginning to respond. // It also assumes that the pilot is holding the throttle constant; // when the pilot does change the throttle, the compensation is // simply recalculated. // This implementation of future thrust isn't perfect. That would // probably require an iterative procedure for solving a // transcendental equation of the form linear(x) = 1/cos(x). It's // shortcomings generally don't hurt anything and work better than // without it. It is designed to work perfectly if the pilot is // using full thrust during flips and it is only activated if 70% or // greater thrust is used. uint32_t time = PIOS_DELAY_GetuS(); // Get roll and pitch angles, calculate combined angle, and begin // the general algorithm. // Example: 45 degrees roll plus 45 degrees pitch = 60 degrees // Do it every 8th iteration to save CPU. if (time != previous_time || previous_time_valid == false) { float angle, angle_unmodified; // spherical right triangle // 0.0 <= angle <= 180.0 angle_unmodified = angle = RAD2DEG(acosf(cos_lookup_deg(attitude->Roll) * cos_lookup_deg(attitude->Pitch))); // Calculate rate as a combined (roll and pitch) bank angle // change; in degrees per second. Rate is calculated over the // most recent 8 loops through stabilization. We could have // asked the gyros. This is probably cheaper. if (previous_time_valid) { float rate; // rate can be negative. rate = (angle - previous_angle) / ((float)(time - previous_time) / 1000000.0f); // Define "within range" to be those transitions that should // be executing now. Recall that each impulse transition is // spread out over a range of time / angle. // There is only one transition and the high power level for // it is either: // 1/fabsf(cos(angle)) * current thrust // or max power factor * current thrust // or full thrust // You can cross the transition with angle either increasing // or decreasing (rate positive or negative). // Thrust is never boosted for negative values of // thrustDemand (negative stick values) // // When the aircraft is upright, thrust is always boosted // . for positive values of thrustDemand // When the aircraft is inverted, thrust is sometimes // . boosted or reversed (or combinations thereof) or zeroed // . for positive values of thrustDemand // It depends on the inverted power settings. // Of course, you can set MaxPowerFactor to 1.0 to // . effectively disable boost. if (thrustDemand > 0.0f) { // to enable the future thrust calculations, make sure // there is a large enough transition that the result // will be roughly on vs. off; without that, it can // exaggerate the length of time the inverted to upright // transition holds full throttle and reduce the length // of time for full throttle when going upright to inverted. if (thrustDemand > 0.7f) { float thrust; thrust = CruiseControlFactorToThrust(CruiseControlAngleToFactor((float)stabSettings.settings.CruiseControlMaxAngle), thrustDemand); // determine if we are in range of the transition // given the thrust at max_angle and thrustDemand // (typically close to 1.0), change variable 'thrust' to // be the proportion of the largest thrust change possible // that occurs when going into inverted mode. // Example: 'thrust' is 0.8 A quad has min_thrust set // to 0.05 The difference is 0.75. The largest possible // difference with this setup is 0.9 - 0.05 = 0.85, so // the proportion is 0.75/0.85 // That is nearly a full throttle stroke. // the 'thrust' variable is non-negative here switch (stabSettings.settings.CruiseControlInvertedPowerOutput) { case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_ZERO: // normal multi-copter case, stroke is max to zero // technically max to constant min_thrust // can be used by CP thrust = (thrust - CruiseControlLimitThrust(0.0f)) / stabSettings.cruiseControl.thrust_difference; break; case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_NORMAL: // reversed but not boosted // : CP heli case, stroke is max to -stick // : thrust = (thrust - CruiseControlLimitThrust(-thrustDemand)) / stabSettings.cruiseControl.thrust_difference; // else it is both unreversed and unboosted // : simply turn off boost, stroke is max to +stick // : thrust = (thrust - CruiseControlLimitThrust(thrustDemand)) / stabSettings.cruiseControl.thrust_difference; thrust = (thrust - CruiseControlLimitThrust( (stabSettings.settings.CruiseControlInvertedThrustReversing == STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED) ? -thrustDemand : thrustDemand)) / stabSettings.cruiseControl.thrust_difference; break; case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_BOOSTED: // if boosted and reversed if (stabSettings.settings.CruiseControlInvertedThrustReversing == STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED) { // CP heli case, stroke is max to min thrust = (thrust - CruiseControlFactorToThrust(-CruiseControlAngleToFactor((float)stabSettings.settings.CruiseControlMaxAngle), thrustDemand)) / stabSettings.cruiseControl.thrust_difference; } // else it is boosted and unreversed so the throttle doesn't change else { // CP heli case, no transition, so stroke is zero thrust = 0.0f; } break; } // 'thrust' is now the proportion of max stroke // multiply this proportion of max stroke, // times the max stroke time, to get this stroke time // we only want half of this time before the transition // (and half after the transition) thrust *= stabSettings.cruiseControl.half_power_delay; // 'thrust' is now the length of time for this stroke // multiply that times angular rate to get the lead angle thrust *= fabsf(rate); // if the transition is within range we use it, // else we just use the current calculated thrust if ((float)stabSettings.settings.CruiseControlMaxAngle - thrust <= angle && angle <= (float)stabSettings.settings.CruiseControlMaxAngle + thrust) { // default to a little above max angle angle = (float)stabSettings.settings.CruiseControlMaxAngle + 0.01f; // if roll direction is downward // then thrust value is taken from below max angle // by the code that knows about the transition angle if (rate < 0.0f) { angle -= 0.02f; } } } // if thrust > 0.7; else just use the angle we already calculated cruisecontrol_factor = CruiseControlAngleToFactor(angle); } else { // if thrust > 0 set factor from angle; else cruisecontrol_factor = 1.0f; } if (angle >= (float)stabSettings.settings.CruiseControlMaxAngle) { switch (stabSettings.settings.CruiseControlInvertedPowerOutput) { case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_ZERO: cruisecontrol_factor = 0.0f; break; case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_NORMAL: cruisecontrol_factor = 1.0f; break; case STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDPOWEROUTPUT_BOOSTED: // no change, leave factor >= 1.0 alone break; } if (stabSettings.settings.CruiseControlInvertedThrustReversing == STABILIZATIONSETTINGS_CRUISECONTROLINVERTEDTHRUSTREVERSING_REVERSED) { cruisecontrol_factor = -cruisecontrol_factor; } } } // if previous_time_valid i.e. we've got a rate; else leave (angle and) factor alone previous_time = time; previous_time_valid = true; previous_angle = angle_unmodified; } // every 8th time }