@@ -46,58 +46,95 @@ bool drvIsm330dhcx::begin() {
4646 _imu->setAccelDataRate (LSM6DS_RATE_104_HZ);
4747 _imu->setGyroRange (LSM6DS_GYRO_RANGE_500_DPS);
4848 _imu->setGyroDataRate (LSM6DS_RATE_104_HZ);
49+ _imu->highPassFilter (true , LSM6DS_HPF_ODR_DIV_100);
4950 _imu->configInt1 (false , false , false , false , true );
5051 _imu->configInt2 (false , false , false );
52+ _imu->enablePedometer (true );
5153 _imu->enableWakeup (true );
5254
5355 WS_DEBUG_PRINTLN (" [drvIsm330dhcx] Sensor initialised successfully"
5456 return true ;
5557}
5658
5759bool drvIsm330dhcx::readAllEvents (sensors_event_t *accel, sensors_event_t *gyro,
58- sensors_event_t *temp) {
60+ sensors_event_t *temp, boolean *shake, uint16_t *steps ) {
5961 if (!_imu) {
6062 return false ;
6163 }
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+ }
6280 return _imu->getEvent (accel, gyro, temp);
6381}
6482
83+ bool drvIsm330dhcx::readAllEvents () {
84+ return readAllEvents (&_lastAccelEvent, &_lastGyroEvent, &_lastTempEvent,
85+ &_last_shake, &_last_steps);
86+ }
87+
6588bool drvIsm330dhcx::computeAccelMagnitude (float &magnitude) {
66- sensors_event_t accel, gyro, temp;
67- if (!readAllEvents (&accel, &gyro, &temp)) {
89+ if (!readAllEvents ()) {
6890 return false ;
6991 }
70- magnitude = sqrtf (accel .acceleration .x * accel .acceleration .x +
71- accel .acceleration .y * accel .acceleration .y +
72- accel .acceleration .z * accel .acceleration .z );
92+ magnitude = sqrtf (_lastAccelEvent .acceleration .x * _lastAccelEvent .acceleration .x +
93+ _lastAccelEvent .acceleration .y * _lastAccelEvent .acceleration .y +
94+ _lastAccelEvent .acceleration .z * _lastAccelEvent .acceleration .z );
7395 return true ;
7496}
7597
7698bool drvIsm330dhcx::getEventRaw (sensors_event_t *rawEvent) {
7799 if (!_imu) {
78100 return false ;
79101 }
80- WS_DEBUG_PRINTLN (" [drvIsm330dhcx] Getting raw magnitude event..."
81- float magnitude = 0 .0f ;
82- if (!computeAccelMagnitude (magnitude)) {
102+
103+ if (!readAllEvents ()) {
83104 return false ;
84105 }
85- rawEvent-> data [ 0 ] = magnitude;
86- WS_DEBUG_PRINT ( " [drvIsm330dhcx] Raw magnitude: " ) ;
87- WS_DEBUG_PRINTLN (magnitude) ;
106+ // Return step count as raw event, counter cleared at read
107+ rawEvent-> data [ 0 ] = ( float )_last_steps ;
108+ _last_steps = 0 ;
88109 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;
89122}
90123
91124bool drvIsm330dhcx::getEventBoolean (sensors_event_t *booleanEvent) {
92125 if (!_imu) {
93126 return false ;
94127 }
95- WS_DEBUG_PRINTLN (" [drvIsm330dhcx] Checking for tap/threshold event..."
96- bool tap = _imu->shake ();
97- booleanEvent->data [0 ] = tap ? 1 .0f : 0 .0f ;
98- if (tap) {
99- WS_DEBUG_PRINTLN (" [drvIsm330dhcx] Threshold event detected"
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 ***"
100136 }
137+ _last_shake = false ;
101138 return true ;
102139}
103140
@@ -106,17 +143,23 @@ bool drvIsm330dhcx::getEventAccelerometer(sensors_event_t *accelEvent) {
106143 return false ;
107144 }
108145 WS_DEBUG_PRINTLN (" [drvIsm330dhcx] Getting accelerometer event..."
109- sensors_event_t gyro, temp;
110- return readAllEvents (accelEvent, &gyro, &temp);
146+ bool result = readAllEvents ()
147+ ? memcpy (accelEvent, &_lastAccelEvent, sizeof (sensors_event_t )),
148+ true
149+ : false ;
150+ return result;
111151}
112152
113153bool drvIsm330dhcx::getEventGyroscope (sensors_event_t *gyroEvent) {
114154 if (!_imu) {
115155 return false ;
116156 }
117157 WS_DEBUG_PRINTLN (" [drvIsm330dhcx] Getting gyroscope event..."
118- sensors_event_t accel, temp;
119- return readAllEvents (&accel, gyroEvent, &temp);
158+ bool result = readAllEvents ()
159+ ? memcpy (gyroEvent, &_lastGyroEvent, sizeof (sensors_event_t )),
160+ true
161+ : false ;
162+ return result;
120163}
121164
122165void drvIsm330dhcx::ConfigureDefaultSensorTypes () {
0 commit comments