rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-02-18 08:54:15 +01:00
Code Issues Releases Activity

Airspeed module: use stored calibration if available

Browse Source
This commit is contained in:
Corvus Corax 2012-11-05 10:59:06 +01:00
parent 5bf18780c1
commit 8f5b2954ac
2 changed files with 72 additions and 59 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

119
flight/Modules/Airspeed/revolution/baro_airspeed_analog.c
View File

@ -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;
}
//Wait until our turn //THIS SHOULD BE, IF OUR TURN GO IN, OTHERWISE CONTINUE
vTaskDelayUntil(lastSysTime, SAMPLING_DELAY_MS_MPXV / portTICK_RATE_MS);
return;
}
//Calibrate sensor by averaging zero point value //THIS SHOULD NOT BE DONE IF THERE IS AN IN-AIR RESET. HOW TO DETECT THIS?
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);
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.
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.
}
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);
if (calibrationCount > (CALIBRATION_IDLE_MS + CALIBRATION_COUNT_MS)/SAMPLING_DELAY_MS_MPXV)
AirspeedSettingsZeroPointSet(&airspeedSettingsData.ZeroPoint);
return;
}
} else if (!calibrationCount) {
// do this only once
calibrationCount++;
//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);
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.
}
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;
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);
//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;
//Set two values, one for the UAVO airspeed sensor reading, and the other for the GPS corrected one
baroAirspeedData->CalibratedAirspeed = filteredAirspeed;
//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;
}

6
flight/Modules/Airspeed/revolution/baro_airspeed_etasv3.c
View File

@ -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 );