From fd54745cc04fa169f9fc33ed797f6e06207934b0 Mon Sep 17 00:00:00 2001 From: "Mccool, Michael" Date: Mon, 10 Nov 2014 09:20:10 +0000 Subject: [PATCH] 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 Signed-off-by: Brendan Le Foll --- examples/lsm303.cxx | 27 +++++++++++--- src/lsm303/lsm303.cxx | 83 +++++++++++++++++++++++++++++-------------- src/lsm303/lsm303.h | 48 +++++++++++++++++++------ 3 files changed, 117 insertions(+), 41 deletions(-) diff --git a/examples/lsm303.cxx b/examples/lsm303.cxx index d578e187..f54096d1 100644 --- a/examples/lsm303.cxx +++ b/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(); - - std::cout << "X " << (int)accel[0] << " - Y " << (int)accel[1] << " - Z " << (int)accel[2] << std::endl; + int16_t* accel = sensor->getRawAccelData(); + std::cout << "acc: rX " << (int)accel[0] + << " - 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; } diff --git a/src/lsm303/lsm303.cxx b/src/lsm303/lsm303.cxx index b6653418..9ebd98be 100644 --- a/src/lsm303/lsm303.cxx +++ b/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 - setRegisterSafe(m_addrAcc, CTRL_REG4_A, 0x00); + // scale can be 2, 4 or 8 + 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) - heading += 360; + if (heading < 0.0) + 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 -LSM303::getAccelY() -{ - return accel[2]; -} - -uint8_t -LSM303::getAccelZ() -{ - return accel[0]; -} - -uint8_t +int16_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[0] = (readThenWrite(OUT_Y_L_A) << 8) | (readThenWrite(OUT_Y_H_A)); - accel[1] = (readThenWrite(OUT_Z_L_A) << 8) | (readThenWrite(OUT_Z_H_A)); + accel[X] = (int16_t(readThenWrite(OUT_X_H_A)) << 8) + | int16_t(readThenWrite(OUT_X_L_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; diff --git a/src/lsm303/lsm303.h b/src/lsm303/lsm303.h index 02fb99ad..728c23b7 100644 --- a/src/lsm303/lsm303.h +++ b/src/lsm303/lsm303.h @@ -57,8 +57,8 @@ namespace upm { #define OUT_Z_H_A 0x2D #define X 0 -#define Z 1 -#define Y 2 +#define Y 1 +#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]; - uint8_t accel[3]; + int16_t coor[3]; + int16_t accel[3]; }; }