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