diff --git a/src/drivers/MS5611.cpp b/src/drivers/MS5611.cpp
new file mode 100644
index 0000000..847a65d
--- /dev/null
+++ b/src/drivers/MS5611.cpp
@@ -0,0 +1,411 @@
+//!
+//! @file an520_I2C.c,v
+//!
+//! Copyright (c) 2009 MEAS Switzerland
+//!
+//!
+//!
+//! @brief This C code is for starter reference only. It is written for the
+//! MEAS Switzerland MS56xx pressure sensor modules and Atmel Atmega644p
+//! microcontroller.
+//!
+//! @version 1.0 $Id: an520_I2C.c,v 1.0
+//!
+//! @todo
+ 
+#include "mbed.h"
+#include "ms5611.h"
+ 
+double P;                       // compensated pressure value (mB)
+double T;                       // compensated temperature value (degC)
+double A;                       // altitude (ft)
+double S;                       // sea level barometer (mB)
+ 
+//uint32_t C[8];                  //coefficient storage
+ 
+//--------------------------------------------------------------------------------------------------------------------------------------//
+// Constructor and destructor - default to be compatible with legacy m5611 driver
+ 
+ms5611::ms5611(PinName sda, PinName scl)  
+: _i2c(sda, scl)
+, m_calibHeight(0)
+{
+        _i2c.frequency(400000);
+        _i2cWAddr = MS5611_ADDR_W;
+        _i2cRAddr = MS5611_ADDR_R;
+}
+ 
+//--------------------------------------------------------------------------------------------------------------------------------------//
+// Constructor and destructor - new, to allow for user to select i2c address based on CSB pin
+ 
+ms5611::ms5611(PinName sda, PinName scl, CSBpolarity CSBpin)  
+: _i2c(sda, scl)
+, m_calibHeight(0)
+{
+        _i2c.frequency(400000);
+        _i2cWAddr = MS5611_ADDR_W;
+        _i2cRAddr = MS5611_ADDR_R;
+        if(CSBpin == CSBpin_1) {
+            _i2cWAddr -= 2;
+            _i2cRAddr -= 2;
+        
+        }
+}
+ 
+//********************************************************
+//! @brief send I2C start condition and the address byte
+//!
+//! @return 0
+//********************************************************
+ 
+int32_t ms5611::m_i2c_start(bool readMode) {
+    int32_t twst;
+    _i2c.start();
+    if(readMode == true) {
+        twst = m_i2c_write(_i2cRAddr);
+    } else {
+        twst = m_i2c_write(_i2cWAddr);
+    }
+    return(twst);
+}
+ 
+//********************************************************
+//! @brief send I2C stop condition
+//!
+//! @return none
+//********************************************************
+ 
+void ms5611::m_i2c_stop() {
+    _i2c.stop();
+}
+ 
+//********************************************************
+//! @brief send I2C stop condition
+//!
+//! @return remote ack status
+//********************************************************
+ 
+uint8_t ms5611::m_i2c_write(uint8_t data) {
+    uint8_t twst = _i2c.write(data);
+    return(twst);
+}
+ 
+//********************************************************
+//! @brief read I2C byte with acknowledgment
+//!
+//! @return read byte
+//********************************************************
+ 
+uint8_t ms5611::m_i2c_readAck() {
+    uint8_t twst = _i2c.read(1);
+    return(twst);
+}
+ 
+//********************************************************
+//! @brief read I2C byte without acknowledgment
+//!
+//! @return read byte
+//********************************************************
+ 
+uint8_t ms5611::m_i2c_readNak() {
+    uint8_t twst = _i2c.read(0);
+    return(twst);
+}
+ 
+//********************************************************
+//! @brief send command using I2C hardware interface
+//!
+//! @return none
+//********************************************************
+ 
+void ms5611::m_i2c_send(uint8_t cmd) {
+    uint8_t ret;
+    ret = m_i2c_start(false);
+    if(!(ret)) {
+        m_i2c_stop();
+    } else {
+        ret = m_i2c_write(cmd);
+        m_i2c_stop();
+    }
+}
+ 
+//********************************************************
+//! @brief send reset sequence
+//!
+//! @return none
+//********************************************************
+ 
+void ms5611::cmd_reset() {
+#if defined  MS5611i2cLOWLEVEL
+    m_i2c_send(MS5611_CMD_RESET);
+#else
+    cobuf[0] = MS5611_CMD_RESET;
+    _i2c.write(_i2cWAddr, cobuf, 1, false);
+#endif
+    wait_ms(4);
+    loadCoefs();
+}
+ 
+//********************************************************
+//! @brief preform adc conversion
+//!
+//! @return 24bit result
+//********************************************************
+ 
+uint64_t ms5611::cmd_adc(uint8_t cmd) {
+    uint64_t temp = 0;
+#if defined  MS5611i2cLOWLEVEL
+    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);
+#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;
+    }
+#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();
+#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;
+}
+ 
+//********************************************************
+//! @brief Read calibration coefficients
+//!
+//! @return coefficient
+//********************************************************
+ 
+uint32_t ms5611::cmd_prom(uint8_t coef_num) {
+    uint32_t rC = 0;
+#if defined  MS5611i2cLOWLEVEL
+    m_i2c_send(MS5611_CMD_PROM_RD + coef_num * 2); // send PROM READ command
+    m_i2c_start(true);
+    rC = m_i2c_readAck();
+    rC = (rC << 8) | m_i2c_readNak();
+    m_i2c_stop();
+#else
+    cobuf[0] = 0;
+    cobuf[1] = 0;
+    cobuf[0] = MS5611_CMD_PROM_RD + coef_num * 2;
+    _i2c.write(_i2cWAddr, cobuf, 1, true);
+    cobuf[0] = 0;
+    _i2c.read(_i2cRAddr, cobuf, 2, false);
+    rC = (cobuf[0] << 8) | cobuf[1];
+#endif
+    return rC;
+}
+ 
+//********************************************************
+//! @brief calculate the CRC code
+//!
+//! @return crc code
+//********************************************************
+ 
+uint8_t ms5611::crc4(uint32_t n_prom[]) {
+    uint32_t n_rem;
+    uint32_t crc_read;
+    uint8_t n_bit;
+    n_rem = 0x00;
+    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 ^= (uint16_t) ((n_prom[cnt>>1]) & 0x00FF);
+            } else {
+                n_rem ^= (uint16_t) (n_prom[cnt>>1]>>8);
+            }
+            for (n_bit = 8; n_bit > 0; n_bit--) {
+                if (n_rem & (0x8000)) {
+                    n_rem = (n_rem << 1) ^ 0x3000;
+                } else {
+                    n_rem = (n_rem << 1);
+                }
+            }
+        }
+    n_rem= (0x000F & (n_rem >> 12));
+    n_prom[7]=crc_read;
+    return (n_rem ^ 0x0);
+}
+ 
+/*
+The CRC code is calculated and written in factory with the LSB byte in the prom n_prom[7] set to 0x00 (see
+Coefficient table below). It is thus important to clear those bytes from the calculation buffer before proceeding
+with the CRC calculation itself:
+n_prom[7]=(0xFF00 & (n_prom[7])); //CRC byte is replaced by 0
+As a simple test of the CRC code, the following coefficient table could be used:
+uint32_t nprom[] = {0x3132,0x3334,0x3536,0x3738,0x3940,0x4142,0x4344,0x4500};
+the resulting calculated CRC should be 0xB.
+ 
+DB  15  14  13  12  11  10  9   8   7   6   5   4   3   2   1   0 
+Addr
+0               16 bit reserved for manufacturer
+1               Coefficient 1 (16 bit unsigned)
+2               Coefficient 2 (16 bit unsigned)
+3               Coefficient 3 (16 bit unsigned)
+4               Coefficient 4 (16 bit unsigned)
+5               Coefficient 5 (16 bit unsigned)
+6               Coefficient 6 (16 bit unsigned)
+7                                   0   0   0   0     CRC(0x0)
+*/    
+/*   
+    //Returns 0x0b as per AP520_004 
+    C[0] = 0x3132;
+    C[1] = 0x3334;
+    C[2] = 0x3536;
+    C[3] = 0x3738;
+    C[4] = 0x3940;
+    C[5] = 0x4142;
+    C[6] = 0x4344;
+    C[7] = 0x4546;
+    n_crc = ms.crc4(C); // calculate the CRC
+    pc.printf("testing CRC: 0x%x\n", n_crc);
+*/
+ 
+//********************************************************
+//! @brief load all calibration coefficients
+//!
+//! @return none
+//********************************************************
+extern RawSerial pc;
+ 
+void ms5611::loadCoefs() {
+    //pc.printf("\r\n   - Coefs: ");
+    for (int i = 0; i < 8; i++){ 
+        wait_ms(50);
+        C[i] = cmd_prom(i);
+        //pc.printf("%04x ", C[i]);
+    }
+    //pc.printf("\r\n");
+    //uint8_t n_crc = crc4(C);
+}
+ 
+//********************************************************
+//! @brief calculate temperature and pressure
+//!
+//! @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;
+ 
+    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;
+    }
+    P = (double)(((((int64_t)D1 * SENS ) >> 21) - OFF) / (double) (1 << 15)) / 100.0;
+}
+ 
+//********************************************************
+//! @brief calculate temperature
+//!
+//! @return double temperature degC
+//********************************************************  
+ 
+double ms5611::calcTemp() {
+    calcPT();
+    return(T);
+} 
+ 
+//********************************************************
+//! @brief calculate pressure
+//!
+//! @return double barometric pressure millibar
+//********************************************************  
+ 
+double ms5611::calcPressure() {
+    calcPT();
+    return(P);
+} 
+ 
+//********************************************************
+//! @brief get pressure, no calculation
+//!
+//! @return double barometric pressure millibar
+//********************************************************  
+ 
+double ms5611::getPressure() {
+    calcPT();
+    return(P);
+} 
+ 
+//********************************************************
+//! @brief get altitude from known sea level barometer, 
+//! @      no pre-pressure calculation
+//!
+//! @enter float sea level barometer
+//! @return float altitude in feet
+//********************************************************  
+ 
+float ms5611::getAltitudeFT(float sea_pressure) {
+    A = (1 - (pow((P / (double)sea_pressure), 0.190284))) * 145366.45;
+    return((float)A);
+} 
+
+float ms5611::getAltitude(float sea_pressure) {
+    return ms5611::getAltitudeFT(sea_pressure) * 0.3048 - m_calibHeight;
+}
+
+void ms5611::resetHeight()
+{
+    m_calibHeight = ms5611::getAltitude();
+}
+ 
+//********************************************************
+//! @brief get sea level pressure from known altitude(ft), 
+//! @      no pre-pressure calculation
+//!
+//! @enter float known altitude in feet
+//! @return float seal level barometer in mb
+//********************************************************  
+ 
+float ms5611::getSeaLevelBaroFT(float known_alt) {
+    S = pow(pow((P * INHG), 0.190284) + 0.00001313 * known_alt , 5.2553026) * MB;
+    return((float)S);
+} 
+ 
+//********************************************************
+//! @brief get sea level pressure from known altitude(m), 
+//! @      no pre-pressure calculation
+//!
+//! @enter float known altitude in meters
+//! @return float seal level barometer in mb
+//********************************************************  
+ 
+float ms5611::getSeaLevelBaroM(float known_alt) {
+    S = pow(pow((P * INHG), 0.190284) + 0.00001313 * known_alt * FTMETERS , 5.2553026) * MB;
+    return((float)S);
+} 
+ 
\ No newline at end of file
diff --git a/src/drivers/MS5611.h b/src/drivers/MS5611.h
new file mode 100644
index 0000000..3e4fcba
--- /dev/null
+++ b/src/drivers/MS5611.h
@@ -0,0 +1,209 @@
+#ifndef MS5611_H
+#define MS5611_H
+ 
+#include "mbed.h"
+ 
+//#define MS5611i2cLOWLEVEL   1               //if the use of low-level I2C routines is needed
+//#warning "MS5611 using low level I2C routines"
+ 
+#define SEA_PRESS   1013.25                 //default sea level pressure level in mb
+#define KNOWNALT    327.0                   //default known altitude, 5200 Franklin Dr., 94588
+#define INHG        0.02952998751           //convert mb to in/Hg constant
+#define MB          33.8638815              //convert in/Hg to mb constant
+#define FTMETERS    0.3048                  //convert feet to meters
+ 
+ 
+/** Software routines to access the Measurement Specialties' MS5611-01BA03 
+ *  Variometer Module using the I2C bus option.  The MS5611 is a 24 bit 
+ *  temperature and pressure transducer for high accuracy Barometer and 
+ *  Altimeter applications.  It also includes compensation coefficients
+ *  stored within the device. 
+ * 
+ *  Code adapted from Measurement Specialties:
+ *  "AN520 C-code example for MS56xx, MS57xx (except analog sensor), and 
+ *  MS58xx series pressure sensors"
+ *
+ *  Note: AN520 has not been updated for use with the MS5611.  Changes
+ *  were necessary to "calcPT()" in order to correct scaling of 
+ *  pressure readings.
+ * 
+ *  Features:
+ *          Altitude resolution to 10cm
+ *          Fast conversion down to 1 ms
+ *          Low power, 1 μA (standby < 0.15 μA)
+ *          QFN package 5.0 x 3.0 x 1.0 mm^3
+ *          Supply voltage 1.8 to 3.6 V
+ *          Integrated digital pressure sensor (24 bit DeltaSigma ADC)
+ *          Operating range: 10 to 1200 mbar, -40 to +85 °C
+ *          I2C and SPI interface up to 20 MHz
+ *          No external components (Internal oscillator)
+ *          Excellent long term stability
+ *
+ * @code
+ * #include "mbed.h"
+ * #include "ms5611.h" 
+ *
+ * //ms5611 ms(p9, p10);                        // i2c pins used
+ * ms5611 ms(p9, p10, ms5611::CSBpin_0);      // NEW!! with rev 7. User can set polarity of CSB pin
+ * //ms5611 ms(p9, p10, ms5611::CSBpin_1);
+ *
+ * Serial pc(USBTX, USBRX);                   // local terminal interface
+ *
+ *
+ * int main (void) {
+ *     pc.baud(921600);                        // set up USB serial speed
+ *
+ *     // set up the ms5611
+ *     pc.printf("\n\nInitializing the MS5611..\n");
+ *     ms.cmd_reset();
+ *     pc.printf("Ready\n");
+ *
+ *     while(1) {
+ *         double Temp = ms.calcTemp();                         //calculate press and temp, then returns current temperature in degC
+ *         double Press = ms.calcPressure();                    //calculate press and temp, then returns current pressure in mb
+ *         double GetPress = ms.getPressure();                  //returns current pressure in mb. Does no calculations.  Ususally done after calcTemp()
+ *         double Altitude = ms.getAltitudeFT(1013.25);         //enter pressure at sea level in mb, returns altitude in feet
+ *         double PressSeaLvlFT = ms.getSeaLevelBaroFT(327.2);  //enter known altitude in feet, returns sea level pressure in mb
+ *         double PressSeaLvlM = ms.getAltitudeFT(99.73);       //enter known altitude in meters, returns seal level pressure in mb
+ *
+ *         pc.printf("Temp: %.2f degC\n", Temp);    
+ *         pc.printf("Barometer: %.1f mB  %.3f in/Hg\n", Press, Press * 0.0295301);
+ *         pc.printf("Alt: %.1f ft\n", Altitude);
+ *         pc.printf("Sea_Lvl: %.1f ft   %.2f m\n", PressSeaLvlFT, PressSeaLvlM);
+ *         wait(2.0);
+ *     }
+ * }
+ *
+ * @endcode
+ */
+ 
+//_____ M A C R O S
+ 
+#define MS5611_ADDR_W 0xEE // Module address write mode (CSBpin = 0);
+#define MS5611_ADDR_R 0xEF // Module address read mode
+#define MS5611_CMD_RESET 0x1E // ADC reset command
+#define MS5611_CMD_ADC_READ 0x00 // ADC read command
+#define MS5611_CMD_ADC_CONV 0x40 // ADC conversion command
+#define MS5611_CMD_ADC_D1 0x00 // ADC D1 conversion
+#define MS5611_CMD_ADC_D2 0x10 // ADC D2 conversion
+#define MS5611_CMD_ADC_256 0x00 // ADC OSR=256
+#define MS5611_CMD_ADC_512 0x02 // ADC OSR=512
+#define MS5611_CMD_ADC_1024 0x04 // ADC OSR=1024
+#define MS5611_CMD_ADC_2048 0x06 // ADC OSR=2048
+#define MS5611_CMD_ADC_4096 0x08 // ADC OSR=4096
+#define MS5611_CMD_PROM_RD 0xA0 // Prom read command
+ 
+    /** Create ms5611 controller class
+     *
+     * @param ms5611 class
+     *
+     */
+class ms5611 {
+ 
+public:
+    enum CSBpolarity {
+        CSBpin_0,  //CSB pin is grounded, I2C address is 0xEE and 0xEF
+        CSBpin_1,  //CSB pin is tied to Vdd, I2C address is 0xEC and 0xED
+    };
+    uint32_t C[8];
+    /** Create a MS5611 object using the specified I2C object
+     *   - User fixed I2C address 0xEE, CSB pin = 0
+     *   - This is the default legacy constructor
+     * @param sda - mbed I2C interface pin
+     * @param scl - mbed I2C interface pin
+     */
+    ms5611(PinName sda, PinName scl);
+    /** Create a MS5611 object using the specified I2C object
+     *   - User defined use of the CSB pin
+     *   - CSB pin = 0, user set I2C address to 0xEE
+     *   - CSB pin = 1, user set I2C address to 0xEC 
+     * @param sda - mbed I2C interface pin
+     * @param scl - mbed I2C interface pin
+     * @param ms5611::CSBpin_0 - CSB pin tied to ground
+     * @param ms5611::CSBpin_1 - CSB pin tied to VDD
+     */
+    ms5611(PinName sda, PinName scl, CSBpolarity CSBpin);
+    /** Initialize the MS5611 and set up the coefficients
+     *    First - reset the MS5611
+     *    Second - load coefficient values from the MS5611 PROM
+     *    Third  - calculate coefficient checksum
+     *  This routine only needs to be run once at boot up
+     *
+     * @param NONE
+     */
+
+    void cmd_reset();
+    /** Calculate and return compensated temperature
+     *    Returns double temperature in degC
+     *
+     * @param NONE
+     */
+
+    double calcTemp();
+    /** Calculate and return compensated barometric pressure
+     *    Returns double pressure in millibars
+     *
+     * @param NONE
+     */
+    double calcPressure();
+    /** Return compensated barometric pressure
+     *    Returns double pressure in millibars
+     *    DOES NOT RE-CALCULATE FIRST!!!
+     *    Saves time if you calcTemp(); first
+     *
+     * @param NONE
+     */
+    double getPressure();
+
+    /** Calculate and returns altitude in feet
+     *    Returns float altitude in feet
+     *
+     * @param float known pressure (mB) at sea level
+     */
+    float getAltitudeFT(float sea_pressure);
+
+    float getAltitude(float sea_pressure = 1013.25);
+
+    void resetHeight();
+
+    /** Calculate and returns sea level baro
+     *    Returns float seal level barometer in feet
+     *
+     * @param float known altitude in feet
+     */
+    float getSeaLevelBaroFT(float known_alt);
+
+    /** Calculate and returns sea level baro
+     *    Returns float seal level barometer in meters
+     *
+     * @param float known altitude in meters
+     */
+    float getSeaLevelBaroM(float known_alt);
+    
+private:
+#if not defined  MS5611i2cLOWLEVEL
+    char cobuf[3];
+#endif
+
+    double m_calibHeight;
+    uint8_t _i2cWAddr;
+    uint8_t _i2cRAddr;
+    int32_t m_i2c_start(bool readMode);
+    void m_i2c_stop();
+    uint8_t m_i2c_write(uint8_t data);
+    uint8_t m_i2c_readAck();
+    uint8_t m_i2c_readNak();
+    void m_i2c_send(uint8_t cmd);
+    void loadCoefs();
+    uint64_t cmd_adc(uint8_t cmd);
+    uint32_t cmd_prom(uint8_t coef_num);
+    uint8_t crc4(uint32_t  n_prom[]);
+    void calcPT();
+    uint32_t PTbuffer[8];       // calibration coefficients
+  
+protected:
+    I2C     _i2c;
+    
+ 
+}; 
+#endif
\ No newline at end of file