aepko/Ardupilot2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Uncommented cold temp compensation after testing. Fixed read throttle so read rate ends up nailing 100hz, not 90hz. minor commenting improvements.

Browse Source
This commit is contained in:
justinbeech 2012-02-13 00:00:06 +00:00
parent 2133b9fb08
commit 4b0c71d6ac
1 changed files with 13 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

22
libraries/AP_Baro/AP_Baro_MS5611.cpp
View File

@ -21,7 +21,7 @@
read() : Read sensor data and _calculate Temperature, Pressure and Altitude
This function is optimized so the main host don´t need to wait
You can call this function in your main loop
Maximun data output frequency 100Hz
Maximum data output frequency 100Hz - this allows maximum oversampling in the chip ADC
It returns a 1 if there are new data.
get_pressure() : return pressure in mbar*100 units
get_temperature() : return temperature in celsius degrees*100 units
@ -49,8 +49,8 @@
#define CMD_MS5611_PROM_C5 0xAA
#define CMD_MS5611_PROM_C6 0xAC
#define CMD_MS5611_PROM_CRC 0xAE
#define CMD_CONVERT_D1_OSR4096 0x48 // Maximun resolution
#define CMD_CONVERT_D2_OSR4096 0x58 // Maximun resolution
#define CMD_CONVERT_D1_OSR4096 0x48 // Maximum resolution (oversampling)
#define CMD_CONVERT_D2_OSR4096 0x58 // Maximum resolution (oversampling)
uint32_t AP_Baro_MS5611::_s_D1;
uint32_t AP_Baro_MS5611::_s_D2;
@ -121,6 +121,7 @@ bool AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
delay(4);
// We read the factory calibration
// The on-chip CRC is not used
C1 = _spi_read_16bits(CMD_MS5611_PROM_C1);
C2 = _spi_read_16bits(CMD_MS5611_PROM_C2);
C3 = _spi_read_16bits(CMD_MS5611_PROM_C3);
@ -150,8 +151,11 @@ void AP_Baro_MS5611::_update(uint32_t tnow)
{
if (_sync_access) return;
if (tnow - _timer < 10000) {
return; // wait for more than 10ms
// Throttle read rate to 100hz maximum.
// note we use 9500us here not 10000us
// the read rate will end up at exactly 100hz because the Periodic Timer fires at 1khz
if (tnow - _timer < 9500) {
return;
}
_timer = tnow;
@ -199,14 +203,14 @@ void AP_Baro_MS5611::_calculate()
long long P;
// Formulas from manufacturer datasheet
// TODO: optimization with shift operations... (shift operations works well on 64 bits variables?)
// We define parameters as 64 bits to prevent overflow on operations
// as per data sheet some intermediate results require over 32 bits, therefore
// we define parameters as 64 bits to prevent overflow on operations
// sub -20c temperature compensation is not included
dT = D2-((long)C5*256);
TEMP = 2000 + ((long long)dT * C6)/8388608;
OFF = (long long)C2 * 65536 + ((long long)C4 * dT ) / 128;
SENS = (long long)C1 * 32768 + ((long long)C3 * dT) / 256;
/*
if (TEMP < 2000){ // second order temperature compensation
long long T2 = (long long)dT*dT / 2147483648;
long long Aux_64 = (TEMP-2000)*(TEMP-2000);
@ -216,7 +220,7 @@ void AP_Baro_MS5611::_calculate()
OFF = OFF - OFF2;
SENS = SENS - SENS2;
}
*/
P = (D1*SENS/2097152 - OFF)/32768;
Temp = TEMP;
Press = P;