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

Convert the pressure sensor code to most of the calculations as int64 to retain

Browse Source 
precision.
This commit is contained in:
James Cotton 2012-02-13 01:22:48 -06:00
parent 4717af1afd
commit 3acbfbe96c
2 changed files with 18 additions and 29 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

13
flight/Modules/Altitude/revolution/altitude.c
View File

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

34
flight/PiOS/Common/pios_ms5611.c
View File

@ -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;
}
/**