Skip to content

Commit 4e8a1c4

Browse files
tyethtyeth
committed
[wip] add LSM303AGR/DLH
1 parent 345fee8 commit 4e8a1c4

File tree

6 files changed

+330
-0
lines changed

6 files changed

+330
-0
lines changed

‎src/components/i2c/controller.cpp‎

Lines changed: 10 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -178,6 +178,16 @@ static const std::map<std::string, FnCreateI2CSensorDriver> I2cFactorySensor = {
178178
const char *driver_name) -> drvBase * {
179179
return new drvLis3mdl(i2c, addr, mux_channel, driver_name);
180180
}},
181+
{"lsm303agr",
182+
[](TwoWire *i2c, uint16_t addr, uint32_t mux_channel,
183+
const char *driver_name) -> drvBase * {
184+
return new drvLsm303agr(i2c, addr, mux_channel, driver_name);
185+
}},
186+
{"lsm303dlh",
187+
[](TwoWire *i2c, uint16_t addr, uint32_t mux_channel,
188+
const char *driver_name) -> drvBase * {
189+
return new drvLsm303dlh(i2c, addr, mux_channel, driver_name);
190+
}},
181191
{"lsm6ds3",
182192
[](TwoWire *i2c, uint16_t addr, uint32_t mux_channel,
183193
const char *driver_name) -> drvBase * {

‎src/components/i2c/controller.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -41,6 +41,8 @@
4141
#include "drivers/drvIna260.h"
4242
#include "drivers/drvLc709203f.h"
4343
#include "drivers/drvLis3dh.h"
44+
#include "drivers/drvLsm303agr.h"
45+
#include "drivers/drvLsm303dlh.h"
4446
#include "drivers/drvIsm330dhcx.h"
4547
#include "drivers/drvLis3mdl.h"
4648
#include "drivers/drvIsm330dlc.h"

‎src/components/i2c/drivers/drvLsm303agr.cpp‎

Lines changed: 117 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,117 @@
1+
/*!
2+
* @file drvLsm303agr.cpp
3+
*
4+
* Driver wrapper for the Adafruit LSM303AGR combo sensor.
5+
*/
6+
7+
#include "drvLsm303agr.h"
8+
9+
#include <math.h>
10+
11+
namespace {
12+
constexpr uint8_t kLsm303agrAccelDefaultAddr = LSM303_ADDRESS_ACCEL;
13+
constexpr uint8_t kLsm303agrMagDefaultAddr = 0x1E; // LIS2MDL address
14+
}
15+
16+
/******************************************************************************/
17+
/*! @brief Destructor */
18+
/******************************************************************************/
19+
drvLsm303agr::~drvLsm303agr() { teardown(); }
20+
21+
/******************************************************************************/
22+
void drvLsm303agr::teardown() {
23+
if (_accel) {
24+
delete _accel;
25+
_accel = nullptr;
26+
}
27+
if (_mag) {
28+
delete _mag;
29+
_mag = nullptr;
30+
}
31+
}
32+
33+
/******************************************************************************/
34+
bool drvLsm303agr::begin() {
35+
teardown();
36+
37+
_accel = new Adafruit_LSM303_Accel_Unified();
38+
_mag = new Adafruit_LIS2MDL();
39+
if (!_accel || !_mag) {
40+
teardown();
41+
return false;
42+
}
43+
44+
const uint8_t accel_addr =
45+
_address == 0 ? kLsm303agrAccelDefaultAddr : (uint8_t)_address;
46+
47+
WS_DEBUG_PRINT("[drvLsm303agr] Initialising accel @ 0x");
48+
WS_DEBUG_PRINTHEX(accel_addr);
49+
WS_DEBUG_PRINTLN("...");
50+
if (!_accel->begin(accel_addr, _i2c)) {
51+
WS_DEBUG_PRINTLN("[drvLsm303agr] Failed to initialise accelerometer");
52+
teardown();
53+
return false;
54+
}
55+
_accel->setRange(LSM303_RANGE_2G);
56+
_accel->setMode(LSM303_MODE_HIGH_RESOLUTION);
57+
58+
WS_DEBUG_PRINT("[drvLsm303agr] Initialising magnetometer @ 0x");
59+
WS_DEBUG_PRINTHEX(kLsm303agrMagDefaultAddr);
60+
WS_DEBUG_PRINTLN("...");
61+
if (!_mag->begin(kLsm303agrMagDefaultAddr, _i2c)) {
62+
WS_DEBUG_PRINTLN("[drvLsm303agr] Failed to initialise LIS2MDL");
63+
teardown();
64+
return false;
65+
}
66+
_mag->setDataRate(LIS2MDL_RATE_50_HZ);
67+
_mag->enableInterrupts(false);
68+
_mag->interruptsActiveHigh(false);
69+
70+
WS_DEBUG_PRINTLN("[drvLsm303agr] Sensor initialised successfully");
71+
return true;
72+
}
73+
74+
bool drvLsm303agr::computeAccelMagnitude(float &magnitude) {
75+
if (!_accel) {
76+
return false;
77+
}
78+
sensors_event_t accel_event;
79+
if (!_accel->getEvent(&accel_event)) {
80+
return false;
81+
}
82+
magnitude = sqrtf(accel_event.acceleration.x * accel_event.acceleration.x +
83+
accel_event.acceleration.y * accel_event.acceleration.y +
84+
accel_event.acceleration.z * accel_event.acceleration.z);
85+
return true;
86+
}
87+
88+
bool drvLsm303agr::getEventRaw(sensors_event_t *rawEvent) {
89+
float magnitude = 0.0f;
90+
if (!computeAccelMagnitude(magnitude)) {
91+
return false;
92+
}
93+
rawEvent->data[0] = magnitude;
94+
return true;
95+
}
96+
97+
bool drvLsm303agr::getEventAccelerometer(sensors_event_t *accelEvent) {
98+
if (!_accel) {
99+
return false;
100+
}
101+
return _accel->getEvent(accelEvent);
102+
}
103+
104+
bool drvLsm303agr::getEventMagneticField(sensors_event_t *magEvent) {
105+
if (!_mag) {
106+
return false;
107+
}
108+
return _mag->getEvent(magEvent);
109+
}
110+
111+
void drvLsm303agr::ConfigureDefaultSensorTypes() {
112+
_default_sensor_types_count = 2;
113+
_default_sensor_types[0] =
114+
wippersnapper_sensor_SensorType_SENSOR_TYPE_ACCELEROMETER;
115+
_default_sensor_types[1] =
116+
wippersnapper_sensor_SensorType_SENSOR_TYPE_MAGNETIC_FIELD;
117+
}

‎src/components/i2c/drivers/drvLsm303agr.h‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
/*!
2+
* @file drvLsm303agr.h
3+
*
4+
* Driver wrapper for the Adafruit LSM303AGR combo sensor (accel + mag).
5+
*/
6+
#ifndef DRV_LSM303AGR_H
7+
#define DRV_LSM303AGR_H
8+
9+
#include "Wippersnapper_V2.h"
10+
#include "drvBase.h"
11+
12+
#include <Adafruit_LIS2MDL.h>
13+
#include <Adafruit_LSM303_Accel.h>
14+
15+
class drvLsm303agr : public drvBase {
16+
public:
17+
drvLsm303agr(TwoWire *i2c, uint16_t sensorAddress, uint32_t mux_channel,
18+
const char *driver_name)
19+
: drvBase(i2c, sensorAddress, mux_channel, driver_name) {}
20+
21+
~drvLsm303agr();
22+
23+
bool begin() override;
24+
25+
bool getEventRaw(sensors_event_t *rawEvent) override;
26+
27+
bool getEventAccelerometer(sensors_event_t *accelEvent) override;
28+
29+
bool getEventMagneticField(sensors_event_t *magEvent) override;
30+
31+
protected:
32+
void ConfigureDefaultSensorTypes() override;
33+
34+
private:
35+
void teardown();
36+
bool computeAccelMagnitude(float &magnitude);
37+
38+
Adafruit_LSM303_Accel_Unified *_accel = nullptr;
39+
Adafruit_LIS2MDL *_mag = nullptr;
40+
};
41+
42+
#endif // DRV_LSM303AGR_H

‎src/components/i2c/drivers/drvLsm303dlh.cpp‎

Lines changed: 117 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,117 @@
1+
/*!
2+
* @file drvLsm303dlh.cpp
3+
*
4+
* Driver wrapper for the legacy Adafruit LSM303DLH combo sensor.
5+
*/
6+
7+
#include "drvLsm303dlh.h"
8+
9+
#include <math.h>
10+
11+
namespace {
12+
constexpr uint8_t kLsm303dlhAccelDefaultAddr = LSM303_ADDRESS_ACCEL;
13+
constexpr uint8_t kLsm303dlhMagDefaultAddr = 0x1E;
14+
}
15+
16+
/******************************************************************************/
17+
/*! @brief Destructor */
18+
/******************************************************************************/
19+
drvLsm303dlh::~drvLsm303dlh() { teardown(); }
20+
21+
/******************************************************************************/
22+
void drvLsm303dlh::teardown() {
23+
if (_accel) {
24+
delete _accel;
25+
_accel = nullptr;
26+
}
27+
if (_mag) {
28+
delete _mag;
29+
_mag = nullptr;
30+
}
31+
}
32+
33+
/******************************************************************************/
34+
bool drvLsm303dlh::begin() {
35+
teardown();
36+
37+
_accel = new Adafruit_LSM303_Accel_Unified();
38+
_mag = new Adafruit_LSM303DLH_Mag_Unified();
39+
if (!_accel || !_mag) {
40+
teardown();
41+
return false;
42+
}
43+
44+
const uint8_t accel_addr =
45+
_address == 0 ? kLsm303dlhAccelDefaultAddr : (uint8_t)_address;
46+
47+
WS_DEBUG_PRINT("[drvLsm303dlh] Initialising accel @ 0x");
48+
WS_DEBUG_PRINTHEX(accel_addr);
49+
WS_DEBUG_PRINTLN("...");
50+
if (!_accel->begin(accel_addr, _i2c)) {
51+
WS_DEBUG_PRINTLN("[drvLsm303dlh] Failed to initialise accelerometer");
52+
teardown();
53+
return false;
54+
}
55+
_accel->setRange(LSM303_RANGE_2G);
56+
_accel->setMode(LSM303_MODE_HIGH_RESOLUTION);
57+
58+
WS_DEBUG_PRINT("[drvLsm303dlh] Initialising magnetometer @ 0x");
59+
WS_DEBUG_PRINTHEX(kLsm303dlhMagDefaultAddr);
60+
WS_DEBUG_PRINTLN("...");
61+
if (!_mag->begin(kLsm303dlhMagDefaultAddr, _i2c)) {
62+
WS_DEBUG_PRINTLN("[drvLsm303dlh] Failed to initialise LSM303DLH mag");
63+
teardown();
64+
return false;
65+
}
66+
_mag->enableAutoRange(true);
67+
_mag->setMagGain(LSM303_MAGGAIN_1_9);
68+
_mag->setMagRate(LSM303_MAGRATE_15);
69+
70+
WS_DEBUG_PRINTLN("[drvLsm303dlh] Sensor initialised successfully");
71+
return true;
72+
}
73+
74+
bool drvLsm303dlh::computeAccelMagnitude(float &magnitude) {
75+
if (!_accel) {
76+
return false;
77+
}
78+
sensors_event_t accel_event;
79+
if (!_accel->getEvent(&accel_event)) {
80+
return false;
81+
}
82+
magnitude = sqrtf(accel_event.acceleration.x * accel_event.acceleration.x +
83+
accel_event.acceleration.y * accel_event.acceleration.y +
84+
accel_event.acceleration.z * accel_event.acceleration.z);
85+
return true;
86+
}
87+
88+
bool drvLsm303dlh::getEventRaw(sensors_event_t *rawEvent) {
89+
float magnitude = 0.0f;
90+
if (!computeAccelMagnitude(magnitude)) {
91+
return false;
92+
}
93+
rawEvent->data[0] = magnitude;
94+
return true;
95+
}
96+
97+
bool drvLsm303dlh::getEventAccelerometer(sensors_event_t *accelEvent) {
98+
if (!_accel) {
99+
return false;
100+
}
101+
return _accel->getEvent(accelEvent);
102+
}
103+
104+
bool drvLsm303dlh::getEventMagneticField(sensors_event_t *magEvent) {
105+
if (!_mag) {
106+
return false;
107+
}
108+
return _mag->getEvent(magEvent);
109+
}
110+
111+
void drvLsm303dlh::ConfigureDefaultSensorTypes() {
112+
_default_sensor_types_count = 2;
113+
_default_sensor_types[0] =
114+
wippersnapper_sensor_SensorType_SENSOR_TYPE_ACCELEROMETER;
115+
_default_sensor_types[1] =
116+
wippersnapper_sensor_SensorType_SENSOR_TYPE_MAGNETIC_FIELD;
117+
}

‎src/components/i2c/drivers/drvLsm303dlh.h‎

Lines changed: 42 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,42 @@
1+
/*!
2+
* @file drvLsm303dlh.h
3+
*
4+
* Driver wrapper for the classic Adafruit LSM303DLH combo sensor.
5+
*/
6+
#ifndef DRV_LSM303DLH_H
7+
#define DRV_LSM303DLH_H
8+
9+
#include "Wippersnapper_V2.h"
10+
#include "drvBase.h"
11+
12+
#include <Adafruit_LSM303DLH_Mag.h>
13+
#include <Adafruit_LSM303_Accel.h>
14+
15+
class drvLsm303dlh : public drvBase {
16+
public:
17+
drvLsm303dlh(TwoWire *i2c, uint16_t sensorAddress, uint32_t mux_channel,
18+
const char *driver_name)
19+
: drvBase(i2c, sensorAddress, mux_channel, driver_name) {}
20+
21+
~drvLsm303dlh();
22+
23+
bool begin() override;
24+
25+
bool getEventRaw(sensors_event_t *rawEvent) override;
26+
27+
bool getEventAccelerometer(sensors_event_t *accelEvent) override;
28+
29+
bool getEventMagneticField(sensors_event_t *magEvent) override;
30+
31+
protected:
32+
void ConfigureDefaultSensorTypes() override;
33+
34+
private:
35+
void teardown();
36+
bool computeAccelMagnitude(float &magnitude);
37+
38+
Adafruit_LSM303_Accel_Unified *_accel = nullptr;
39+
Adafruit_LSM303DLH_Mag_Unified *_mag = nullptr;
40+
};
41+
42+
#endif // DRV_LSM303DLH_H

0 commit comments

Comments
 (0)