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;
}
}
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;
}
}