示例#1
0
/**
 * 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);
		}

	}
}
示例#2
0
static void calculate_pids()
{

	// This scale will be calculated and allows suppressing the PID
	// controller gain
	float roll_scale = 1.0f;
	float pitch_scale = 1.0f;
	float yaw_scale = 1.0f;

	// Fetch the current throttle settings
	float throttle;
	StabilizationDesiredThrottleGet(&throttle);

	// Calculate the desired PID suppression based on throttle settings. This is
	// similar to an algorithm used by MultiWii and empirically works well. It
	// creates a piecewise linear suppression of PIDs versus throttle.
	for (uint32_t i = 0; i < 3; i++) {
		float attenuation;
		float threshold;
		float scale = 1.0f;

		switch(i) {
		case 0:
			attenuation = settings.RollRateTPA[STABILIZATIONSETTINGS_ROLLRATETPA_ATTENUATION] / 100.0f;
			threshold = settings.RollRateTPA[STABILIZATIONSETTINGS_ROLLRATETPA_THRESHOLD] / 100.0f;
			break;
		case 1:
			attenuation = settings.RollRateTPA[STABILIZATIONSETTINGS_PITCHRATETPA_ATTENUATION] / 100.0f;
			threshold = settings.RollRateTPA[STABILIZATIONSETTINGS_PITCHRATETPA_THRESHOLD] / 100.0f;
			break;
		case 2:
			attenuation = settings.RollRateTPA[STABILIZATIONSETTINGS_YAWRATETPA_ATTENUATION] / 100.0f;
			threshold = settings.RollRateTPA[STABILIZATIONSETTINGS_YAWRATETPA_THRESHOLD] / 100.0f;
			break;
		}

		// Ensure everything is in a valid range to keep scale well behaved
		if (throttle > 0 && throttle < 1.0f &&
			attenuation > 0 && attenuation < 0.9f &&
			threshold > 0 && threshold < 1) {

			if (throttle > threshold)
				scale = 1.0f - attenuation * (throttle - threshold) / (1.0f - threshold);
		}

		switch(i) {
		case 0:
			roll_scale = scale;
			break;
		case 1:
			pitch_scale = scale;
			break;
		case 2:
			yaw_scale = scale;
			break;
		}
	}

	// Set the roll rate PID constants
	pid_configure(&pids[PID_RATE_ROLL],
	              settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KP] * roll_scale,
	              settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KI],
	              settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_KD] * roll_scale,
	              settings.RollRatePID[STABILIZATIONSETTINGS_ROLLRATEPID_ILIMIT]);

	// Set the pitch rate PID constants
	pid_configure(&pids[PID_RATE_PITCH],
	              settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KP] * pitch_scale,
	              settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KI],
	              settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_KD] * pitch_scale,
	              settings.PitchRatePID[STABILIZATIONSETTINGS_PITCHRATEPID_ILIMIT]);

	// Set the yaw rate PID constants
	pid_configure(&pids[PID_RATE_YAW],
	              settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KP] * yaw_scale,
	              settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KI],
	              settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_KD] * yaw_scale,
	              settings.YawRatePID[STABILIZATIONSETTINGS_YAWRATEPID_ILIMIT]);

	// Set the roll attitude PI constants
	pid_configure(&pids[PID_ATT_ROLL],
	              settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KP],
	              settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_KI], 0,
	              settings.RollPI[STABILIZATIONSETTINGS_ROLLPI_ILIMIT]);

	// Set the pitch attitude PI constants
	pid_configure(&pids[PID_ATT_PITCH],
	              settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KP],
	              settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_KI], 0,
	              settings.PitchPI[STABILIZATIONSETTINGS_PITCHPI_ILIMIT]);

	// Set the yaw attitude PI constants
	pid_configure(&pids[PID_ATT_YAW],
	              settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KP],
	              settings.YawPI[STABILIZATIONSETTINGS_YAWPI_KI], 0,
	              settings.YawPI[STABILIZATIONSETTINGS_YAWPI_ILIMIT]);

	// Set the vbar roll settings
	pid_configure(&pids[PID_VBAR_ROLL],
	              settings.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KP] * roll_scale,
	              settings.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KI],
	              settings.VbarRollPID[STABILIZATIONSETTINGS_VBARROLLPID_KD] * roll_scale,
	              0);

	// Set the vbar pitch settings
	pid_configure(&pids[PID_VBAR_PITCH],
	              settings.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KP] * pitch_scale,
	              settings.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KI],
	              settings.VbarPitchPID[STABILIZATIONSETTINGS_VBARPITCHPID_KD] * pitch_scale,
	              0);

	// Set the vbar yaw settings
	pid_configure(&pids[PID_VBAR_YAW],
	              settings.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KP] * yaw_scale,
	              settings.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KI],
	              settings.VbarYawPID[STABILIZATIONSETTINGS_VBARYAWPID_KD] * yaw_scale,
	              0);

	// Set the coordinated flight settings
	pid_configure(&pids[PID_COORDINATED_FLIGHT_YAW],
	              settings.CoordinatedFlightYawPI[STABILIZATIONSETTINGS_COORDINATEDFLIGHTYAWPI_KP],
	              settings.CoordinatedFlightYawPI[STABILIZATIONSETTINGS_COORDINATEDFLIGHTYAWPI_KI],
	              0, /* No derivative term */
	              settings.CoordinatedFlightYawPI[STABILIZATIONSETTINGS_COORDINATEDFLIGHTYAWPI_ILIMIT]);

	// Set the mwrate roll settings
	pid_configure(&pids[PID_MWR_ROLL],
	              mwrate_settings.RollRatePID[MWRATESETTINGS_ROLLRATEPID_KP] * roll_scale,
	              mwrate_settings.RollRatePID[MWRATESETTINGS_ROLLRATEPID_KI],
	              mwrate_settings.RollRatePID[MWRATESETTINGS_ROLLRATEPID_KD] * roll_scale,
	              mwrate_settings.RollRatePID[MWRATESETTINGS_ROLLRATEPID_ILIMIT]);

	// Set the mwrate pitch settings
	pid_configure(&pids[PID_MWR_PITCH],
	              mwrate_settings.PitchRatePID[MWRATESETTINGS_PITCHRATEPID_KP] * pitch_scale,
	              mwrate_settings.PitchRatePID[MWRATESETTINGS_PITCHRATEPID_KI],
	              mwrate_settings.PitchRatePID[MWRATESETTINGS_PITCHRATEPID_KD] * pitch_scale,
	              mwrate_settings.PitchRatePID[MWRATESETTINGS_PITCHRATEPID_ILIMIT]);

	// Set the mwrate yaw settings
	pid_configure(&pids[PID_MWR_YAW],
	              mwrate_settings.YawRatePID[MWRATESETTINGS_YAWRATEPID_KP] * yaw_scale,
	              mwrate_settings.YawRatePID[MWRATESETTINGS_YAWRATEPID_KI],
	              mwrate_settings.YawRatePID[MWRATESETTINGS_YAWRATEPID_KD] * yaw_scale,
	              mwrate_settings.YawRatePID[MWRATESETTINGS_YAWRATEPID_ILIMIT]);

	// Set the coordinated flight settings
	pid_configure(&pids[PID_COORDINATED_FLIGHT_YAW],
	              settings.CoordinatedFlightYawPI[STABILIZATIONSETTINGS_COORDINATEDFLIGHTYAWPI_KP],
	              settings.CoordinatedFlightYawPI[STABILIZATIONSETTINGS_COORDINATEDFLIGHTYAWPI_KI],
	              0, /* No derivative term */
	              settings.CoordinatedFlightYawPI[STABILIZATIONSETTINGS_COORDINATEDFLIGHTYAWPI_ILIMIT]);

	// Set up the derivative term
	pid_configure_derivative(settings.DerivativeCutoff, settings.DerivativeGamma);

}