MS5611Base.h

00001 /*
00002 Copyright (c) 2012, Senio Networks, Inc.
00003 
00004 Permission is hereby granted, free of charge, to any person obtaining a copy
00005 of this software and associated documentation files (the "Software"), to deal
00006 in the Software without restriction, including without limitation the rights
00007 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00008 copies of the Software, and to permit persons to whom the Software is
00009 furnished to do so, subject to the following conditions:
00010 
00011 The above copyright notice and this permission notice shall be included in
00012 all copies or substantial portions of the Software.
00013 
00014 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00015 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00016 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00017 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00018 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00019 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00020 THE SOFTWARE.
00021 */
00022 
00023 /*  MS5611 Modifications
00024 Aerodyne Labs
00025 Matthew Nelson - 2014
00026 This library was originally the MS5607 library.  It has been modified to 
00027 to be used the MS5611 Pressure sensor
00028 ------------
00029 IndNav: Made adaptions, even though, the original works well, problem:
00030 the original has a delay of 8ms from writing to reading (see line 73 in MS5611I2c
00031 Solution: seperate write and read in 2 processes (hene's hint):
00032     - write at end of data_read thread, without waiting
00033     - at beginning of next call, read the data
00034 since there are 2 seperate reading/writings (one for pressure(altitude), one for Temperature -and we need both!)
00035 toggle the 2 processes, this results, that only every 2nd pressure/altitude value is a new value!
00036 
00037 altb 20.2.2020
00038 */
00039 
00040 #ifndef MS5611_BASE_H
00041 #define MS5611_BASE_H
00042 
00043 class MS5611Base {
00044 public:
00045     void printCoefficients() {
00046         printf("%d, %d, %d, %d, %d, %d \r\n", c1, c2, c3, c4, c5, c6);
00047     }
00048 
00049     int getRawPressure() {
00050         return readADC(ADC_D1 | OSR_4096);  // fuer 1024: gibt das 4
00051     }
00052     int getRawPressure_read() {
00053         return readADC_read(ADC_D1 | OSR_4096);  // fuer 1024: gibt das 4
00054     }
00055     int getRawTemperature() {
00056         return readADC(ADC_D2 | OSR_4096);  // fuer 1024: gibt das 20
00057     }
00058     int getRawTemperature_read() {
00059         return readADC_read(ADC_D2 | OSR_4096);  // fuer 1024: gibt das 20
00060     }
00061  
00062     float getTemperature() {
00063         int dT = getRawTemperature() - (c5 << 8);
00064         int temp = 2000 + ((dT * c6) >> 23);
00065         
00066         // 2nd order temperature compensation
00067         if (temp < 2000) {
00068             int t2 = (int64_t) dT * dT >> 31;
00069             temp -= t2;
00070         }
00071 
00072         return float(temp) / 100;
00073     }
00074 
00075     float getPressure() {
00076         int dT = getRawTemperature() - (c5 << 8);
00077         int temp = 2000 + ((dT * c6) >> 23);
00078         int64_t off = ((int64_t) c2 << 16) + ((int64_t) dT * c4 >> 7);
00079         int64_t sens = ((int64_t) c1 << 15) + ((int64_t) dT * c3 >> 8);
00080 
00081         // 2nd order temperature compensation
00082         if (temp < 2000) {
00083             int64_t off2 = (int64_t) 5 * (temp - 2000) * (temp - 2000) >> 1;
00084             int64_t sens2 = (int64_t) 5 * (temp - 2000) * (temp - 2000) >> 2;
00085             if (temp < -1500) {
00086                 off2 += (int64_t) 7 * (temp + 1500) * (temp + 1500);
00087                 sens2 += (int64_t) 11 * (temp + 1500) * (temp + 1500) >> 1;
00088             }
00089             off -= off2;
00090             sens -= sens2;
00091         }
00092 
00093         return float((((int64_t) getRawPressure() * sens >> 21) - off) >> 15);
00094     }
00095     float getPressure(int dT) {
00096 //        int dT = getRawTemperature() - (c5 << 8);
00097         int temp = 2000 + ((dT * c6) >> 23);
00098         int64_t off = ((int64_t) c2 << 16) + ((int64_t) dT * c4 >> 7);
00099         int64_t sens = ((int64_t) c1 << 15) + ((int64_t) dT * c3 >> 8);
00100 
00101         // 2nd order temperature compensation
00102         if (temp < 2000) {
00103             int64_t off2 = (int64_t) 5 * (temp - 2000) * (temp - 2000) >> 1;
00104             int64_t sens2 = (int64_t) 5 * (temp - 2000) * (temp - 2000) >> 2;
00105             if (temp < -1500) {
00106                 off2 += (int64_t) 7 * (temp + 1500) * (temp + 1500);
00107                 sens2 += (int64_t) 11 * (temp + 1500) * (temp + 1500) >> 1;
00108             }
00109             off -= off2;
00110             sens -= sens2;
00111         }
00112 
00113         return float((((int64_t) getRawPressure() * sens >> 21) - off) >> 15);
00114     }
00115 
00116     float getAltitude(int presssure = 0) {
00117         return toAltitude(presssure ? presssure : (int) getPressure());
00118     }
00119 
00120     float getAltitude_toggle_Temp_read(int presssure = 0) {
00121         if(temp_toggle)
00122             {
00123                 dT_keep = getRawTemperature_read() - (c5 << 8);
00124                 temp_toggle = false;
00125             }
00126         else
00127             {
00128                 temp_toggle = true;
00129                 old_alt = toAltitude(presssure ? presssure : (int) getPressure(dT_keep));
00130             }
00131         return old_alt;  
00132     }
00133 
00134     int getAltitude_toggle_Temp_write(int presssure = 0) {
00135         if(temp_toggle)
00136             {
00137                return readADC_write(ADC_D2 | OSR_4096);
00138             }
00139         else
00140             {
00141                 return readADC_write(ADC_D1 | OSR_4096);
00142             }
00143     }
00144     int dT_keep;
00145 protected:
00146     int32_t c1, c2, c3, c4, c5, c6;
00147     bool temp_toggle;
00148     float old_alt;      // get altitude only every 2nd time.
00149     enum {
00150         RESET     = 0x1E,
00151         ADC_READ  = 0x00,
00152         ADC_CONV  = 0x40,
00153         ADC_D1    = 0x00,
00154         ADC_D2    = 0x10,
00155         OSR_256   = 0x00,
00156         OSR_512   = 0x02,
00157         OSR_1024  = 0x04,
00158         OSR_2048  = 0x06,
00159         OSR_4096  = 0x08,
00160         PROM_READ = 0xA0
00161     };
00162 
00163     virtual void writeCommand(int command, int ms = 0) = 0;
00164     virtual int readPROM(int address) = 0;
00165     virtual int readADC(int command) = 0;
00166     virtual int readADC_write(int command) = 0;
00167     virtual int readADC_read(int command) = 0;
00168 
00169     void init() {
00170         writeCommand(RESET, 3);
00171         c1 = readPROM(1);
00172         c2 = readPROM(2);
00173         c3 = readPROM(3);
00174         c4 = readPROM(4);
00175         c5 = readPROM(5);
00176         c6 = readPROM(6);
00177         dT_keep = 2000;
00178         temp_toggle = true;
00179         old_alt = 0.0;
00180     }
00181 
00182     float toAltitude(int pressure) {
00183         // Ref. 29124-AltimeterAppNote1.pdf
00184         const float R = 287.052; // specific gas constant R*/M0
00185         const float g = 9.80665; // standard gravity 
00186         const float t_grad = 0.0065; // gradient of temperature
00187         const float t0 = 273.15 + 15; // temperature at 0 altitude
00188         const float p0 = 101325; // pressure at 0 altitude
00189 
00190         return t0 / t_grad * (1 - exp((t_grad * R / g) * log(pressure / p0)));
00191     }
00192 };
00193 
00194 #endif