Skip to content

Commit a653fae

Browse files
tyethtyeth
committed
[wip] Base class for LSM6DS
1 parent 3936e2e commit a653fae

File tree

10 files changed

+186
-384
lines changed

10 files changed

+186
-384
lines changed

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

Lines changed: 118 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,118 @@
1+
/*!
2+
* @file drvBaseAccelLsm6.cpp
3+
*/
4+
5+
#include "drvBaseAccelLsm6.h"
6+
7+
#include <math.h>
8+
9+
drvBaseAccelLsm6::drvBaseAccelLsm6(TwoWire *i2c, uint16_t sensorAddress,
10+
uint32_t mux_channel,
11+
const char *driver_name)
12+
: drvBase(i2c, sensorAddress, mux_channel, driver_name) {}
13+
14+
drvBaseAccelLsm6::~drvBaseAccelLsm6() {}
15+
16+
void drvBaseAccelLsm6::setInternalPollingInterval(uint32_t interval_ms) {
17+
_internalPollPeriod = interval_ms;
18+
}
19+
20+
bool drvBaseAccelLsm6::readAllEvents() {
21+
Adafruit_LSM6DS *imu = getLSM6Sensor();
22+
if (!imu) {
23+
return false;
24+
}
25+
26+
uint32_t now = millis();
27+
if (_has_last_events && _internalPollPeriod > 0 &&
28+
(now - _lastPoll) < _internalPollPeriod) {
29+
// too soon reuse cached data, except first run or interval=0
30+
return true;
31+
}
32+
33+
_lastPoll = now;
34+
35+
if (imu->shake()) {
36+
WS_DEBUG_PRINT("[");
37+
WS_DEBUG_PRINT(_name);
38+
WS_DEBUG_PRINTLN("] Shake detected!");
39+
_last_shake = true;
40+
}
41+
42+
uint16_t step_change = imu->readPedometer();
43+
if (step_change > 0) {
44+
WS_DEBUG_PRINT("[");
45+
WS_DEBUG_PRINT(_name);
46+
WS_DEBUG_PRINT("] Steps detected: ");
47+
WS_DEBUG_PRINTLN(step_change);
48+
_last_steps += step_change;
49+
imu->resetPedometer();
50+
}
51+
52+
bool success = imu->getEvent(&_lastAccelEvent, &_lastGyroEvent, &_lastTempEvent);
53+
_has_last_events = success;
54+
return success;
55+
}
56+
57+
bool drvBaseAccelLsm6::computeAccelMagnitude(float &magnitude) {
58+
if (!readAllEvents()) {
59+
return false;
60+
}
61+
62+
magnitude = sqrtf(_lastAccelEvent.acceleration.x *
63+
_lastAccelEvent.acceleration.x +
64+
_lastAccelEvent.acceleration.y *
65+
_lastAccelEvent.acceleration.y +
66+
_lastAccelEvent.acceleration.z *
67+
_lastAccelEvent.acceleration.z);
68+
return true;
69+
}
70+
71+
bool drvBaseAccelLsm6::getEventRaw(sensors_event_t *rawEvent) {
72+
if (!readAllEvents()) {
73+
return false;
74+
}
75+
76+
rawEvent->data[0] = static_cast<float>(_last_steps);
77+
_last_steps = 0;
78+
return true;
79+
}
80+
81+
bool drvBaseAccelLsm6::getEventBoolean(sensors_event_t *booleanEvent) {
82+
if (!readAllEvents()) {
83+
return false;
84+
}
85+
86+
booleanEvent->data[0] = _last_shake ? 1.0f : 0.0f;
87+
if (_last_shake) {
88+
WS_DEBUG_PRINT("[");
89+
WS_DEBUG_PRINT(_name);
90+
WS_DEBUG_PRINTLN("] *** Threshold event detected ***");
91+
_last_shake = false;
92+
}
93+
return true;
94+
}
95+
96+
bool drvBaseAccelLsm6::getEventAccelerometer(sensors_event_t *accelEvent) {
97+
if (!readAllEvents()) {
98+
return false;
99+
}
100+
101+
*accelEvent = _lastAccelEvent;
102+
return true;
103+
}
104+
105+
bool drvBaseAccelLsm6::getEventGyroscope(sensors_event_t *gyroEvent) {
106+
if (!readAllEvents()) {
107+
return false;
108+
}
109+
110+
*gyroEvent = _lastGyroEvent;
111+
return true;
112+
}
113+
114+
void drvBaseAccelLsm6::ConfigureDefaultSensorTypes() {
115+
_default_sensor_types_count = 1;
116+
_default_sensor_types[0] =
117+
wippersnapper_sensor_SensorType_SENSOR_TYPE_ACCELEROMETER;
118+
}

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

Lines changed: 45 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,45 @@
1+
/*!
2+
* @file drvBaseAccelLsm6.h
3+
*
4+
* Shared helper for Adafruit LSM6-series accelerometer/gyroscope drivers
5+
* that expose shake and pedometer functionality.
6+
*/
7+
#ifndef DRV_BASE_ACCEL_LSM6_H
8+
#define DRV_BASE_ACCEL_LSM6_H
9+
10+
#include "drvBase.h"
11+
12+
#include <Adafruit_LSM6DS.h>
13+
14+
class drvBaseAccelLsm6 : public drvBase {
15+
public:
16+
drvBaseAccelLsm6(TwoWire *i2c, uint16_t sensorAddress,
17+
uint32_t mux_channel, const char *driver_name);
18+
virtual ~drvBaseAccelLsm6();
19+
20+
bool getEventBoolean(sensors_event_t *booleanEvent) override;
21+
bool getEventRaw(sensors_event_t *rawEvent) override;
22+
bool getEventAccelerometer(sensors_event_t *accelEvent) override;
23+
bool getEventGyroscope(sensors_event_t *gyroEvent) override;
24+
25+
void ConfigureDefaultSensorTypes() override;
26+
27+
void setInternalPollingInterval(uint32_t interval_ms);
28+
29+
protected:
30+
virtual Adafruit_LSM6DS *getLSM6Sensor() const = 0;
31+
32+
bool readAllEvents();
33+
bool computeAccelMagnitude(float &magnitude);
34+
35+
bool _has_last_events = false; ///< Flag to track if last events are stored
36+
bool _last_shake = false; ///< Last state of shake / tap detection
37+
sensors_event_t _lastAccelEvent; ///< Last accelerometer event
38+
sensors_event_t _lastGyroEvent; ///< Last gyroscope event
39+
sensors_event_t _lastTempEvent; ///< Last temperature event (raw)
40+
uint16_t _last_steps = 0; ///< Last step count
41+
uint32_t _lastPoll = 0; ///< Last poll time
42+
uint32_t _internalPollPeriod = 200; ///< Internal Polling interval in ms
43+
};
44+
45+
#endif // DRV_BASE_ACCEL_LSM6_H

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

Lines changed: 1 addition & 114 deletions
Original file line numberDiff line numberDiff line change
@@ -6,11 +6,9 @@
66

77
#include "drvIsm330dhcx.h"
88

9-
#include <math.h>
10-
119
drvIsm330dhcx::drvIsm330dhcx(TwoWire *i2c, uint16_t sensorAddress,
1210
uint32_t mux_channel, const char *driver_name)
13-
: drvBase(i2c, sensorAddress, mux_channel, driver_name) {}
11+
: drvBaseAccelLsm6(i2c, sensorAddress, mux_channel, driver_name) {}
1412

1513
drvIsm330dhcx::~drvIsm330dhcx() {
1614
if (_imu) {
@@ -56,114 +54,3 @@ bool drvIsm330dhcx::begin() {
5654
return true;
5755
}
5856

59-
bool drvIsm330dhcx::readAllEvents(sensors_event_t *accel, sensors_event_t *gyro,
60-
sensors_event_t *temp, boolean *shake, uint16_t *steps) {
61-
if (!_imu) {
62-
return false;
63-
}
64-
if (_lastPoll > 0 && _internalPollPeriod > 0 && millis() - _lastPoll < _internalPollPeriod) {
65-
// Limit polling to every 200ms, return cache, but not first time
66-
return true;
67-
}
68-
_lastPoll = millis(); // TODO: set in fastTicks, if used, instead
69-
if (_imu->shake()) {
70-
WS_DEBUG_PRINTLN("[drvIsm330dhcx] Shake detected!");
71-
*shake = true;
72-
}
73-
uint16_t step_change = _imu->readPedometer();
74-
if (step_change > 0) {
75-
WS_DEBUG_PRINT("[drvIsm330dhcx] Steps detected: ")
76-
WS_DEBUG_PRINTLN(step_change);
77-
steps = steps + step_change;
78-
_imu->resetPedometer();
79-
}
80-
return _imu->getEvent(accel, gyro, temp);
81-
}
82-
83-
bool drvIsm330dhcx::readAllEvents() {
84-
return readAllEvents(&_lastAccelEvent, &_lastGyroEvent, &_lastTempEvent,
85-
&_last_shake, &_last_steps);
86-
}
87-
88-
bool drvIsm330dhcx::computeAccelMagnitude(float &magnitude) {
89-
if (!readAllEvents()) {
90-
return false;
91-
}
92-
magnitude = sqrtf(_lastAccelEvent.acceleration.x * _lastAccelEvent.acceleration.x +
93-
_lastAccelEvent.acceleration.y * _lastAccelEvent.acceleration.y +
94-
_lastAccelEvent.acceleration.z * _lastAccelEvent.acceleration.z);
95-
return true;
96-
}
97-
98-
bool drvIsm330dhcx::getEventRaw(sensors_event_t *rawEvent) {
99-
if (!_imu) {
100-
return false;
101-
}
102-
103-
if (!readAllEvents()) {
104-
return false;
105-
}
106-
// Return step count as raw event, counter cleared at read
107-
rawEvent->data[0] = (float)_last_steps;
108-
_last_steps = 0;
109-
return true;
110-
111-
112-
////MAGNITUDE ONLY - DISABLED FOR NOW
113-
// WS_DEBUG_PRINTLN("[drvIsm330dhcx] Getting raw magnitude event...");
114-
// float magnitude = 0.0f;
115-
// if (!computeAccelMagnitude(magnitude)) {
116-
// return false;
117-
// }
118-
// rawEvent->data[0] = magnitude;
119-
// WS_DEBUG_PRINT("[drvIsm330dhcx] Raw magnitude: ");
120-
// WS_DEBUG_PRINTLN(magnitude);
121-
// return true;
122-
}
123-
124-
bool drvIsm330dhcx::getEventBoolean(sensors_event_t *booleanEvent) {
125-
if (!_imu) {
126-
return false;
127-
}
128-
if (!readAllEvents()) {
129-
return false;
130-
}
131-
WS_DEBUG_PRINT("[drvIsm330dhcx] Checking for shake/tap/threshold event...");
132-
WS_DEBUG_PRINTLN(_last_shake ? " DETECTED!" : " none.");
133-
booleanEvent->data[0] = _last_shake ? 1.0f : 0.0f;
134-
if (_last_shake) {
135-
WS_DEBUG_PRINTLN("[drvIsm330dhcx] *** Threshold event detected ***");
136-
}
137-
_last_shake = false;
138-
return true;
139-
}
140-
141-
bool drvIsm330dhcx::getEventAccelerometer(sensors_event_t *accelEvent) {
142-
if (!_imu) {
143-
return false;
144-
}
145-
WS_DEBUG_PRINTLN("[drvIsm330dhcx] Getting accelerometer event...");
146-
bool result = readAllEvents()
147-
? memcpy(accelEvent, &_lastAccelEvent, sizeof(sensors_event_t)),
148-
true
149-
: false;
150-
return result;
151-
}
152-
153-
bool drvIsm330dhcx::getEventGyroscope(sensors_event_t *gyroEvent) {
154-
if (!_imu) {
155-
return false;
156-
}
157-
WS_DEBUG_PRINTLN("[drvIsm330dhcx] Getting gyroscope event...");
158-
bool result = readAllEvents()
159-
? memcpy(gyroEvent, &_lastGyroEvent, sizeof(sensors_event_t)),
160-
true
161-
: false;
162-
return result;
163-
}
164-
165-
void drvIsm330dhcx::ConfigureDefaultSensorTypes() {
166-
_default_sensor_types_count = 1;
167-
_default_sensor_types[0] =
168-
wippersnapper_sensor_SensorType_SENSOR_TYPE_ACCELEROMETER;
169-
}

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

Lines changed: 4 additions & 26 deletions
Original file line numberDiff line numberDiff line change
@@ -7,45 +7,23 @@
77
#define DRV_ISM330DHCX_H
88

99
#include "Wippersnapper_V2.h"
10-
#include "drvBase.h"
10+
#include "drvBaseAccelLsm6.h"
1111
#include <Adafruit_ISM330DHCX.h>
1212

1313
#define ISM330_TAP_THRESHOLD_MSS 15.0f
1414

15-
class drvIsm330dhcx : public drvBase {
15+
class drvIsm330dhcx : public drvBaseAccelLsm6 {
1616
public:
1717
drvIsm330dhcx(TwoWire *i2c, uint16_t sensorAddress, uint32_t mux_channel,
1818
const char *driver_name);
1919
~drvIsm330dhcx();
2020

2121
bool begin() override;
22-
bool getEventBoolean(sensors_event_t *booleanEvent) override;
23-
bool getEventRaw(sensors_event_t *rawEvent) override;
24-
bool getEventAccelerometer(sensors_event_t *accelEvent) override;
25-
bool getEventGyroscope(sensors_event_t *gyroEvent) override;
2622

27-
void ConfigureDefaultSensorTypes() override;
28-
void setInternalPollingInterval(uint32_t interval_ms) { // override {
29-
// Polling interval is managed internally.
30-
_internalPollPeriod = interval_ms < 0 ? 0 : interval_ms;
31-
}
23+
protected:
24+
Adafruit_LSM6DS *getLSM6Sensor() const override { return _imu; }
3225

3326
private:
34-
bool readAllEvents(sensors_event_t *accel, sensors_event_t *gyro,
35-
sensors_event_t *temp, boolean *shake,
36-
uint16_t *steps);
37-
bool readAllEvents();
38-
bool computeAccelMagnitude(float &magnitude);
39-
40-
bool _has_last_events = false; ///< Flag to track if last events are stored
41-
bool _last_shake = false; ///< Last state of shake / tap detection
42-
sensors_event_t _lastAccelEvent; ///< Last accelerometer event
43-
sensors_event_t _lastGyroEvent; ///< Last gyroscope event
44-
sensors_event_t _lastTempEvent; ///< Last temperature event (raw)
45-
uint16_t _last_steps = 0; ///< Last step count
46-
uint32_t _lastPoll = 0; ///< Last poll time
47-
uint32_t _internalPollPeriod = 200; ///< Internal Polling interval in ms
48-
4927
Adafruit_ISM330DHCX *_imu = nullptr; ///< Pointer to the ISM330DHCX sensor
5028
};
5129

0 commit comments

Comments
 (0)