static void airspeedActualUpdatedCb(UAVObjEvent * ev) { AirspeedActualData airspeedActual; VelocityActualData velocityActual; AirspeedActualGet(&airspeedActual); VelocityActualGet(&velocityActual); float groundspeed = sqrtf(velocityActual.East*velocityActual.East + velocityActual.North*velocityActual.North ); indicatedAirspeedActualBias = airspeedActual.CalibratedAirspeed - groundspeed; // 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. }
/** * 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 velocity from the current position and path * * Takes in @ref PositionActual and compares it to @ref PathDesired * and computes @ref VelocityDesired */ static void updatePathVelocity() { PositionActualData positionActual; PositionActualGet(&positionActual); VelocityActualData velocityActual; VelocityActualGet(&velocityActual); float cur[3] = {positionActual.North, positionActual.East, positionActual.Down}; struct path_status progress; path_progress(&pathDesired, cur, &progress); float groundspeed = 0; float altitudeSetpoint = 0; switch (pathDesired.Mode) { case PATHDESIRED_MODE_CIRCLERIGHT: case PATHDESIRED_MODE_CIRCLELEFT: groundspeed = pathDesired.EndingVelocity; altitudeSetpoint = pathDesired.End[2]; break; case PATHDESIRED_MODE_ENDPOINT: case PATHDESIRED_MODE_VECTOR: default: groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * bound_min_max(progress.fractional_progress,0,1); altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * bound_min_max(progress.fractional_progress,0,1); break; } // this ensures a significant forward component at least close to the real trajectory if (groundspeed<fixedWingAirspeeds.BestClimbRateSpeed/10.0f) groundspeed=fixedWingAirspeeds.BestClimbRateSpeed/10.0f; // calculate velocity - can be zero if waypoints are too close VelocityDesiredData velocityDesired; velocityDesired.North = progress.path_direction[0] * groundspeed; velocityDesired.East = progress.path_direction[1] * groundspeed; float error_speed = progress.error * fixedwingpathfollowerSettings.HorizontalPosP; // calculate correction - can also be zero if correction vector is 0 or no error present velocityDesired.North += progress.correction_direction[0] * error_speed; velocityDesired.East += progress.correction_direction[1] * error_speed; float downError = altitudeSetpoint - positionActual.Down; velocityDesired.Down = downError * fixedwingpathfollowerSettings.VerticalPosP; // update pathstatus pathStatus.error = progress.error; pathStatus.fractional_progress = progress.fractional_progress; pathStatus.fractional_progress = progress.fractional_progress; if (pathStatus.fractional_progress < 1) pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; else pathStatus.Status = PATHSTATUS_STATUS_COMPLETED; pathStatus.Waypoint = pathDesired.Waypoint; 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 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 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 * * 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); }
/** * Controller to maintain/seek a position and optionally descend. * @param[in] dT time since last eval * @param[in] hold_pos_ned a position to hold * @param[in] landing whether to descend * @param[in] update_status does this update path_status, or does somoene else? */ static int32_t vtol_follower_control_simple(const float dT, const float *hold_pos_ned, bool landing, bool update_status) { PositionActualData positionActual; VelocityDesiredData velocityDesired; PositionActualGet(&positionActual); VelocityActualData velocityActual; VelocityActualGet(&velocityActual); /* Where would we be in ___ second at current rates? */ const float cur_pos_ned[3] = { positionActual.North + velocityActual.North * guidanceSettings.PositionFeedforward, positionActual.East + velocityActual.East * guidanceSettings.PositionFeedforward, positionActual.Down }; float errors_ned[3]; /* Calculate the difference between where we want to be and the * above position */ vtol_calculate_difference(cur_pos_ned, hold_pos_ned, errors_ned, false); float horiz_error_mag = vtol_magnitude(errors_ned, 2); float scale_horiz_error_mag = 0; /* Apply a cubic deadband; if we are already really close don't work * as hard to fix it as if we're far away. Prevents chasing high * frequency noise in direction of correction. * * That is, when we're far, noise in estimated position mostly results * in noise/dither in how fast we go towards the target. When we're * close, there's a large directional / hunting component. This * attenuates that. */ if (horiz_error_mag > 0.00001f) { scale_horiz_error_mag = vtol_deadband(horiz_error_mag, guidanceSettings.EndpointDeadbandWidth, guidanceSettings.EndpointDeadbandCenterGain, vtol_end_m, vtol_end_r) / horiz_error_mag; } float damped_ne[2] = { errors_ned[0] * scale_horiz_error_mag, errors_ned[1] * scale_horiz_error_mag }; float commands_ned[3]; // Compute desired north command velocity from position error commands_ned[0] = pid_apply_antiwindup(&vtol_pids[NORTH_POSITION], damped_ne[0], -guidanceSettings.HorizontalVelMax, guidanceSettings.HorizontalVelMax, dT); // Compute desired east command velocity from position error commands_ned[1] = pid_apply_antiwindup(&vtol_pids[EAST_POSITION], damped_ne[1], -guidanceSettings.HorizontalVelMax, guidanceSettings.HorizontalVelMax, dT); if (!landing) { // Compute desired down comand velocity from the position difference commands_ned[2] = pid_apply_antiwindup(&vtol_pids[DOWN_POSITION], errors_ned[2], -guidanceSettings.VerticalVelMax, guidanceSettings.VerticalVelMax, dT); } else { // Just use the landing rate. commands_ned[2] = guidanceSettings.LandingRate; } // Limit the maximum horizontal velocity any direction (not north and east separately) vtol_limit_velocity(commands_ned, guidanceSettings.HorizontalVelMax); velocityDesired.North = commands_ned[0]; velocityDesired.East = commands_ned[1]; velocityDesired.Down = commands_ned[2]; VelocityDesiredSet(&velocityDesired); if (update_status) { uint8_t path_status = PATHSTATUS_STATUS_INPROGRESS; if (!landing) { const bool criterion_altitude = (errors_ned[2]> -guidanceSettings.WaypointAltitudeTol) || (!guidanceSettings.ThrottleControl); // Indicate whether we are in radius of this endpoint // And at/above the altitude requested if ((vtol_magnitude(errors_ned, 2) < guidanceSettings.EndpointRadius) && criterion_altitude) { path_status = PATHSTATUS_STATUS_COMPLETED; } } // landing never terminates. PathStatusStatusSet(&path_status); } 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 * * 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 */ uint8_t waypointFollowing(uint8_t flightMode, FixedWingPathFollowerSettingsCCData fixedwingpathfollowerSettings) { float dT = fixedwingpathfollowerSettings.UpdatePeriod / 1000.0f; //Convert from [ms] to [s] VelocityActualData velocityActual; StabilizationDesiredData stabDesired; float trueAirspeed; float calibratedAirspeedActual; float airspeedDesired; float airspeedError; float pitchCommand; float powerCommand; float headingError_R; float rollCommand; //TODO: Move these out of the loop FixedWingAirspeedsData fixedwingAirspeeds; FixedWingAirspeedsGet(&fixedwingAirspeeds); VelocityActualGet(&velocityActual); StabilizationDesiredGet(&stabDesired); // TODO: Create UAVO that merges airspeed together AirspeedActualTrueAirspeedGet(&trueAirspeed); PositionActualData positionActual; PositionActualGet(&positionActual); PathDesiredData pathDesired; PathDesiredGet(&pathDesired); if (flightStatusUpdate) { //Reset integrals integral->totalEnergyError = 0; integral->airspeedError = 0; integral->lineError = 0; integral->circleError = 0; if (flightMode == FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME) { // Simple Return To Home mode: climb 10 meters and fly to home position pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North; pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; pathDesired.End[PATHDESIRED_END_NORTH] = 0; pathDesired.End[PATHDESIRED_END_EAST] = 0; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down - 10; pathDesired.StartingVelocity = fixedwingAirspeeds.BestClimbRateSpeed; pathDesired.EndingVelocity = fixedwingAirspeeds.BestClimbRateSpeed; pathDesired.Mode = PATHDESIRED_MODE_FLYVECTOR; homeOrbit = false; } else if (flightMode == FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD) { // Simple position hold: stay at present altitude and position //Offset by one so that the start and end points don't perfectly coincide pathDesired.Start[PATHDESIRED_START_NORTH] = positionActual.North - 1; pathDesired.Start[PATHDESIRED_START_EAST] = positionActual.East; pathDesired.Start[PATHDESIRED_START_DOWN] = positionActual.Down; pathDesired.End[PATHDESIRED_END_NORTH] = positionActual.North; pathDesired.End[PATHDESIRED_END_EAST] = positionActual.East; pathDesired.End[PATHDESIRED_END_DOWN] = positionActual.Down; pathDesired.StartingVelocity = fixedwingAirspeeds.BestClimbRateSpeed; pathDesired.EndingVelocity = fixedwingAirspeeds.BestClimbRateSpeed; pathDesired.Mode = PATHDESIRED_MODE_FLYVECTOR; } PathDesiredSet(&pathDesired); flightStatusUpdate = false; } /** * Compute speed error (required for throttle and pitch) */ // Current airspeed calibratedAirspeedActual = trueAirspeed; //BOOOOOOOOOO!!! Where's the conversion from TAS to CAS? // Current heading float headingActual_R = atan2f(velocityActual.East, velocityActual.North); // Desired airspeed airspeedDesired = pathDesired.EndingVelocity; // Airspeed error airspeedError = airspeedDesired - calibratedAirspeedActual; /** * Compute desired throttle command */ //Proxy because instead of m*(1/2*v^2+g*h), it's v^2+2*gh. This saves processing power float totalEnergyProxySetpoint = powf(pathDesired.EndingVelocity, 2.0f) - 2.0f * GRAVITY * pathDesired.End[2]; float totalEnergyProxyActual = powf(trueAirspeed, 2.0f) - 2.0f * GRAVITY * positionActual.Down; float errorTotalEnergy = totalEnergyProxySetpoint - totalEnergyProxyActual; float throttle_kp = fixedwingpathfollowerSettings. ThrottlePI[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLEPI_KP]; float throttle_ki = fixedwingpathfollowerSettings.ThrottlePI[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLEPI_KI]; float throttle_ilimit = fixedwingpathfollowerSettings.ThrottlePI[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLEPI_ILIMIT]; //Integrate with bound. Make integral leaky for better performance. Approximately 30s time constant. if (throttle_ilimit > 0.0f) { integral->totalEnergyError = bound(integral->totalEnergyError + errorTotalEnergy * dT, -throttle_ilimit / throttle_ki, throttle_ilimit / throttle_ki) * (1.0f - 1.0f / (1.0f + 30.0f / dT)); } powerCommand = errorTotalEnergy * throttle_kp + integral->totalEnergyError * throttle_ki; float throttlelimit_neutral = fixedwingpathfollowerSettings. ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLELIMIT_NEUTRAL]; float throttlelimit_min = fixedwingpathfollowerSettings. ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLELIMIT_MIN]; float throttlelimit_max = fixedwingpathfollowerSettings. ThrottleLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_THROTTLELIMIT_MAX]; // set throttle stabDesired.Throttle = bound(powerCommand + throttlelimit_neutral, throttlelimit_min, throttlelimit_max); /** * Compute desired pitch command */ float airspeed_kp = fixedwingpathfollowerSettings. AirspeedPI[FIXEDWINGPATHFOLLOWERSETTINGSCC_AIRSPEEDPI_KP]; float airspeed_ki = fixedwingpathfollowerSettings. AirspeedPI[FIXEDWINGPATHFOLLOWERSETTINGSCC_AIRSPEEDPI_KI]; float airspeed_ilimit = fixedwingpathfollowerSettings. AirspeedPI[FIXEDWINGPATHFOLLOWERSETTINGSCC_AIRSPEEDPI_ILIMIT]; if (airspeed_ki > 0.0f) { //Integrate with saturation integral->airspeedError = bound(integral->airspeedError + airspeedError * dT, -airspeed_ilimit / airspeed_ki, airspeed_ilimit / airspeed_ki); } //Compute the cross feed from altitude to pitch, with saturation float pitchcrossfeed_kp = fixedwingpathfollowerSettings. VerticalToPitchCrossFeed [FIXEDWINGPATHFOLLOWERSETTINGSCC_VERTICALTOPITCHCROSSFEED_KP]; float pitchcrossfeed_min = fixedwingpathfollowerSettings. VerticalToPitchCrossFeed [FIXEDWINGPATHFOLLOWERSETTINGSCC_VERTICALTOPITCHCROSSFEED_MAX]; float pitchcrossfeed_max = fixedwingpathfollowerSettings. VerticalToPitchCrossFeed [FIXEDWINGPATHFOLLOWERSETTINGSCC_VERTICALTOPITCHCROSSFEED_MAX]; float alitudeError = -(pathDesired.End[2] - positionActual.Down); //Negative to convert from Down to altitude float altitudeToPitchCommandComponent = bound(alitudeError * pitchcrossfeed_kp, -pitchcrossfeed_min, pitchcrossfeed_max); //Compute the pitch command as err*Kp + errInt*Ki + X_feed. pitchCommand = -(airspeedError * airspeed_kp + integral->airspeedError * airspeed_ki) + altitudeToPitchCommandComponent; //Saturate pitch command float pitchlimit_neutral = fixedwingpathfollowerSettings. PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_PITCHLIMIT_NEUTRAL]; float pitchlimit_min = fixedwingpathfollowerSettings. PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_PITCHLIMIT_MIN]; float pitchlimit_max = fixedwingpathfollowerSettings. PitchLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_PITCHLIMIT_MAX]; stabDesired.Pitch = bound(pitchlimit_neutral + pitchCommand, pitchlimit_min, pitchlimit_max); /** * Compute desired roll command */ float p[3] = { positionActual.North, positionActual.East, positionActual.Down }; float *c = pathDesired.End; float *r = pathDesired.Start; float q[3] = { pathDesired.End[0] - pathDesired.Start[0], pathDesired.End[1] - pathDesired.Start[1], pathDesired.End[2] - pathDesired.Start[2] }; float k_path = fixedwingpathfollowerSettings.VectorFollowingGain / pathDesired.EndingVelocity; //Divide gain by airspeed so that the turn rate is independent of airspeed float k_orbit = fixedwingpathfollowerSettings.OrbitFollowingGain / pathDesired.EndingVelocity; //Divide gain by airspeed so that the turn rate is independent of airspeed float k_psi_int = fixedwingpathfollowerSettings.FollowerIntegralGain; //======================================== //SHOULD NOT BE HARD CODED bool direction; float chi_inf = PI / 4.0f; //THIS NEEDS TO BE A FUNCTION OF HOW LONG OUR PATH IS. //Saturate chi_inf. I.e., never approach the path at a steeper angle than 45 degrees chi_inf = chi_inf < PI / 4.0f ? PI / 4.0f : chi_inf; //======================================== float rho; float headingCommand_R; float pncn = p[0] - c[0]; float pece = p[1] - c[1]; float d = sqrtf(pncn * pncn + pece * pece); //Assume that we want a 15 degree bank angle. This should yield a nice, non-agressive turn #define ROLL_FOR_HOLDING_CIRCLE 15.0f //Calculate radius, rho, using r*omega=v and omega = g/V_g * tan(phi) //THIS SHOULD ONLY BE CALCULATED ONCE, INSTEAD OF EVERY TIME rho = powf(pathDesired.EndingVelocity, 2) / (GRAVITY * tanf(fabs(ROLL_FOR_HOLDING_CIRCLE * DEG2RAD))); typedef enum { LINE, ORBIT } pathTypes_t; pathTypes_t pathType; //Determine if we should fly on a line or orbit path. switch (flightMode) { case FLIGHTSTATUS_FLIGHTMODE_RETURNTOHOME: if (d < rho + 5.0f * pathDesired.EndingVelocity || homeOrbit == true) { //When approx five seconds from the circle, start integrating into it pathType = ORBIT; homeOrbit = true; } else { pathType = LINE; } break; case FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD: pathType = ORBIT; break; default: pathType = LINE; break; } //Check to see if we've gone past our destination. Since the path follower //is simply following a vector, it has no concept of where the vector ends. //It will simply keep following it to infinity if we don't stop it. So while //we don't know why the commutation to the next point failed, we don't know //we don't want the plane flying off. if (pathType == LINE) { //Compute the norm squared of the horizontal path length //IT WOULD BE NICE TO ONLY DO THIS ONCE PER WAYPOINT UPDATE, INSTEAD OF //EVERY LOOP float pathLength2 = q[0] * q[0] + q[1] * q[1]; //Perform a quick vector math operation, |a| < a.b/|a| = |b|cos(theta), //to test if we've gone past the waypoint. Add in a distance equal to 5s //of flight time, for good measure to make sure we don't add jitter. if (((p[0] - r[0]) * q[0] + (p[1] - r[1]) * q[1]) > pathLength2 + 5.0f * pathDesired.EndingVelocity) { //Whoops, we've really overflown our destination point, and haven't //received any instructions. Start circling //flightMode will reset the next time a waypoint changes, so there's //no danger of it getting stuck in orbit mode. flightMode = FLIGHTSTATUS_FLIGHTMODE_POSITIONHOLD; pathType = ORBIT; } } switch (pathType) { case ORBIT: if (pathDesired.Mode == PATHDESIRED_MODE_FLYCIRCLELEFT) { direction = false; } else { //In the case where the direction is undefined, always fly in a //clockwise fashion direction = true; } headingCommand_R = followOrbit(c, rho, direction, p, headingActual_R, k_orbit, k_psi_int, dT); break; case LINE: headingCommand_R = followStraightLine(r, q, p, headingActual_R, chi_inf, k_path, k_psi_int, dT); break; } //Calculate heading error headingError_R = headingCommand_R - headingActual_R; //Wrap heading error around circle if (headingError_R < -PI) headingError_R += 2.0f * PI; if (headingError_R > PI) headingError_R -= 2.0f * PI; //GET RID OF THE RAD2DEG. IT CAN BE FACTORED INTO HeadingPI float rolllimit_neutral = fixedwingpathfollowerSettings. RollLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_ROLLLIMIT_NEUTRAL]; float rolllimit_min = fixedwingpathfollowerSettings. RollLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_ROLLLIMIT_MIN]; float rolllimit_max = fixedwingpathfollowerSettings. RollLimit[FIXEDWINGPATHFOLLOWERSETTINGSCC_ROLLLIMIT_MAX]; float headingpi_kp = fixedwingpathfollowerSettings. HeadingPI[FIXEDWINGPATHFOLLOWERSETTINGSCC_HEADINGPI_KP]; rollCommand = (headingError_R * headingpi_kp) * RAD2DEG; //Turn heading stabDesired.Roll = bound(rolllimit_neutral + rollCommand, rolllimit_min, rolllimit_max); #ifdef SIM_OSX fprintf(stderr, " headingError_R: %f, rollCommand: %f\n", headingError_R, rollCommand); #endif /** * Compute desired yaw command */ // Coordinated flight, so we reset the desired yaw. 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; StabilizationDesiredSet(&stabDesired); //Stuff some debug variables into PathDesired, because right now these //fields aren't being used. pathDesired.ModeParameters[0] = pitchCommand; pathDesired.ModeParameters[1] = airspeedError; pathDesired.ModeParameters[2] = integral->airspeedError; pathDesired.ModeParameters[3] = alitudeError; pathDesired.UID = errorTotalEnergy; PathDesiredSet(&pathDesired); return 0; }
/** * @brief Use the INSGPS fusion algorithm in either indoor or outdoor mode (use GPS) * @params[in] first_run This is the first run so trigger reinitialization * @params[in] outdoor_mode If true use the GPS for position, if false weakly pull to (0,0) * @return 0 for success, -1 for failure */ static int32_t updateAttitudeINSGPS(bool first_run, bool outdoor_mode) { UAVObjEvent ev; GyrosData gyrosData; AccelsData accelsData; MagnetometerData magData; BaroAltitudeData baroData; GPSPositionData gpsData; GPSVelocityData gpsVelData; GyrosBiasData gyrosBias; static bool mag_updated = false; static bool baro_updated; static bool gps_updated; static bool gps_vel_updated; static float baroOffset = 0; static uint32_t ins_last_time = 0; static bool inited; float NED[3] = {0.0f, 0.0f, 0.0f}; float vel[3] = {0.0f, 0.0f, 0.0f}; float zeros[3] = {0.0f, 0.0f, 0.0f}; // Perform the update uint16_t sensors = 0; float dT; // Wait until the gyro and accel object is updated, if a timeout then go to failsafe if ( (xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE) || (xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE) ) { // Do not set attitude timeout warnings in simulation mode if (!AttitudeActualReadOnly()){ AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING); return -1; } } if (inited) { mag_updated = 0; baro_updated = 0; gps_updated = 0; gps_vel_updated = 0; } if (first_run) { inited = false; init_stage = 0; mag_updated = 0; baro_updated = 0; gps_updated = 0; gps_vel_updated = 0; ins_last_time = PIOS_DELAY_GetRaw(); return 0; } mag_updated |= (xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE); baro_updated |= xQueueReceive(baroQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE; gps_updated |= (xQueueReceive(gpsQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; gps_vel_updated |= (xQueueReceive(gpsVelQueue, &ev, 0 / portTICK_RATE_MS) == pdTRUE) && outdoor_mode; // Get most recent data GyrosGet(&gyrosData); AccelsGet(&accelsData); MagnetometerGet(&magData); BaroAltitudeGet(&baroData); GPSPositionGet(&gpsData); GPSVelocityGet(&gpsVelData); GyrosBiasGet(&gyrosBias); // Discard mag if it has NAN (normally from bad calibration) mag_updated &= (magData.x == magData.x && magData.y == magData.y && magData.z == magData.z); // Don't require HomeLocation.Set to be true but at least require a mag configuration (allows easily // switching between indoor and outdoor mode with Set = false) mag_updated &= (homeLocation.Be[0] != 0 || homeLocation.Be[1] != 0 || homeLocation.Be[2]); // Have a minimum requirement for gps usage gps_updated &= (gpsData.Satellites >= 7) && (gpsData.PDOP <= 4.0f) && (homeLocation.Set == HOMELOCATION_SET_TRUE); if (!inited) AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR); else if (outdoor_mode && gpsData.Satellites < 7) AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_ERROR); else AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); if (!inited && mag_updated && baro_updated && (gps_updated || !outdoor_mode)) { // Don't initialize until all sensors are read if (init_stage == 0 && !outdoor_mode) { float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f}; float q[4]; float pos[3] = {0.0f, 0.0f, 0.0f}; // Initialize barometric offset to homelocation altitude baroOffset = -baroData.Altitude; pos[2] = -(baroData.Altitude + baroOffset); // Reset the INS algorithm INSGPSInit(); INSSetMagVar(revoCalibration.mag_var); INSSetAccelVar(revoCalibration.accel_var); INSSetGyroVar(revoCalibration.gyro_var); INSSetBaroVar(revoCalibration.baro_var); // Initialize the gyro bias from the settings float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f}; INSSetGyroBias(gyro_bias); xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); MagnetometerGet(&magData); // Set initial attitude AttitudeActualData attitudeActual; attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI; attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI; attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI; RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); AttitudeActualSet(&attitudeActual); q[0] = attitudeActual.q1; q[1] = attitudeActual.q2; q[2] = attitudeActual.q3; q[3] = attitudeActual.q4; INSSetState(pos, zeros, q, zeros, zeros); INSResetP(Pdiag); } else if (init_stage == 0 && outdoor_mode) { float Pdiag[16]={25.0f,25.0f,25.0f,5.0f,5.0f,5.0f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-5f,1e-4f,1e-4f,1e-4f}; float q[4]; float NED[3]; // Reset the INS algorithm INSGPSInit(); INSSetMagVar(revoCalibration.mag_var); INSSetAccelVar(revoCalibration.accel_var); INSSetGyroVar(revoCalibration.gyro_var); INSSetBaroVar(revoCalibration.baro_var); INSSetMagNorth(homeLocation.Be); // Initialize the gyro bias from the settings float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f}; INSSetGyroBias(gyro_bias); GPSPositionData gpsPosition; GPSPositionGet(&gpsPosition); // Transform the GPS position into NED coordinates getNED(&gpsPosition, NED); // Initialize barometric offset to cirrent GPS NED coordinate baroOffset = -NED[2] - baroData.Altitude; xQueueReceive(magQueue, &ev, 100 / portTICK_RATE_MS); MagnetometerGet(&magData); // Set initial attitude AttitudeActualData attitudeActual; attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI; attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI; attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI; RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); AttitudeActualSet(&attitudeActual); q[0] = attitudeActual.q1; q[1] = attitudeActual.q2; q[2] = attitudeActual.q3; q[3] = attitudeActual.q4; INSSetState(NED, zeros, q, zeros, zeros); INSResetP(Pdiag); } else if (init_stage > 0) { // Run prediction a bit before any corrections dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f; GyrosBiasGet(&gyrosBias); float gyros[3] = {(gyrosData.x + gyrosBias.x) * F_PI / 180.0f, (gyrosData.y + gyrosBias.y) * F_PI / 180.0f, (gyrosData.z + gyrosBias.z) * F_PI / 180.0f}; INSStatePrediction(gyros, &accelsData.x, dT); AttitudeActualData attitude; AttitudeActualGet(&attitude); attitude.q1 = Nav.q[0]; attitude.q2 = Nav.q[1]; attitude.q3 = Nav.q[2]; attitude.q4 = Nav.q[3]; Quaternion2RPY(&attitude.q1,&attitude.Roll); AttitudeActualSet(&attitude); } init_stage++; if(init_stage > 10) inited = true; ins_last_time = PIOS_DELAY_GetRaw(); return 0; } if (!inited) return 0; dT = PIOS_DELAY_DiffuS(ins_last_time) / 1.0e6f; ins_last_time = PIOS_DELAY_GetRaw(); // This should only happen at start up or at mode switches if(dT > 0.01f) dT = 0.01f; else if(dT <= 0.001f) dT = 0.001f; // If the gyro bias setting was updated we should reset // the state estimate of the EKF if(gyroBiasSettingsUpdated) { float gyro_bias[3] = {gyrosBias.x * F_PI / 180.0f, gyrosBias.y * F_PI / 180.0f, gyrosBias.z * F_PI / 180.0f}; INSSetGyroBias(gyro_bias); gyroBiasSettingsUpdated = false; } // Because the sensor module remove the bias we need to add it // back in here so that the INS algorithm can track it correctly float gyros[3] = {gyrosData.x * F_PI / 180.0f, gyrosData.y * F_PI / 180.0f, gyrosData.z * F_PI / 180.0f}; if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE) { gyros[0] += gyrosBias.x * F_PI / 180.0f; gyros[1] += gyrosBias.y * F_PI / 180.0f; gyros[2] += gyrosBias.z * F_PI / 180.0f; } // Advance the state estimate INSStatePrediction(gyros, &accelsData.x, dT); // Copy the attitude into the UAVO AttitudeActualData attitude; AttitudeActualGet(&attitude); attitude.q1 = Nav.q[0]; attitude.q2 = Nav.q[1]; attitude.q3 = Nav.q[2]; attitude.q4 = Nav.q[3]; Quaternion2RPY(&attitude.q1,&attitude.Roll); AttitudeActualSet(&attitude); // Advance the covariance estimate INSCovariancePrediction(dT); if(mag_updated) sensors |= MAG_SENSORS; if(baro_updated) sensors |= BARO_SENSOR; INSSetMagNorth(homeLocation.Be); if (gps_updated && outdoor_mode) { INSSetPosVelVar(revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_POS], revoCalibration.gps_var[REVOCALIBRATION_GPS_VAR_VEL]); sensors |= POS_SENSORS; if (0) { // Old code to take horizontal velocity from GPS Position update sensors |= HORIZ_SENSORS; vel[0] = gpsData.Groundspeed * cosf(gpsData.Heading * F_PI / 180.0f); vel[1] = gpsData.Groundspeed * sinf(gpsData.Heading * F_PI / 180.0f); vel[2] = 0; } // Transform the GPS position into NED coordinates getNED(&gpsData, NED); // Track barometric altitude offset with a low pass filter baroOffset = BARO_OFFSET_LOWPASS_ALPHA * baroOffset + (1.0f - BARO_OFFSET_LOWPASS_ALPHA ) * ( -NED[2] - baroData.Altitude ); } else if (!outdoor_mode) { INSSetPosVelVar(1e2f, 1e2f); vel[0] = vel[1] = vel[2] = 0; NED[0] = NED[1] = 0; NED[2] = -(baroData.Altitude + baroOffset); sensors |= HORIZ_SENSORS | HORIZ_POS_SENSORS; sensors |= POS_SENSORS |VERT_SENSORS; } if (gps_vel_updated && outdoor_mode) { sensors |= HORIZ_SENSORS | VERT_SENSORS; vel[0] = gpsVelData.North; vel[1] = gpsVelData.East; vel[2] = gpsVelData.Down; } /* * TODO: Need to add a general sanity check for all the inputs to make sure their kosher * although probably should occur within INS itself */ if (sensors) INSCorrection(&magData.x, NED, vel, ( baroData.Altitude + baroOffset ), sensors); // Copy the position and velocity into the UAVO PositionActualData positionActual; PositionActualGet(&positionActual); positionActual.North = Nav.Pos[0]; positionActual.East = Nav.Pos[1]; positionActual.Down = Nav.Pos[2]; PositionActualSet(&positionActual); VelocityActualData velocityActual; VelocityActualGet(&velocityActual); velocityActual.North = Nav.Vel[0]; velocityActual.East = Nav.Vel[1]; velocityActual.Down = Nav.Vel[2]; VelocityActualSet(&velocityActual); if (revoCalibration.BiasCorrectedRaw == REVOCALIBRATION_BIASCORRECTEDRAW_TRUE && !gyroBiasSettingsUpdated) { // Copy the gyro bias into the UAVO except when it was updated // from the settings during the calculation, then consume it // next cycle gyrosBias.x = Nav.gyro_bias[0] * 180.0f / F_PI; gyrosBias.y = Nav.gyro_bias[1] * 180.0f / F_PI; gyrosBias.z = Nav.gyro_bias[2] * 180.0f / F_PI; GyrosBiasSet(&gyrosBias); } return 0; }
static int32_t updateAttitudeComplementary(bool first_run) { UAVObjEvent ev; GyrosData gyrosData; AccelsData accelsData; static int32_t timeval; float dT; static uint8_t init = 0; // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if ( xQueueReceive(gyroQueue, &ev, FAILSAFE_TIMEOUT_MS / portTICK_RATE_MS) != pdTRUE || xQueueReceive(accelQueue, &ev, 1 / portTICK_RATE_MS) != pdTRUE ) { // When one of these is updated so should the other // Do not set attitude timeout warnings in simulation mode if (!AttitudeActualReadOnly()){ AlarmsSet(SYSTEMALARMS_ALARM_ATTITUDE,SYSTEMALARMS_ALARM_WARNING); return -1; } } AccelsGet(&accelsData); // During initialization and FlightStatusData flightStatus; FlightStatusGet(&flightStatus); if(first_run) { #if defined(PIOS_INCLUDE_HMC5883) // To initialize we need a valid mag reading if ( xQueueReceive(magQueue, &ev, 0 / portTICK_RATE_MS) != pdTRUE ) return -1; MagnetometerData magData; MagnetometerGet(&magData); #else MagnetometerData magData; magData.x = 100; magData.y = 0; magData.z = 0; #endif AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); init = 0; attitudeActual.Roll = atan2f(-accelsData.y, -accelsData.z) * 180.0f / F_PI; attitudeActual.Pitch = atan2f(accelsData.x, -accelsData.z) * 180.0f / F_PI; attitudeActual.Yaw = atan2f(-magData.y, magData.x) * 180.0f / F_PI; RPY2Quaternion(&attitudeActual.Roll,&attitudeActual.q1); AttitudeActualSet(&attitudeActual); timeval = PIOS_DELAY_GetRaw(); return 0; } if((init == 0 && xTaskGetTickCount() < 7000) && (xTaskGetTickCount() > 1000)) { // For first 7 seconds use accels to get gyro bias attitudeSettings.AccelKp = 1; attitudeSettings.AccelKi = 0.9; attitudeSettings.YawBiasRate = 0.23; magKp = 1; } else if ((attitudeSettings.ZeroDuringArming == ATTITUDESETTINGS_ZERODURINGARMING_TRUE) && (flightStatus.Armed == FLIGHTSTATUS_ARMED_ARMING)) { attitudeSettings.AccelKp = 1; attitudeSettings.AccelKi = 0.9; attitudeSettings.YawBiasRate = 0.23; magKp = 1; init = 0; } else if (init == 0) { // Reload settings (all the rates) AttitudeSettingsGet(&attitudeSettings); magKp = 0.01f; init = 1; } GyrosGet(&gyrosData); // Compute the dT using the cpu clock dT = PIOS_DELAY_DiffuS(timeval) / 1000000.0f; timeval = PIOS_DELAY_GetRaw(); float q[4]; AttitudeActualData attitudeActual; AttitudeActualGet(&attitudeActual); float grot[3]; float accel_err[3]; // Get the current attitude estimate quat_copy(&attitudeActual.q1, q); // Rotate gravity to body frame and cross with accels grot[0] = -(2 * (q[1] * q[3] - q[0] * q[2])); grot[1] = -(2 * (q[2] * q[3] + q[0] * q[1])); grot[2] = -(q[0] * q[0] - q[1]*q[1] - q[2]*q[2] + q[3]*q[3]); CrossProduct((const float *) &accelsData.x, (const float *) grot, accel_err); // Account for accel magnitude accel_mag = accelsData.x*accelsData.x + accelsData.y*accelsData.y + accelsData.z*accelsData.z; accel_mag = sqrtf(accel_mag); accel_err[0] /= accel_mag; accel_err[1] /= accel_mag; accel_err[2] /= accel_mag; if ( xQueueReceive(magQueue, &ev, 0) != pdTRUE ) { // Rotate gravity to body frame and cross with accels float brot[3]; float Rbe[3][3]; MagnetometerData mag; Quaternion2R(q, Rbe); MagnetometerGet(&mag); // If the mag is producing bad data don't use it (normally bad calibration) if (mag.x == mag.x && mag.y == mag.y && mag.z == mag.z) { rot_mult(Rbe, homeLocation.Be, brot); float mag_len = sqrtf(mag.x * mag.x + mag.y * mag.y + mag.z * mag.z); mag.x /= mag_len; mag.y /= mag_len; mag.z /= mag_len; float bmag = sqrtf(brot[0] * brot[0] + brot[1] * brot[1] + brot[2] * brot[2]); brot[0] /= bmag; brot[1] /= bmag; brot[2] /= bmag; // Only compute if neither vector is null if (bmag < 1 || mag_len < 1) mag_err[0] = mag_err[1] = mag_err[2] = 0; else CrossProduct((const float *) &mag.x, (const float *) brot, mag_err); } } else { mag_err[0] = mag_err[1] = mag_err[2] = 0; } // Accumulate integral of error. Scale here so that units are (deg/s) but Ki has units of s GyrosBiasData gyrosBias; GyrosBiasGet(&gyrosBias); gyrosBias.x -= accel_err[0] * attitudeSettings.AccelKi; gyrosBias.y -= accel_err[1] * attitudeSettings.AccelKi; gyrosBias.z -= mag_err[2] * magKi; GyrosBiasSet(&gyrosBias); // Correct rates based on error, integral component dealt with in updateSensors gyrosData.x += accel_err[0] * attitudeSettings.AccelKp / dT; gyrosData.y += accel_err[1] * attitudeSettings.AccelKp / dT; gyrosData.z += accel_err[2] * attitudeSettings.AccelKp / dT + mag_err[2] * magKp / dT; // Work out time derivative from INSAlgo writeup // Also accounts for the fact that gyros are in deg/s float qdot[4]; qdot[0] = (-q[1] * gyrosData.x - q[2] * gyrosData.y - q[3] * gyrosData.z) * dT * F_PI / 180 / 2; qdot[1] = (q[0] * gyrosData.x - q[3] * gyrosData.y + q[2] * gyrosData.z) * dT * F_PI / 180 / 2; qdot[2] = (q[3] * gyrosData.x + q[0] * gyrosData.y - q[1] * gyrosData.z) * dT * F_PI / 180 / 2; qdot[3] = (-q[2] * gyrosData.x + q[1] * gyrosData.y + q[0] * gyrosData.z) * dT * F_PI / 180 / 2; // Take a time step q[0] = q[0] + qdot[0]; q[1] = q[1] + qdot[1]; q[2] = q[2] + qdot[2]; q[3] = q[3] + qdot[3]; if(q[0] < 0) { q[0] = -q[0]; q[1] = -q[1]; q[2] = -q[2]; q[3] = -q[3]; } // Renomalize qmag = sqrtf(q[0]*q[0] + q[1]*q[1] + q[2]*q[2] + q[3]*q[3]); q[0] = q[0] / qmag; q[1] = q[1] / qmag; q[2] = q[2] / qmag; q[3] = q[3] / qmag; // If quaternion has become inappropriately short or is nan reinit. // THIS SHOULD NEVER ACTUALLY HAPPEN if((fabs(qmag) < 1.0e-3f) || (qmag != qmag)) { q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0; } quat_copy(q, &attitudeActual.q1); // Convert into eueler degrees (makes assumptions about RPY order) Quaternion2RPY(&attitudeActual.q1,&attitudeActual.Roll); AttitudeActualSet(&attitudeActual); // Flush these queues for avoid errors xQueueReceive(baroQueue, &ev, 0); if ( xQueueReceive(gpsQueue, &ev, 0) == pdTRUE && homeLocation.Set == HOMELOCATION_SET_TRUE ) { float NED[3]; // Transform the GPS position into NED coordinates GPSPositionData gpsPosition; GPSPositionGet(&gpsPosition); getNED(&gpsPosition, NED); PositionActualData positionActual; PositionActualGet(&positionActual); positionActual.North = NED[0]; positionActual.East = NED[1]; positionActual.Down = NED[2]; PositionActualSet(&positionActual); } if ( xQueueReceive(gpsVelQueue, &ev, 0) == pdTRUE ) { // Transform the GPS position into NED coordinates GPSVelocityData gpsVelocity; GPSVelocityGet(&gpsVelocity); VelocityActualData velocityActual; VelocityActualGet(&velocityActual); velocityActual.North = gpsVelocity.North; velocityActual.East = gpsVelocity.East; velocityActual.Down = gpsVelocity.Down; VelocityActualSet(&velocityActual); } AlarmsClear(SYSTEMALARMS_ALARM_ATTITUDE); return 0; }
bool vtol_follower_control_loiter(float dT, float *hold_pos, float *att_adj, float *alt_adj) { LoiterCommandData cmd; LoiterCommandGet(&cmd); // XXX TODO reproject when we're not issuing body-centric commands float commands_rp[2] = { cmd.Roll, cmd.Pitch }; const float CMD_THRESHOLD = 0.2f; float command_mag = vectorn_magnitude(commands_rp, 2); float deadband_mag = loiter_deadband(command_mag, CMD_THRESHOLD, 40); float down_cmd = 0; if (guidanceSettings.ThrottleControl && guidanceSettings.LoiterAllowAltControl) { // Inverted because we want units in "Down" frame // Doubled to recenter to 1 to -1 scale from 0-1. // loiter_deadband clips appropriately. down_cmd = loiter_deadband(1 - (cmd.Throttle * 2), altitudeHoldSettings.Deadband / 100.0f, altitudeHoldSettings.Expo); } // Peak detect and decay of the past command magnitude static float historic_mag = 0.0f; // Initialize altitude command *alt_adj = 0; // First reduce by decay constant historic_mag *= loiter_brakealpha; // If our current magnitude is greater than the result, increase it. if (deadband_mag > historic_mag) { historic_mag = deadband_mag; } // And if we haven't had any significant command lately, bug out and // do nothing. if ((historic_mag < 0.001f) && (fabsf(down_cmd) < 0.001f)) { att_adj[0] = 0; att_adj[1] = 0; return false; } // Normalize our command magnitude. Command vectors from this // point are normalized. if (command_mag >= 0.001f) { commands_rp[0] /= command_mag; commands_rp[1] /= command_mag; } else { // Just pick a direction commands_rp[0] = 0.0f; commands_rp[1] = -1.0f; } // Find our current position error PositionActualData positionActual; PositionActualGet(&positionActual); float cur_pos_ned[3] = { positionActual.North, positionActual.East, positionActual.Down }; float total_poserr_ned[3]; vector3_distances(cur_pos_ned, hold_pos, total_poserr_ned, false); if (deadband_mag >= 0.001f) { float yaw; AttitudeActualYawGet(&yaw); float commands_ne[2]; // 90 degrees here compensates for the above being in roll-pitch // order vs. north-east (and where yaw is defined). vector2_rotate(commands_rp, commands_ne, 90 + yaw); VelocityActualData velocityActual; VelocityActualGet(&velocityActual); // Come up with a target velocity for us to fly the command // at, considering our current momentum in that direction. float target_vel = guidanceSettings.LoiterInitialMaxVel * deadband_mag; // Plus whatever current velocity we're making good in // that direction.. // find the portion of our current velocity vector parallel to // cmd. float parallel_sign = velocityActual.North * commands_ne[0] + velocityActual.East * commands_ne[1]; if (parallel_sign > 0) { float parallel_mag = sqrtf( powf(velocityActual.North * commands_ne[0], 2) + powf(velocityActual.East * commands_ne[1], 2)); target_vel += deadband_mag * parallel_mag; } // Feed the target velocity forward for our new desired position hold_pos[0] = cur_pos_ned[0] + commands_ne[0] * target_vel * guidanceSettings.LoiterLookaheadTimeConstant; hold_pos[1] = cur_pos_ned[1] + commands_ne[1] * target_vel * guidanceSettings.LoiterLookaheadTimeConstant; } // Now put a portion of the error back in. At full stick // deflection, decay error at specified time constant float scaled_error_alpha = 1 - historic_mag * (1 - loiter_errordecayalpha); hold_pos[0] -= scaled_error_alpha * total_poserr_ned[0]; hold_pos[1] -= scaled_error_alpha * total_poserr_ned[1]; // Compute attitude feedforward att_adj[0] = deadband_mag * commands_rp[0] * guidanceSettings.LoiterAttitudeFeedthrough; att_adj[1] = deadband_mag * commands_rp[1] * guidanceSettings.LoiterAttitudeFeedthrough; // If we are being commanded to climb or descend... if (fabsf(down_cmd) >= 0.001f) { // Forgive all altitude error so when position controller comes // back we do something sane hold_pos[2] = cur_pos_ned[2]; // and output an adjustment for velocity control use */ *alt_adj = down_cmd * guidanceSettings.VerticalVelMax; } return true; }