void baro_airspeedGetMPXV(AirspeedSensorData *airspeedSensor, AirspeedSettingsData *airspeedSettings, uint8_t airspeedADCPin){
// Ensure that the ADC pin is properly configured
if(airspeedADCPin < 0){
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
return;
}
if (sensor.type == PIOS_MPXV_UNKNOWN) {
switch (airspeedSettings->AirspeedSensorType) {
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002:
sensor = PIOS_MPXV_7002_DESC(airspeedADCPin);
break;
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004:
sensor = PIOS_MPXV_5004_DESC(airspeedADCPin);
break;
case AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_MP3V5010:
sensor = PIOS_MP3V5010_DESC(airspeedADCPin);
break;
default:
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_FALSE;
return;
}
}
airspeedSensor->SensorValue = PIOS_MPXV_Measure(&sensor);
if (!airspeedSettings->ZeroPoint) {
// Calibrate sensor by averaging zero point value
if (calibrationCount < CALIBRATION_IDLE_MS / airspeedSettings->SamplePeriod) { //First let sensor warm up and stabilize.
calibrationCount++;
return;
} else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) { //Then compute the average.
calibrationCount++; /*DO NOT MOVE FROM BEFORE sensorCalibration=... LINE, OR ELSE WILL HAVE DIVIDE BY ZERO */
PIOS_MPXV_Calibrate(&sensor, airspeedSensor->SensorValue);
// Set settings UAVO. The airspeed UAVO is set elsewhere in the function.
if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS) / airspeedSettings->SamplePeriod) {
airspeedSettings->ZeroPoint = sensor.zeroPoint;
AirspeedSettingsZeroPointSet(&airspeedSettings->ZeroPoint);
}
return;
}
}
sensor.zeroPoint = airspeedSettings->ZeroPoint;
//Filter CAS
float alpha = airspeedSettings->SamplePeriod / (airspeedSettings->SamplePeriod + ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS); //Low pass filter.
airspeedSensor->CalibratedAirspeed = PIOS_MPXV_CalcAirspeed(&sensor, airspeedSensor->SensorValue) * alpha + airspeedSensor->CalibratedAirspeed * (1.0f-alpha);
airspeedSensor->SensorConnected = AIRSPEEDSENSOR_SENSORCONNECTED_TRUE;
}
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;
}
void baro_airspeedGetAnalog(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin)
{
//Wait until our turn
vTaskDelayUntil(lastSysTime, MS2TICKS(SAMPLING_DELAY_MS_MPXV));
//Ensure that the ADC pin is properly configured
if(airspeedADCPin <0){ //It's not, so revert to former sensor type
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_FALSE;
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_IDLE_MS/SAMPLING_DELAY_MS_MPXV) { //First let sensor warm up and stabilize.
calibrationCount++;
return;
} else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_MPXV) { //Then compute the average.
calibrationCount++; /*DO NOT MOVE FROM BEFORE sensorCalibration=... LINE, OR ELSE WILL HAVE DIVIDE BY ZERO */
uint16_t sensorCalibration;
if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002){
sensorCalibration=PIOS_MPXV7002_Calibrate(airspeedADCPin, calibrationCount-CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV);
PIOS_MPXV7002_UpdateCalibration(sensorCalibration); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
else if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){
sensorCalibration=PIOS_MPXV5004_Calibrate(airspeedADCPin, calibrationCount-CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV);
PIOS_MPXV5004_UpdateCalibration(sensorCalibration); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot.
}
baroAirspeedData->BaroConnected = BAROAIRSPEED_BAROCONNECTED_TRUE;
//Set settings UAVO. The airspeed UAVO is set elsewhere in the function.
if (calibrationCount == (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_MPXV)
AirspeedSettingsZeroPointSet(&sensorCalibration);
return;
}
//Get CAS
float calibratedAirspeed=0;
if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002){
calibratedAirspeed = PIOS_MPXV7002_ReadAirspeed(airspeedADCPin);
if (calibratedAirspeed < 0) //This only occurs when there's a bad ADC reading.
return;
//Get sensor value, just for telemetry purposes.
//This is a silly waste of resources, and should probably be removed at some point in the future.
//At this time, PIOS_MPXV7002_Measure() should become a static function and be removed from the header file.
//
//Moreover, due to the way the ADC driver is currently written, this code will return 0 more often than
//not. This is something that will have to change on the ADC side of things.
baroAirspeedData->SensorValue=PIOS_MPXV7002_Measure(airspeedADCPin);
}
else if(airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV5004){
calibratedAirspeed = PIOS_MPXV5004_ReadAirspeed(airspeedADCPin);
if (calibratedAirspeed < 0) //This only occurs when there's a bad ADC reading.
return;
//Get sensor value, just for telemetry purposes.
//This is a silly waste of resources, and should probably be removed at some point in the future.
//At this time, PIOS_MPXV7002_Measure() should become a static function and be removed from the header file.
//
//Moreover, due to the way the ADC driver is currently written, this code will return 0 more often than
//not. This is something that will have to change on the ADC side of things.
baroAirspeedData->SensorValue=PIOS_MPXV5004_Measure(airspeedADCPin);
}
//Filter CAS
float alpha=SAMPLING_DELAY_MS_MPXV/(SAMPLING_DELAY_MS_MPXV + ANALOG_BARO_AIRSPEED_TIME_CONSTANT_MS); //Low pass filter.
float filteredAirspeed = calibratedAirspeed*(alpha) + baroAirspeedData->CalibratedAirspeed*(1.0f-alpha);
//Set two values, one for the UAVO airspeed sensor reading, and the other for the GPS corrected one
baroAirspeedData->CalibratedAirspeed = filteredAirspeed;
}
