/** * Compute desired velocity from the current position * * Takes in @ref PositionActual and compares it to @ref PositionDesired * and computes @ref VelocityDesired */ void updateVtolDesiredVelocity() { GuidanceSettingsData guidanceSettings; PositionActualData positionActual; PositionDesiredData positionDesired; VelocityDesiredData velocityDesired; GuidanceSettingsGet(&guidanceSettings); PositionActualGet(&positionActual); PositionDesiredGet(&positionDesired); VelocityDesiredGet(&velocityDesired); // Note all distances in cm float dNorth = positionDesired.North - positionActual.North; float dEast = positionDesired.East - positionActual.East; float distance = sqrt(pow(dNorth, 2) + pow(dEast, 2)); float heading = atan2f(dEast, dNorth); float groundspeed = bound(distance * guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_KP], 0, guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_MAX]); velocityDesired.North = groundspeed * cosf(heading); velocityDesired.East = groundspeed * sinf(heading); float dDown = positionDesired.Down - positionActual.Down; velocityDesired.Down = bound(dDown * guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_KP], -guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_MAX], guidanceSettings.VerticalP[GUIDANCESETTINGS_VERTICALP_MAX]); VelocityDesiredSet(&velocityDesired); }
/** * Compute desired velocity from the current position * * Takes in @ref PositionActual and compares it to @ref PositionDesired * and computes @ref VelocityDesired */ void updateVtolDesiredVelocity() { static portTickType lastSysTime; portTickType thisSysTime = xTaskGetTickCount();; float dT = 0; GuidanceSettingsData guidanceSettings; PositionActualData positionActual; PositionDesiredData positionDesired; VelocityDesiredData velocityDesired; GuidanceSettingsGet(&guidanceSettings); PositionActualGet(&positionActual); PositionDesiredGet(&positionDesired); VelocityDesiredGet(&velocityDesired); float northError; float eastError; float downError; float northCommand; float eastCommand; 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; // Note all distances in cm // Compute desired north command northError = positionDesired.North - positionActual.North; northPosIntegral = bound(northPosIntegral + northError * dT * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KI], -guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT], guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT]); northCommand = (northError * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP] + northPosIntegral); eastError = positionDesired.East - positionActual.East; eastPosIntegral = bound(eastPosIntegral + eastError * dT * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KI], -guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT], guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_ILIMIT]); eastCommand = (eastError * guidanceSettings.HorizontalPosPI[GUIDANCESETTINGS_HORIZONTALPOSPI_KP] + eastPosIntegral); velocityDesired.North = bound(northCommand,-guidanceSettings.HorizontalVelMax,guidanceSettings.HorizontalVelMax); velocityDesired.East = bound(eastCommand,-guidanceSettings.HorizontalVelMax,guidanceSettings.HorizontalVelMax); downError = positionDesired.Down - positionActual.Down; downPosIntegral = bound(downPosIntegral + downError * dT * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KI], -guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT], guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_ILIMIT]); downCommand = (downError * guidanceSettings.VerticalPosPI[GUIDANCESETTINGS_VERTICALPOSPI_KP] + downPosIntegral); velocityDesired.Down = bound(downCommand, -guidanceSettings.VerticalVelMax, guidanceSettings.VerticalVelMax); VelocityDesiredSet(&velocityDesired); }
/** * Compute desired velocity from the current position * * Takes in @ref PositionActual and compares it to @ref PositionDesired * and computes @ref VelocityDesired */ void updateEndpointVelocity() { float dT = guidanceSettings.UpdatePeriod / 1000.0f; PositionActualData positionActual; VelocityDesiredData velocityDesired; PositionActualGet(&positionActual); VelocityDesiredGet(&velocityDesired); float northError; float eastError; float downError; float northCommand; float eastCommand; float northPos = positionActual.North; float eastPos = positionActual.East; float downPos = positionActual.Down; // Compute desired north command velocity from position error northError = pathDesired.End[PATHDESIRED_END_NORTH] - northPos; northCommand = pid_apply_antiwindup(&vtol_pids[NORTH_POSITION], northError, -guidanceSettings.HorizontalVelMax, guidanceSettings.HorizontalVelMax, dT); // Compute desired east command velocity from position error eastError = pathDesired.End[PATHDESIRED_END_EAST] - eastPos; eastCommand = pid_apply_antiwindup(&vtol_pids[EAST_POSITION], eastError, -guidanceSettings.HorizontalVelMax, guidanceSettings.HorizontalVelMax, dT); // Limit the maximum velocity any direction (not north and east separately) float total_vel = sqrtf(powf(northCommand,2) + powf(eastCommand,2)); float scale = 1; if(total_vel > guidanceSettings.HorizontalVelMax) scale = guidanceSettings.HorizontalVelMax / total_vel; velocityDesired.North = northCommand * scale; velocityDesired.East = eastCommand * scale; // Compute the desired velocity from the position difference downError = pathDesired.End[PATHDESIRED_END_DOWN] - downPos; velocityDesired.Down = pid_apply_antiwindup(&vtol_pids[DOWN_POSITION], downError, -guidanceSettings.VerticalVelMax, guidanceSettings.VerticalVelMax, dT); VelocityDesiredSet(&velocityDesired); // Indicate whether we are in radius of this endpoint uint8_t path_status = PATHSTATUS_STATUS_INPROGRESS; float distance2 = powf(northError, 2) + powf(eastError, 2); if (distance2 < (guidanceSettings.EndpointRadius * guidanceSettings.EndpointRadius)) { path_status = PATHSTATUS_STATUS_COMPLETED; } PathStatusStatusSet(&path_status); }
/** * Compute desired attitude for vtols - emergency fallback */ void VtolFlyController::UpdateDesiredAttitudeEmergencyFallback() { VelocityDesiredData velocityDesired; VelocityStateData velocityState; StabilizationDesiredData stabDesired; float courseError; float courseCommand; VelocityStateGet(&velocityState); VelocityDesiredGet(&velocityDesired); ManualControlCommandData manualControlData; ManualControlCommandGet(&manualControlData); courseError = RAD2DEG(atan2f(velocityDesired.East, velocityDesired.North) - atan2f(velocityState.East, velocityState.North)); if (courseError < -180.0f) { courseError += 360.0f; } if (courseError > 180.0f) { courseError -= 360.0f; } courseCommand = (courseError * vtolPathFollowerSettings->EmergencyFallbackYawRate.kP); stabDesired.Yaw = boundf(courseCommand, -vtolPathFollowerSettings->EmergencyFallbackYawRate.Max, vtolPathFollowerSettings->EmergencyFallbackYawRate.Max); controlDown.UpdateVelocitySetpoint(velocityDesired.Down); controlDown.UpdateVelocityState(velocityState.Down); stabDesired.Thrust = controlDown.GetDownCommand(); stabDesired.Roll = vtolPathFollowerSettings->EmergencyFallbackAttitude.Roll; stabDesired.Pitch = vtolPathFollowerSettings->EmergencyFallbackAttitude.Pitch; if (vtolPathFollowerSettings->ThrustControl == VTOLPATHFOLLOWERSETTINGS_THRUSTCONTROL_MANUAL) { stabDesired.Thrust = manualControlData.Thrust; } stabDesired.StabilizationMode.Roll = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.Pitch = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode.Yaw = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; stabDesired.StabilizationMode.Thrust = STABILIZATIONDESIRED_STABILIZATIONMODE_CRUISECONTROL; StabilizationDesiredSet(&stabDesired); }
/** * Set the desired velocity from the input sticks */ static void manualSetDesiredVelocity() { ManualControlCommandData cmd; VelocityDesiredData velocityDesired; ManualControlCommandGet(&cmd); VelocityDesiredGet(&velocityDesired); GuidanceSettingsData guidanceSettings; GuidanceSettingsGet(&guidanceSettings); velocityDesired.North = -guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_MAX] * cmd.Pitch; velocityDesired.East = guidanceSettings.HorizontalP[GUIDANCESETTINGS_HORIZONTALP_MAX] * cmd.Roll; velocityDesired.Down = 0; VelocityDesiredSet(&velocityDesired); }
/** * Set the desired velocity from the input sticks */ static void manualSetDesiredVelocity() { ManualControlCommandData cmd; VelocityDesiredData velocityDesired; ManualControlCommandGet(&cmd); VelocityDesiredGet(&velocityDesired); GuidanceSettingsData guidanceSettings; GuidanceSettingsGet(&guidanceSettings); velocityDesired.North = -guidanceSettings.HorizontalVelMax * cmd.Pitch; velocityDesired.East = guidanceSettings.HorizontalVelMax * cmd.Roll; velocityDesired.Down = 0; 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 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 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 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); }
/** * 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 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; }
/** * Compute desired attitude from the desired velocity * @param[in] dT the time since last evaluation * * 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 */ int32_t vtol_follower_control_attitude(float dT) { vtol_follower_control_accel(dT); AccelDesiredData accelDesired; AccelDesiredGet(&accelDesired); StabilizationDesiredData stabDesired; float northCommand = accelDesired.North; float eastCommand = accelDesired.East; // Project the north and east acceleration signals into body frame float yaw; AttitudeActualYawGet(&yaw); float forward_accel_desired = -northCommand * cosf(yaw * DEG2RAD) + -eastCommand * sinf(yaw * DEG2RAD); float right_accel_desired = -northCommand * sinf(yaw * DEG2RAD) + eastCommand * cosf(yaw * DEG2RAD); // Set the angle that would achieve the desired acceleration given the thrust is enough for a hover stabDesired.Pitch = bound_min_max(RAD2DEG * atanf(forward_accel_desired / GRAVITY), -guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch); stabDesired.Roll = bound_min_max(RAD2DEG * atanf(right_accel_desired / GRAVITY), -guidanceSettings.MaxRollPitch, guidanceSettings.MaxRollPitch); stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; // Calculate the throttle setting or use pass through from transmitter if (guidanceSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) { ManualControlCommandThrottleGet(&stabDesired.Throttle); } else { float downCommand = accelDesired.Down; AltitudeHoldStateData altitudeHoldState; altitudeHoldState.VelocityDesired = downCommand; altitudeHoldState.Integral = vtol_pids[DOWN_VELOCITY].iAccumulator / 1000.0f; altitudeHoldState.AngleGain = 1.0f; if (altitudeHoldSettings.AttitudeComp > 0) { // Throttle desired is at this point the mount desired in the up direction, we can // account for the attitude if desired AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); // Project a unit vector pointing up into the body frame and // get the z component float fraction = attitudeActual.q1 * attitudeActual.q1 - attitudeActual.q2 * attitudeActual.q2 - attitudeActual.q3 * attitudeActual.q3 + attitudeActual.q4 * attitudeActual.q4; // Add ability to scale up the amount of compensation to achieve // level forward flight fraction = powf(fraction, (float) altitudeHoldSettings.AttitudeComp / 100.0f); // Dividing by the fraction remaining in the vertical projection will // attempt to compensate for tilt. This acts like the thrust is linear // with the output which isn't really true. If the fraction is starting // to go negative we are inverted and should shut off throttle downCommand = (fraction > 0.1f) ? (downCommand / fraction) : 0.0f; altitudeHoldState.AngleGain = 1.0f / fraction; } altitudeHoldState.Throttle = downCommand; AltitudeHoldStateSet(&altitudeHoldState); stabDesired.Throttle = bound_min_max(downCommand, 0, 1); } // Various ways to control the yaw that are essentially manual passthrough. However, because we do not have a fine // grained mechanism of manual setting the yaw as it normally would we need to duplicate that code here float manual_rate[STABILIZATIONSETTINGS_MANUALRATE_NUMELEM]; switch(guidanceSettings.YawMode) { case VTOLPATHFOLLOWERSETTINGS_YAWMODE_RATE: /* This is awkward. This allows the transmitter to control the yaw while flying navigation */ ManualControlCommandYawGet(&yaw); StabilizationSettingsManualRateGet(manual_rate); stabDesired.Yaw = manual_rate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_AXISLOCK: ManualControlCommandYawGet(&yaw); StabilizationSettingsManualRateGet(manual_rate); stabDesired.Yaw = manual_rate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_ATTITUDE: { uint8_t yaw_max; StabilizationSettingsYawMaxGet(&yaw_max); ManualControlCommandYawGet(&yaw); stabDesired.Yaw = yaw_max * yaw; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; } break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_PATH: { // Face forward on the path VelocityDesiredData velocityDesired; VelocityDesiredGet(&velocityDesired); float total_vel2 = velocityDesired.East*velocityDesired.East + velocityDesired.North*velocityDesired.North; float path_direction = atan2f(velocityDesired.East, velocityDesired.North) * RAD2DEG; if (total_vel2 > 1) { stabDesired.Yaw = path_direction; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; } else { stabDesired.Yaw = 0; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; } } break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_POI: stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI; break; } StabilizationDesiredSet(&stabDesired); 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 * @param[in] dT the time since last evaluation * @param[in] att_adj an adjustment to the attitude for loiter mode * * 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 */ int32_t vtol_follower_control_attitude(float dT, const float *att_adj) { vtol_follower_control_accel(dT); float default_adj[2] = {0,0}; if (!att_adj) { att_adj = default_adj; } AccelDesiredData accelDesired; AccelDesiredGet(&accelDesired); StabilizationSettingsData stabSet; StabilizationSettingsGet(&stabSet); float northCommand = accelDesired.North; float eastCommand = accelDesired.East; // Project the north and east acceleration signals into body frame float yaw; AttitudeActualYawGet(&yaw); float forward_accel_desired = -northCommand * cosf(yaw * DEG2RAD) + -eastCommand * sinf(yaw * DEG2RAD); float right_accel_desired = -northCommand * sinf(yaw * DEG2RAD) + eastCommand * cosf(yaw * DEG2RAD); StabilizationDesiredData stabDesired; // Set the angle that would achieve the desired acceleration given the thrust is enough for a hover stabDesired.Pitch = bound_sym(RAD2DEG * atanf(forward_accel_desired / GRAVITY), guidanceSettings.MaxRollPitch) + att_adj[1]; stabDesired.Roll = bound_sym(RAD2DEG * atanf(right_accel_desired / GRAVITY), guidanceSettings.MaxRollPitch) + att_adj[0]; // Re-bound based on maximum attitude settings stabDesired.Pitch = bound_sym(stabDesired.Pitch, stabSet.PitchMax); stabDesired.Roll = bound_sym(stabDesired.Roll, stabSet.RollMax); stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; // Calculate the throttle setting or use pass through from transmitter if (guidanceSettings.ThrottleControl == VTOLPATHFOLLOWERSETTINGS_THROTTLECONTROL_FALSE) { ManualControlCommandThrottleGet(&stabDesired.Throttle); } else { float downCommand = vtol_follower_control_altitude(accelDesired.Down); stabDesired.Throttle = bound_min_max(downCommand, 0, 1); } // Various ways to control the yaw that are essentially manual passthrough. However, because we do not have a fine // grained mechanism of manual setting the yaw as it normally would we need to duplicate that code here 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 = stabSet.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_AXISLOCK: ManualControlCommandYawGet(&yaw); stabDesired.Yaw = stabSet.ManualRate[STABILIZATIONSETTINGS_MANUALRATE_YAW] * yaw; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_ATTITUDE: { ManualControlCommandYawGet(&yaw); stabDesired.Yaw = stabSet.YawMax * yaw; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; } break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_PATH: { // Face forward on the path VelocityDesiredData velocityDesired; VelocityDesiredGet(&velocityDesired); float total_vel2 = velocityDesired.East*velocityDesired.East + velocityDesired.North*velocityDesired.North; float path_direction = atan2f(velocityDesired.East, velocityDesired.North) * RAD2DEG; if (total_vel2 > 1) { stabDesired.Yaw = path_direction; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; } else { stabDesired.Yaw = 0; stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_RATE; } } break; case VTOLPATHFOLLOWERSETTINGS_YAWMODE_POI: stabDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_POI; break; } StabilizationDesiredSet(&stabDesired); return 0; }