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