Diff: ALTIMU.cpp

Revision:

0:08c9e51e6fd6

Child:

1:8cc36ccb8d58

--- /dev/null	Thu Jan 01 00:00:00 1970 +0000
+++ b/ALTIMU.cpp	Mon Oct 15 02:14:19 2018 +0000
@@ -0,0 +1,105 @@
+/**
+ */
+ #include "ALTIMU.h"
+
+// Public Methods //////////////////////////////////////////////////////////////
+
+// Constructor
+Altimu::Altimu(PinName sda, PinName scl):
+    _ALTIMU(sda, scl)
+{
+    // Setting I2C operation frequency.
+    _ALTIMU.frequency(200000);
+
+    // L3GD20 configuration.
+    // DRDY_HL (DRDY active high);I2C_dis = (I2C & SPI enable); SW = (Normal Mode); Low_ODR = (Low speed disable).
+    write_reg(L3GD20_ADDR,0x39,0x00);
+    // DR = 01 (200 Hz ODR); BW = 10 (50 Hz bandwidth); PD = 1 (normal mode); Zen = Yen = Xen = 1 (all axes enabled).
+    write_reg(L3GD20_ADDR,0x20,0x5F); //Setting CTRL_REG1
+    // End L3GD20 configuration.
+
+    // LSM303 configuration
+        // Acell configuration
+          // 50 Hz X/Y/Z axis enable.
+    write_reg(LSM303_ADDR, 0x20, 0x57);
+        // mag
+            //continuous mag
+    write_reg(LSM303_ADDR, 0x24, 0x78);
+    write_reg(LSM303_ADDR, 0x26, 0x00);
+    // End LSM303 configuration.
+    // LPS25H configuration
+    write_reg(LPS25H_ADDR, 0x10, 0x05);
+    write_reg(LPS25H_ADDR, 0x2e, 0xdf);
+    write_reg(LPS25H_ADDR, 0x21, 0x40);
+    write_reg(LPS25H_ADDR, 0x20, 0x90);
+}
+// L3GD30 read data function,
+bool Altimu::read_L3GD20(float *gx, float *gy, float *gz) {
+    char gyr[6];
+    if (recv(L3GD20_ADDR, 0x28, gyr, 6)) {
+    //scale is 8.75 mdps/digit
+        *gx = float(short(gyr[1] << 8 | gyr[0]))*0.00875;
+        *gy =  float(short(gyr[3] << 8 | gyr[2]))*0.00875;
+        *gz =  float(short(gyr[5] << 8 | gyr[4]))*0.00875;
+        return true;
+    }
+    return false;
+}
+
+// LSM303D read data function.
+bool Altimu::read_LSM303D(float *ax, float *ay, float *az, float *mx, float *my, float *mz) {
+    char acc[6], mag[6];
+
+    if (recv(LSM303_ADDR, 0x28, acc, 6) && recv(LSM303_ADDR, 0x08, mag, 6)) {
+        *ax = float(short(acc[1] << 8 | acc[0]))*0.061;  //32768/4=8192
+        *ay =  float(short(acc[3] << 8 | acc[2]))*0.061;
+        *az =  float(short(acc[5] << 8 | acc[4]))*0.061;
+        //+-4gauss
+        *mx = float(short(mag[0] << 8 | mag[1]))*0.16;
+        *mz = float(short(mag[2] << 8 | mag[3]))*0.16;
+        *my = float(short(mag[4] << 8 | mag[5]))*0.16;
+
+        return true;
+    }
+
+    return false;
+}
+
+// LPS25H read data function.
+void Altimu::read_LPS25H(float *press, float *alt)
+{
+    char dt[3];
+    dt[0] = 0x28 | 0x80;
+    _ALTIMU.write(LPS25H_ADDR, dt, 1, true);
+    _ALTIMU.read(LPS25H_ADDR, dt, 3, false);
+    *press = dt[2] << 16 | dt[1] << 8 | dt[0];
+    *press /= 4096;
+    *alt = (1-pow(*press / 1013.25, 0.190263))*44330.8;
+}
+
+// I2C functions
+bool Altimu::write_reg(int addr_i2c,int addr_reg, char v)
+{
+    char data[2] = {addr_reg, v};
+    return Altimu::_ALTIMU.write(addr_i2c, data, 2) == 0;
+}
+
+bool Altimu::read_reg(int addr_i2c,int addr_reg, char *v)
+{
+    char data = addr_reg;
+    bool result = false;
+
+    __disable_irq();
+    if ((_ALTIMU.write(addr_i2c, &data, 1) == 0) && (_ALTIMU.read(addr_i2c, &data, 1) == 0)){
+        *v = data;
+        result = true;
+    }
+    __enable_irq();
+    return result;
+}
+
+bool Altimu::recv(char sad, char sub, char *buf, int length) {
+    if (length > 1) sub |= 0x80;
+
+    return _ALTIMU.write(sad, &sub, 1, true) == 0 && _ALTIMU.read(sad, buf, length) == 0;
+}