Revision 0:369a1b265ddb, committed 2019-03-21

Comitter:

ThunderSoft

Date:

Thu Mar 21 09:03:32 2019 +0000

Commit message:

Add code for FRDM_FXS_MULTI_B

Changed in this revision

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FRDM_FXS_MULTI_B/FXAS21000.cpp	Thu Mar 21 09:03:32 2019 +0000
@@ -0,0 +1,71 @@
+ #include "FXAS21000.h"
+ 
+FXAS21000::FXAS21000(PinName sda, PinName scl) : _i2c(sda, scl) {
+ 
+    begin();
+}
+              
+void FXAS21000::RegRead( char reg, char * d, int len)
+{
+    char cmd[1];
+    cmd[0] = reg;
+    char i2c_addr = FXAS21000_SLAVE_ADDR;
+    _i2c.write( i2c_addr, cmd, 1, true);
+    _i2c.read ( i2c_addr, d, len);
+}
+
+void FXAS21000::begin(void)
+{
+    char data[2];
+    // write 0000 1000 = 0x08 to gyro control register 1 to place FXAS21000 into
+    // standby
+    // [7-1] = 0000 000
+    // [0]: active=0
+    data[0] = FXAS21000_CTRL_REG1;
+    data[1] = 0x08;
+    _i2c.write( FXAS21000_SLAVE_ADDR, data, 2);
+    
+    // write 0001 1011 to CRTL_REG0 register
+    data[0] = FXAS21000_CTRL_REG0;
+    data[1] = 0x1B;
+    _i2c.write( FXAS21000_SLAVE_ADDR, data, 2);
+
+    // write 0000 1001 to gyro control register 1
+    data[0] = FXAS21000_CTRL_REG1;
+    data[1] = 0x0A;
+    _i2c.write( FXAS21000_SLAVE_ADDR, data, 2);
+}
+
+char FXAS21000::getWhoAmI(void)
+{
+    char d;
+    RegRead( FXAS21000_WHOAMI, &d, 1);
+    return(d);
+}
+
+void FXAS21000::ReadXYZ(float * a)
+{
+    char d[7];
+    int16_t t[6];
+
+    RegRead( FXAS21000_STATUS, d, 7);
+    t[0] = ((d[1] * 256) + ((unsigned short) d[2]));
+    t[1] = ((d[3] * 256) + ((unsigned short) d[4]));
+    t[2] = ((d[5] * 256) + ((unsigned short) d[6]));
+    //printf("%X\r\n", (int) d[0]);
+
+    a[0] = (float) t[0] * 0.003125;
+    a[1] = (float) t[1] * 0.003125;
+    a[2] = (float) t[2] * 0.003125;
+  
+}
+
+void FXAS21000::ReadXYZraw(int16_t * t)
+{
+    char d[7];
+
+    RegRead( FXAS21000_STATUS, d, 7);
+    t[0] = ((d[1] * 256) + ((unsigned short) d[2]));
+    t[1] = ((d[3] * 256) + ((unsigned short) d[4]));
+    t[2] = ((d[5] * 256) + ((unsigned short) d[6]));
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FRDM_FXS_MULTI_B/FXAS21000.h	Thu Mar 21 09:03:32 2019 +0000
@@ -0,0 +1,86 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef FXAS21000_H
+#define FXAS21000_H
+
+#include "mbed.h"
+
+
+/**
+ * MMA8652 Slave Address
+ */
+
+#define FXAS21000_SLAVE_ADDR (0x20 << 1)       //(0x20)
+
+// MMA8652 internal register addresses
+#define FXAS21000_STATUS 0x00
+#define FXAS21000_WHOAMI 0x0C
+#define FXAS21000_XYZ_DATA_CFG 0x0E
+#define FXAS21000_CTRL_REG0 0x0D
+#define FXAS21000_CTRL_REG1 0x13
+#define FXAS21000_WHOAMI_VAL 0xD1
+
+/**
+ * FXAS21000 gyroscope on I2C
+ */
+class FXAS21000
+{
+public:
+    /**
+     * FXAS21000 constructor
+     *
+     * @param sda SDA pin
+     * @param sdl SCL pin
+     */
+    FXAS21000(PinName sda, PinName scl);
+ 
+    /**
+     * Get the Gyro values as floating point degrees / second
+     *
+     * @param floating point array where the results will be placed
+     */
+    void ReadXYZ(float * a);
+
+    /**
+     * Get the Gyro values as signed 16 bit value
+     *
+     * @param int16_t point array where the results will be placed
+     */
+    void ReadXYZraw(int16_t * t);
+   
+    /**
+     * Get the value of the WHO_AM_I register
+     *
+     * @returns DEVICE_ID value == 0xD1
+     */
+    char getWhoAmI(void);
+    
+private:
+    I2C _i2c;
+
+    /**
+     * Set the device in active mode
+     */
+    void begin( void);
+    
+    void RegRead( char reg, char * d, int len);
+};
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FRDM_FXS_MULTI_B/FXLS8471Q.cpp	Thu Mar 21 09:03:32 2019 +0000
@@ -0,0 +1,130 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+
+#include "FXLS8471Q.h"
+#define UINT14_MAX        16383
+
+FXLS8471Q::FXLS8471Q(PinName mosi, PinName miso, PinName sck, PinName cs) : _spi(mosi, miso, sck), _spi_cs(cs)
+{
+    _spi_cs = 1;
+    wait(0.1);
+    begin();
+}
+
+void FXLS8471Q::RegWrite( int reg, int *d, int len)
+{
+    int ob[2];
+    int c = 0;
+    ob[1] = 0;
+    ob[0] = reg + 128;
+    _spi_cs = 0;
+    _spi.write(ob[0]);
+    _spi.write(ob[1]);
+    while(c < len) _spi.write(d[c++]);
+    _spi_cs = 1;
+}
+
+void FXLS8471Q::RegRead( int reg, int *d, int len)
+{
+    int ob[2];
+    int c = 0;
+    ob[0] = reg;
+    ob[1] = 0;
+    _spi_cs = 0;
+    _spi.write(ob[0]);
+    _spi.write(ob[1]);
+    while(c < len) d[c++] = _spi.write(0x00);
+    _spi_cs = 1;
+}
+
+char FXLS8471Q::getWhoAmI(void)
+{
+    int d;
+    RegRead( FXLS8471Q_WHOAMI, &d, 1);
+    return((char) d);
+}
+
+
+void FXLS8471Q::begin(void)
+{
+    int databyte;
+
+    // write 0000 0000 = 0x00 to accelerometer control register 1 to place FXLS8471Q into
+    // standby
+    // [7-1] = 0000 000
+    // [0]: active=0
+    databyte = 0x00;
+    RegWrite(FXLS8471Q_CTRL_REG1, &databyte, 1);
+
+    // write 0000 0001= 0x01 to XYZ_DATA_CFG register
+    // [7]: reserved
+    // [6]: reserved
+    // [5]: reserved
+    // [4]: hpf_out=0
+    // [3]: reserved
+    // [2]: reserved
+    // [1-0]: fs=00 for accelerometer range of +/-2g with 0.244mg/LSB
+    databyte = 0x00;
+    RegWrite(FXLS8471Q_XYZ_DATA_CFG, &databyte, 1);
+
+    // write 0001 0101b = 0x15 to accelerometer control register 1
+    // [7-6]: aslp_rate=00
+    // [5-3]: dr=010 for 200Hz data rate
+    // [2]: lnoise=1 for low noise mode
+    // [1]: f_read=0 for normal 16 bit reads
+    // [0]: active=1 to take the part out of standby and enable sampling
+    databyte = 0x15;
+    RegWrite(FXLS8471Q_CTRL_REG1, &databyte, 1);
+}
+
+void FXLS8471Q::ReadXYZ(float * fdata)
+{
+    int raw[7];
+    int16_t ix, iy, iz;
+    RegRead(FXLS8471Q_STATUS, raw, 7);
+
+    ix = ((raw[1] * 256) + raw[2]);// / 4;
+    iy = ((raw[3] * 256) + raw[4]);// / 4;
+    iz = ((raw[5] * 256) + raw[6]);// / 4;
+    fdata[0] = ((float) ix) / 16384.0;
+    fdata[1] = ((float) iy) / 16384.0;
+    fdata[2] = ((float) iz) / 16384.0;
+}
+
+void FXLS8471Q::ReadXYZraw(int16_t * d)
+{
+    int res[6];
+    int16_t acc;
+    RegRead(FXLS8471Q_OUT_X_MSB, res, 6);
+
+    acc = (res[0] << 6) | (res[1] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    d[0] = acc;
+    acc = (res[2] << 6) | (res[3] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    d[1] = acc;
+    acc = (res[4] << 6) | (res[5] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    d[2] = acc;
+}
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FRDM_FXS_MULTI_B/FXLS8471Q.h	Thu Mar 21 09:03:32 2019 +0000
@@ -0,0 +1,90 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+
+#ifndef FXLS8471Q_H
+#define FXLS8471Q_H
+
+#include "mbed.h"
+
+// FXLS8471Q internal register addresses
+
+#define FXLS8471Q_STATUS 0x00
+#define FXLS8471Q_OUT_X_MSB 0x01
+#define FXLS8471Q_WHOAMI 0x0D
+#define FXLS8471Q_XYZ_DATA_CFG 0x0E
+#define FXLS8471Q_CTRL_REG1 0x2A
+#define FXLS8471Q_WHOAMI_VAL 0x6A
+
+// MMA8652 Slave Address (For I2C)
+//#define FXLS8471Q_ADDRESS 0x1E
+
+/**
+ * FXLS8471Q Xtrinsic accelerometer on SPI
+ */
+class FXLS8471Q
+{
+public:
+    /**
+    * FXLS8471Q constructor
+    *
+    * @param mosi MOSI pin
+    * @param miso MISO pin
+    * @param scl  SCL  pin
+    * @param cs   Device Chip Select pin
+    */
+    FXLS8471Q(PinName mosi, PinName miso, PinName scl, PinName cs);
+
+    /**
+     * Get XYZ axis acceleration in G's as floating point
+     *
+     * @param res array where acceleration data will be stored
+     */
+    void ReadXYZ(float * a);
+
+    /**
+     * Get XYZ axis acceleration as signed 16 bit values
+     *
+     * @param res array where acceleration data will be stored
+     */
+    void ReadXYZraw(int16_t * d);
+
+    /**
+     * Get the value of the WHO_AM_I register
+     *
+     * @returns WHO_AM_I value
+     */
+    char getWhoAmI(void);
+
+private:
+    SPI _spi;
+
+    DigitalOut _spi_cs;
+
+    /**
+     * Set the device in active mode
+     */
+    void begin( void);
+
+    void RegWrite( int reg, int * d, int len);
+
+    void RegRead( int reg, int * d, int len);
+};
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FRDM_FXS_MULTI_B/FXOS8700Q.cpp	Thu Mar 21 09:03:32 2019 +0000
@@ -0,0 +1,309 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "FXOS8700Q.h"
+#define UINT14_MAX        16383
+
+uint8_t SensorBuffer[12];
+int     MagReadStatus;
+
+FXOS8700Q_acc::FXOS8700Q_acc(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) {
+    // activate the peripheral
+    uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00};
+    m_i2c.frequency(400000);
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_M_CTRL_REG1;
+    data[1] = 0x1F;
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_M_CTRL_REG2;
+    data[1] = 0x20;
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_XYZ_DATA_CFG;
+    data[1] = 0x00;
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_CTRL_REG1;
+    data[1] = 0x1C;
+    writeRegs(data, 2);
+    MagReadStatus = 0;
+}
+
+FXOS8700Q_acc::~FXOS8700Q_acc() { }
+
+void FXOS8700Q_acc::enable(void) {
+    uint8_t data[2];
+    readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
+    data[1] |= 0x01;
+    data[0] = FXOS8700Q_CTRL_REG1;
+    writeRegs(data, 2);
+    MagReadStatus = 0;
+}
+
+void FXOS8700Q_acc::disable(void) {
+    uint8_t data[2];
+    readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
+    data[1] &= 0xFE;
+    data[0] = FXOS8700Q_CTRL_REG1;
+    writeRegs(data, 2);
+    MagReadStatus = 0;
+}
+
+
+
+uint32_t FXOS8700Q_acc::whoAmI() {
+    uint8_t who_am_i = 0;
+    readRegs(FXOS8700Q_WHOAMI, &who_am_i, 1);
+    return (uint32_t) who_am_i;
+}
+
+uint32_t FXOS8700Q_acc::dataReady(void) {
+    uint8_t stat = 0;
+    readRegs(FXOS8700Q_STATUS, &stat, 1);
+    return (uint32_t) stat;
+}
+
+uint32_t FXOS8700Q_acc::sampleRate(uint32_t f) {
+    return(50); // for now sample rate is fixed at 50Hz
+}
+
+void FXOS8700Q_acc::getX(float * x) {
+    *x = (float(getAccAxis(FXOS8700Q_OUT_X_MSB))/4096.0f);
+}
+
+void FXOS8700Q_acc::getY(float * y) {
+    *y = (float(getAccAxis(FXOS8700Q_OUT_Y_MSB))/4096.0f);
+}
+
+void FXOS8700Q_acc::getZ(float * z) {
+    *z = (float(getAccAxis(FXOS8700Q_OUT_Z_MSB))/4096.0f);
+}
+
+void FXOS8700Q_acc::getX(int16_t * d) {
+    *d = getAccAxis(FXOS8700Q_OUT_X_MSB);
+}
+
+void FXOS8700Q_acc::getY(int16_t * d) {
+    *d = getAccAxis(FXOS8700Q_OUT_Y_MSB);
+}
+
+void FXOS8700Q_acc::getZ(int16_t * d) {
+    *d = getAccAxis(FXOS8700Q_OUT_Z_MSB);
+}
+
+
+void FXOS8700Q_acc::getAxis(MotionSensorDataUnits &data) {
+    int16_t acc, t[3];
+
+   readRegs(FXOS8700Q_OUT_X_MSB, SensorBuffer, 12);
+
+    acc = (SensorBuffer[0] << 6) | (SensorBuffer[1] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    t[0] = acc;
+    acc = (SensorBuffer[2] << 6) | (SensorBuffer[3] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    t[1] = acc;
+    acc = (SensorBuffer[4] << 6) | (SensorBuffer[5] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    t[2] = acc;
+    data.x = ((float) t[0]) / 4096.0f;
+    data.y = ((float) t[1]) / 4096.0f;
+    data.z = ((float) t[2]) / 4096.0f;
+    MagReadStatus = 1;
+}
+
+
+void FXOS8700Q_acc::getAxis(MotionSensorDataCounts &data) {
+    int16_t acc;
+    readRegs(FXOS8700Q_OUT_X_MSB, SensorBuffer, 12);
+
+    acc = (SensorBuffer[0] << 6) | (SensorBuffer[1] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    data.x = acc;
+    acc = (SensorBuffer[2] << 6) | (SensorBuffer[3] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    data.y = acc;
+    acc = (SensorBuffer[4] << 6) | (SensorBuffer[5] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    data.z = acc;
+    MagReadStatus = 1;
+}
+
+void FXOS8700Q_acc::readRegs(int addr, uint8_t * data, int len) {
+    char t[1] = {addr};
+    m_i2c.write(m_addr, t, 1,true);
+    m_i2c.read(m_addr, (char *)data, len);
+}
+
+void FXOS8700Q_acc::writeRegs(uint8_t * data, int len) {
+    m_i2c.write(m_addr, (char *)data, len);
+}
+
+
+int16_t FXOS8700Q_acc::getAccAxis(uint8_t addr) {
+    int16_t acc;
+    uint8_t res[2];
+    readRegs(addr, res, 2);
+
+    acc = (res[0] << 6) | (res[1] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+
+    return acc;
+}
+
+
+
+FXOS8700Q_mag::FXOS8700Q_mag(PinName sda, PinName scl, int addr) : m_i2c(sda, scl), m_addr(addr) {
+    // activate the peripheral
+    uint8_t data[2] = {FXOS8700Q_CTRL_REG1, 0x00};
+    m_i2c.frequency(400000);
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_M_CTRL_REG1;
+    data[1] = 0x1F;
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_M_CTRL_REG2;
+    data[1] = 0x20;
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_XYZ_DATA_CFG;
+    data[1] = 0x00;
+    writeRegs(data, 2);
+    data[0] = FXOS8700Q_CTRL_REG1;
+    data[1] = 0x18;//0x1D;
+    writeRegs(data, 2);
+    MagReadStatus = 0;
+}
+
+FXOS8700Q_mag::~FXOS8700Q_mag() { }
+
+void FXOS8700Q_mag::enable(void) {
+    uint8_t data[2];
+    readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
+    data[1] |= 0x01;
+    data[0] = FXOS8700Q_CTRL_REG1;
+    writeRegs(data, 2);
+    MagReadStatus = 0;
+}
+
+void FXOS8700Q_mag::disable(void) {
+    uint8_t data[2];
+    readRegs( FXOS8700Q_CTRL_REG1, &data[1], 1);
+    data[1] &= 0xFE;
+    data[0] = FXOS8700Q_CTRL_REG1;
+    writeRegs(data, 2);
+    MagReadStatus = 0;
+}
+
+
+
+uint32_t FXOS8700Q_mag::whoAmI() {
+    uint8_t who_am_i = 0;
+    readRegs(FXOS8700Q_WHOAMI, &who_am_i, 1);
+    return (uint32_t) who_am_i;
+}
+
+uint32_t FXOS8700Q_mag::dataReady(void) {
+    uint8_t stat = 0;
+    readRegs(FXOS8700Q_STATUS, &stat, 1);
+    return (uint32_t) stat;
+}
+
+uint32_t FXOS8700Q_mag::sampleRate(uint32_t f) {
+    return(50); // for now sample rate is fixed at 50Hz
+}
+
+void FXOS8700Q_mag::getX(float * x) {
+    *x = (float(getAccAxis(FXOS8700Q_M_OUT_X_MSB)) * 0.1f);
+}
+
+void FXOS8700Q_mag::getY(float * y) {
+    *y = (float(getAccAxis(FXOS8700Q_M_OUT_Y_MSB)) * 0.1f);
+}
+
+void FXOS8700Q_mag::getZ(float * z) {
+    *z = (float(getAccAxis(FXOS8700Q_M_OUT_Z_MSB)) * 0.1f);
+}
+
+void FXOS8700Q_mag::getX(int16_t * d) {
+    *d = getAccAxis(FXOS8700Q_M_OUT_X_MSB);
+}
+
+void FXOS8700Q_mag::getY(int16_t * d) {
+    *d = getAccAxis(FXOS8700Q_M_OUT_Y_MSB);
+}
+
+void FXOS8700Q_mag::getZ(int16_t * d) {
+    *d = getAccAxis(FXOS8700Q_M_OUT_Z_MSB);
+}
+
+
+void FXOS8700Q_mag::getAxis(MotionSensorDataUnits &data) {
+    int16_t t[3];
+    uint8_t res[6];
+    
+   if(MagReadStatus) {
+        t[0] = (SensorBuffer[6] << 8) | SensorBuffer[7];
+        t[1] = (SensorBuffer[8] << 8) | SensorBuffer[9];
+        t[2] = (SensorBuffer[10] << 8) | SensorBuffer[11];        
+        } else {
+        readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6);
+        t[0] = (res[0] << 8) | res[1];
+        t[1] = (res[2] << 8) | res[3];
+        t[2] = (res[4] << 8) | res[5];
+        }
+    data.x = ((float) t[0]) * 0.1f;
+    data.y = ((float) t[1]) * 0.1f;
+    data.z = ((float) t[2]) * 0.1f;
+}
+
+
+void FXOS8700Q_mag::getAxis(MotionSensorDataCounts &data) {
+
+    uint8_t res[6];
+    readRegs(FXOS8700Q_M_OUT_X_MSB, res, 6);
+
+    data.x = (res[0] << 8) | res[1];
+    data.y = (res[2] << 8) | res[3];
+    data.z = (res[4] << 8) | res[5];
+}
+
+void FXOS8700Q_mag::readRegs(int addr, uint8_t * data, int len) {
+    char t[1] = {addr};
+    m_i2c.write(m_addr, t, 1, true);
+    m_i2c.read(m_addr, (char *)data, len);
+}
+
+void FXOS8700Q_mag::writeRegs(uint8_t * data, int len) {
+    m_i2c.write(m_addr, (char *)data, len);
+}
+
+
+int16_t FXOS8700Q_mag::getAccAxis(uint8_t addr) {
+    int16_t acc;
+    uint8_t res[2];
+    readRegs(addr, res, 2);
+
+    acc = (res[0] << 8) | res[1];
+
+    return acc;
+}
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FRDM_FXS_MULTI_B/FXOS8700Q.h	Thu Mar 21 09:03:32 2019 +0000
@@ -0,0 +1,151 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef FXOS8700Q_H
+#define FXOS8700Q_H
+
+#include "mbed.h"
+#include "MotionSensor.h"
+// FXOS8700CQ I2C address
+#define FXOS8700CQ_SLAVE_ADDR0 (0x1E<<1) // with pins SA0=0, SA1=0
+#define FXOS8700CQ_SLAVE_ADDR1 (0x1D<<1) // with pins SA0=1, SA1=0
+#define FXOS8700CQ_SLAVE_ADDR2 (0x1C<<1) // with pins SA0=0, SA1=1
+#define FXOS8700CQ_SLAVE_ADDR3 (0x1F<<1) // with pins SA0=1, SA1=1
+// FXOS8700CQ internal register addresses
+#define FXOS8700Q_STATUS 0x00
+#define FXOS8700Q_OUT_X_MSB 0x01
+#define FXOS8700Q_OUT_Y_MSB 0x03
+#define FXOS8700Q_OUT_Z_MSB 0x05
+#define FXOS8700Q_M_OUT_X_MSB 0x33
+#define FXOS8700Q_M_OUT_Y_MSB 0x35
+#define FXOS8700Q_M_OUT_Z_MSB 0x37
+#define FXOS8700Q_WHOAMI 0x0D
+#define FXOS8700Q_XYZ_DATA_CFG 0x0E
+#define FXOS8700Q_CTRL_REG1 0x2A
+#define FXOS8700Q_M_CTRL_REG1 0x5B
+#define FXOS8700Q_M_CTRL_REG2 0x5C
+#define FXOS8700Q_WHOAMI_VAL 0xC7
+
+
+/**
+* MMA8451Q accelerometer example
+*
+* @code
+* #include "mbed.h"
+* #include "FXOS8700Q.h"
+* 
+* 
+* int main(void) {
+* 
+* FXOS8700Q combo( A4, A5, FXOS8700Q_I2C_ADDRESS0);
+* PwmOut rled(LED_RED);
+* PwmOut gled(LED_GREEN);
+* PwmOut bled(LED_BLUE);
+* 
+*     while (true) {       
+*         rled1.0 - combo(acc.getAccX());
+*         gled1.0 - combo(acc.getAccY());
+*         bled1.0 - combo(acc.getAccZ());
+*         wait(0.1);
+*     }
+* }
+* @endcode
+*/
+
+class FXOS8700Q_acc : public MotionSensor
+{
+public:
+  /**
+  * FXOS8700Q constructor
+  *
+  * @param sda SDA pin
+  * @param sdl SCL pin
+  * @param addr addr of the I2C peripheral
+  */
+  
+  FXOS8700Q_acc(PinName sda, PinName scl, int addr);
+
+  /**
+  * FXOS8700Q destructor
+  */
+  ~FXOS8700Q_acc();
+
+    void enable(void);
+    void disable(void);
+    uint32_t sampleRate(uint32_t frequency);
+    uint32_t whoAmI(void);
+    uint32_t dataReady(void);
+    void getX(int16_t * x);
+    void getY(int16_t * y);
+    void getZ(int16_t * z);
+    void getX(float * x);
+    void getY(float * y);
+    void getZ(float * z);
+    void getAxis(MotionSensorDataCounts &data);
+    void getAxis(MotionSensorDataUnits &data);
+  
+  void readRegs(int addr, uint8_t * data, int len);
+  
+private:
+  I2C m_i2c;
+  int m_addr;
+
+  void writeRegs(uint8_t * data, int len);
+  int16_t getAccAxis(uint8_t addr);
+
+};
+
+class FXOS8700Q_mag : public MotionSensor
+{
+public:
+  FXOS8700Q_mag(PinName sda, PinName scl, int addr);
+
+  /**
+  * FXOS8700Q destructor
+  */
+  ~FXOS8700Q_mag();
+
+    void enable(void);
+    void disable(void);
+    uint32_t sampleRate(uint32_t fequency);
+    uint32_t whoAmI(void);
+    uint32_t dataReady(void);
+    void getX(int16_t * x);
+    void getY(int16_t * y);
+    void getZ(int16_t * z);
+    void getX(float * x);
+    void getY(float * y);
+    void getZ(float * z);
+    void getAxis(MotionSensorDataCounts &data);
+    void getAxis(MotionSensorDataUnits &data);
+  
+  void readRegs(int addr, uint8_t * data, int len);
+  
+private:
+  I2C m_i2c;
+  int m_addr;
+  char sbuf[12];
+  int sstatus;
+  
+  void writeRegs(uint8_t * data, int len);
+  int16_t getAccAxis(uint8_t addr);
+
+};
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FRDM_FXS_MULTI_B/MAG3110.cpp	Thu Mar 21 09:03:32 2019 +0000
@@ -0,0 +1,195 @@
+
+#include "MAG3110.h"
+#include "mbed.h"
+#include "MotionSensor.h"
+
+/******************************************************************************
+ * Constructors
+ ******************************************************************************/
+MAG3110::MAG3110(PinName sda, PinName scl): m_i2c(sda, scl), 
+    m_addr(MAG_ADDR)
+{
+    char cmd[2];
+
+    cmd[0] = MAG_CTRL_REG2;
+    cmd[1] = 0x80;
+    m_i2c.write(m_addr, cmd, 2);
+
+}
+
+void MAG3110::enable(void) {
+    uint8_t data[2];
+    readRegs( MAG_CTRL_REG1, &data[1], 1);
+    data[1] |= 0x01;
+    data[0] = MAG_CTRL_REG1;
+    writeRegs(data, 2);
+}
+
+
+void MAG3110::disable(void) {
+    uint8_t data[2];
+    readRegs( MAG_CTRL_REG1, &data[1], 1);
+    data[1] &= 0xFE;
+    data[0] = MAG_CTRL_REG1;
+    writeRegs(data, 2);
+}
+
+
+void MAG3110::readRegs(int addr, uint8_t * data, int len)
+{
+    char cmd[1];
+
+    cmd[0] = addr;
+    m_i2c.write( m_addr, cmd, 1, true);
+    m_i2c.read( m_addr, (char *) data, len);
+    return;
+}
+
+
+void MAG3110::writeRegs(uint8_t * data, int len) {
+    m_i2c.write(m_addr, (char *)data, len);
+}
+
+
+uint32_t MAG3110::whoAmI() {
+    uint8_t who_am_i = 0;
+    readRegs(MAG_WHOAMI, &who_am_i, 1);
+    return (uint32_t) who_am_i;
+}
+
+uint32_t MAG3110::dataReady(void) {
+    uint8_t stat = 0;
+    readRegs(MAG_DR_STATUS, &stat, 1);
+    return (uint32_t) stat;
+}
+
+uint32_t MAG3110::sampleRate(uint32_t f) {
+    return(50); // for now sample rate is fixed at 50Hz
+}
+
+
+void MAG3110::getX(float * x) {
+    *x = (float(getMagAxis(MAG_OUT_X_MSB)) * 0.1f);
+}
+
+void MAG3110::getY(float * y) {
+    *y = (float(getMagAxis(MAG_OUT_Y_MSB)) * 0.1f);
+}
+
+void MAG3110::getZ(float * z) {
+    *z = (float(getMagAxis(MAG_OUT_Z_MSB)) * 0.1f);
+}
+
+void MAG3110::getX(int16_t * d) {
+    *d = getMagAxis(MAG_OUT_X_MSB);
+}
+
+void MAG3110::getY(int16_t * d) {
+    *d = getMagAxis(MAG_OUT_Y_MSB);
+}
+
+void MAG3110::getZ(int16_t * d) {
+    *d = getMagAxis(MAG_OUT_Z_MSB);
+}
+
+int16_t MAG3110::getMagAxis(uint8_t addr) {
+    int16_t acc;
+    uint8_t res[2];
+    readRegs(addr, res, 2);
+
+    acc = (res[0] << 8) | res[1];
+
+    return acc;
+}
+
+
+void MAG3110::getAxis(MotionSensorDataUnits &data) {
+    int16_t t[3];
+    uint8_t res[6];
+
+    readRegs(MAG_OUT_X_MSB, res, 6);
+    t[0] = (res[0] << 8) | res[1];
+    t[1] = (res[2] << 8) | res[3];
+    t[2] = (res[4] << 8) | res[5];
+    data.x = ((float) t[0]) * 0.1f;
+    data.y = ((float) t[1]) * 0.1f;
+    data.z = ((float) t[2]) * 0.1f;
+}
+
+
+void MAG3110::getAxis(MotionSensorDataCounts &data) {
+
+    uint8_t res[6];
+    readRegs(MAG_OUT_X_MSB, res, 6);
+
+    data.x = (res[0] << 8) | res[1];
+    data.y = (res[2] << 8) | res[3];
+    data.z = (res[4] << 8) | res[5];
+}
+/*
+
+// read a register per, pass first reg value, reading 2 bytes increments register
+// Reads MSB first then LSB
+int MAG3110::readVal(char regAddr)
+{
+    char cmd[2];
+    int16_t t;
+    cmd[0] = regAddr;
+    if(_i2c.write(m_addr, cmd, 1)) {
+        printf("MAG3110 write error\r\n");
+        _i2c.stop();
+        _i2c.start();
+        }
+
+    cmd[0] = 0x00;
+    cmd[1] = 0x00;
+    _i2c.read(m_addr, cmd, 2);
+    t = (cmd[0] * 256) + (unsigned short) cmd[1];
+    return ((int) t); //concatenate the MSB and LSB
+}
+
+
+float MAG3110::getHeading()
+{
+    int xVal = readVal(MAG_OUT_X_MSB);
+    int yVal = readVal(MAG_OUT_Y_MSB);
+    return (atan2((double)(yVal - _avgY),(double)(xVal - _avgX)))*180/PI;
+}
+
+void MAG3110::getValues(int *xVal, int *yVal, int *zVal)
+{
+    *xVal = readVal(MAG_OUT_X_MSB);
+    *yVal = readVal(MAG_OUT_Y_MSB);
+    *zVal = readVal(MAG_OUT_Z_MSB);
+}
+
+void MAG3110::ReadXYZ(float * mag)
+{
+    int x, y, z;
+    x = readVal(MAG_OUT_X_MSB);
+    y = readVal(MAG_OUT_Y_MSB);
+    z = readVal(MAG_OUT_Z_MSB);
+    mag[0] = (float) x / 10.0;
+    mag[1] = (float) y / 10.0;
+    mag[2] = (float) z / 10.0;
+    
+}
+
+void MAG3110::ReadXYZraw(int16_t * mag_raw)
+{
+    mag_raw[0] = readVal(MAG_OUT_X_MSB);
+    mag_raw[1] = readVal(MAG_OUT_Y_MSB);
+    mag_raw[2] = readVal(MAG_OUT_Z_MSB);
+}
+
+void MAG3110::setCalibration(int minX, int maxX, int minY, int maxY )
+{
+    _avgX=(maxX+minX)/2;
+    _avgY=(maxY+minY)/2;
+}
+*/
+
+
+
+
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FRDM_FXS_MULTI_B/MAG3110.h	Thu Mar 21 09:03:32 2019 +0000
@@ -0,0 +1,115 @@
+/*
+ * MAG3110 Sensor Library for mbed
+ * TODO: Add proper header
+ */
+
+#ifndef MAG3110_H
+#define MAG3110_H
+
+#include "mbed.h"
+#include "MotionSensor.h"
+
+#define PI 3.14159265359
+
+#define MAG_ADDR (0x0E << 1)
+
+// define registers
+#define MAG_DR_STATUS 0x00
+#define MAG_OUT_X_MSB 0x01
+#define MAG_OUT_X_LSB 0x02
+#define MAG_OUT_Y_MSB 0x03
+#define MAG_OUT_Y_LSB 0x04
+#define MAG_OUT_Z_MSB 0x05
+#define MAG_OUT_Z_LSB 0x06
+#define MAG_WHOAMI  0x07
+#define MAG_SYSMOD    0x08
+#define MAG_OFF_X_MSB 0x09
+#define MAG_OFF_X_LSB 0x0A
+#define MAG_OFF_Y_MSB 0x0B
+#define MAG_OFF_Y_LSB 0x0C
+#define MAG_OFF_Z_MSB 0x0D
+#define MAG_OFF_Z_LSB 0x0E
+#define MAG_DIE_TEMP  0x0F
+#define MAG_CTRL_REG1 0x10
+#define MAG_CTRL_REG2 0x11
+
+// what should WHO_AM_I return?
+#define MAG_3110_WHO_AM_I_VALUE 0xC4
+
+
+// Fields in registers
+// CTRL_REG1: dr2,dr1,dr0  os1,os0  fr tm ac
+
+// Sampling rate from 80Hz down to 0.625Hz
+#define MAG_3110_SAMPLE80 0
+#define MAG_3110_SAMPLE40 0x20
+#define MAG_3110_SAMPLE20 0x40
+#define MAG_3110_SAMPLE10 0x60
+#define MAG_3110_SAMPLE5 0x80
+#define MAG_3110_SAMPLE2_5 0xA0
+#define MAG_3110_SAMPLE1_25 0xC0
+#define MAG_3110_SAMPLE0_625 0xE0
+
+// How many samples to average (lowers data rate)
+#define MAG_3110_OVERSAMPLE1 0
+#define MAG_3110_OVERSAMPLE2 0x08
+#define MAG_3110_OVERSAMPLE3 0x10
+#define MAG_3110_OVERSAMPLE4 0x18
+
+// read only 1 byte per axis
+#define MAG_3110_FASTREAD 0x04
+// do one measurement (even if in standby mode)
+#define MAG_3110_TRIGGER 0x02
+// put in active mode
+#define MAG_3110_ACTIVE 0x01
+
+// CTRL_REG2: AUTO_MRST_EN  _ RAW MAG_RST _ _ _ _ _
+// reset sensor after each reading
+#define MAG_3110_AUTO_MRST_EN 0x80
+// don't subtract user offsets
+#define MAG_3110_RAW 0x20
+// reset magnetic sensor after too-large field
+#define MAG_3110_MAG_RST 0x10
+
+// DR_STATUS Register ZYXOW ZOW YOW XOW ZYXDR ZDR YDR XDR
+#define MAG_3110_ZYXDR  0x08
+
+/**
+ * MAG3110 Class to read X/Y/Z data from the magentometer
+ *
+ */
+class MAG3110 : public MotionSensor
+{
+public:
+    /**
+     * Main constructor
+     * @param sda SDA pin
+     * @param sdl SCL pin
+     * @param addr addr of the I2C peripheral
+     */
+    MAG3110(PinName sda, PinName scl);
+
+    void enable(void);
+    void disable(void);
+    uint32_t sampleRate(uint32_t fequency);
+    uint32_t whoAmI(void);
+    uint32_t dataReady(void);
+    void getX(int16_t * x);
+    void getY(int16_t * y);
+    void getZ(int16_t * z);
+    void getX(float * x);
+    void getY(float * y);
+    void getZ(float * z);
+    void getAxis(MotionSensorDataCounts &data);
+    void getAxis(MotionSensorDataUnits &data);
+    void readRegs(int addr, uint8_t * data, int len);
+  
+private:
+  I2C m_i2c;
+  char m_addr;
+  int16_t getMagAxis(uint8_t addr);
+  void writeRegs(uint8_t * data, int len);
+
+};
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FRDM_FXS_MULTI_B/MMA8652.cpp	Thu Mar 21 09:03:32 2019 +0000
@@ -0,0 +1,130 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#include "MMA8652.h"
+#include "common_define.h"
+#define UINT14_MAX        16383
+  
+MMA8652::MMA8652(PinName sda, PinName scl) : _i2c(sda, scl) {
+ 
+    begin();
+}
+
+
+MMA8652::~MMA8652() { }
+
+              
+void MMA8652::RegRead( char reg, char * d, int len)
+{
+    // char cmd = reg;
+    // char i2c_addr = MMA8652_SLAVE_ADDR;
+    // int status;
+    // printf("i2c_addr = 0x%x \r\n",i2c_addr);
+    // status = _i2c.write( i2c_addr, &cmd, 1);
+    // status = _i2c.read (i2c_addr, d, len);
+    // if(status == 0)
+    // {
+    //     printf("success \r\n");
+    // }
+    // else
+    // {
+    //     printf("failer \r\n");
+    // }
+    i2cReadForFXS_MUTIL(&_i2c,MMA8652_SLAVE_ADDR,(uint8_t*)d,reg,len);
+
+}
+
+void MMA8652::begin(void)
+{
+    char data[2];
+    // write 0000 0000 = 0x00 to accelerometer control register 1 to place MMA8652 into
+    // standby
+    // [7-1] = 0000 000
+    // [0]: active=0
+    data[0] = MMA8652_CTRL_REG1;
+    data[1] = 0x00;
+    _i2c.write( MMA8652_SLAVE_ADDR, data, 2);
+    
+    // write 0000 0001= 0x01 to XYZ_DATA_CFG register
+    // [7]: reserved
+    // [6]: reserved
+    // [5]: reserved
+    // [4]: hpf_out=0
+    // [3]: reserved
+    // [2]: reserved
+    // [1-0]: fs=00 for accelerometer range of +/-2g range with 0.244mg/LSB
+    data[0] = MMA8652_XYZ_DATA_CFG;
+    data[1] = 0x00;
+    _i2c.write( MMA8652_SLAVE_ADDR, data, 2);
+
+    // write 0000 1101 = 0x0D to accelerometer control register 1
+    // [7-6]: aslp_rate=00
+    // [5-3]: dr=100 for 50Hz data rate
+    // [2]: 0
+    // [1]: 0
+    // [0]: active=1 to take the part out of standby and enable sampling
+    data[0] = MMA8652_CTRL_REG1;
+    data[1] = 0x21;
+    _i2c.write( MMA8652_SLAVE_ADDR, data, 2);
+}
+
+char  MMA8652::getWhoAmI(void)
+{
+    char Id = 0x00;
+    RegRead(MMA8652_WHOAMI, &Id, 1);
+    return Id;
+}
+
+void MMA8652::ReadXYZ(float * a)
+{
+    char d[7];
+    int16_t t[6];
+
+    RegRead( MMA8652_STATUS, d, 7);
+    t[0] = ((d[1] * 256) + ((unsigned short) d[2]));
+    t[1] = ((d[3] * 256) + ((unsigned short) d[4]));
+    t[2] = ((d[5] * 256) + ((unsigned short) d[6]));
+
+    a[0] = (float) t[0] / 16384.0;
+    a[1] = (float) t[1] / 16384.0;
+    a[2] = (float) t[2] / 16384.0;
+  
+}
+
+
+void MMA8652::ReadXYZraw(int16_t * d)
+{
+    char res[6];
+    int16_t acc;
+    RegRead( MMA8652_OUT_X_MSB, res, 6);
+
+    acc = (res[0] << 6) | (res[1] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    d[0] = acc;
+    acc = (res[2] << 6) | (res[3] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    d[1] = acc;
+    acc = (res[4] << 6) | (res[5] >> 2);
+    if (acc > UINT14_MAX/2)
+        acc -= UINT14_MAX;
+    d[2] = acc;
+}
+  
+   

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FRDM_FXS_MULTI_B/MMA8652.h	Thu Mar 21 09:03:32 2019 +0000
@@ -0,0 +1,89 @@
+/* Copyright (c) 2010-2011 mbed.org, MIT License
+*
+* Permission is hereby granted, free of charge, to any person obtaining a copy of this software
+* and associated documentation files (the "Software"), to deal in the Software without
+* restriction, including without limitation the rights to use, copy, modify, merge, publish,
+* distribute, sublicense, and/or sell copies of the Software, and to permit persons to whom the
+* Software is furnished to do so, subject to the following conditions:
+*
+* The above copyright notice and this permission notice shall be included in all copies or
+* substantial portions of the Software.
+*
+* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING
+* BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND
+* NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,
+* DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
+* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
+*/
+
+#ifndef MMA8652_H
+#define MMA8652_H
+
+#include "mbed.h"
+
+
+// MMA8652 Slave Address
+#define MMA8652_SLAVE_ADDR (0x1D << 1)
+
+// MMA8652 internal register addresses
+#define MMA8652_STATUS 0x00
+#define MMA8652_OUT_X_MSB 0x01
+#define MMA8652_WHOAMI 0x0D
+#define MMA8652_XYZ_DATA_CFG 0x0E
+#define MMA8652_CTRL_REG1 0x2A
+#define MMA8652_WHOAMI_VAL 0x4A
+
+/**
+ * MMA8652 Xtrinsic accelerometer on I2C
+ */
+class MMA8652
+{
+public:
+    /**
+     * MMA8652 constructor
+     *
+     * @param sda SDA pin
+     * @param sdl SCL pin
+     */
+    MMA8652(PinName sda, PinName scl);
+
+    /**
+     * MMA8652 destructor
+     */
+    ~MMA8652();
+    
+    /**
+     * Get XYZ axis acceleration in floating point G's
+     *
+     * @param res array where acceleration data will be stored
+     */
+    void ReadXYZ(float * a);
+
+    /**
+     * Get XYZ axis acceleration, signed 16 bit values
+     *
+     * @param res array where acceleration data will be stored
+     */
+    void ReadXYZraw(int16_t * d);
+    
+    /**
+     * Get the value of the WHO_AM_I register
+     *
+     * @returns DEVICE_ID value == 0x1D
+     */
+    char getWhoAmI(void);
+
+private:
+    I2C _i2c;
+
+    /**
+     * Set the device in active mode
+     */
+    void begin( void);
+
+    void RegRead( char reg, char * d, int len);
+
+};
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FRDM_FXS_MULTI_B/MPL3115A2.cpp	Thu Mar 21 09:03:32 2019 +0000
@@ -0,0 +1,583 @@
+#include "MPL3115A2.h"
+
+#define REG_WHO_AM_I        0x0C        // return 0xC4 by default
+#define REG_STATUS          0x00
+#define REG_CTRL_REG_1      0x26
+#define REG_CTRL_REG_3      0x28
+#define REG_CTRL_REG_4      0x29
+#define REG_CTRL_REG_5      0x2A
+#define REG_PRESSURE_MSB    0x01        // 3 byte pressure data
+#define REG_ALTIMETER_MSB   0x01        // 3 byte altimeter data
+#define REG_TEMP_MSB        0x04        // 2 byte temperature data
+#define REG_PT_DATA_CFG     0x13
+#define REG_P_TGT_MSB       0x16
+#define REG_P_WND_MSB       0x19
+#define REG_OFF_P           0x2b
+#define REG_OFF_T           0x2c
+#define REG_OFF_H           0x2d
+#define REG_PRES_MIN_MSB    0x1c
+#define REG_ALTI_MIN_MSB    0x1c
+#define REG_TEMP_MIN_MSB    0x1f
+#define REG_PRES_MAX_MSB    0x21
+#define REG_ALTI_MAX_MSB    0x21
+#define REG_TEMP_MAX_MSB    0x24
+#define REG_PRES_DELTA_MSB  0x07
+#define REG_ALTI_DELTA_MSB  0x07
+#define REG_TEMP_DELTA_MSB  0x0a
+
+
+#define UINT14_MAX        16383
+
+// Status flag for data ready.
+#define PTDR_STATUS       0x03        // Pressure Altitude and Temperature ready
+#define PDR_STATUS        0x02        // Pressure and Altitude data ready
+#define TDR_STATUS        0x01        // Temperature data ready
+
+
+void (*MPL3115A2_usr2_fptr)(void);               // Pointers to user function called after
+void (*MPL3115A2_usr1_fptr)(void);               // IRQ assertion.
+
+
+MPL3115A2::MPL3115A2(PinName sda, PinName scl, int addr, PinName int1, PinName int2) : m_i2c(sda, scl), m_addr(addr), MPL3115A2_Int1(int1), MPL3115A2_Int2(int2) {
+    unsigned char data[6];
+
+    MPL3115A2_mode = BAROMETRIC_MODE;
+    MPL3115A2_oversampling = OVERSAMPLE_RATIO_1;
+    //
+    MPL3115A2_usr1_fptr = NULL;
+    MPL3115A2_usr2_fptr = NULL;
+    MPL3115A2_Int1.fall( NULL);
+    MPL3115A2_Int2.fall( NULL);
+    
+    Reset();
+    
+    data[0]=REG_PRES_MIN_MSB;
+    data[1]=0;data[2]=0;data[3]=0;data[4]=0;data[5]=0;
+    writeRegs( &data[0], 6);
+}
+
+void MPL3115A2::Reset( void)
+{
+    unsigned char t;
+    
+    // soft reset...
+    readRegs( REG_CTRL_REG_1, &t, 1);
+    unsigned char data[2] = { REG_CTRL_REG_1, t|0x04};
+    writeRegs(data, 2);    
+    wait( 0.1);
+
+}
+
+void MPL3115A2::DataReady( void(*fptr)(void), unsigned char OS)
+{
+    unsigned char dt[5];
+    unsigned char data[2];
+    
+    // Soft Reset
+    Reset();
+    
+    Standby();
+    
+    // Clear all interrupts by reading the output registers.
+    readRegs( REG_ALTIMETER_MSB, &dt[0], 5);
+    getStatus();
+    // Configure INT active low and pullup
+    data[0] = REG_CTRL_REG_3;
+    data[1] = 0x00;
+    writeRegs(data, 2);    
+    // Enable Interrupt fot data ready
+    data[0] = REG_CTRL_REG_4;
+    data[1] = 0x80;
+    writeRegs(data, 2);    
+    // Configure Interrupt to route to INT2
+    data[0] = REG_CTRL_REG_5;
+    data[1] = 0x00;
+    writeRegs(data, 2);    
+    data[0] = REG_PT_DATA_CFG;
+    data[1] = 0x07;
+    writeRegs(data, 2);    
+
+    // Configure the OverSampling rate, Altimeter/Barometer mode and set the sensor Active
+    data[0] = REG_CTRL_REG_1;
+    data[1] = (OS<<3);
+    //
+    if (MPL3115A2_mode == BAROMETRIC_MODE)
+        data[1] &= 0x7F;
+    else
+        data[1] |= 0x80;
+    //
+    data[1] |= 0x01; 
+    writeRegs(data, 2);    
+
+    MPL3115A2_usr2_fptr = fptr;
+    MPL3115A2_Int2.fall( this, &MPL3115A2::DataReady_IRQ);
+
+}
+
+void MPL3115A2::DataReady_IRQ( void)
+{
+    // Clear the IRQ flag
+    getStatus();
+    // Run the user supplied function
+    MPL3115A2_usr2_fptr();   
+}
+
+void MPL3115A2::AltitudeTrigger( void(*fptr)(void), unsigned short level)
+{
+    unsigned char dt[5];
+    unsigned char data[2];
+
+    // Soft Reset
+    Reset();
+    
+    // The device is on standby
+    Standby();
+    
+    // Clear all interrupts by reading the output registers.
+    readRegs( REG_ALTIMETER_MSB, &dt[0], 5);
+    getStatus();
+
+    // Write Target and Window Values
+    dt[0] = REG_P_TGT_MSB;
+    dt[1] = (level>>8);
+    dt[2] = (level&0xFF);
+    writeRegs( dt, 3);
+    
+    // Window values are zero
+    dt[0] = REG_P_WND_MSB;
+    dt[1] = 0;
+    dt[2] = 0;
+    writeRegs( dt, 3);
+
+    // Enable Pressure Threshold interrupt
+    data[0] = REG_CTRL_REG_4;
+    data[1] = 0x08;
+    writeRegs( data, 2);
+    // Interrupt is routed to INT1
+    data[0] = REG_CTRL_REG_5;
+    data[1] = 0x08;
+    writeRegs( data, 2);
+    data[0] = REG_PT_DATA_CFG;
+    data[1] = 0x07;
+    writeRegs(data, 2);    
+    // Configure the OverSampling rate, Altimeter mode and set the sensor Active
+    data[0] = REG_CTRL_REG_1;
+    data[1] = 0x81 | (MPL3115A2_oversampling<<3);    
+    writeRegs(data, 2);    
+
+    MPL3115A2_usr1_fptr = fptr;
+    MPL3115A2_Int1.fall( this, &MPL3115A2::AltitudeTrg_IRQ);
+
+}
+
+void MPL3115A2::AltitudeTrg_IRQ( void)
+{
+    // Clear the IRQ flag
+    getStatus();
+    // Run the user supplied function
+    MPL3115A2_usr1_fptr();   
+
+}
+
+void MPL3115A2::Barometric_Mode( void)
+{
+    unsigned char t;
+    unsigned char data[2];
+    
+    Standby();
+
+    // soft reset...
+    Reset();
+        
+    Standby();
+    readRegs( REG_CTRL_REG_1, &t, 1);
+    
+    // Set the Barometric mode
+    data[0] = REG_CTRL_REG_1;
+    data[1] = t&0x7F;
+    writeRegs(data, 2);    
+
+    data[0] = REG_PT_DATA_CFG;
+    data[1] = 0x07;
+    writeRegs(data, 2);    
+
+    Oversample_Ratio( MPL3115A2_oversampling);
+    
+    Active();
+    
+    MPL3115A2_mode = BAROMETRIC_MODE;
+}
+
+void MPL3115A2::Altimeter_Mode( void)
+{
+    unsigned char t;
+    unsigned char data[2];
+    
+    Standby();
+
+    // soft reset...
+    Reset();    
+    
+    Standby();
+    readRegs( REG_CTRL_REG_1, &t, 1);
+
+    data[0] = REG_CTRL_REG_1;
+    data[1] = t|0x80;
+    writeRegs(data, 2);    
+
+    data[0] = REG_PT_DATA_CFG;
+    data[1] = 0x07;
+    writeRegs(data, 2);    
+
+    Oversample_Ratio( MPL3115A2_oversampling);
+    
+    Active();
+    
+    MPL3115A2_mode = ALTIMETER_MODE;
+}
+
+void MPL3115A2::Oversample_Ratio( unsigned int ratio)
+{
+    unsigned char t;
+    
+    Standby();
+    readRegs( REG_CTRL_REG_1, &t, 1);
+
+    t = t & 0xE7;
+    t = t | ( ratio<<3);
+
+    unsigned char data[2] = { REG_CTRL_REG_1, t};
+    writeRegs(data, 2);    
+
+    Active();
+    
+    MPL3115A2_oversampling = ratio;
+}
+
+
+void MPL3115A2::Active( void)
+{
+    unsigned char t;
+    
+    // Activate the peripheral
+    readRegs(REG_CTRL_REG_1, &t, 1);
+    unsigned char data[2] = {REG_CTRL_REG_1, t|0x01};
+    writeRegs(data, 2);
+}
+
+void MPL3115A2::Standby( void)
+{
+    unsigned char t;
+    
+    // Standby
+    readRegs(REG_CTRL_REG_1, &t, 1);
+    unsigned char data[2] = {REG_CTRL_REG_1, t&0xFE};
+    writeRegs(data, 2);
+}
+
+unsigned char MPL3115A2::getDeviceID() {
+    unsigned char device_id = 0;
+    readRegs(REG_WHO_AM_I, &device_id, 1);
+    return device_id;
+}
+
+unsigned int MPL3115A2::isDataAvailable( void)
+{
+    unsigned char status;
+    
+    readRegs( REG_STATUS, &status, 1);
+
+    return ((status>>1));
+    
+}
+
+unsigned char MPL3115A2::getStatus( void)
+{
+    unsigned char status;
+    
+    readRegs( REG_STATUS, &status, 1);
+    return status;
+}
+
+unsigned int MPL3115A2::getAllData( float *f)
+{
+    if ( isDataAvailable() & PTDR_STATUS) {
+        if ( MPL3115A2_mode == ALTIMETER_MODE) {
+            f[0] = getAltimeter( REG_ALTIMETER_MSB);
+        } else {
+            f[0] = getPressure( REG_PRESSURE_MSB);
+        }
+        
+        f[1] = getTemperature( REG_TEMP_MSB);
+        //
+        return 1;
+    } else
+        return 0;
+}
+
+unsigned int MPL3115A2::getAllData( float *f, float *d)
+{
+    if ( isDataAvailable() & PTDR_STATUS) {
+        if ( MPL3115A2_mode == ALTIMETER_MODE) {
+            f[0] = getAltimeter();
+            d[0] = getAltimeter( REG_ALTI_DELTA_MSB);
+        } else {
+            f[0] = getPressure();
+            d[0] = getPressure( REG_PRES_DELTA_MSB);
+        }
+        
+        f[1] = getTemperature();
+        d[1] = getTemperature( REG_TEMP_DELTA_MSB);
+        //
+        return 1;
+    } else
+        return 0;
+}
+
+void MPL3115A2::getAllMaximumData( float *f)
+{
+    if ( MPL3115A2_mode == ALTIMETER_MODE) {
+        f[0] = getAltimeter( REG_ALTI_MAX_MSB);
+    } else {
+        f[0] = getPressure( REG_PRES_MAX_MSB);
+    }
+    
+    f[1] = getTemperature( REG_TEMP_MAX_MSB);
+}
+
+void MPL3115A2::getAllMinimumData( float *f)
+{
+    if ( MPL3115A2_mode == ALTIMETER_MODE) {
+        f[0] = getAltimeter( REG_ALTI_MIN_MSB);
+    } else {
+        f[0] = getPressure( REG_PRES_MIN_MSB);
+    }
+    
+    f[1] = getTemperature( REG_TEMP_MIN_MSB);
+}
+
+float MPL3115A2::getAltimeter( void)
+{
+    float a;
+    
+    a = getAltimeter( REG_ALTIMETER_MSB);
+    return a;
+}
+
+float MPL3115A2::getAltimeter( unsigned char reg)
+{
+    unsigned char dt[3];
+    unsigned short altm;
+    short tmp;
+    float faltm;
+    
+    /*
+    * dt[0] = Bits 12-19 of 20-bit real-time Altitude sample. (b7-b0)
+    * dt[1] = Bits 4-11 of 20-bit real-time Altitude sample. (b7-b0)
+    * dt[2] = Bits 0-3 of 20-bit real-time Altitude sample (b7-b4)
+    */
+    readRegs( reg, &dt[0], 3);
+    altm = (dt[0]<<8) | dt[1];
+    //
+    if ( dt[0] > 0x7F) {
+        // negative number
+        tmp = ~altm + 1;
+        faltm = (float)tmp * -1.0f;
+    } else {
+        faltm = (float)altm * 1.0f;
+    }
+    //
+    faltm = faltm+((float)(dt[2]>>4) * 0.0625f);
+    return faltm;
+}
+
+float MPL3115A2::getPressure( void)
+{
+    float a;
+    
+    a = getPressure( REG_PRESSURE_MSB);
+    return a;
+}
+
+float MPL3115A2::getPressure( unsigned char reg)
+{
+    unsigned char dt[3];
+    unsigned int prs;
+    int tmp;
+    float fprs;
+    
+    /*
+    * dt[0] = Bits 12-19 of 20-bit real-time Pressure sample. (b7-b0)
+    * dt[1] = Bits 4-11 of 20-bit real-time Pressure sample. (b7-b0)
+    * dt[2] = Bits 0-3 of 20-bit real-time Pressure sample (b7-b4)
+    */
+    readRegs( reg, &dt[0], 3);
+    prs = ((dt[0]<<10) | (dt[1]<<2) | (dt[2]>>6));
+    //
+    if ( dt[0] > 0x7f) {
+        // negative number
+        if ( dt[0] & 0x80)
+            prs |= 0xFFFC0000;      // set at 1 the bits to complete the word len
+        else
+            prs |= 0xFFFE0000;
+        tmp = ~prs + 1;             // make the complemets. At this point all the bits are inverted.
+        fprs = (float)tmp * -1.0f;  // set the signe..
+    } else {
+        fprs = (float)prs * 1.0f;
+    }
+
+    if ( dt[2] & 0x10)              // I did some experiment to set the fractional parte.
+        fprs += 0.25f;              // ** Warning: the DS is wrong! **
+    if ( dt[2] & 0x20)
+        fprs += 0.5f;
+        
+    return fprs;
+}
+
+float MPL3115A2::getTemperature( void)
+{
+    float a;
+    
+    a = getTemperature( REG_TEMP_MSB);
+    return a;
+}
+
+float MPL3115A2::getTemperature( unsigned char reg)
+{
+    unsigned char dt[2];
+    unsigned short temp;
+    float ftemp;
+    
+    /*
+    * dt[0] = Bits 4-11 of 16-bit real-time temperature sample. (b7-b0)
+    * dt[1] = Bits 0-3 of 16-bit real-time temperature sample. (b7-b4)
+    */
+    readRegs( reg, &dt[0], 2);
+    temp = dt[0];
+    //
+    if ( dt[0] > 0x7F) {
+        temp = ~temp + 1;
+        ftemp = (float)temp * -1.0f;
+    } else {
+        ftemp = (float)temp * 1.0f;
+    }
+    //
+    ftemp = ftemp+((float)(dt[1]>>4) * 0.0625f);
+    return ftemp;
+
+}
+
+
+unsigned int MPL3115A2::getAllDataRaw( unsigned char *dt)
+{
+    // Check for Press/Alti and Temp value ready
+    if ( isDataAvailable() & PTDR_STATUS) {
+        if ( MPL3115A2_mode == ALTIMETER_MODE) {
+            getAltimeterRaw( &dt[0]);               // 3 bytes
+        } else {
+            getPressureRaw( &dt[0]);                   // 3 bytes
+        }
+        
+        getTemperatureRaw( &dt[3]);                    // 2 bytes
+        
+        return 1;
+    } else {
+        return 0;
+    }
+}
+
+unsigned int MPL3115A2::getAltimeterRaw( unsigned char *dt)
+{
+    
+    /*
+    * dt[0] = Bits 12-19 of 20-bit real-time Pressure sample. (b7-b0)
+    * dt[1] = Bits 4-11 of 20-bit real-time Pressure sample. (b7-b0)
+    * dt[2] = Bits 0-3 of 20-bit real-time Pressure sample (b7-b4)
+    */
+    
+    // Check for Press/Alti value ready
+    if ( isDataAvailable() & PDR_STATUS) {
+        readRegs( REG_ALTIMETER_MSB, &dt[0], 3);    
+        return 1;
+    } else
+        return 0;
+}
+
+unsigned int  MPL3115A2::getPressureRaw( unsigned char *dt)
+{
+    
+    /*
+    * dt[0] = Bits 12-19 of 20-bit real-time Pressure sample. (b7-b0)
+    * dt[1] = Bits 4-11 of 20-bit real-time Pressure sample. (b7-b0)
+    * dt[2] = Bits 0-3 of 20-bit real-time Pressure sample (b7-b4)
+    */
+    
+    // Check for Press/Alti value ready
+    if ( isDataAvailable() & PDR_STATUS) {
+        readRegs( REG_PRESSURE_MSB, &dt[0], 3);
+        return 1;
+    } else 
+        return 0;
+        
+}
+
+unsigned int MPL3115A2::getTemperatureRaw( unsigned char *dt)
+{
+    
+    /*
+    * dt[0] = Bits 4-11 of 16-bit real-time temperature sample. (b7-b0)
+    * dt[1] = Bits 0-3 of 16-bit real-time temperature sample. (b7-b4)
+    */
+    
+    // Check for Temp value ready
+    if ( isDataAvailable() & TDR_STATUS) {
+        readRegs( REG_TEMP_MSB, &dt[0], 2);
+        return 1;
+    } else
+        return 0;        
+}
+
+void MPL3115A2::SetPressureOffset( char offset)
+{
+    unsigned char data [2] = {REG_OFF_P, offset};
+
+    Standby();
+    writeRegs(data,2);
+
+    Active(); 
+}
+
+void MPL3115A2::SetTemperatureOffset( char offset)
+{
+    unsigned char data [2] = {REG_OFF_T, offset};
+
+    Standby();
+    writeRegs(data,2);
+
+    Active(); 
+}
+
+void MPL3115A2::SetAltitudeOffset( char offset)
+{
+    unsigned char data [2] = {REG_OFF_H, offset};
+
+    Standby();
+    writeRegs(data,2);
+
+    Active(); 
+}
+
+void MPL3115A2::readRegs(int addr, uint8_t * data, int len) {
+    char t[1] = {addr};
+    m_i2c.write(m_addr, t, 1, true);
+    m_i2c.read(m_addr, (char *)data, len);
+}
+
+void MPL3115A2::writeRegs(uint8_t * data, int len) {
+    m_i2c.write(m_addr, (char *)data, len);
+}
+
+uint8_t MPL3115A2::getMode()
+{
+    return MPL3115A2_mode;
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FRDM_FXS_MULTI_B/MPL3115A2.h	Thu Mar 21 09:03:32 2019 +0000
@@ -0,0 +1,324 @@
+#ifndef MPL3115A2_H
+#define MPL3115A2_H
+
+#include "mbed.h"
+
+
+#define MPL3115A2_DEFAULT_ADDRESS (0x60 << 1)
+
+// Oversampling value and minimum time between sample
+#define OVERSAMPLE_RATIO_1      0       // 6 ms
+#define OVERSAMPLE_RATIO_2      1       // 10 ms
+#define OVERSAMPLE_RATIO_4      2       // 18 ms
+#define OVERSAMPLE_RATIO_8      3       // 34 ms
+#define OVERSAMPLE_RATIO_16     4       // 66 ms
+#define OVERSAMPLE_RATIO_32     5       // 130 ms
+#define OVERSAMPLE_RATIO_64     6       // 258 ms
+#define OVERSAMPLE_RATIO_128    7       // 512 ms
+
+/* Mode */
+#define ALTIMETER_MODE      1
+#define BAROMETRIC_MODE     2
+
+/**
+* MPL3115A2 Altimeter example
+*
+* @code
+* #include "mbed.h"
+* #include "MPL3115A2.h"
+* 
+* #define MPL3115A2_I2C_ADDRESS (0x60<<1)
+* MPL3115A2 wigo_sensor1(PTE0, PTE1, MPL3115A2_I2C_ADDRESS);
+* Serial pc(USBTX, USBRX);
+*
+* // pos [0] = altitude or pressure value 
+* // pos [1] = temperature value 
+* float sensor_data[2];
+*
+* int main(void) {
+*
+*    pc.baud( 230400);
+*    pc.printf("MPL3115A2 Altimeter mode. [%d]\r\n", wigo_sensor1.getDeviceID()); 
+*
+*    wigo_sensor1.Oversample_Ratio( OVERSAMPLE_RATIO_32);
+*    
+*    while(1) {
+*        //
+*        if ( wigo_sensor1.isDataAvailable()) {
+*            wigo_sensor1.getAllData( &sensor_data[0]);
+*            pc.printf("\tAltitude: %f\tTemperature: %f\r\n", sensor_data[0], sensor_data[1]);
+*        }
+*        //
+*        wait( 0.001);
+*    }
+*
+* }
+* @endcode
+*/
+class MPL3115A2
+{
+public:
+    /**
+     * MPL3115A2 constructor
+     *
+     * @param sda SDA pin
+     * @param sdl SCL pin
+     * @param addr addr of the I2C peripheral
+     * @param int1 InterruptIn
+     * @param int2 InterruptIn
+     *
+     * Interrupt schema:
+     *
+     * * The Altitude Trigger use the IRQ1 - Altitude Trigger -> MPL3115A2_Int1.fall -> AltitudeTrg_IRQ -> MPL3115A2_usr1_fptr
+     *
+     * * The Data ready use the IRQ2 - Data Ready -> MPL3115A2_Int2.fall -> DataReady_IRQ -> MPL3115A2_usr2_fptr
+     */
+    MPL3115A2(PinName sda, PinName scl, int addr, PinName int1, PinName int2);
+    
+    /**
+    * Get the value of the WHO_AM_I register
+    *
+    * @returns DEVICE_ID value == 0xC4
+    */
+    uint8_t getDeviceID();
+    
+    /**
+    * Return the STATUS register value
+    *
+    * @returns STATUS register value
+    */
+    unsigned char getStatus( void);
+    
+    /**
+    * Get the altimeter value
+    *
+    * @returns altimeter value as float
+    */
+    float getAltimeter( void);
+
+    /**
+    * Get the altimeter value in raw mode
+    *
+    * @param    dt      pointer to unsigned char array
+    * @returns 1 if data are available, 0 if not.
+    */
+    unsigned int getAltimeterRaw( unsigned char *dt);
+    
+    /**
+    * Get the pressure value
+    *
+    * @returns pressure value as float
+    */
+    float getPressure( void);
+
+    /**
+    * Get the pressure value in raw mode
+    *
+    * @param    dt      pointer to unsigned char array
+    * @returns 1 if data are available, 0 if not.
+    */
+    unsigned int  getPressureRaw( unsigned char *dt);
+    
+    /**
+    * Get the temperature value
+    *
+    * @returns temperature value as float
+    */
+    float getTemperature( void);
+
+    /**
+    * Get the temperature value in raw mode
+    *
+    * @param    dt      pointer to unsigned char array
+    * @returns 1 if data are available, 0 if not.
+    */
+    unsigned int getTemperatureRaw( unsigned char *dt);
+    
+    /**
+    * Set the Altimeter Mode
+    *
+    * @returns none
+    */
+    void Altimeter_Mode( void);
+    
+    /**
+    * Set the Barometric Mode
+    *
+    * @returns none
+    */
+    void Barometric_Mode( void);
+    
+    /**
+    * Get the altimeter or pressure and temperature values
+    *
+    * @param array of float f[2]
+    * @returns 0 no data available, 1 for data available
+    */
+    unsigned int  getAllData( float *f);
+
+    /**
+    * Get the altimeter or pressure and temperature values and the delta values
+    *
+    * @param array of float f[2], array of float d[2]
+    * @returns 0 no data available, 1 for data available
+    */
+    unsigned int getAllData( float *f, float *d);
+    
+    /**
+    * Get the altimeter or pressure and temperature captured maximum value
+    *
+    * @param array of float f[2]
+    * @returns 0 no data available, 1 for data available
+    */
+    void getAllMaximumData( float *f);
+
+    /**
+    * Get the altimeter or pressure and temperature captured minimum value
+    *
+    * @param array of float f[2]
+    * @returns 0 no data available, 1 for data available
+    */
+    void getAllMinimumData( float *f);
+
+    /**
+    * Get the altimeter or pressure, and temperature values in raw mode
+    *
+    * @param array of unsigned char[5]
+    * @returns 1 if data are available, 0 if not.
+    */    
+    unsigned int getAllDataRaw( unsigned char *dt);
+    
+    /** 
+    * Return if there are date available
+    * 
+    * @return 0 for no data available, bit0 set for Temp data available, bit1 set for Press/Alti data available
+    *         bit2 set for both Temp and Press/Alti data available
+    */
+    unsigned int isDataAvailable( void);
+    
+    /** 
+    * Set the oversampling rate value
+    *
+    * @param oversampling values. See MPL3115A2.h
+    * @return none
+    */
+    void Oversample_Ratio( unsigned int ratio);
+
+    /**
+    * Configure the sensor to streaming data using Interrupt
+    *
+    * @param user functin callback, oversampling values. See MPL3115A2.h
+    * @return none
+    */
+    void DataReady( void(*fptr)(void), unsigned char OS);
+
+    /**
+    * Configure the sensor to generate an Interrupt crossing the center threshold
+    *
+    * @param user functin callback, level in meter
+    * @return none
+    */
+    void AltitudeTrigger( void(*fptr)(void), unsigned short level);
+    
+    /**
+    * Soft Reset
+    *
+    * @param none
+    * @return none
+    */    
+    void Reset( void);
+
+    /**
+    * Configure the Pressure offset.
+    * Pressure user accessible offset trim value expressed as an 8-bit 2's complement number. 
+    * The user offset registers may be adjusted to enhance accuracy and optimize the system performance. 
+    * Range is from -512 to +508 Pa, 4 Pa per LSB.
+    * In RAW output mode no scaling or offsets will be applied in the digital domain
+    *
+    * @param    offset
+    * @return   none
+    */    
+    void SetPressureOffset( char offset);
+    
+    /**
+    * Configure the Temperature offset.
+    * Temperature user accessible offset trim value expressed as an 8-bit 2's complement number. 
+    * The user offset registers may be adjusted to enhance accuracy and optimize the system performance. 
+    * Range is from -8 to +7.9375°C 0.0625°C per LSB.
+    * In RAW output mode no scaling or offsets will be applied in the digital domain
+    *
+    * @param    offset
+    * @return   none
+    */    
+    void SetTemperatureOffset( char offset);
+    
+    /**
+    * Configure the Altitude offset.
+    * Altitude Data User Offset Register (OFF_H) is expressed as a 2’s complement number in meters. 
+    * The user offset register provides user adjustment to the vertical height of the Altitude output. 
+    * The range of values are from -128 to +127 meters.
+    * In RAW output mode no scaling or offsets will be applied in the digital domain
+    *
+    * @param    offset
+    * @return   none
+    */    
+    void SetAltitudeOffset( char offset);        
+
+    /**
+     * @brief  Get current device's mode.
+     * @return [ALTIMETER_MODE] or [BAROMETRIC_MODE]
+     */
+    uint8_t getMode();
+private:
+    I2C m_i2c;
+    int m_addr;
+    InterruptIn MPL3115A2_Int1;
+    InterruptIn MPL3115A2_Int2;
+
+    unsigned char MPL3115A2_mode;
+    unsigned char MPL3115A2_oversampling;
+    void DataReady_IRQ( void);
+    void AltitudeTrg_IRQ( void);
+    
+    /** Set the device in active mode
+    */
+    void Active( void);
+    
+    /** Set the device in standby mode
+    */
+    void Standby( void);
+    
+    /** Get the altimiter value from the sensor.
+    *
+    * @param    reg the register from which read the data. 
+    *           Can be: REG_ALTIMETER_MSB for altimeter value
+    *                   REG_ALTI_MIN_MSB for the minimum value captured
+    *                   REG_ALTI_MAX_MSB for the maximum value captured
+    */
+    float getAltimeter( unsigned char reg);    
+
+    /** Get the pressure value from the sensor.
+    *
+    * @param    reg the register from which read the data. 
+    *           Can be: REG_PRESSURE_MSB for altimeter value
+    *                   REG_PRES_MIN_MSB for the minimum value captured
+    *                   REG_PRES_MAX_MSB for the maximum value captured
+    */    
+    float getPressure( unsigned char reg);
+
+    /** Get the altimiter value from the sensor.
+    *
+    * @param    reg the register from which read the data. 
+    *           Can be: REG_TEMP_MSB for altimeter value
+    *                   REG_TEMP_MIN_MSB for the minimum value captured
+    *                   REG_TEMP_MAX_MSB for the maximum value captured
+    */
+    float getTemperature( unsigned char reg);
+    
+    void readRegs(int addr, uint8_t * data, int len);
+    void writeRegs(uint8_t * data, int len);
+
+};
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/FRDM_FXS_MULTI_B/MotionSensor.h	Thu Mar 21 09:03:32 2019 +0000
@@ -0,0 +1,114 @@
+/* MotionSensor Base Class
+ * Copyright (c) 2014-2015 ARM Limited
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef MOTIONSENSOR_H
+#define MOTIONSENSOR_H
+
+#include "stdint.h"
+
+/** motion_data_counts_t struct
+ */
+typedef struct {
+    int16_t x;      /*!< x-axis counts */
+    int16_t y;      /*!< y-axis counts */
+    int16_t z;      /*!< z-axis counts */
+} motion_data_counts_t;
+
+/** motion_data_units_t struct
+ */
+typedef struct {
+    float x;        /*!< x-axis counts */
+    float y;        /*!< y-axis counts */
+    float z;        /*!< z-axis counts */
+} motion_data_units_t;
+
+typedef motion_data_units_t MotionSensorDataUnits;
+typedef motion_data_counts_t  MotionSensorDataCounts;
+/** Motion Sensor Base Class
+    Useful for accessing data in a common way
+ */
+class MotionSensor
+{
+//public:
+
+    // /** Enable the sensor for operation
+    //  */
+    // virtual void enable(void) const = 0;
+
+    // /** disable the sensors operation
+    //  */
+    // virtual void disable(void) const = 0;
+
+    // /** Set the sensor sample rate
+    //     @param frequency The desires sample frequency
+    //     @return The amount of error in Hz between desired and actual frequency
+    //  */
+    // virtual uint32_t sampleRate(uint32_t frequency) const = 0;
+
+    // /** Tells of new data is ready
+    //     @return The amount of data samples ready to be read from a device
+    //  */
+    // virtual uint32_t dataReady(void) const = 0;
+
+    // /** Get the x data in counts
+    //     @param x A referene to the variable to put the data in, 0 denotes not used
+    //     @return The x data in counts
+    //  */
+    // virtual int16_t getX(int16_t &x) const = 0;
+
+    // /** Get the y data in counts
+    //     @param y A referene to the variable to put the data in, 0 denotes not used
+    //     @return The y data in counts
+    //  */
+    // virtual int16_t getY(int16_t &y) const = 0;
+
+    // * Get the z data in counts
+    //     @param z A referene to the variable to put the data in, 0 denotes not used
+    //     @return The z data in counts
+     
+    // virtual int16_t getZ(int16_t &z) const = 0;
+
+    // /** Get the x data in units
+    //     @param x A referene to the variable to put the data in, 0 denotes not used
+    //     @return The x data in units
+    //  */
+    // virtual float getX(float &x) const = 0;
+
+    // /** Get the y data in units
+    //     @param y A referene to the variable to put the data in, 0 denotes not used
+    //     @return The y data in units
+    //  */
+    // virtual float getY(float &y) const = 0;
+
+    // /** Get the z data in units
+    //     @param z A referene to the variable to put the data in, 0 denotes not used
+    //     @return The z data in units
+    //  */
+    // virtual float getZ(float &z) const = 0;
+
+    // /** Get the x,y,z data in counts
+    //     @param xyz A referene to the variable to put the data in, 0 denotes not used
+    //  */
+    // virtual void getAxis(motion_data_counts_t &xyz) const = 0;
+
+    // /** Get the x,y,z data in units
+    //     @param xyz A referene to the variable to put the data in, 0 denotes not used
+    //  */
+    // virtual void getAxis(motion_data_units_t &xyz) const = 0;
+};
+
+#endif
+

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/common_define.cpp	Thu Mar 21 09:03:32 2019 +0000
@@ -0,0 +1,51 @@
+#include "common_define.h"
+
+
+
+
+void i2cWrite(I2C * dev_i2c,uint8_t address,uint8_t* pBuffer, uint8_t RegisterAddr, uint16_t NumByteToWrite)
+{
+    uint8_t temp_buffer[NumByteToWrite + 1];
+    temp_buffer[0] = RegisterAddr;
+    memcpy(temp_buffer + 1,pBuffer,NumByteToWrite);
+    dev_i2c->write((int)address,(const char *)temp_buffer,NumByteToWrite + 1);
+}
+
+
+void i2cRead(I2C * dev_i2c,uint8_t address,uint8_t* pBuffer, uint8_t RegisterAddr, uint16_t NumByteToRead)
+{
+    dev_i2c->write((int)address,(const char *)&RegisterAddr,1);
+    dev_i2c->read((int)address,(char *)pBuffer,NumByteToRead);
+}
+
+void i2cWriteForVL6180X(I2C * dev_i2c,uint8_t address,uint8_t* pBuffer, uint16_t RegisterAddr, uint16_t NumByteToWrite)
+{   
+    uint8_t temp_buffer[NumByteToWrite + 2];
+    temp_buffer[0] = (RegisterAddr >> 8) & 0xff;
+    temp_buffer[1] = (RegisterAddr) & 0xff;
+    memcpy(temp_buffer + 2,pBuffer,NumByteToWrite);
+    dev_i2c->write((int)address,(const char *)temp_buffer,NumByteToWrite + 2);
+}
+
+void i2cReadForVL6180X(I2C * dev_i2c,uint8_t address,uint8_t* pBuffer, uint16_t RegisterAddr, uint16_t NumByteToRead)
+{
+    uint8_t temp_buffer[2];
+    temp_buffer[0] = (RegisterAddr >> 8) & 0xff;
+    temp_buffer[1] = (RegisterAddr) & 0xff;
+    dev_i2c->write((int)address,(const char *)temp_buffer,2);
+    dev_i2c->read((int)address,(char *)pBuffer,NumByteToRead);
+}
+
+void i2cReadForFXS_MUTIL(I2C * dev_i2c,uint8_t address,uint8_t* pBuffer, uint16_t RegisterAddr, uint16_t NumByteToRead)
+{
+    dev_i2c->write((int)address,(const char *)&RegisterAddr,1,true);
+    dev_i2c->read((int)address,(char *)pBuffer,NumByteToRead);
+}
+
+void i2cWriteForFXS_MUTIL(I2C * dev_i2c,uint8_t address,uint8_t* pBuffer, uint16_t RegisterAddr, uint16_t NumByteToWrite)
+{
+    uint8_t temp_buffer[NumByteToWrite + 1];
+    temp_buffer[0] = RegisterAddr;
+    memcpy(temp_buffer + 1,pBuffer,NumByteToWrite);
+    dev_i2c->write((int)address,(const char *)temp_buffer,NumByteToWrite + 1);
+}

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/common_define.h	Thu Mar 21 09:03:32 2019 +0000
@@ -0,0 +1,24 @@
+#ifndef COMMON_DEFINE_H
+#define COMMON_DEFINE_H
+
+
+#include "mbed.h"
+
+
+#define TwoWire I2C
+#define delay(x)   wait_ms(x)
+
+
+void i2cWrite(I2C * dev_i2c,uint8_t address,uint8_t* pBuffer, uint8_t RegisterAddr, uint16_t NumByteToWrite);
+
+void i2cRead(I2C * dev_i2c,uint8_t address,uint8_t* pBuffer, uint8_t RegisterAddr, uint16_t NumByteToRead);
+
+void i2cWriteForVL6180X(I2C * dev_i2c,uint8_t address,uint8_t* pBuffer, uint16_t RegisterAddr, uint16_t NumByteToWrite);
+
+void i2cReadForVL6180X(I2C * dev_i2c,uint8_t address,uint8_t* pBuffer, uint16_t RegisterAddr, uint16_t NumByteToRead);
+
+void i2cReadForFXS_MUTIL(I2C * dev_i2c,uint8_t address,uint8_t* pBuffer, uint16_t RegisterAddr, uint16_t NumByteToRead);
+
+void i2cWriteForFXS_MUTIL(I2C * dev_i2c,uint8_t address,uint8_t* pBuffer, uint16_t RegisterAddr, uint16_t NumByteToWrite);
+
+#endif

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/main.cpp	Thu Mar 21 09:03:32 2019 +0000
@@ -0,0 +1,87 @@
+#include "mbed.h"
+#include "MAG3110.h"
+#include "FXOS8700Q.h"
+#include "MMA8652.h"
+#include "FXLS8471Q.h"
+#include "FXAS21000.h"
+#include "MPL3115A2.h"
+
+
+
+
+#if __TT_M3HQ__
+#define DISPLAY_TMPM_STRING "Welcome to Thundersoft TT_M3HQ"
+#endif
+
+
+#if __TT_M4G9__
+#define DISPLAY_TMPM_STRING "Welcome to Thundersoft TT_M4G9"
+#endif
+
+
+int main()
+{
+    FXLS8471Q acc1(D11, D12, D13, D10);
+    MMA8652   acc2(D14, D15);
+    FXOS8700Q_acc combo_acc(D14, D15, FXOS8700CQ_SLAVE_ADDR0);
+    FXOS8700Q_mag combo_mag(D14, D15, FXOS8700CQ_SLAVE_ADDR0);
+    MAG3110   mag2(D14, D15);
+    FXAS21000 gyro(D14, D15);
+    MPL3115A2 wigo_sensor1(D14, D15, MPL3115A2_DEFAULT_ADDRESS,(PinName)0,(PinName)0);
+    float acc_data[3], gyro_data[3],sensor_data[2];
+    MotionSensorDataUnits adata;
+    MotionSensorDataUnits mdata;
+    int16_t acc_raw[3];
+    printf("%s\r\n",DISPLAY_TMPM_STRING);
+    combo_acc.enable();
+    combo_mag.enable();
+    mag2.enable();
+    wigo_sensor1.Barometric_Mode();
+    wigo_sensor1.Oversample_Ratio( OVERSAMPLE_RATIO_32);
+     printf("FXLS8471 Acc   = 0x%x\r\n", acc1.getWhoAmI());
+     printf("MMA8652 Acc    = 0x%x\r\n", acc2.getWhoAmI());
+     printf("FXOS8700 Combo = 0x%x\r\n", combo_acc.whoAmI());
+     printf("MAG3110 Mag    = 0x%x\r\n", mag2.whoAmI());
+     printf("FXAS21000 Gyro = 0x%x\r\n", gyro.getWhoAmI());
+     printf("MPL3115A2 = 0x%x \r\n",wigo_sensor1.getDeviceID());    
+     while(1) {
+        acc1.ReadXYZ(acc_data);
+        acc1.ReadXYZraw(acc_raw);
+        printf("FXLS8471 Acc:   X:%6.3f Y:%6.3f Z:%6.3f (Raw X:%4d Y:%4d Z:%4d)\r\n", acc_data[0], acc_data[1], acc_data[2], acc_raw[0], acc_raw[1], acc_raw[2]);
+
+        acc2.ReadXYZ(acc_data);
+        acc2.ReadXYZraw(acc_raw);
+        printf("MMA8652 Acc:    X:%6.3f Y:%6.3f Z:%6.3f (Raw X:%4d Y:%4d Z:%4d)\r\n", acc_data[0], acc_data[1], acc_data[2], acc_raw[0], acc_raw[1], acc_raw[2]);
+
+        combo_acc.getAxis(adata);
+        printf("FXOS8700 Acc:   X:%6.3f Y:%6.3f Z:%6.3f\r\n", adata.x, adata.y, adata.z);
+        
+        combo_mag.getAxis(mdata);
+        printf("FXOS8700 Mag:   X:%6.2f Y:%6.2f Z:%6.2f\r\n", mdata.x, mdata.y, mdata.z);
+
+        mag2.getAxis(mdata);
+        printf("MAG3110 Mag:    X:%6.2f Y:%6.2f Z:%6.2f\r\n", mdata.x, mdata.y, mdata.z);
+
+        gyro.ReadXYZ(gyro_data);
+        printf("FXAS21000 Gyro: X:%6.2f Y:%6.2f Z:%6.2f\r\n", gyro_data[0], gyro_data[1], gyro_data[2]);
+        
+        
+        if ( wigo_sensor1.isDataAvailable()) 
+        {
+            if(wigo_sensor1.getMode() == BAROMETRIC_MODE)
+            {
+              wigo_sensor1.getAllData( &sensor_data[0]);
+              printf("Pressure: %f Temperature: %f\r\n", sensor_data[0], sensor_data[1]);
+              wigo_sensor1.Altimeter_Mode();
+            }
+            else
+            {
+              wigo_sensor1.getAllData( &sensor_data[0]);
+              printf("Altitude: %f Temperature: %f\r\n", sensor_data[0], sensor_data[1]);
+              wigo_sensor1.Barometric_Mode();
+            }
+        }
+        printf("\r\n");
+        wait_ms(1000);
+     }
+}
\ No newline at end of file

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/mbed-os.lib	Thu Mar 21 09:03:32 2019 +0000
@@ -0,0 +1,1 @@
+https://github.com/ARMmbed/mbed-os/#51d55508e8400b60af467005646c4e2164738d48