mirror of
https://github.com/eclipse/upm.git
synced 2025-03-14 20:47:30 +03:00
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>
This commit is contained in:
parent
4bf30df933
commit
fd54745cc0
@ -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;
|
||||
}
|
||||
|
@ -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;
|
||||
|
@ -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];
|
||||
};
|
||||
|
||||
}
|
||||
|
Loading…
x
Reference in New Issue
Block a user