UAVRO
High resolution barometer and altimeter using i2c mode
Fork of
ms5611
by 
Kevin Braun
Diff: ms5611.cpp
- Revision:
- 9:6104e8cdb3ec
- Parent:
- 8:461f68bc94f2
diff -r 461f68bc94f2 -r 6104e8cdb3ec ms5611.cpp
--- a/ms5611.cpp Tue Apr 28 23:36:22 2015 +0000
+++ b/ms5611.cpp Tue Jul 14 16:19:59 2015 +0000
@@ -22,6 +22,7 @@
double S; // sea level barometer (mB)
uint32_t C[8]; //coefficient storage
+int MStype = 0;
//--------------------------------------------------------------------------------------------------------------------------------------//
// Constructor and destructor - default to be compatible with legacy m5611 driver
@@ -39,7 +40,8 @@
_i2c.frequency(400000);
_i2cWAddr = MS5611_ADDR_W;
_i2cRAddr = MS5611_ADDR_R;
- if(CSBpin == CSBpin_1) {
+ MStype = CSBpin;
+ if((CSBpin == CSBpin_1) || (CSBpin == MS5805)) {
_i2cWAddr -= 2;
_i2cRAddr -= 2;
@@ -112,7 +114,7 @@
//! @return none
//********************************************************
-void ms5611::m_i2c_send(char cmd) {
+int ms5611::m_i2c_send(char cmd) {
unsigned char ret;
ret = m_i2c_start(false);
if(!(ret)) {
@@ -121,6 +123,7 @@
ret = m_i2c_write(cmd);
m_i2c_stop();
}
+ return(ret);
}
//********************************************************
@@ -129,16 +132,29 @@
//! @return none
//********************************************************
-void ms5611::cmd_reset() {
+int ms5611::cmd_reset() {
#if defined MS5611i2cLOWLEVEL
- m_i2c_send(MS5611_CMD_RESET);
+ int ret = m_i2c_send(MS5611_CMD_RESET);
+ if(!(ret)) return(ret);
#else
char cobuf[1];
cobuf[0] = MS5611_CMD_RESET;
- _i2c.write(_i2cWAddr, cobuf, 1, false);
+ int ret = _i2c.write(_i2cWAddr, cobuf, 1, false);
+ if((ret)) return(0);
#endif
+
+#ifdef RTOS_H
+ Thread::wait(4);
+#else
wait_ms(4);
+#endif
+
loadCoefs();
+#if defined MS5611i2cLOWLEVEL
+ return(ret);
+#else
+ return(1);
+#endif
}
//********************************************************
@@ -148,33 +164,47 @@
//********************************************************
unsigned long ms5611::cmd_adc(char cmd) {
- char cobuf[3];
- cobuf[0] = 0;
- cobuf[1] = 0;
- cobuf[2] = 0;
- unsigned int ret;
+ char cobuf[3] = {0, 0, 0};
unsigned long temp = 0;
#if defined MS5611i2cLOWLEVEL
- m_i2c_send(MS5611_CMD_ADC_CONV + cmd);
+ m_i2c_start(false);
+ m_i2c_write(MS5611_CMD_ADC_CONV + cmd);
+ m_i2c_stop();
#else
cobuf[0] = MS5611_CMD_ADC_CONV + cmd;
_i2c.write(_i2cWAddr, cobuf, 1, false);
#endif
switch (cmd & 0x0f) {
+#ifdef RTOS_H
+ case MS5611_CMD_ADC_256 : Thread::wait(1); break;
+ case MS5611_CMD_ADC_512 : Thread::wait(3); break;
+ case MS5611_CMD_ADC_1024: Thread::wait(4); break;
+ case MS5611_CMD_ADC_2048: Thread::wait(6); break;
+ case MS5611_CMD_ADC_4096: Thread::wait(10); break;
+ case MS5805_CMD_ADC_8192: Thread::wait(19); break;
+#else
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;
+ case MS5805_CMD_ADC_8192: wait_ms(19); break;
+#endif
}
#if defined MS5611i2cLOWLEVEL
- m_i2c_send(MS5611_CMD_ADC_READ);
+ m_i2c_start(false);
+ m_i2c_write(MS5611_CMD_ADC_READ);
+ m_i2c_start(true);
+ cobuf[0] = m_i2c_readAck();
+ cobuf[1] = m_i2c_readAck();
+ cobuf[2] = 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);
#endif
- ret = _i2c.read(_i2cRAddr, cobuf, 3, false);
//if(ret) printf("\n*** ms5611 ADC Read Error ");
temp = (cobuf[0] << 16) + (cobuf[1] << 8) + cobuf[2];
return temp;
@@ -187,19 +217,21 @@
//********************************************************
unsigned int ms5611::cmd_prom(char coef_num) {
- char cobuf[2];
- unsigned int ret;
+ char cobuf[2] = {0, 0};
unsigned int rC = 0;
- cobuf[0] = 0;
- cobuf[1] = 0;
#if defined MS5611i2cLOWLEVEL
- m_i2c_send(MS5611_CMD_PROM_RD + coef_num * 2); // send PROM READ command
+ m_i2c_start(false);
+ m_i2c_write(MS5611_CMD_PROM_RD + coef_num * 2);// send PROM READ command
+ m_i2c_start(true);
+ cobuf[0] = m_i2c_readAck();
+ cobuf[1] = m_i2c_readNak();
+ m_i2c_stop();
#else
cobuf[0] = MS5611_CMD_PROM_RD + coef_num * 2;
_i2c.write(_i2cWAddr, cobuf, 1, true);
cobuf[0] = 0;
+ _i2c.read(_i2cRAddr, cobuf, 2, false);
#endif
- ret = _i2c.read(_i2cRAddr, cobuf, 2, false);
//if(ret) printf("\n*** ms5611 PROM Read Error ");
rC = cobuf[0] * 256 + cobuf[1];
return rC;
@@ -212,12 +244,16 @@
//********************************************************
unsigned char ms5611::crc4(unsigned int n_prom[]) {
- unsigned int n_rem;
- unsigned int crc_read;
- unsigned char n_bit;
- n_rem = 0x00;
- crc_read = n_prom[7];
- n_prom[7]=(0xFF00 & (n_prom[7]));
+ unsigned int n_rem = 0;
+ unsigned int crc_read = 0;
+ unsigned char n_bit = 0;
+ if(MStype == ms5611::MS5805) {
+ n_prom[0] = (n_prom[0] & 0x0FFF);
+ n_prom[7] = 0;
+ } else {
+ crc_read = n_prom[7];
+ n_prom[7]=(0xFF00 & (n_prom[7]));
+ }
for (int cnt = 0; cnt < 16; cnt++) {
if (cnt%2 == 1) {
n_rem ^= (unsigned short) ((n_prom[cnt>>1]) & 0x00FF);
@@ -232,8 +268,10 @@
}
}
}
- n_rem= (0x000F & (n_rem >> 12));
- n_prom[7]=crc_read;
+ n_rem= ((n_rem >> 12) & 0x000F);
+ if(!(MStype == ms5611::MS5805)) {
+ n_prom[7]=crc_read;
+ }
return (n_rem ^ 0x0);
}
@@ -295,8 +333,16 @@
//********************************************************
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
+ int32_t D1 = 0;
+ int32_t D2 = 0;
+ if(MStype == ms5611::MS5805) {
+ D2 = cmd_adc(MS5611_CMD_ADC_D2 + MS5805_CMD_ADC_8192); // read D2
+ D1 = cmd_adc(MS5611_CMD_ADC_D1 + MS5805_CMD_ADC_8192); // read D1
+ } else {
+ D2 = cmd_adc(MS5611_CMD_ADC_D2 + MS5611_CMD_ADC_4096); // read D2
+ 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);
@@ -304,18 +350,26 @@
int32_t TEMP = 2000 + (int64_t)dT * (int64_t)C[6] / (int64_t)(1 << 23);
if(TEMP < 2000) { // if temperature lower than 20 Celsius
- float T1 = (TEMP - 2000) * (TEMP - 2000);
- int64_t OFF1 = (5 * T1) / 2;
- int64_t SENS1 = (5 * T1) / 4;
-
- if(TEMP < -1500) { // if temperature lower than -15 Celsius
- T1 = (TEMP + 1500) * (TEMP + 1500);
- OFF1 += 7 * T1;
- SENS1 += 11 * T1 / 2;
- }
+ float T1 = 0.0;
+ int64_t OFF1, SENS1 = 0;
+ if(MStype == ms5611::MS5805) {
+ T1 = 11 * ((dT * dT) >> 35);
+ OFF1 = 31 * ((TEMP - 2000) * (TEMP - 2000)) >> 3;
+ SENS1 = 63 * ((TEMP - 2000) * (TEMP - 2000)) >> 5;
+ } else {
+ T1 = (TEMP - 2000) * (TEMP - 2000);
+ OFF1 = (5 * T1) / 2;
+ SENS1 = (5 * T1) / 4;
+
+ if(TEMP < -1500) { // if temperature lower than -15 Celsius
+ T1 = (TEMP + 1500) * (TEMP + 1500);
+ OFF1 += 7 * T1;
+ SENS1 += 11 * T1 / 2;
+ }
+ }
OFF -= OFF1;
SENS -= SENS1;
- T = (float)TEMP / 100;
+ T = ((double)TEMP - T1)/ 100.0;
}
// int64_t P1 = ((((int64_t)D1 * SENS) >> 21) - OFF) >> 15;
P = ((((int64_t)D1 * SENS ) >> 21) - OFF) / (double) (1 << 15) / 100.0;
@@ -376,7 +430,7 @@
//********************************************************
float ms5611::getSeaLevelBaroFT(float known_alt) {
- S = pow(pow((P * INHG), 0.190284) + 0.00001313 * known_alt , 5.2553026) * MB;
+ S = pow(pow((P * MB_INHG_DOUBLE), 0.190284) + 0.00001313 * known_alt , 5.2553026) * INHG_MB_DOUBLE;
return((float)S);
}
@@ -389,6 +443,7 @@
//********************************************************
float ms5611::getSeaLevelBaroM(float known_alt) {
- S = pow(pow((P * INHG), 0.190284) + 0.00001313 * known_alt * FTMETERS , 5.2553026) * MB;
+ S = pow(pow((P * MB_INHG_DOUBLE), 0.190284) + 0.00001313 * known_alt * FEET_METERS , 5.2553026) * INHG_MB_DOUBLE;
return((float)S);
}
+