UAVRO
High resolution barometer and altimeter using i2c mode
Fork of
ms5611
by 
Kevin Braun
Diff: ms5611.cpp
- Revision:
- 12:af100556f99e
- Parent:
- 11:e0417b67a4b5
--- a/ms5611.cpp Thu Dec 03 19:47:10 2015 +0000
+++ b/ms5611.cpp Mon May 30 08:10:56 2016 +0000
@@ -30,6 +30,7 @@
_i2c.frequency(400000);
_i2cWAddr = MS5611_ADDR_W;
_i2cRAddr = MS5611_ADDR_R;
+ step = 1;
}
//--------------------------------------------------------------------------------------------------------------------------------------//
@@ -44,6 +45,7 @@
_i2cRAddr -= 2;
}
+ step = 1;
}
//********************************************************
@@ -148,37 +150,47 @@
uint64_t ms5611::cmd_adc(uint8_t cmd) {
uint64_t temp = 0;
+ if( step & 0b101 ){ // or 0b1 either 0b100
#if defined MS5611i2cLOWLEVEL
- m_i2c_send(MS5611_CMD_ADC_CONV + cmd);
+ m_i2c_send(MS5611_CMD_ADC_CONV + cmd);
#else
- cobuf[0] = 0;
- cobuf[1] = 0;
- cobuf[2] = 0;
- cobuf[0] = MS5611_CMD_ADC_CONV + cmd;
- _i2c.write(_i2cWAddr, cobuf, 1, false);
+ cobuf[0] = 0;
+ cobuf[1] = 0;
+ cobuf[2] = 0;
+ cobuf[0] = MS5611_CMD_ADC_CONV + cmd;
+ _i2c.write(_i2cWAddr, cobuf, 1, false);
#endif
- switch (cmd & 0x0f) {
- case MS5611_CMD_ADC_256 : wait_us(900); break;
- case MS5611_CMD_ADC_512 : wait_ms(3); break;
- case MS5611_CMD_ADC_1024: wait_ms(4); break;
- case MS5611_CMD_ADC_2048: wait_ms(6); break;
- case MS5611_CMD_ADC_4096: wait_ms(10); break;
+ /*
+ switch (cmd & 0x0f) {
+ case MS5611_CMD_ADC_256 : wait_us(900); break;
+ case MS5611_CMD_ADC_512 : wait_ms(3); break;
+ case MS5611_CMD_ADC_1024: wait_ms(4); break;
+ case MS5611_CMD_ADC_2048: wait_ms(6); break;
+ case MS5611_CMD_ADC_4096: wait_ms(10); break;
+ }
+ */
+ step = step<<1; // will be 0b10 or 0b1000
+ return 0;
}
+ else if( step & 0b1010 ){ // or 0b10 either 0b1000
#if defined MS5611i2cLOWLEVEL
- m_i2c_send(MS5611_CMD_ADC_READ);
- m_i2c_start(true);
- temp = m_i2c_readAck();
- temp = (temp << 8) | m_i2c_readAck();
- temp = (temp << 8) | m_i2c_readNak();
- m_i2c_stop();
+ m_i2c_send(MS5611_CMD_ADC_READ);
+ m_i2c_start(true);
+ temp = m_i2c_readAck();
+ temp = (temp << 8) | m_i2c_readAck();
+ temp = (temp << 8) | m_i2c_readNak();
+ m_i2c_stop();
#else
- cobuf[0] = MS5611_CMD_ADC_READ;
- _i2c.write(_i2cWAddr, cobuf, 1, true);
- cobuf[0] = 0;
- _i2c.read(_i2cRAddr, cobuf, 3, false);
- temp = (cobuf[0] << 16) | (cobuf[1] << 8) | cobuf[2];
-#endif
- return temp;
+ cobuf[0] = MS5611_CMD_ADC_READ;
+ _i2c.write(_i2cWAddr, cobuf, 1, true);
+ cobuf[0] = 0;
+ _i2c.read(_i2cRAddr, cobuf, 3, false);
+ temp = (cobuf[0] << 16) | (cobuf[1] << 8) | cobuf[2];
+#endif
+ step = step<<1; // will be 0b100 or 0b10000
+ return temp;
+ }
+ return 0;
}
//********************************************************
@@ -293,29 +305,38 @@
//! @return none
//********************************************************
-void ms5611::calcPT() {
- int32_t D2 = cmd_adc(MS5611_CMD_ADC_D2 + MS5611_CMD_ADC_4096); // read D2
- int32_t D1 = cmd_adc(MS5611_CMD_ADC_D1 + MS5611_CMD_ADC_4096); // read D1
- int64_t dT = D2 - ((uint64_t)C[5] << 8);
- int64_t OFF = ((uint32_t)C[2] << 16) + ((dT * (C[4]) >> 7)); //was OFF = (C[2] << 17) + dT * C[4] / (1 << 6);
- int64_t SENS = ((uint32_t)C[1] << 15) + ((dT * (C[3]) >> 8)); //was SENS = (C[1] << 16) + dT * C[3] / (1 << 7);
- int32_t TEMP = 2000 + (int64_t)dT * (int64_t)C[6] / (int64_t)(1 << 23);
- T = (double) TEMP / 100.0;
+uint8_t ms5611::calcPT() {
+ if( step & 0b11 ){ // or 0b1 either 0b10
+ D2 = cmd_adc(MS5611_CMD_ADC_D2 + MS5611_CMD_ADC_2048); // read D2
+ }
+ if( step & 0b1100 ){ // or 0b10 either 0b100
+ D1 = cmd_adc(MS5611_CMD_ADC_D1 + MS5611_CMD_ADC_2048); // read D1
+ }
+ if( step & 0b10000 ){
+ int64_t dT = D2 - ((uint64_t)C[5] << 8);
+ int64_t OFF = ((uint32_t)C[2] << 16) + ((dT * (C[4]) >> 7)); //was OFF = (C[2] << 17) + dT * C[4] / (1 << 6);
+ int64_t SENS = ((uint32_t)C[1] << 15) + ((dT * (C[3]) >> 8)); //was SENS = (C[1] << 16) + dT * C[3] / (1 << 7);
+ int32_t TEMP = 2000 + (int64_t)dT * (int64_t)C[6] / (int64_t)(1 << 23);
+ T = (double) TEMP / 100.0;
- if(TEMP < 2000) { // if temperature lower than +20 Celsius
- int64_t T1 = ((int64_t)TEMP - 2000) * ((int64_t)TEMP - 2000);
- int64_t OFF1 = (5 * T1) >> 1;
- int64_t SENS1 = (5 * T1) >> 2;
+ if(TEMP < 2000) { // if temperature lower than +20 Celsius
+ int64_t T1 = ((int64_t)TEMP - 2000) * ((int64_t)TEMP - 2000);
+ int64_t OFF1 = (5 * T1) >> 1;
+ int64_t SENS1 = (5 * T1) >> 2;
- if(TEMP < -1500) { // if temperature lower than -15 Celsius
- T1 = ((int64_t)TEMP + 1500) * ((int64_t)TEMP + 1500);
- OFF1 += 7 * T1;
- SENS1 += 11 * T1 >> 1;
- }
- OFF -= OFF1;
- SENS -= SENS1;
+ if(TEMP < -1500) { // if temperature lower than -15 Celsius
+ T1 = ((int64_t)TEMP + 1500) * ((int64_t)TEMP + 1500);
+ OFF1 += 7 * T1;
+ SENS1 += 11 * T1 >> 1;
+ }
+ OFF -= OFF1;
+ SENS -= SENS1;
+ }
+ P = (double)(((((int64_t)D1 * SENS ) >> 21) - OFF) / (double) (1 << 15)) / 100.0;
+ step = 1;
+ return 1;
}
- P = (double)(((((int64_t)D1 * SENS ) >> 21) - OFF) / (double) (1 << 15)) / 100.0;
+ return 0;
}
//********************************************************
@@ -347,10 +368,16 @@
//********************************************************
double ms5611::getPressure() {
- calcPT();
+ //calcPT();
return(P);
}
+float ms5611::getAltitude() {
+ A = (1 - pow(P/(double)1013.250, 0.190295)) * 44330.0;
+ //A = (1 - (pow((P / (double)sea_pressure), 0.190284))) * 145366.45;
+ return((float)A);
+}
+
//********************************************************
//! @brief get altitude from known sea level barometer,
//! @ no pre-pressure calculation