From 4b0c71d6ac573072c2f74004975bfe2c5d689f77 Mon Sep 17 00:00:00 2001 From: justinbeech Date: Mon, 13 Feb 2012 00:00:06 +0000 Subject: [PATCH] Uncommented cold temp compensation after testing. Fixed read throttle so read rate ends up nailing 100hz, not 90hz. minor commenting improvements. --- libraries/AP_Baro/AP_Baro_MS5611.cpp | 22 +++++++++++++--------- 1 file changed, 13 insertions(+), 9 deletions(-) diff --git a/libraries/AP_Baro/AP_Baro_MS5611.cpp b/libraries/AP_Baro/AP_Baro_MS5611.cpp index 497fb11d45..59a0b96ebd 100644 --- a/libraries/AP_Baro/AP_Baro_MS5611.cpp +++ b/libraries/AP_Baro/AP_Baro_MS5611.cpp @@ -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;