bool lps25hSelfTest(void)
{
float pressure;
float temperature;
float asl;
if (!isInit)
return false;
lps25hGetData(&pressure, &temperature, &asl);
if (lps25hEvaluateSelfTest(LPS25H_ST_PRESS_MIN, LPS25H_ST_PRESS_MAX, pressure, "pressure") &&
lps25hEvaluateSelfTest(LPS25H_ST_TEMP_MIN, LPS25H_ST_TEMP_MAX, temperature, "temperature"))
{
return true;
}
else
{
return false;
}
}
void sensorsAcquire(sensorData_t *sensors, const uint32_t tick)
{
if (RATE_DO_EXECUTE(IMU_RATE, tick)) {
imu9Read(&sensors->gyro, &sensors->acc, &sensors->mag);
}
if (RATE_DO_EXECUTE(BARO_RATE, tick) && imuHasBarometer()) {
#ifdef PLATFORM_CF1
ms5611GetData(&sensors->baro.pressure,
&sensors->baro.temperature,
&sensors->baro.asl);
#else
lps25hGetData(&sensors->baro.pressure,
&sensors->baro.temperature,
&sensors->baro.asl);
#endif
// Experimental: receive the position from parameters
if (position.timestamp) {
sensors->position = position;
}
}
}
bool lps25hSelfTest(void)
{
bool testStatus = true;
float pressure;
float temperature;
float asl;
if (!isInit)
return false;
lps25hGetData(&pressure, &temperature, &asl);
if (lps25hEvaluateSelfTest(LPS25H_ST_PRESS_MIN, LPS25H_ST_PRESS_MAX, pressure, "pressure") &&
lps25hEvaluateSelfTest(LPS25H_ST_TEMP_MIN, LPS25H_ST_TEMP_MAX, temperature, "temperature"))
{
DEBUG_PRINT("Self test [OK].\n");
}
else
{
testStatus = false;
}
return testStatus;
}
static void stabilizerAltHoldUpdate(void)
{
// Get altitude hold commands from pilot
commanderGetAltHold(&altHold, &setAltHold, &altHoldChange);
// Get barometer height estimates
//TODO do the smoothing within getData
lps25hGetData(&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;
}
}
