コード例 #1
0
static void stabilizerAltHoldUpdate() {
	// Get the time
	float altitudeError = 0;
	static float altitudeError_i = 0;
	float instAcceleration = 0;
	float deltaVertSpeed = 0;
	static uint32_t timeStart = 0;
	static uint32_t timeCurrent = 0;


	// Get altitude hold commands from pilot
	commanderGetAltHold(&altHold, &setAltHold, &altHoldChange);

	// Get barometer height estimates
	//TODO do the smoothing within getData
	ms5611GetData(&pressure, &temperature, &aslRaw);

	// Compute the altitude
	altitudeError = aslRaw - estimatedAltitude;
	altitudeError_i = fmin(50.0, fmax(-50.0, altitudeError_i + altitudeError));

	instAcceleration = deadband(accWZ, vAccDeadband) * 9.80665 + altitudeError_i * altEstKi;

	deltaVertSpeed = instAcceleration * ALTHOLD_UPDATE_DT + (altEstKp1 * ALTHOLD_UPDATE_DT) * altitudeError;
    estimatedAltitude += (vSpeedComp * 2.0 + deltaVertSpeed) * (ALTHOLD_UPDATE_DT / 2) + (altEstKp2 * ALTHOLD_UPDATE_DT) * altitudeError;
    vSpeedComp += deltaVertSpeed;

	aslLong = estimatedAltitude;		// Override aslLong

	// Estimate vertical speed based on Acc - fused with baro to reduce drift
	vSpeedComp = constrain(vSpeedComp, -vSpeedLimit, vSpeedLimit);

	// Reset Integral gain of PID controller if being charged
	if (!pmIsDischarging()) {
		altHoldPID.integ = 0.0;
	}

	// Altitude hold mode just activated, set target altitude as current altitude. Reuse previous integral term as a starting point
	if (setAltHold) {
		// Set to current altitude
		altHoldTarget = estimatedAltitude;

		// Set the start time
		timeStart = xTaskGetTickCount();
		timeCurrent = 0;

		// Reset PID controller
		pidInit(&altHoldPID, estimatedAltitude, altHoldKp, altHoldKi, altHoldKd, ALTHOLD_UPDATE_DT);

		// Cache last integral term for reuse after pid init
		// const float pre_integral = hoverPID.integ;

		// Reset the PID controller for the hover controller. We want zero vertical velocity
		pidInit(&hoverPID, 0, hoverKp, hoverKi, hoverKd, ALTHOLD_UPDATE_DT);
		pidSetIntegralLimit(&hoverPID, 3);
		// TODO set low and high limits depending on voltage
		// TODO for now just use previous I value and manually set limits for whole voltage range
		//                    pidSetIntegralLimit(&altHoldPID, 12345);
		//                    pidSetIntegralLimitLow(&altHoldPID, 12345);              /


		// hoverPID.integ = pre_integral;
	}

	// In altitude hold mode
	if (altHold) {
		// Get current time
		timeCurrent = xTaskGetTickCount();

		// Update target altitude from joy controller input
		altHoldTarget += altHoldChange / altHoldChange_SENS;
		pidSetDesired(&altHoldPID, altHoldTarget);

		// Compute error (current - target), limit the error
		altHoldErr = constrain(deadband(estimatedAltitude - altHoldTarget, errDeadband), -altHoldErrMax, altHoldErrMax);
		pidSetError(&altHoldPID, -altHoldErr);

		// Get control from PID controller, dont update the error (done above)
		float altHoldPIDVal = pidUpdate(&altHoldPID, estimatedAltitude, false);

		// Get the PID value for the hover
		float hoverPIDVal = pidUpdate(&hoverPID, vSpeedComp, true);

		float thrustValFloat;

		// Use different weights depending on time into altHold mode
		if (timeCurrent > 150) {
			// Compute the mixture between the alt hold and the hover PID
			thrustValFloat = 0.5*hoverPIDVal + 0.5*altHoldPIDVal;
		} else {
			// Compute the mixture between the alt hold and the hover PID
			thrustValFloat = 0.1*hoverPIDVal + 0.9*altHoldPIDVal;
		}
		// float thrustVal = 0.5*hoverPIDVal + 0.5*altHoldPIDVal;
		uint32_t thrustVal = altHoldBaseThrust + (int32_t)(thrustValFloat*pidAslFac);

		// compute new thrust
		actuatorThrust = max(altHoldMinThrust, min(altHoldMaxThrust, limitThrust( thrustVal )));

		// i part should compensate for voltage drop
	} else {
		altHoldTarget = 0.0;
		altHoldErr = 0.0;
	}
}
コード例 #2
0
static void stabilizerAltHoldUpdate(void)
{
  // Get altitude hold commands from pilot
  commanderGetAltHold(&altHold, &setAltHold, &altHoldChange);

  // Get barometer height estimates
  //TODO do the smoothing within getData
  ms5611GetData(&pressure, &temperature, &aslRaw);
  asl = asl * aslAlpha + aslRaw * (1 - aslAlpha);
  aslLong = aslLong * aslAlphaLong + aslRaw * (1 - aslAlphaLong);
  // Estimate vertical speed based on successive barometer readings. This is ugly :)
  vSpeedASL = deadband(asl - aslLong, vSpeedASLDeadband);

  // Estimate vertical speed based on Acc - fused with baro to reduce drift
  vSpeed = constrain(vSpeed, -vSpeedLimit, vSpeedLimit);
  vSpeed = vSpeed * vBiasAlpha + vSpeedASL * (1.f - vBiasAlpha);
  vSpeedAcc = vSpeed;

  // Reset Integral gain of PID controller if being charged
  if (!pmIsDischarging())
  {
    altHoldPID.integ = 0.0;
  }

  // Altitude hold mode just activated, set target altitude as current altitude. Reuse previous integral term as a starting point
  if (setAltHold)
  {
    // Set to current altitude
    altHoldTarget = asl;

    // Cache last integral term for reuse after pid init
    const float pre_integral = altHoldPID.integ;

    // Reset PID controller
    pidInit(&altHoldPID, asl, altHoldKp, altHoldKi, altHoldKd,
            ALTHOLD_UPDATE_DT);
    // TODO set low and high limits depending on voltage
    // TODO for now just use previous I value and manually set limits for whole voltage range
    //                    pidSetIntegralLimit(&altHoldPID, 12345);
    //                    pidSetIntegralLimitLow(&altHoldPID, 12345);              /

    altHoldPID.integ = pre_integral;

    // Reset altHoldPID
    altHoldPIDVal = pidUpdate(&altHoldPID, asl, false);
  }

  // In altitude hold mode
  if (altHold)
  {
    // Update target altitude from joy controller input
    altHoldTarget += altHoldChange / altHoldChange_SENS;
    pidSetDesired(&altHoldPID, altHoldTarget);

    // Compute error (current - target), limit the error
    altHoldErr = constrain(deadband(asl - altHoldTarget, errDeadband),
                           -altHoldErrMax, altHoldErrMax);
    pidSetError(&altHoldPID, -altHoldErr);

    // Get control from PID controller, dont update the error (done above)
    // Smooth it and include barometer vspeed
    // TODO same as smoothing the error??
    altHoldPIDVal = (pidAlpha) * altHoldPIDVal + (1.f - pidAlpha) * ((vSpeedAcc * vSpeedAccFac) +
                    (vSpeedASL * vSpeedASLFac) + pidUpdate(&altHoldPID, asl, false));

    // compute new thrust
    actuatorThrust =  max(altHoldMinThrust, min(altHoldMaxThrust,
                          limitThrust( altHoldBaseThrust + (int32_t)(altHoldPIDVal*pidAslFac))));

    // i part should compensate for voltage drop

  }
  else
  {
    altHoldTarget = 0.0;
    altHoldErr = 0.0;
    altHoldPIDVal = 0.0;
  }
}