Skip to content

Commit 345fee8

Browse files
tyethtyeth
committed
[wip] Add LSM6DS3, LSM6DSO32
1 parent 08d773e commit 345fee8

File tree

6 files changed

+341
-9
lines changed

6 files changed

+341
-9
lines changed

‎src/components/i2c/controller.cpp‎

Lines changed: 19 additions & 9 deletions
Original file line numberDiff line numberDiff line change
@@ -173,21 +173,31 @@ static const std::map<std::string, FnCreateI2CSensorDriver> I2cFactorySensor = {
173173
const char *driver_name) -> drvBase * {
174174
return new drvLis3dh(i2c, addr, mux_channel, driver_name);
175175
}},
176-
{"lis3mdl",
177-
[](TwoWire *i2c, uint16_t addr, uint32_t mux_channel,
176+
{"lis3mdl",
177+
[](TwoWire *i2c, uint16_t addr, uint32_t mux_channel,
178178
const char *driver_name) -> drvBase * {
179179
return new drvLis3mdl(i2c, addr, mux_channel, driver_name);
180-
}},
181-
{"ism330dlc",
182-
[](TwoWire *i2c, uint16_t addr, uint32_t mux_channel,
180+
}},
181+
{"lsm6ds3",
182+
[](TwoWire *i2c, uint16_t addr, uint32_t mux_channel,
183+
const char *driver_name) -> drvBase * {
184+
return new drvLsm6ds3(i2c, addr, mux_channel, driver_name);
185+
}},
186+
{"lsm6dso32",
187+
[](TwoWire *i2c, uint16_t addr, uint32_t mux_channel,
188+
const char *driver_name) -> drvBase * {
189+
return new drvLsm6dso32(i2c, addr, mux_channel, driver_name);
190+
}},
191+
{"ism330dlc",
192+
[](TwoWire *i2c, uint16_t addr, uint32_t mux_channel,
183193
const char *driver_name) -> drvBase * {
184194
return new drvIsm330dlc(i2c, addr, mux_channel, driver_name);
185-
}},
186-
{"ism330dhcx",
187-
[](TwoWire *i2c, uint16_t addr, uint32_t mux_channel,
195+
}},
196+
{"ism330dhcx",
197+
[](TwoWire *i2c, uint16_t addr, uint32_t mux_channel,
188198
const char *driver_name) -> drvBase * {
189199
return new drvIsm330dhcx(i2c, addr, mux_channel, driver_name);
190-
}},
200+
}},
191201
{"lsm9ds1",
192202
[](TwoWire *i2c, uint16_t addr, uint32_t mux_channel,
193203
const char *driver_name) -> drvBase * {

‎src/components/i2c/controller.h‎

Lines changed: 2 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -44,6 +44,8 @@
4444
#include "drivers/drvIsm330dhcx.h"
4545
#include "drivers/drvLis3mdl.h"
4646
#include "drivers/drvIsm330dlc.h"
47+
#include "drivers/drvLsm6ds3.h"
48+
#include "drivers/drvLsm6dso32.h"
4749
#include "drivers/drvLsm9ds1.h"
4850
#include "drivers/drvLps22hb.h"
4951
#include "drivers/drvLps25hb.h"

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

Lines changed: 118 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,118 @@
1+
/*!
2+
* @file drvLsm6ds3.cpp
3+
*
4+
* Driver wrapper for the Adafruit LSM6DS3 6-DoF IMU.
5+
*/
6+
7+
#include "drvLsm6ds3.h"
8+
9+
#include <math.h>
10+
11+
/******************************************************************************/
12+
/*! @brief Destructor */
13+
/******************************************************************************/
14+
drvLsm6ds3::~drvLsm6ds3() {
15+
if (_imu) {
16+
delete _imu;
17+
_imu = nullptr;
18+
}
19+
}
20+
21+
/******************************************************************************/
22+
bool drvLsm6ds3::begin() {
23+
if (_imu) {
24+
delete _imu;
25+
_imu = nullptr;
26+
}
27+
28+
_imu = new Adafruit_LSM6DS3();
29+
if (!_imu) {
30+
return false;
31+
}
32+
33+
uint8_t addr = _address == 0 ? LSM6DS_I2CADDR_DEFAULT : (uint8_t)_address;
34+
WS_DEBUG_PRINT("[drvLsm6ds3] Initialising @ 0x");
35+
WS_DEBUG_PRINTHEX(addr);
36+
WS_DEBUG_PRINTLN("...");
37+
38+
if (!_imu->begin_I2C(addr, _i2c)) {
39+
WS_DEBUG_PRINTLN("[drvLsm6ds3] Failed to initialise sensor");
40+
delete _imu;
41+
_imu = nullptr;
42+
return false;
43+
}
44+
45+
_imu->setAccelRange(LSM6DS_ACCEL_RANGE_4_G);
46+
_imu->setAccelDataRate(LSM6DS_RATE_104_HZ);
47+
_imu->setGyroRange(LSM6DS_GYRO_RANGE_500_DPS);
48+
_imu->setGyroDataRate(LSM6DS_RATE_104_HZ);
49+
_imu->configInt1(false, false, false, false, true);
50+
_imu->configInt2(false, false, false);
51+
_imu->enableWakeup(true);
52+
53+
WS_DEBUG_PRINTLN("[drvLsm6ds3] Sensor initialised successfully");
54+
return true;
55+
}
56+
57+
bool drvLsm6ds3::readAllEvents(sensors_event_t *accel,
58+
sensors_event_t *gyro,
59+
sensors_event_t *temp) {
60+
if (!_imu) {
61+
return false;
62+
}
63+
return _imu->getEvent(accel, gyro, temp);
64+
}
65+
66+
bool drvLsm6ds3::computeAccelMagnitude(float &magnitude) {
67+
sensors_event_t accel, gyro, temp;
68+
if (!readAllEvents(&accel, &gyro, &temp)) {
69+
return false;
70+
}
71+
magnitude = sqrtf(accel.acceleration.x * accel.acceleration.x +
72+
accel.acceleration.y * accel.acceleration.y +
73+
accel.acceleration.z * accel.acceleration.z);
74+
return true;
75+
}
76+
77+
bool drvLsm6ds3::getEventRaw(sensors_event_t *rawEvent) {
78+
if (!_imu) {
79+
return false;
80+
}
81+
float magnitude = 0.0f;
82+
if (!computeAccelMagnitude(magnitude)) {
83+
return false;
84+
}
85+
rawEvent->data[0] = magnitude;
86+
return true;
87+
}
88+
89+
bool drvLsm6ds3::getEventBoolean(sensors_event_t *booleanEvent) {
90+
if (!_imu) {
91+
return false;
92+
}
93+
bool shake = _imu->shake();
94+
booleanEvent->data[0] = shake ? 1.0f : 0.0f;
95+
return true;
96+
}
97+
98+
bool drvLsm6ds3::getEventAccelerometer(sensors_event_t *accelEvent) {
99+
if (!_imu) {
100+
return false;
101+
}
102+
sensors_event_t gyro, temp;
103+
return readAllEvents(accelEvent, &gyro, &temp);
104+
}
105+
106+
bool drvLsm6ds3::getEventGyroscope(sensors_event_t *gyroEvent) {
107+
if (!_imu) {
108+
return false;
109+
}
110+
sensors_event_t accel, temp;
111+
return readAllEvents(&accel, gyroEvent, &temp);
112+
}
113+
114+
void drvLsm6ds3::ConfigureDefaultSensorTypes() {
115+
_default_sensor_types_count = 1;
116+
_default_sensor_types[0] =
117+
wippersnapper_sensor_SensorType_SENSOR_TYPE_ACCELEROMETER;
118+
}

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

Lines changed: 43 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,43 @@
1+
/*!
2+
* @file drvLsm6ds3.h
3+
*
4+
* Driver wrapper for the Adafruit LSM6DS3 6-DoF IMU.
5+
*/
6+
#ifndef DRV_LSM6DS3_H
7+
#define DRV_LSM6DS3_H
8+
9+
#include "Wippersnapper_V2.h"
10+
#include "drvBase.h"
11+
12+
#include <Adafruit_LSM6DS3.h>
13+
14+
class drvLsm6ds3 : public drvBase {
15+
public:
16+
drvLsm6ds3(TwoWire *i2c, uint16_t sensorAddress, uint32_t mux_channel,
17+
const char *driver_name)
18+
: drvBase(i2c, sensorAddress, mux_channel, driver_name) {}
19+
20+
~drvLsm6ds3();
21+
22+
bool begin() override;
23+
24+
bool getEventRaw(sensors_event_t *rawEvent) override;
25+
26+
bool getEventBoolean(sensors_event_t *booleanEvent) override;
27+
28+
bool getEventAccelerometer(sensors_event_t *accelEvent) override;
29+
30+
bool getEventGyroscope(sensors_event_t *gyroEvent) override;
31+
32+
protected:
33+
void ConfigureDefaultSensorTypes() override;
34+
35+
private:
36+
bool readAllEvents(sensors_event_t *accel, sensors_event_t *gyro,
37+
sensors_event_t *temp);
38+
bool computeAccelMagnitude(float &magnitude);
39+
40+
Adafruit_LSM6DS3 *_imu = nullptr;
41+
};
42+
43+
#endif // DRV_LSM6DS3_H

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

Lines changed: 116 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,116 @@
1+
/*!
2+
* @file drvLsm6dso32.cpp
3+
*
4+
* Driver wrapper for the Adafruit LSM6DSO32 6-DoF IMU.
5+
*/
6+
7+
#include "drvLsm6dso32.h"
8+
9+
#include <math.h>
10+
11+
/******************************************************************************/
12+
drvLsm6dso32::~drvLsm6dso32() {
13+
if (_imu) {
14+
delete _imu;
15+
_imu = nullptr;
16+
}
17+
}
18+
19+
/******************************************************************************/
20+
bool drvLsm6dso32::begin() {
21+
if (_imu) {
22+
delete _imu;
23+
_imu = nullptr;
24+
}
25+
26+
_imu = new Adafruit_LSM6DSO32();
27+
if (!_imu) {
28+
return false;
29+
}
30+
31+
uint8_t addr = _address == 0 ? LSM6DS_I2CADDR_DEFAULT : (uint8_t)_address;
32+
WS_DEBUG_PRINT("[drvLsm6dso32] Initialising @ 0x");
33+
WS_DEBUG_PRINTHEX(addr);
34+
WS_DEBUG_PRINTLN("...");
35+
36+
if (!_imu->begin_I2C(addr, _i2c)) {
37+
WS_DEBUG_PRINTLN("[drvLsm6dso32] Failed to initialise sensor");
38+
delete _imu;
39+
_imu = nullptr;
40+
return false;
41+
}
42+
43+
_imu->setAccelRange(LSM6DSO32_ACCEL_RANGE_8_G);
44+
_imu->setAccelDataRate(LSM6DS_RATE_104_HZ);
45+
_imu->setGyroRange(LSM6DS_GYRO_RANGE_500_DPS);
46+
_imu->setGyroDataRate(LSM6DS_RATE_104_HZ);
47+
_imu->configInt1(false, false, false, false, true);
48+
_imu->configInt2(false, false, false);
49+
_imu->enableWakeup(true);
50+
51+
WS_DEBUG_PRINTLN("[drvLsm6dso32] Sensor initialised successfully");
52+
return true;
53+
}
54+
55+
bool drvLsm6dso32::readAllEvents(sensors_event_t *accel,
56+
sensors_event_t *gyro,
57+
sensors_event_t *temp) {
58+
if (!_imu) {
59+
return false;
60+
}
61+
return _imu->getEvent(accel, gyro, temp);
62+
}
63+
64+
bool drvLsm6dso32::computeAccelMagnitude(float &magnitude) {
65+
sensors_event_t accel, gyro, temp;
66+
if (!readAllEvents(&accel, &gyro, &temp)) {
67+
return false;
68+
}
69+
magnitude = sqrtf(accel.acceleration.x * accel.acceleration.x +
70+
accel.acceleration.y * accel.acceleration.y +
71+
accel.acceleration.z * accel.acceleration.z);
72+
return true;
73+
}
74+
75+
bool drvLsm6dso32::getEventRaw(sensors_event_t *rawEvent) {
76+
if (!_imu) {
77+
return false;
78+
}
79+
float magnitude = 0.0f;
80+
if (!computeAccelMagnitude(magnitude)) {
81+
return false;
82+
}
83+
rawEvent->data[0] = magnitude;
84+
return true;
85+
}
86+
87+
bool drvLsm6dso32::getEventBoolean(sensors_event_t *booleanEvent) {
88+
if (!_imu) {
89+
return false;
90+
}
91+
bool shake = _imu->shake();
92+
booleanEvent->data[0] = shake ? 1.0f : 0.0f;
93+
return true;
94+
}
95+
96+
bool drvLsm6dso32::getEventAccelerometer(sensors_event_t *accelEvent) {
97+
if (!_imu) {
98+
return false;
99+
}
100+
sensors_event_t gyro, temp;
101+
return readAllEvents(accelEvent, &gyro, &temp);
102+
}
103+
104+
bool drvLsm6dso32::getEventGyroscope(sensors_event_t *gyroEvent) {
105+
if (!_imu) {
106+
return false;
107+
}
108+
sensors_event_t accel, temp;
109+
return readAllEvents(&accel, gyroEvent, &temp);
110+
}
111+
112+
void drvLsm6dso32::ConfigureDefaultSensorTypes() {
113+
_default_sensor_types_count = 1;
114+
_default_sensor_types[0] =
115+
wippersnapper_sensor_SensorType_SENSOR_TYPE_ACCELEROMETER;
116+
}

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

Lines changed: 43 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,43 @@
1+
/*!
2+
* @file drvLsm6dso32.h
3+
*
4+
* Driver wrapper for the Adafruit LSM6DSO32 6-DoF IMU.
5+
*/
6+
#ifndef DRV_LSM6DSO32_H
7+
#define DRV_LSM6DSO32_H
8+
9+
#include "Wippersnapper_V2.h"
10+
#include "drvBase.h"
11+
12+
#include <Adafruit_LSM6DSO32.h>
13+
14+
class drvLsm6dso32 : public drvBase {
15+
public:
16+
drvLsm6dso32(TwoWire *i2c, uint16_t sensorAddress, uint32_t mux_channel,
17+
const char *driver_name)
18+
: drvBase(i2c, sensorAddress, mux_channel, driver_name) {}
19+
20+
~drvLsm6dso32();
21+
22+
bool begin() override;
23+
24+
bool getEventRaw(sensors_event_t *rawEvent) override;
25+
26+
bool getEventBoolean(sensors_event_t *booleanEvent) override;
27+
28+
bool getEventAccelerometer(sensors_event_t *accelEvent) override;
29+
30+
bool getEventGyroscope(sensors_event_t *gyroEvent) override;
31+
32+
protected:
33+
void ConfigureDefaultSensorTypes() override;
34+
35+
private:
36+
bool readAllEvents(sensors_event_t *accel, sensors_event_t *gyro,
37+
sensors_event_t *temp);
38+
bool computeAccelMagnitude(float &magnitude);
39+
40+
Adafruit_LSM6DSO32 *_imu = nullptr;
41+
};
42+
43+
#endif // DRV_LSM6DSO32_H

0 commit comments

Comments
 (0)