diff --git a/flight/Modules/Airspeed/revolution/baro_airspeed_analog.c b/flight/Modules/Airspeed/revolution/baro_airspeed_analog.c index c5bbac3d6..693a34046 100644 --- a/flight/Modules/Airspeed/revolution/baro_airspeed_analog.c +++ b/flight/Modules/Airspeed/revolution/baro_airspeed_analog.c @@ -60,78 +60,91 @@ static uint16_t calibrationCount=0; void baro_airspeedGetAnalog(BaroAirspeedData *baroAirspeedData, portTickType *lastSysTime, uint8_t airspeedSensorType, int8_t airspeedADCPin){ - //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; + //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; + } + + AirspeedSettingsData airspeedSettingsData; + AirspeedSettingsGet(&airspeedSettingsData); + + //Wait until our turn //THIS SHOULD BE, IF OUR TURN GO IN, OTHERWISE CONTINUE + vTaskDelayUntil(lastSysTime, SAMPLING_DELAY_MS_MPXV / portTICK_RATE_MS); - return; - } - //Wait until our turn //THIS SHOULD BE, IF OUR TURN GO IN, OTHERWISE CONTINUE - vTaskDelayUntil(lastSysTime, SAMPLING_DELAY_MS_MPXV / portTICK_RATE_MS); - - //Calibrate sensor by averaging zero point value //THIS SHOULD NOT BE DONE IF THERE IS AN IN-AIR RESET. HOW TO DETECT THIS? + if (!airspeedSettingsData.ZeroPoint) { + //Calibrate sensor by averaging zero point value 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. + if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002) { + airspeedSettingsData.ZeroPoint=PIOS_MPXV7002_Calibrate(airspeedADCPin, calibrationCount-CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV); + PIOS_MPXV7002_UpdateCalibration(airspeedSettingsData.ZeroPoint); //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) { + airspeedSettingsData.ZeroPoint=PIOS_MPXV5004_Calibrate(airspeedADCPin, calibrationCount-CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_MPXV); + PIOS_MPXV5004_UpdateCalibration(airspeedSettingsData.ZeroPoint); //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); + if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_MPXV) + AirspeedSettingsZeroPointSet(&airspeedSettingsData.ZeroPoint); 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); + } else if (!calibrationCount) { + // do this only once + calibrationCount++; + if (airspeedSensorType==AIRSPEEDSETTINGS_AIRSPEEDSENSORTYPE_DIYDRONESMPXV7002) { + PIOS_MPXV7002_UpdateCalibration(airspeedSettingsData.ZeroPoint); //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) { + PIOS_MPXV5004_UpdateCalibration(airspeedSettingsData.ZeroPoint); //This makes sense for the user if the initial calibration was not good and the user does not wish to reboot. } - //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); + return; - //Set two values, one for the UAVO airspeed sensor reading, and the other for the GPS corrected one - baroAirspeedData->CalibratedAirspeed = filteredAirspeed; + } + + //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->BaroConnected = BAROAIRSPEED_BAROCONNECTED_TRUE; + baroAirspeedData->CalibratedAirspeed = filteredAirspeed; } diff --git a/flight/Modules/Airspeed/revolution/baro_airspeed_etasv3.c b/flight/Modules/Airspeed/revolution/baro_airspeed_etasv3.c index fbde4e598..f98ada92e 100644 --- a/flight/Modules/Airspeed/revolution/baro_airspeed_etasv3.c +++ b/flight/Modules/Airspeed/revolution/baro_airspeed_etasv3.c @@ -80,13 +80,13 @@ void baro_airspeedGetETASV3(BaroAirspeedData *baroAirspeedData, portTickType *la // only calibrate if no stored calibration is available if (!airspeedSettingsData.ZeroPoint) { //Calibrate sensor by averaging zero point value - if (calibrationCount < CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_ETASV3) { + if (calibrationCount <= CALIBRATION_IDLE_MS/SAMPLING_DELAY_MS_ETASV3) { calibrationCount++; return; - } else if (calibrationCount < (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) { + } else if (calibrationCount <= (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) { calibrationCount++; calibrationSum += baroAirspeedData->SensorValue; - if (calibrationCount == (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) { + if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_ETASV3) { airspeedSettingsData.ZeroPoint = (int16_t) (((float)calibrationSum) / CALIBRATION_COUNT_MS +0.5f); AirspeedSettingsZeroPointSet( &airspeedSettingsData.ZeroPoint );