lsm303: fix types returned by module and heading formula

* getRawAccelData() & other accel functions now return int16_t
* scale is now a supported constructor parameter
* heading returns 0-360 values

Signed-off-by: Mccool, Michael <michael.mccool@intel.com>
Signed-off-by: Brendan Le Foll <brendan.le.foll@intel.com>

examples/lsm303.cxx

@@ -32,12 +32,31 @@ main(int argc, char **argv)
 {
     upm::LSM303 *sensor = new upm::LSM303(0);
 
-    std::cout << sensor->getHeading() << std::endl;
+    sensor->getCoordinates();
+    int16_t* coor = sensor->getRawCoorData(); // in XZY order
+    std::cout << "coor: rX " << (int)coor[0]
+              << " - rY " << (int)coor[2]  // note: index is 2
+              << " - rZ " << (int)coor[1]  // note: index is 1
+              << std::endl;
+    std::cout << "coor: gX " << sensor->getCoorX()
+              << " - gY " << sensor->getCoorY()
+              << " - gZ " << sensor->getCoorZ()
+              << std::endl;
+
+    std::cout << "heading: "
+              << sensor->getHeading()
+              << std::endl;
 
     sensor->getAcceleration();
-    uint8_t* accel = sensor->getRawAccelData();
+    int16_t* accel = sensor->getRawAccelData();
-
+    std::cout << "acc: rX " << (int)accel[0]
-    std::cout << "X " << (int)accel[0] << " - Y " << (int)accel[1] << " - Z " << (int)accel[2] << std::endl;
+              << " - rY " << (int)accel[1]
+              << " - Z " << (int)accel[2]
+              << std::endl;
+    std::cout << "acc: gX " << sensor->getAccelX()
+              << " - gY " << sensor->getAccelY()
+              << " - gZ " << sensor->getAccelZ()
+              << std::endl;
 
     return 0;
 }

src/lsm303/lsm303.cxx

@@ -33,7 +33,7 @@
 
 using namespace upm;
 
-LSM303::LSM303(int bus, int addrMag, int addrAcc)
+LSM303::LSM303(int bus, int addrMag, int addrAcc, int accScale)
 {
     mraa_result_t ret = MRAA_SUCCESS;
 
@@ -45,8 +45,14 @@ LSM303::LSM303(int bus, int addrMag, int addrAcc)
     // 0x27 is the 'normal' mode with X/Y/Z enable
     setRegisterSafe(m_addrAcc, CTRL_REG1_A, 0x27);
 
-    // scale == 2, can be 4 or 8
+    // scale can be 2, 4 or 8
-    setRegisterSafe(m_addrAcc, CTRL_REG4_A, 0x00);
+    if (2 == accScale) {
+      setRegisterSafe(m_addrAcc, CTRL_REG4_A, 0x00);
+    } else if (4 == accScale) {
+      setRegisterSafe(m_addrAcc, CTRL_REG4_A, 0x10);
+    } else { // default; equivalent to 8g
+      setRegisterSafe(m_addrAcc, CTRL_REG4_A, 0x30);
+    }
 
     // 0x10 = minimum datarate ~15Hz output rate
     setRegisterSafe(m_addrMag, CRA_REG_M, 0x10);
@@ -71,42 +77,42 @@ LSM303::getHeading()
         return -1;
     }
 
-    float heading = 180 * atan2(coor[Y], coor[X])/M_PI;
+    float heading = 180.0 * atan2(double(coor[Y]), double(coor[X]))/M_PI;
 
-    if (heading < 0)
+    if (heading < 0.0)
-        heading += 360;
+        heading += 360.0;
 
     return heading;
 }
 
-uint8_t*
+int16_t*
 LSM303::getRawAccelData()
 {
     return &accel[0];
 }
 
-uint8_t*
+int16_t*
 LSM303::getRawCoorData()
 {
     return &coor[0];
 }
 
-uint8_t
+int16_t
-LSM303::getAccelY()
-{
-  return accel[2];
-}
-
-uint8_t
-LSM303::getAccelZ()
-{
-  return accel[0];
-}
-
-uint8_t
 LSM303::getAccelX()
 {
-  return accel[1];
+  return accel[X];
+}
+
+int16_t
+LSM303::getAccelY()
+{
+  return accel[Y];
+}
+
+int16_t
+LSM303::getAccelZ()
+{
+  return accel[Z];
 }
 
 mraa_result_t
@@ -124,15 +130,35 @@ LSM303::getCoordinates()
     }
     // convert to coordinates
     for (int i=0; i<3; i++) {
-        coor[i] = (buf[2*i] << 8) | buf[(2*i)+1];
+        coor[i] = (int16_t(buf[2*i] << 8))
+                |  int16_t(buf[(2*i)+1]);
     }
-    // note that coor array is in XZY order
+    // swap elements 1 and 2 to get things in natural XYZ order
+    int16_t t = coor[2];
+    coor[2] = coor[1];
+    coor[1] = t;
     //printf("X=%x, Y=%x, Z=%x\n", coor[X], coor[Y], coor[Z]);
 
     return ret;
 }
 
+int16_t
+LSM303::getCoorX() {
+  return coor[X];
+}
+
+int16_t
+LSM303::getCoorY() {
+  return coor[Y];
+}
+
+int16_t
+LSM303::getCoorZ() {
+  return coor[Z];
+}
+
 // helper function that writes a value to the acc and then reads
+// FIX: shouldn't this be write-then-read?
 int
 LSM303::readThenWrite(uint8_t reg)
 {
@@ -147,9 +173,12 @@ LSM303::getAcceleration()
 {
     mraa_result_t ret = MRAA_SUCCESS;
 
-    accel[2] = (readThenWrite(OUT_X_L_A) << 8) | (readThenWrite(OUT_X_H_A));
+    accel[X] = (int16_t(readThenWrite(OUT_X_H_A)) << 8)
-    accel[0] = (readThenWrite(OUT_Y_L_A) << 8) | (readThenWrite(OUT_Y_H_A));
+             |  int16_t(readThenWrite(OUT_X_L_A));
-    accel[1] = (readThenWrite(OUT_Z_L_A) << 8) | (readThenWrite(OUT_Z_H_A));
+    accel[Y] = (int16_t(readThenWrite(OUT_Y_H_A)) << 8)
+             |  int16_t(readThenWrite(OUT_Y_L_A));
+    accel[Z] = (int16_t(readThenWrite(OUT_Z_H_A)) << 8)
+             |  int16_t(readThenWrite(OUT_Z_L_A));
     //printf("X=%x, Y=%x, Z=%x\n", accel[X], accel[Y], accel[Z]);
 
     return ret;

src/lsm303/lsm303.h

@@ -57,8 +57,8 @@ namespace upm {
 #define OUT_Z_H_A 0x2D
 
 #define X 0
-#define Z 1
+#define Y 1
-#define Y 2
+#define Z 2
 
 /**
  * @brief LSM303 accelerometer/compass library
@@ -87,7 +87,10 @@ class LSM303 {
          * @param addr magometer
      * @param addr accelerometer
          */
-        LSM303 (int bus, int addrMag=LSM303_MAG, int addrAcc=LSM303_ACC);
+        LSM303 (int bus,
+                int addrMag=LSM303_MAG,
+                int addrAcc=LSM303_ACC,
+                int accScale=8);
 
         /**
          * LSM303 object destructor
@@ -108,24 +111,49 @@ class LSM303 {
 
         /**
          * Get accelerometer values
+         * Call before calling other "get" functions for acceleration
          */
         mraa_result_t getAcceleration();
 
         /**
          * Get the raw coordinate data, this will get updated when getCoordinates() is called
          */
-        uint8_t* getRawCoorData();
+        int16_t* getRawCoorData();
 
-        uint8_t getAccelY();
+        /**
+         * Just get the X component of the coordinate data
+         */
+        int16_t getCoorX();
 
-        uint8_t getAccelZ();
+        /**
+         * Just get the Y component of the coordinate data
+         */
+        int16_t getCoorY();
 
-        uint8_t getAccelX();
+        /**
+         * Just get the Z component of the coordinate data
+         */
+        int16_t getCoorZ();
 
         /**
          * Get the raw accelerometer data, this will get updated when getAcceleration() is called
          */
-        uint8_t* getRawAccelData();
+        int16_t* getRawAccelData();
+
+        /**
+         * Just get the X component of the acceleration
+         */
+        int16_t getAccelX();
+
+        /**
+         * Just get the Y component of the acceleration
+         */
+        int16_t getAccelY();
+
+        /**
+         * Just get the Z component of the acceleration
+         */
+        int16_t getAccelZ();
 
     private:
         int readThenWrite(uint8_t reg);
@@ -135,8 +163,8 @@ class LSM303 {
         int m_addrMag;
     int m_addrAcc;
         uint8_t buf[6];
-        uint8_t coor[3];
+        int16_t coor[3];
-        uint8_t accel[3];
+        int16_t accel[3];
 };
 
 }