Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-02-05 15:33:57 -04:00
Code Issues 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 f1a41b041f
commit 69f1613f2c
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 read() : Read sensor data and _calculate Temperature, Pressure and Altitude
This function is optimized so the main host don´t need to wait This function is optimized so the main host don´t need to wait
You can call this function in your main loop 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. It returns a 1 if there are new data.
get_pressure() : return pressure in mbar*100 units get_pressure() : return pressure in mbar*100 units
get_temperature() : return temperature in celsius degrees*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_C5 0xAA
#define CMD_MS5611_PROM_C6 0xAC #define CMD_MS5611_PROM_C6 0xAC
#define CMD_MS5611_PROM_CRC 0xAE #define CMD_MS5611_PROM_CRC 0xAE
#define CMD_CONVERT_D1_OSR4096 0x48 // Maximun resolution #define CMD_CONVERT_D1_OSR4096 0x48 // Maximum resolution (oversampling)
#define CMD_CONVERT_D2_OSR4096 0x58 // Maximun resolution #define CMD_CONVERT_D2_OSR4096 0x58 // Maximum resolution (oversampling)
uint32_t AP_Baro_MS5611::_s_D1; uint32_t AP_Baro_MS5611::_s_D1;
uint32_t AP_Baro_MS5611::_s_D2; uint32_t AP_Baro_MS5611::_s_D2;
@ -121,6 +121,7 @@ bool AP_Baro_MS5611::init( AP_PeriodicProcess *scheduler )
delay(4); delay(4);
// We read the factory calibration // We read the factory calibration
// The on-chip CRC is not used
C1 = _spi_read_16bits(CMD_MS5611_PROM_C1); C1 = _spi_read_16bits(CMD_MS5611_PROM_C1);
C2 = _spi_read_16bits(CMD_MS5611_PROM_C2); C2 = _spi_read_16bits(CMD_MS5611_PROM_C2);
C3 = _spi_read_16bits(CMD_MS5611_PROM_C3); 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 (_sync_access) return;
if (tnow - _timer < 10000) { // Throttle read rate to 100hz maximum.
return; // wait for more than 10ms // 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; _timer = tnow;
@ -199,14 +203,14 @@ void AP_Baro_MS5611::_calculate()
long long P; long long P;
// Formulas from manufacturer datasheet // Formulas from manufacturer datasheet
// TODO: optimization with shift operations... (shift operations works well on 64 bits variables?) // as per data sheet some intermediate results require over 32 bits, therefore
// We define parameters as 64 bits to prevent overflow on operations // we define parameters as 64 bits to prevent overflow on operations
// sub -20c temperature compensation is not included
dT = D2-((long)C5*256); dT = D2-((long)C5*256);
TEMP = 2000 + ((long long)dT * C6)/8388608; TEMP = 2000 + ((long long)dT * C6)/8388608;
OFF = (long long)C2 * 65536 + ((long long)C4 * dT ) / 128; OFF = (long long)C2 * 65536 + ((long long)C4 * dT ) / 128;
SENS = (long long)C1 * 32768 + ((long long)C3 * dT) / 256; SENS = (long long)C1 * 32768 + ((long long)C3 * dT) / 256;
/*
if (TEMP < 2000){ // second order temperature compensation if (TEMP < 2000){ // second order temperature compensation
long long T2 = (long long)dT*dT / 2147483648; long long T2 = (long long)dT*dT / 2147483648;
long long Aux_64 = (TEMP-2000)*(TEMP-2000); long long Aux_64 = (TEMP-2000)*(TEMP-2000);
@ -216,7 +220,7 @@ void AP_Baro_MS5611::_calculate()
OFF = OFF - OFF2; OFF = OFF - OFF2;
SENS = SENS - SENS2; SENS = SENS - SENS2;
} }
*/
P = (D1*SENS/2097152 - OFF)/32768; P = (D1*SENS/2097152 - OFF)/32768;
Temp = TEMP; Temp = TEMP;
Press = P; Press = P;