void baro_airspeedGetETASV3(BaroAirspeedData *baroAirspeedData, uint32_t *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin)
{
//Wait until our turn.
PIOS_Thread_Sleep_Until(lastSysTime, SAMPLING_DELAY_MS_ETASV3);
AirspeedSettingsData airspeedSettingsData;
AirspeedSettingsGet(&airspeedSettingsData);
//Check to see if airspeed sensor is returning baroAirspeedData
baroAirspeedData->SensorValue = PIOS_ETASV3_ReadAirspeed();
if (baroAirspeedData->SensorValue == -1) {
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
baroAirspeedData->CalibratedAirspeed = 0;
return;
}
//Calibrate sensor by averaging zero point value //THIS SHOULD NOT BE DONE IF THERE IS AN IN-AIR RESET. HOW TO DETECT THIS?
if (calibrationCount < CALIBRATION_COUNT_IDLE) {
calibrationCount++;
calibrationSum = 0;
return;
} else if (calibrationCount < CALIBRATION_COUNT_IDLE + CALIBRATION_COUNT) {
calibrationCount++;
calibrationSum += baroAirspeedData->SensorValue;
if (calibrationCount == CALIBRATION_COUNT_IDLE + CALIBRATION_COUNT) {
airspeedSettingsData.ZeroPoint = (uint16_t) roundf(((float)calibrationSum) / CALIBRATION_COUNT);
AirspeedSettingsZeroPointSet( &airspeedSettingsData.ZeroPoint );
} else {
return;
}
}
//Compute airspeed
float calibratedAirspeed;
if (baroAirspeedData->SensorValue > airspeedSettingsData.ZeroPoint)
calibratedAirspeed = ETS_AIRSPEED_SCALE * sqrtf(baroAirspeedData->SensorValue - airspeedSettingsData.ZeroPoint);
else
calibratedAirspeed = 0;
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_TRUE;
baroAirspeedData->CalibratedAirspeed = calibratedAirspeed;
}
void baro_airspeedGetETASV3(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings)
{
// Check to see if airspeed sensor is returning airspeedSensor
airspeedSensor->SensorValue = PIOS_ETASV3_ReadAirspeed();
if (airspeedSensor->SensorValue == (uint16_t)-1) {
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
airspeedSensor->CalibratedAirspeed = 0;
return;
}
// only calibrate if no stored calibration is available
if (!airspeedSettings->ZeroPoint) {
// Calibrate sensor by averaging zero point value
if (calibrationCount <= CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) {
calibrationCount++;
calibrationSum = 0;
calibrationCount2 = 0;
return;
} else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) {
calibrationCount++;
calibrationCount2++;
calibrationSum += airspeedSensor->SensorValue;
if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) {
airspeedSettings->ZeroPoint = (int16_t)(((float)calibrationSum) / calibrationCount2);
AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint);
calibrationCount = 0;
calibrationSum = 0;
calibrationCount2 = 0;
}
return;
}
}
// Compute airspeed
airspeedSensor->CalibratedAirspeed = airspeedSettings->Scale * sqrtf((float)abs(airspeedSensor->SensorValue - airspeedSettings->ZeroPoint));
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
}
