TEST_F(BoundMinMax, PositiveMinMax) { float min = 1.0f; float max = 10.0f; // Below Lower Bound EXPECT_EQ(min, bound_min_max(min - 1.0f, min, max)); // At Lower Bound EXPECT_EQ(min, bound_min_max(min, min, max)); // In Bounds EXPECT_EQ(2.0f, bound_min_max(2.0f, min, max)); // At Upper Bound EXPECT_EQ(max, bound_min_max(max, min, max)); // Above Upper Bound EXPECT_EQ(max, bound_min_max(max + 1.0f, min, max)); }
TEST_F(BoundMinMax, StraddleZeroMinMax) { float min = -10.0f; float max = 10.0f; // Below Lower Bound EXPECT_EQ(min, bound_min_max(min - 1.0f, min, max)); // At Lower Bound EXPECT_EQ(min, bound_min_max(min, min, max)); // In Bounds EXPECT_EQ(0.0f, bound_min_max(0.0f, min, max)); // At Upper Bound EXPECT_EQ(max, bound_min_max(max, min, max)); // Above Upper Bound EXPECT_EQ(max, bound_min_max(max + 1.0f, min, max)); }
/** * 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() { float dT = guidanceSettings.UpdatePeriod / 1000.0f; PositionActualData positionActual; PositionActualGet(&positionActual); float cur[3] = {positionActual.North, positionActual.East, positionActual.Down}; struct path_status progress; path_progress(&pathDesired, cur, &progress); // Update the path status UAVO PathStatusData pathStatus; PathStatusGet(&pathStatus); pathStatus.fractional_progress = progress.fractional_progress; if (pathStatus.fractional_progress < 1) pathStatus.Status = PATHSTATUS_STATUS_INPROGRESS; else pathStatus.Status = PATHSTATUS_STATUS_COMPLETED; PathStatusSet(&pathStatus); float groundspeed = pathDesired.StartingVelocity + (pathDesired.EndingVelocity - pathDesired.StartingVelocity) * progress.fractional_progress; if(progress.fractional_progress > 1) groundspeed = 0; VelocityDesiredData velocityDesired; velocityDesired.North = progress.path_direction[0] * groundspeed; velocityDesired.East = progress.path_direction[1] * groundspeed; float error_speed = progress.error * guidanceSettings.HorizontalPosPI[VTOLPATHFOLLOWERSETTINGS_HORIZONTALPOSPI_KP]; float correction_velocity[2] = {progress.correction_direction[0] * error_speed, progress.correction_direction[1] * error_speed}; float total_vel = sqrtf(powf(correction_velocity[0],2) + powf(correction_velocity[1],2)); float scale = 1; if(total_vel > guidanceSettings.HorizontalVelMax) scale = guidanceSettings.HorizontalVelMax / total_vel; // Currently not apply a PID loop for horizontal corrections velocityDesired.North += progress.correction_direction[0] * error_speed * scale; velocityDesired.East += progress.correction_direction[1] * error_speed * scale; // Interpolate desired velocity along the path float altitudeSetpoint = pathDesired.Start[2] + (pathDesired.End[2] - pathDesired.Start[2]) * bound_min_max(progress.fractional_progress,0,1); float downError = altitudeSetpoint - positionActual.Down; velocityDesired.Down = pid_apply_antiwindup(&vtol_pids[DOWN_POSITION], downError, -guidanceSettings.VerticalVelMax, guidanceSettings.VerticalVelMax, dT); VelocityDesiredSet(&velocityDesired); }
void PID_configure(struct ParseState *Parser, struct Value *ReturnValue, struct Value **Param, int NumArgs) { if (Param[0]->Val->Pointer == NULL) return; struct pid *pid = Param[0]->Val->Pointer; pid->p = Param[1]->Val->FP; pid->i = Param[2]->Val->FP; pid->d = Param[3]->Val->FP; pid->iLim = Param[4]->Val->FP; double cutoff = bound_min_max(Param[5]->Val->FP,1,100); pid->dTau = 1.0 / (2 * 3.14159265358979323846 * cutoff); }
/** * 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 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 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); }
/** * Module thread, should not return. */ static void altitudeHoldTask(void *parameters) { bool engaged = false; AltitudeHoldDesiredData altitudeHoldDesired; StabilizationDesiredData stabilizationDesired; AltitudeHoldSettingsData altitudeHoldSettings; UAVObjEvent ev; struct pid velocity_pid; // Listen for object updates. AltitudeHoldDesiredConnectQueue(queue); AltitudeHoldSettingsConnectQueue(queue); FlightStatusConnectQueue(queue); AltitudeHoldSettingsGet(&altitudeHoldSettings); pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp, altitudeHoldSettings.VelocityKi, 0.0f, 1.0f); AlarmsSet(SYSTEMALARMS_ALARM_ALTITUDEHOLD, SYSTEMALARMS_ALARM_OK); // Main task loop const uint32_t dt_ms = 5; const float dt_s = dt_ms * 0.001f; uint32_t timeout = dt_ms; while (1) { if (PIOS_Queue_Receive(queue, &ev, timeout) != true) { } else if (ev.obj == FlightStatusHandle()) { uint8_t flight_mode; FlightStatusFlightModeGet(&flight_mode); if (flight_mode == FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD && !engaged) { // Copy the current throttle as a starting point for integral StabilizationDesiredThrottleGet(&velocity_pid.iAccumulator); velocity_pid.iAccumulator *= 1000.0f; // pid library scales up accumulator by 1000 engaged = true; } else if (flight_mode != FLIGHTSTATUS_FLIGHTMODE_ALTITUDEHOLD) engaged = false; // Run loop at 20 Hz when engaged otherwise just slowly wait for it to be engaged timeout = engaged ? dt_ms : 100; } else if (ev.obj == AltitudeHoldDesiredHandle()) { AltitudeHoldDesiredGet(&altitudeHoldDesired); } else if (ev.obj == AltitudeHoldSettingsHandle()) { AltitudeHoldSettingsGet(&altitudeHoldSettings); pid_configure(&velocity_pid, altitudeHoldSettings.VelocityKp, altitudeHoldSettings.VelocityKi, 0.0f, 1.0f); } bool landing = altitudeHoldDesired.Land == ALTITUDEHOLDDESIRED_LAND_TRUE; // For landing mode allow throttle to go negative to allow the integrals // to stop winding up const float min_throttle = landing ? -0.1f : 0.0f; // When engaged compute altitude controller output if (engaged) { float position_z, velocity_z, altitude_error; PositionActualDownGet(&position_z); VelocityActualDownGet(&velocity_z); position_z = -position_z; // Use positive up convention velocity_z = -velocity_z; // Use positive up convention // Compute the altitude error altitude_error = altitudeHoldDesired.Altitude - position_z; // Velocity desired is from the outer controller plus the set point float velocity_desired = altitude_error * altitudeHoldSettings.PositionKp + altitudeHoldDesired.ClimbRate; float throttle_desired = pid_apply_antiwindup(&velocity_pid, velocity_desired - velocity_z, min_throttle, 1.0f, // positive limits since this is throttle dt_s); 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 throttle_desired = (fraction > 0.1f) ? (throttle_desired / fraction) : 0.0f; } StabilizationDesiredGet(&stabilizationDesired); stabilizationDesired.Throttle = bound_min_max(throttle_desired, min_throttle, 1.0f); if (landing) { stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabilizationDesired.Roll = 0; stabilizationDesired.Pitch = 0; stabilizationDesired.Yaw = 0; } else { stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_ROLL] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_PITCH] = STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE; stabilizationDesired.StabilizationMode[STABILIZATIONDESIRED_STABILIZATIONMODE_YAW] = STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK; stabilizationDesired.Roll = altitudeHoldDesired.Roll; stabilizationDesired.Pitch = altitudeHoldDesired.Pitch; stabilizationDesired.Yaw = altitudeHoldDesired.Yaw; } StabilizationDesiredSet(&stabilizationDesired); } } }
/** * Module task */ static void stabilizationTask(void* parameters) { UAVObjEvent ev; uint32_t timeval = PIOS_DELAY_GetRaw(); ActuatorDesiredData actuatorDesired; StabilizationDesiredData stabDesired; RateDesiredData rateDesired; AttitudeActualData attitudeActual; GyrosData gyrosData; FlightStatusData flightStatus; float *stabDesiredAxis = &stabDesired.Roll; float *actuatorDesiredAxis = &actuatorDesired.Roll; float *rateDesiredAxis = &rateDesired.Roll; float horizonRateFraction = 0.0f; // Force refresh of all settings immediately before entering main task loop SettingsUpdatedCb((UAVObjEvent *) NULL); // Settings for system identification uint32_t iteration = 0; const uint32_t SYSTEM_IDENT_PERIOD = 75; uint32_t system_ident_timeval = PIOS_DELAY_GetRaw(); float dT_filtered = 0; // Main task loop zero_pids(); while(1) { iteration++; PIOS_WDG_UpdateFlag(PIOS_WDG_STABILIZATION); // Wait until the AttitudeRaw object is updated, if a timeout then go to failsafe if (PIOS_Queue_Receive(queue, &ev, FAILSAFE_TIMEOUT_MS) != true) { AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_WARNING); continue; } calculate_pids(); float dT = PIOS_DELAY_DiffuS(timeval) * 1.0e-6f; timeval = PIOS_DELAY_GetRaw(); // exponential moving averaging (EMA) of dT to reduce jitter; ~200points // to have more or less equivalent noise reduction to a normal N point moving averaging: alpha = 2 / (N + 1) // run it only at the beginning for the first samples, to reduce CPU load, and the value should converge to a constant value if (iteration < 100) { dT_filtered = dT; } else if (iteration < 2000) { dT_filtered = 0.01f * dT + (1.0f - 0.01f) * dT_filtered; } else if (iteration == 2000) { gyro_filter_updated = true; } if (gyro_filter_updated) { if (settings.GyroCutoff < 1.0f) { gyro_alpha = 0; } else { gyro_alpha = expf(-2.0f * (float)(M_PI) * settings.GyroCutoff * dT_filtered); } // Compute time constant for vbar decay term if (settings.VbarTau < 0.001f) { vbar_decay = 0; } else { vbar_decay = expf(-dT_filtered / settings.VbarTau); } gyro_filter_updated = false; } FlightStatusGet(&flightStatus); StabilizationDesiredGet(&stabDesired); AttitudeActualGet(&attitudeActual); GyrosGet(&gyrosData); ActuatorDesiredGet(&actuatorDesired); #if defined(RATEDESIRED_DIAGNOSTICS) RateDesiredGet(&rateDesired); #endif struct TrimmedAttitudeSetpoint { float Roll; float Pitch; float Yaw; } trimmedAttitudeSetpoint; // Mux in level trim values, and saturate the trimmed attitude setpoint. trimmedAttitudeSetpoint.Roll = bound_min_max( stabDesired.Roll + trimAngles.Roll, -settings.RollMax + trimAngles.Roll, settings.RollMax + trimAngles.Roll); trimmedAttitudeSetpoint.Pitch = bound_min_max( stabDesired.Pitch + trimAngles.Pitch, -settings.PitchMax + trimAngles.Pitch, settings.PitchMax + trimAngles.Pitch); trimmedAttitudeSetpoint.Yaw = stabDesired.Yaw; // For horizon mode we need to compute the desire attitude from an unscaled value and apply the // trim offset. Also track the stick with the most deflection to choose rate blending. horizonRateFraction = 0.0f; if (stabDesired.StabilizationMode[ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON) { trimmedAttitudeSetpoint.Roll = bound_min_max( stabDesired.Roll * settings.RollMax + trimAngles.Roll, -settings.RollMax + trimAngles.Roll, settings.RollMax + trimAngles.Roll); horizonRateFraction = fabsf(stabDesired.Roll); } if (stabDesired.StabilizationMode[PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON) { trimmedAttitudeSetpoint.Pitch = bound_min_max( stabDesired.Pitch * settings.PitchMax + trimAngles.Pitch, -settings.PitchMax + trimAngles.Pitch, settings.PitchMax + trimAngles.Pitch); horizonRateFraction = MAX(horizonRateFraction, fabsf(stabDesired.Pitch)); } if (stabDesired.StabilizationMode[YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON) { trimmedAttitudeSetpoint.Yaw = stabDesired.Yaw * settings.YawMax; horizonRateFraction = MAX(horizonRateFraction, fabsf(stabDesired.Yaw)); } // For weak leveling mode the attitude setpoint is the trim value (drifts back towards "0") if (stabDesired.StabilizationMode[ROLL] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) { trimmedAttitudeSetpoint.Roll = trimAngles.Roll; } if (stabDesired.StabilizationMode[PITCH] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) { trimmedAttitudeSetpoint.Pitch = trimAngles.Pitch; } if (stabDesired.StabilizationMode[YAW] == STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING) { trimmedAttitudeSetpoint.Yaw = 0; } // Note we divide by the maximum limit here so the fraction ranges from 0 to 1 depending on // how much is requested. horizonRateFraction = bound_sym(horizonRateFraction, HORIZON_MODE_MAX_BLEND) / HORIZON_MODE_MAX_BLEND; // Calculate the errors in each axis. The local error is used in the following modes: // ATTITUDE, HORIZON, WEAKLEVELING float local_attitude_error[3]; local_attitude_error[0] = trimmedAttitudeSetpoint.Roll - attitudeActual.Roll; local_attitude_error[1] = trimmedAttitudeSetpoint.Pitch - attitudeActual.Pitch; local_attitude_error[2] = trimmedAttitudeSetpoint.Yaw - attitudeActual.Yaw; // Wrap yaw error to [-180,180] local_attitude_error[2] = circular_modulus_deg(local_attitude_error[2]); static float gyro_filtered[3]; gyro_filtered[0] = gyro_filtered[0] * gyro_alpha + gyrosData.x * (1 - gyro_alpha); gyro_filtered[1] = gyro_filtered[1] * gyro_alpha + gyrosData.y * (1 - gyro_alpha); gyro_filtered[2] = gyro_filtered[2] * gyro_alpha + gyrosData.z * (1 - gyro_alpha); // A flag to track which stabilization mode each axis is in static uint8_t previous_mode[MAX_AXES] = {255,255,255}; bool error = false; //Run the selected stabilization algorithm on each axis: for(uint8_t i=0; i< MAX_AXES; i++) { // Check whether this axis mode needs to be reinitialized bool reinit = (stabDesired.StabilizationMode[i] != previous_mode[i]); // The unscaled input (-1,1) float *raw_input = &stabDesired.Roll; previous_mode[i] = stabDesired.StabilizationMode[i]; // Apply the selected control law switch(stabDesired.StabilizationMode[i]) { case STABILIZATIONDESIRED_STABILIZATIONMODE_RATE: if(reinit) pids[PID_GROUP_RATE + i].iAccumulator = 0; // Store to rate desired variable for storing to UAVO rateDesiredAxis[i] = bound_sym(stabDesiredAxis[i], settings.ManualRate[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_ACROPLUS: // this implementation is based on the Openpilot/Librepilot Acro+ flightmode // and our existing rate & MWRate flightmodes if(reinit) pids[PID_GROUP_RATE + i].iAccumulator = 0; // The factor for gyro suppression / mixing raw stick input into the output; scaled by raw stick input float factor = fabsf(raw_input[i]) * settings.AcroInsanityFactor / 100; // Store to rate desired variable for storing to UAVO rateDesiredAxis[i] = bound_sym(raw_input[i] * settings.ManualRate[i], settings.ManualRate[i]); // Zero integral for aggressive maneuvers, like it is done for MWRate if ((i < 2 && fabsf(gyro_filtered[i]) > 150.0f) || (i == 0 && fabsf(raw_input[i]) > 0.2f)) { pids[PID_GROUP_RATE + i].iAccumulator = 0; pids[PID_GROUP_RATE + i].i = 0; } // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = factor * raw_input[i] + (1.0f - factor) * actuatorDesiredAxis[i]; actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i], 1.0f); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE: if(reinit) { pids[PID_GROUP_ATT + i].iAccumulator = 0; pids[PID_GROUP_RATE + i].iAccumulator = 0; } // Compute the outer loop rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i], dT); rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.MaximumRate[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_VIRTUALBAR: // Store for debugging output rateDesiredAxis[i] = stabDesiredAxis[i]; // Run a virtual flybar stabilization algorithm on this axis stabilization_virtual_flybar(gyro_filtered[i], rateDesiredAxis[i], &actuatorDesiredAxis[i], dT, reinit, i, &pids[PID_GROUP_VBAR + i], &settings); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_WEAKLEVELING: { if (reinit) pids[PID_GROUP_RATE + i].iAccumulator = 0; float weak_leveling = local_attitude_error[i] * weak_leveling_kp; weak_leveling = bound_sym(weak_leveling, weak_leveling_max); // Compute desired rate as input biased towards leveling rateDesiredAxis[i] = stabDesiredAxis[i] + weak_leveling; actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; } case STABILIZATIONDESIRED_STABILIZATIONMODE_AXISLOCK: if (reinit) pids[PID_GROUP_RATE + i].iAccumulator = 0; if (fabsf(stabDesiredAxis[i]) > max_axislock_rate) { // While getting strong commands act like rate mode rateDesiredAxis[i] = bound_sym(stabDesiredAxis[i], settings.ManualRate[i]); // Reset accumulator axis_lock_accum[i] = 0; } else { // For weaker commands or no command simply lock (almost) on no gyro change axis_lock_accum[i] += (stabDesiredAxis[i] - gyro_filtered[i]) * dT; axis_lock_accum[i] = bound_sym(axis_lock_accum[i], max_axis_lock); // Compute the inner loop float tmpRateDesired = pid_apply(&pids[PID_GROUP_ATT + i], axis_lock_accum[i], dT); rateDesiredAxis[i] = bound_sym(tmpRateDesired, settings.MaximumRate[i]); } actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_HORIZON: if(reinit) { pids[PID_GROUP_RATE + i].iAccumulator = 0; } // Do not allow outer loop integral to wind up in this mode since the controller // is often disengaged. pids[PID_GROUP_ATT + i].iAccumulator = 0; // Compute the outer loop for the attitude control float rateDesiredAttitude = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i], dT); // Compute the desire rate for a rate control float rateDesiredRate = raw_input[i] * settings.ManualRate[i]; // Blend from one rate to another. The maximum of all stick positions is used for the // amount so that when one axis goes completely to rate the other one does too. This // prevents doing flips while one axis tries to stay in attitude mode. rateDesiredAxis[i] = rateDesiredAttitude * (1.0f-horizonRateFraction) + rateDesiredRate * horizonRateFraction; rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.ManualRate[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_MWRATE: { if(reinit) { pids[PID_GROUP_MWR + i].iAccumulator = 0; } /* Conversion from MultiWii PID settings to our units. Kp = Kp_mw * 4 / 80 / 500 Kd = Kd_mw * looptime * 1e-6 * 4 * 3 / 32 / 500 Ki = Ki_mw * 4 / 125 / 64 / (looptime * 1e-6) / 500 These values will just be approximate and should help you get started. */ // The unscaled input (-1,1) - note in MW this is from (-500,500) float *raw_input = &stabDesired.Roll; // dynamic PIDs are scaled both by throttle and stick position float scale = (i == 0 || i == 1) ? mwrate_settings.RollPitchRate : mwrate_settings.YawRate; float pid_scale = (100.0f - scale * fabsf(raw_input[i])) / 100.0f; float dynP8 = pids[PID_GROUP_MWR + i].p * pid_scale; float dynD8 = pids[PID_GROUP_MWR + i].d * pid_scale; // these terms are used by the integral loop this proportional term is scaled by throttle (this is different than MW // that does not apply scale float cfgP8 = pids[PID_GROUP_MWR + i].p; float cfgI8 = pids[PID_GROUP_MWR + i].i; // Dynamically adjust PID settings struct pid mw_pid; mw_pid.p = 0; // use zero Kp here because of strange setpoint. applied later. mw_pid.d = dynD8; mw_pid.i = cfgI8; mw_pid.iLim = pids[PID_GROUP_MWR + i].iLim; mw_pid.iAccumulator = pids[PID_GROUP_MWR + i].iAccumulator; mw_pid.lastErr = pids[PID_GROUP_MWR + i].lastErr; mw_pid.lastDer = pids[PID_GROUP_MWR + i].lastDer; // Zero integral for aggressive maneuvers if ((i < 2 && fabsf(gyro_filtered[i]) > 150.0f) || (i == 0 && fabsf(raw_input[i]) > 0.2f)) { mw_pid.iAccumulator = 0; mw_pid.i = 0; } // Apply controller as if we want zero change, then add stick input afterwards actuatorDesiredAxis[i] = pid_apply_setpoint(&mw_pid, raw_input[i] / cfgP8, gyro_filtered[i], dT); actuatorDesiredAxis[i] += raw_input[i]; // apply input actuatorDesiredAxis[i] -= dynP8 * gyro_filtered[i]; // apply Kp term actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); // Store PID accumulators for next cycle pids[PID_GROUP_MWR + i].iAccumulator = mw_pid.iAccumulator; pids[PID_GROUP_MWR + i].lastErr = mw_pid.lastErr; pids[PID_GROUP_MWR + i].lastDer = mw_pid.lastDer; } break; case STABILIZATIONDESIRED_STABILIZATIONMODE_SYSTEMIDENT: if(reinit) { pids[PID_GROUP_ATT + i].iAccumulator = 0; pids[PID_GROUP_RATE + i].iAccumulator = 0; } static uint32_t ident_iteration = 0; static float ident_offsets[3] = {0}; if (PIOS_DELAY_DiffuS(system_ident_timeval) / 1000.0f > SYSTEM_IDENT_PERIOD && SystemIdentHandle()) { ident_iteration++; system_ident_timeval = PIOS_DELAY_GetRaw(); SystemIdentData systemIdent; SystemIdentGet(&systemIdent); const float SCALE_BIAS = 7.1f; float roll_scale = expf(SCALE_BIAS - systemIdent.Beta[SYSTEMIDENT_BETA_ROLL]); float pitch_scale = expf(SCALE_BIAS - systemIdent.Beta[SYSTEMIDENT_BETA_PITCH]); float yaw_scale = expf(SCALE_BIAS - systemIdent.Beta[SYSTEMIDENT_BETA_YAW]); if (roll_scale > 0.25f) roll_scale = 0.25f; if (pitch_scale > 0.25f) pitch_scale = 0.25f; if (yaw_scale > 0.25f) yaw_scale = 0.2f; switch(ident_iteration & 0x07) { case 0: ident_offsets[0] = 0; ident_offsets[1] = 0; ident_offsets[2] = yaw_scale; break; case 1: ident_offsets[0] = roll_scale; ident_offsets[1] = 0; ident_offsets[2] = 0; break; case 2: ident_offsets[0] = 0; ident_offsets[1] = 0; ident_offsets[2] = -yaw_scale; break; case 3: ident_offsets[0] = -roll_scale; ident_offsets[1] = 0; ident_offsets[2] = 0; break; case 4: ident_offsets[0] = 0; ident_offsets[1] = 0; ident_offsets[2] = yaw_scale; break; case 5: ident_offsets[0] = 0; ident_offsets[1] = pitch_scale; ident_offsets[2] = 0; break; case 6: ident_offsets[0] = 0; ident_offsets[1] = 0; ident_offsets[2] = -yaw_scale; break; case 7: ident_offsets[0] = 0; ident_offsets[1] = -pitch_scale; ident_offsets[2] = 0; break; } } if (i == ROLL || i == PITCH) { // Compute the outer loop rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], local_attitude_error[i], dT); rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.MaximumRate[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] += ident_offsets[i]; actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); } else { // Get the desired rate. yaw is always in rate mode in system ident. rateDesiredAxis[i] = bound_sym(stabDesiredAxis[i], settings.ManualRate[i]); // Compute the inner loop only for yaw actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] += ident_offsets[i]; actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); } break; case STABILIZATIONDESIRED_STABILIZATIONMODE_COORDINATEDFLIGHT: switch (i) { case YAW: if (reinit) { pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0; pids[PID_RATE_YAW].iAccumulator = 0; axis_lock_accum[YAW] = 0; } //If we are not in roll attitude mode, trigger an error if (stabDesired.StabilizationMode[ROLL] != STABILIZATIONDESIRED_STABILIZATIONMODE_ATTITUDE) { error = true; break ; } if (fabsf(stabDesired.Yaw) < COORDINATED_FLIGHT_MAX_YAW_THRESHOLD) { //If yaw is within the deadband... if (fabsf(stabDesired.Roll) > COORDINATED_FLIGHT_MIN_ROLL_THRESHOLD) { // We're requesting more roll than the threshold float accelsDataY; AccelsyGet(&accelsDataY); //Reset integral if we have changed roll to opposite direction from rudder. This implies that we have changed desired turning direction. if ((stabDesired.Roll > 0 && actuatorDesiredAxis[YAW] < 0) || (stabDesired.Roll < 0 && actuatorDesiredAxis[YAW] > 0)){ pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0; } // Coordinate flight can simply be seen as ensuring that there is no lateral acceleration in the // body frame. As such, we use the (noisy) accelerometer data as our measurement. Ideally, at // some point in the future we will estimate acceleration and then we can use the estimated value // instead of the measured value. float errorSlip = -accelsDataY; float command = pid_apply(&pids[PID_COORDINATED_FLIGHT_YAW], errorSlip, dT); actuatorDesiredAxis[YAW] = bound_sym(command ,1.0); // Reset axis-lock integrals pids[PID_RATE_YAW].iAccumulator = 0; axis_lock_accum[YAW] = 0; } else if (fabsf(stabDesired.Roll) <= COORDINATED_FLIGHT_MIN_ROLL_THRESHOLD) { // We're requesting less roll than the threshold // Axis lock on no gyro change axis_lock_accum[YAW] += (0 - gyro_filtered[YAW]) * dT; rateDesiredAxis[YAW] = pid_apply(&pids[PID_ATT_YAW], axis_lock_accum[YAW], dT); rateDesiredAxis[YAW] = bound_sym(rateDesiredAxis[YAW], settings.MaximumRate[YAW]); actuatorDesiredAxis[YAW] = pid_apply_setpoint(&pids[PID_RATE_YAW], rateDesiredAxis[YAW], gyro_filtered[YAW], dT); actuatorDesiredAxis[YAW] = bound_sym(actuatorDesiredAxis[YAW],1.0f); // Reset coordinated-flight integral pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0; } } else { //... yaw is outside the deadband. Pass the manual input directly to the actuator. actuatorDesiredAxis[YAW] = bound_sym(stabDesiredAxis[YAW], 1.0); // Reset all integrals pids[PID_COORDINATED_FLIGHT_YAW].iAccumulator = 0; pids[PID_RATE_YAW].iAccumulator = 0; axis_lock_accum[YAW] = 0; } break; case ROLL: case PITCH: default: //Coordinated Flight has no effect in these modes. Trigger a configuration error. error = true; break; } break; case STABILIZATIONDESIRED_STABILIZATIONMODE_POI: // The sanity check enforces this is only selectable for Yaw // for a gimbal you can select pitch too. if(reinit) { pids[PID_GROUP_ATT + i].iAccumulator = 0; pids[PID_GROUP_RATE + i].iAccumulator = 0; } float error; float angle; if (CameraDesiredHandle()) { switch(i) { case PITCH: CameraDesiredDeclinationGet(&angle); error = circular_modulus_deg(angle - attitudeActual.Pitch); break; case ROLL: { uint8_t roll_fraction = 0; #ifdef GIMBAL if (BrushlessGimbalSettingsHandle()) { BrushlessGimbalSettingsRollFractionGet(&roll_fraction); } #endif /* GIMBAL */ // For ROLL POI mode we track the FC roll angle (scaled) to // allow keeping some motion CameraDesiredRollGet(&angle); angle *= roll_fraction / 100.0f; error = circular_modulus_deg(angle - attitudeActual.Roll); } break; case YAW: CameraDesiredBearingGet(&angle); error = circular_modulus_deg(angle - attitudeActual.Yaw); break; default: error = true; } } else error = true; // Compute the outer loop rateDesiredAxis[i] = pid_apply(&pids[PID_GROUP_ATT + i], error, dT); rateDesiredAxis[i] = bound_sym(rateDesiredAxis[i], settings.PoiMaximumRate[i]); // Compute the inner loop actuatorDesiredAxis[i] = pid_apply_setpoint(&pids[PID_GROUP_RATE + i], rateDesiredAxis[i], gyro_filtered[i], dT); actuatorDesiredAxis[i] = bound_sym(actuatorDesiredAxis[i],1.0f); break; case STABILIZATIONDESIRED_STABILIZATIONMODE_NONE: actuatorDesiredAxis[i] = bound_sym(stabDesiredAxis[i],1.0f); break; default: error = true; break; } } if (settings.VbarPiroComp == STABILIZATIONSETTINGS_VBARPIROCOMP_TRUE) stabilization_virtual_flybar_pirocomp(gyro_filtered[2], dT); #if defined(RATEDESIRED_DIAGNOSTICS) RateDesiredSet(&rateDesired); #endif // Save dT actuatorDesired.UpdateTime = dT * 1000; actuatorDesired.Throttle = stabDesired.Throttle; if(flightStatus.FlightMode != FLIGHTSTATUS_FLIGHTMODE_MANUAL) { ActuatorDesiredSet(&actuatorDesired); } else { // Force all axes to reinitialize when engaged for(uint8_t i=0; i< MAX_AXES; i++) previous_mode[i] = 255; } if(flightStatus.Armed != FLIGHTSTATUS_ARMED_ARMED || (lowThrottleZeroIntegral && stabDesired.Throttle < 0)) { // Force all axes to reinitialize when engaged for(uint8_t i=0; i< MAX_AXES; i++) previous_mode[i] = 255; } // Clear or set alarms. Done like this to prevent toggling each cycle // and hammering system alarms if (error) AlarmsSet(SYSTEMALARMS_ALARM_STABILIZATION,SYSTEMALARMS_ALARM_ERROR); else AlarmsClear(SYSTEMALARMS_ALARM_STABILIZATION); } }
TEST_F(BoundMinMax, ValWithinZeroRange) { // Test bounding when min = max = val EXPECT_EQ(-1.0f, bound_min_max(-1.0f, -1.0f, -1.0f)); EXPECT_EQ(0.0f, bound_min_max(0.0f, 0.0f, 0.0f)); EXPECT_EQ(1.0f, bound_min_max(1.0f, 1.0f, 1.0f)); };
TEST_F(BoundMinMax, ValBelowZeroRange) { // Test lower bounding when min = max with (val < min) EXPECT_EQ(-1.0f, bound_min_max(-10.0f, -1.0f, -1.0f)); EXPECT_EQ(0.0f, bound_min_max(-10.0f, 0.0f, 0.0f)); EXPECT_EQ(1.0f, bound_min_max(-10.0f, 1.0f, 1.0f)); };
/** * Bound input value within range (plus or minus) */ static float bound_sym(float val, float range) { return (bound_min_max(val, -range, range)); }
/** * Interpolate values (groundspeeds, altitudes) over flight legs * @param[in] fraction how far we are through the leg * @param[in] beginVal the configured value for the beginning of the leg * @param[in] endVal the configured value for the end of the leg * @returns the interpolated value * * Simple linear interpolation with clipping to ends (fraction>=0, <=1). */ static float vtol_interpolate(const float fraction, const float beginVal, const float endVal) { return beginVal + (endVal - beginVal) * bound_min_max(fraction, 0, 1); }
/** * 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; }
TEST_F(BoundMinMax, ValAboveZeroRange) { // Test upper bounding when min = max with (val > max) EXPECT_EQ(-1.0f, bound_min_max(10.0f, -1.0f, -1.0f)); EXPECT_EQ(0.0f, bound_min_max(10.0f, 0.0f, 0.0f)); EXPECT_EQ(1.0f, bound_min_max(10.0f, 1.0f, 1.0f)); }
/** * 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 * @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; }