mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-18 08:54:15 +01:00
Convert the pressure sensor code to most of the calculations as int64 to retain
precision.
This commit is contained in:
parent
4717af1afd
commit
3acbfbe96c
@ -46,20 +46,12 @@
|
||||
// Private constants
|
||||
#define STACK_SIZE_BYTES 500
|
||||
#define TASK_PRIORITY (tskIDLE_PRIORITY+1)
|
||||
//#define UPDATE_PERIOD 100
|
||||
#define UPDATE_PERIOD 25
|
||||
|
||||
// Private types
|
||||
|
||||
// Private variables
|
||||
static xTaskHandle taskHandle;
|
||||
|
||||
// down sampling variables
|
||||
#define alt_ds_size 4
|
||||
static int32_t alt_ds_temp = 0;
|
||||
static int32_t alt_ds_pres = 0;
|
||||
static int alt_ds_count = 0;
|
||||
|
||||
// Private functions
|
||||
static void altitudeTask(void *parameters);
|
||||
|
||||
@ -88,11 +80,6 @@ int32_t AltitudeInitialize()
|
||||
{
|
||||
BaroAltitudeInitialize();
|
||||
|
||||
// init down-sampling data
|
||||
alt_ds_temp = 0;
|
||||
alt_ds_pres = 0;
|
||||
alt_ds_count = 0;
|
||||
|
||||
return 0;
|
||||
}
|
||||
MODULE_INITCALL(AltitudeInitialize, AltitudeStart)
|
||||
|
@ -45,10 +45,10 @@ MS5611CalibDataTypeDef CalibData;
|
||||
/* Straight from the datasheet */
|
||||
static int32_t X1, X2, X3, B3, B5, B6, P;
|
||||
static uint32_t B4, B7;
|
||||
static volatile int32_t RawTemperature;
|
||||
static volatile int32_t RawPressure;
|
||||
static volatile float Pressure;
|
||||
static volatile float Temperature;
|
||||
static volatile uint32_t RawTemperature;
|
||||
static volatile uint32_t RawPressure;
|
||||
static volatile int64_t Pressure;
|
||||
static volatile int64_t Temperature;
|
||||
|
||||
static int32_t PIOS_MS5611_Read(uint8_t address, uint8_t * buffer, uint8_t len);
|
||||
static int32_t PIOS_MS5611_WriteCommand(uint8_t command);
|
||||
@ -107,6 +107,10 @@ int32_t PIOS_MS5611_StartADC(ConversionTypeTypeDef Type)
|
||||
* \param[in] PresOrTemp BMP085_PRES_ADDR or BMP085_TEMP_ADDR
|
||||
* \return 0 if successfully read the ADC, -1 if failed
|
||||
*/
|
||||
volatile int64_t Offset;
|
||||
volatile int64_t Sens;
|
||||
volatile int64_t deltaTemp;
|
||||
|
||||
int32_t PIOS_MS5611_ReadADC(void)
|
||||
{
|
||||
uint8_t Data[3];
|
||||
@ -114,7 +118,6 @@ int32_t PIOS_MS5611_ReadADC(void)
|
||||
Data[1] = 0;
|
||||
Data[2] = 0;
|
||||
|
||||
static float dT;
|
||||
/* Read and store the 16bit result */
|
||||
if (CurrentRead == TemperatureConv) {
|
||||
/* Read the temperature conversion */
|
||||
@ -123,24 +126,23 @@ int32_t PIOS_MS5611_ReadADC(void)
|
||||
|
||||
RawTemperature = (Data[0] << 16) | (Data[1] << 8) | Data[2];
|
||||
|
||||
dT = RawTemperature - CalibData.C[4] * (1 << 8);
|
||||
Temperature = 2000 + dT * CalibData.C[5] / (1<<23);
|
||||
Temperature /= 100.0f;
|
||||
deltaTemp = RawTemperature - (CalibData.C[4] << 8);
|
||||
Temperature = ((2000l + deltaTemp * CalibData.C[5]) >> 23);
|
||||
// Temperature /= 100.0f;
|
||||
|
||||
} else {
|
||||
float Offset;
|
||||
float Sens;
|
||||
|
||||
/* Read the pressure conversion */
|
||||
if (PIOS_MS5611_Read(MS5611_ADC_READ, Data, 3) != 0)
|
||||
return -1;
|
||||
RawPressure = ((Data[0] << 16) | (Data[1] << 8) | Data[2]);
|
||||
|
||||
Offset = (float) CalibData.C[1] * (1<<16) +(float) CalibData.C[3] * (float) dT / (1<<7);
|
||||
Sens = (float) CalibData.C[0] * (1<<15) + ((float) CalibData.C[2] * (float) dT) / (1<<8);
|
||||
Offset = (((int64_t) CalibData.C[1]) << 16) + ((((int64_t) CalibData.C[3]) * deltaTemp) >> 7);
|
||||
Sens = ((int64_t) CalibData.C[0]) << 15;
|
||||
Sens = Sens + ((((int64_t) CalibData.C[2]) * deltaTemp) >> 8);
|
||||
|
||||
Pressure = (RawPressure * Sens / (1<<21) - Offset) / (1<<15);
|
||||
Pressure /= 1000.0f;
|
||||
Pressure = (((((int64_t) RawPressure) * Sens) >> 21) - Offset) >> 15;
|
||||
// Pressure /= 1000.0f;
|
||||
}
|
||||
return 0;
|
||||
}
|
||||
@ -150,7 +152,7 @@ int32_t PIOS_MS5611_ReadADC(void)
|
||||
*/
|
||||
float PIOS_MS5611_GetTemperature(void)
|
||||
{
|
||||
return Temperature;
|
||||
return ((float) Temperature) / 100.0f;
|
||||
}
|
||||
|
||||
/**
|
||||
@ -158,7 +160,7 @@ float PIOS_MS5611_GetTemperature(void)
|
||||
*/
|
||||
float PIOS_MS5611_GetPressure(void)
|
||||
{
|
||||
return Pressure;
|
||||
return ((float) Pressure) / 1000.0f;
|
||||
}
|
||||
|
||||
/**
|
||||
|
Loading…
x
Reference in New Issue
Block a user