Skip to content

Commit c201486

Browse files
giulcioffigiulcioffi
authored
Merge pull request #44 from arduino/handle_multiple_sensor_payloads
Handle short and long sensor payloads
2 parents 5915ed6 + 6917ff8 commit c201486

19 files changed

+244
-44
lines changed

‎src/Arduino_BHY2Host.cpp

+18
Original file line numberDiff line numberDiff line change
@@ -51,6 +51,14 @@ void Arduino_BHY2Host::update()
5151
for (int i = 0; i < available; i++) {
5252
SensorDataPacket data;
5353
readSensorData(data);
54+
SensorLongDataPacket longData;
55+
memcpy(&longData, &data, sizeof(SensorDataPacket));
56+
sensorManager.process(longData);
57+
}
58+
uint8_t availableLong = availableSensorLongData();
59+
for (int i = 0; i < available; i++) {
60+
SensorLongDataPacket data;
61+
readSensorLongData(data);
5462
sensorManager.process(data);
5563
}
5664
}
@@ -99,11 +107,21 @@ uint8_t Arduino_BHY2Host::availableSensorData()
99107
return eslovHandler.requestAvailableData();
100108
}
101109

110+
uint8_t Arduino_BHY2Host::availableSensorLongData()
111+
{
112+
return eslovHandler.requestAvailableLongData();
113+
}
114+
102115
bool Arduino_BHY2Host::readSensorData(SensorDataPacket &data)
103116
{
104117
return eslovHandler.requestSensorData(data);
105118
}
106119

120+
bool Arduino_BHY2Host::readSensorLongData(SensorLongDataPacket &data)
121+
{
122+
return eslovHandler.requestSensorLongData(data);
123+
}
124+
107125
void Arduino_BHY2Host::parse(SensorDataPacket& data, DataXYZ& vector)
108126
{
109127
DataParser::parse3DVector(data, vector);

‎src/Arduino_BHY2Host.h

+2
Original file line numberDiff line numberDiff line change
@@ -41,7 +41,9 @@ class Arduino_BHY2Host {
4141
void configureSensor(uint8_t sensorId, float sampleRate, uint32_t latency);
4242
uint8_t requestAck();
4343
uint8_t availableSensorData();
44+
uint8_t availableSensorLongData();
4445
bool readSensorData(SensorDataPacket &data);
46+
bool readSensorLongData(SensorLongDataPacket &data);
4547

4648
void parse(SensorDataPacket& data, DataXYZ& vector);
4749
void parse(SensorDataPacket& data, DataOrientation& vector);

‎src/BLEHandler.cpp

+28-29
Original file line numberDiff line numberDiff line change
@@ -7,10 +7,12 @@
77

88
auto sensorServiceUuid = '34c2e3bb-34aa-11eb-adc1-0242ac120002';
99
auto sensorDataUuid = "34c2e3bc-34aa-11eb-adc1-0242ac120002";
10+
auto sensorLongDataUuid = "34c2e3be-34aa-11eb-adc1-0242ac120002";
1011
auto sensorConfigUuid = "34c2e3bd-34aa-11eb-adc1-0242ac120002";
1112

1213
BLECharacteristic configCharacteristic;
1314
BLECharacteristic dataCharacteristic;
15+
BLECharacteristic longDataCharacteristic;
1416
BLEDevice niclaPeripheral;
1517

1618
BLEHandler::BLEHandler() :
@@ -87,6 +89,7 @@ bool BLEHandler::connectToNicla()
8789
// retrieve characteristics
8890
configCharacteristic = peripheral.characteristic(sensorConfigUuid);
8991
dataCharacteristic = peripheral.characteristic(sensorDataUuid);
92+
longDataCharacteristic = peripheral.characteristic(sensorLongDataUuid);
9093

9194
if (!configCharacteristic) {
9295
end();
@@ -114,6 +117,21 @@ bool BLEHandler::connectToNicla()
114117
_debug->println("Subscribed to dataCharacteristic");
115118
}
116119

120+
if (!longDataCharacteristic) {
121+
end();
122+
return false;
123+
} else if (!longDataCharacteristic.canSubscribe()) {
124+
end();
125+
return false;
126+
} else if (!longDataCharacteristic.subscribe()) {
127+
end();
128+
return false;
129+
}
130+
longDataCharacteristic.setEventHandler(BLEWritten, receivedLongSensorData);
131+
if (_debug) {
132+
_debug->println("Subscribed to longDataCharacteristic");
133+
}
134+
117135
niclaPeripheral = peripheral;
118136
nicla_found = true;
119137
}
@@ -133,34 +151,6 @@ void BLEHandler::update()
133151
begin();
134152
}
135153

136-
//if (niclaPeripheral.connected()) {
137-
//if (dataCharacteristic.valueUpdated()) {
138-
/*
139-
uint8_t data[sizeof(SensorDataPacket)];
140-
dataCharacteristic.readValue(data, sizeof(SensorDataPacket));
141-
142-
if (_debug) {
143-
_debug->print("Data packet received: ");
144-
for (int i=0; i<sizeof(SensorDataPacket); i++) {
145-
_debug->print(data[i], HEX);
146-
_debug->print(", ");
147-
}
148-
_debug->println();
149-
}
150-
151-
SensorDataPacket sensorData;
152-
memcpy(&sensorData, &data, sizeof(SensorDataPacket));
153-
154-
sensorManager.process(sensorData);
155-
*/
156-
//}
157-
/*
158-
} else {
159-
if (_debug) {
160-
_debug->println("Peripheral disconnected");
161-
}
162-
}
163-
*/
164154
}
165155

166156
void BLEHandler::end()
@@ -188,7 +178,16 @@ void BLEHandler::receivedSensorData(BLEDevice central, BLECharacteristic charact
188178
{
189179
SensorDataPacket sensorData;
190180
characteristic.readValue(&sensorData, sizeof(sensorData));
191-
sensorManager.process(sensorData);
181+
SensorLongDataPacket sensorLongData;
182+
memcpy(&sensorLongData, &sensorData, sizeof(SensorLongDataPacket));
183+
sensorManager.process(sensorLongData);
184+
}
185+
186+
void BLEHandler::receivedLongSensorData(BLEDevice central, BLECharacteristic characteristic)
187+
{
188+
SensorLongDataPacket sensorLongData;
189+
characteristic.readValue(&sensorLongData, sizeof(sensorLongData));
190+
sensorManager.process(sensorLongData);
192191
}
193192

194193
void BLEHandler::debug(Stream &stream)

‎src/BLEHandler.h

+1
Original file line numberDiff line numberDiff line change
@@ -21,6 +21,7 @@ class BLEHandler {
2121
friend class Arduino_BHY2Host;
2222

2323
static void receivedSensorData(BLEDevice central, BLECharacteristic characteristic);
24+
static void receivedLongSensorData(BLEDevice central, BLECharacteristic characteristic);
2425
bool connectToNicla();
2526

2627
void debug(Stream &stream);

‎src/EslovHandler.cpp

+47-1
Original file line numberDiff line numberDiff line change
@@ -95,7 +95,26 @@ void EslovHandler::update()
9595
//delay(ESLOV_DELAY);
9696
requestSensorData(sensorData);
9797
delay(ESLOV_DELAY);
98-
Serial.write((uint8_t*)&sensorData, sizeof(sensorData));
98+
Serial.write((uint8_t*)&sensorData, sizeof(SensorDataPacket));
99+
availableData--;
100+
}
101+
102+
_rxIndex = 0;
103+
104+
} else if (_rxBuffer[0] == HOST_READ_LONG_SENSOR_OPCODE) {
105+
106+
if (_debug) {
107+
_debug->print("received read sensor opcode\r\n");
108+
}
109+
uint8_t availableData = requestAvailableLongData();
110+
Serial.write(availableData);
111+
112+
SensorLongDataPacket sensorData;
113+
while (availableData) {
114+
//delay(ESLOV_DELAY);
115+
requestSensorLongData(sensorData);
116+
delay(ESLOV_DELAY);
117+
Serial.write((uint8_t*)&sensorData, sizeof(SensorLongDataPacket));
99118
availableData--;
100119
}
101120

@@ -214,12 +233,23 @@ uint8_t EslovHandler::requestAvailableData()
214233
delay(ESLOV_DELAY);
215234
}
216235

236+
uint8_t EslovHandler::requestAvailableLongData()
237+
{
238+
writeStateChange(ESLOV_AVAILABLE_LONG_SENSOR_STATE);
239+
while(!digitalRead(_eslovIntPin)) {}
240+
uint8_t ret = Wire.requestFrom(ESLOV_DEFAULT_ADDRESS, 1);
241+
if (!ret) return 0;
242+
return Wire.read();
243+
delay(ESLOV_DELAY);
244+
}
245+
217246
bool EslovHandler::requestSensorData(SensorDataPacket &sData)
218247
{
219248
if (_eslovState != ESLOV_READ_SENSOR_STATE) {
220249
writeStateChange(ESLOV_READ_SENSOR_STATE);
221250
while(!digitalRead(_eslovIntPin)) {}
222251
}
252+
//uint8_t ret = Wire.requestFrom(ESLOV_DEFAULT_ADDRESS, sizeof(SensorDataPacket));
223253
uint8_t ret = Wire.requestFrom(ESLOV_DEFAULT_ADDRESS, sizeof(SensorDataPacket));
224254
if (!ret) return false;
225255

@@ -230,6 +260,22 @@ bool EslovHandler::requestSensorData(SensorDataPacket &sData)
230260
return true;
231261
}
232262

263+
bool EslovHandler::requestSensorLongData(SensorLongDataPacket &sData)
264+
{
265+
if (_eslovState != ESLOV_READ_LONG_SENSOR_STATE) {
266+
writeStateChange(ESLOV_READ_LONG_SENSOR_STATE);
267+
while(!digitalRead(_eslovIntPin)) {}
268+
}
269+
uint8_t ret = Wire.requestFrom(ESLOV_DEFAULT_ADDRESS, sizeof(SensorLongDataPacket));
270+
if (!ret) return false;
271+
272+
uint8_t *data = (uint8_t*)&sData;
273+
for (uint8_t i = 0; i < sizeof(SensorLongDataPacket); i++) {
274+
data[i] = Wire.read();
275+
}
276+
return true;
277+
}
278+
233279
void EslovHandler::toggleEslovIntPin()
234280
{
235281
if (!_intPinAsserted) {

‎src/EslovHandler.h

+8-3
Original file line numberDiff line numberDiff line change
@@ -24,14 +24,17 @@ enum HostOpcode {
2424
HOST_DFU_INTERNAL_OPCODE = ESLOV_DFU_INTERNAL_OPCODE,
2525
HOST_DFU_EXTERNAL_OPCODE = ESLOV_DFU_EXTERNAL_OPCODE,
2626
HOST_READ_SENSOR_OPCODE,
27-
HOST_CONFIG_SENSOR_OPCODE
27+
HOST_CONFIG_SENSOR_OPCODE,
28+
HOST_READ_LONG_SENSOR_OPCODE
2829
};
2930

3031
enum EslovState {
3132
ESLOV_AVAILABLE_SENSOR_STATE = 0x00,
3233
ESLOV_READ_SENSOR_STATE = 0x01,
3334
ESLOV_DFU_ACK_STATE = 0x02,
34-
ESLOV_SENSOR_ACK_STATE = 0x03
35+
ESLOV_SENSOR_ACK_STATE = 0x03,
36+
ESLOV_AVAILABLE_LONG_SENSOR_STATE = 0x04,
37+
ESLOV_READ_LONG_SENSOR_STATE = 0x05
3538

3639
};
3740

@@ -47,8 +50,10 @@ class EslovHandler {
4750
void writeStateChange(EslovState state);
4851
void writeConfigPacket(SensorConfigurationPacket& config);
4952
uint8_t requestPacketAck();
50-
uint8_t requestAvailableData() ;
53+
uint8_t requestAvailableData();
54+
uint8_t requestAvailableLongData();
5155
bool requestSensorData(SensorDataPacket &sData);
56+
bool requestSensorLongData(SensorLongDataPacket &sData);
5257
void toggleEslovIntPin();
5358

5459
protected:

‎src/sensors/DataParser.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -24,7 +24,7 @@ void DataParser::parseQuaternion(SensorDataPacket& data, DataQuaternion& vector,
2424
vector.accuracy = data.getUint16(8) * scaleFactor;
2525
}
2626

27-
void DataParser::parseBSEC(SensorDataPacket& data, DataBSEC& vector) {
27+
void DataParser::parseBSEC(SensorLongDataPacket& data, DataBSEC& vector) {
2828
const float SCALE_BSEC_BVOC_EQ = 0.01f;
2929
const float SCALE_BSEC_COMP_T = 1.0f / 256;
3030
const float SCALE_BSEC_COMP_H = 1.0f / 500;
@@ -39,7 +39,7 @@ void DataParser::parseBSEC(SensorDataPacket& data, DataBSEC& vector) {
3939
vector.comp_g = (uint32_t)(data.getFloat(14));
4040
}
4141

42-
void DataParser::parseBSECLegacy(SensorDataPacket& data, DataBSEC& vector) {
42+
void DataParser::parseBSECLegacy(SensorLongDataPacket& data, DataBSEC& vector) {
4343
vector.comp_t = data.getFloat(0);
4444
vector.comp_h = data.getFloat(4);
4545
//note that: SENSOR_DATA_FIXED_LENGTH is defined as 10 by default,

‎src/sensors/DataParser.h

+2-2
Original file line numberDiff line numberDiff line change
@@ -75,8 +75,8 @@ class DataParser {
7575
static void parseEuler(SensorDataPacket& data, DataOrientation& vector);
7676
static void parseEuler(SensorDataPacket& data, DataOrientation& vector, float scaleFactor);
7777
static void parseQuaternion(SensorDataPacket& data, DataQuaternion& vector, float scaleFactor);
78-
static void parseBSEC(SensorDataPacket& data, DataBSEC& vector);
79-
static void parseBSECLegacy(SensorDataPacket& data, DataBSEC& vector);
78+
static void parseBSEC(SensorLongDataPacket& data, DataBSEC& vector);
79+
static void parseBSECLegacy(SensorLongDataPacket& data, DataBSEC& vector);
8080
static void parseData(SensorDataPacket& data, float& value, float scaleFactor, SensorPayload format);
8181
static void parseActivity(SensorDataPacket& data, uint16_t value);
8282
};

‎src/sensors/Sensor.h

+2
Original file line numberDiff line numberDiff line change
@@ -35,6 +35,8 @@ class Sensor : public SensorClass {
3535
DataParser::parseData(data, _value, _factor, _format);
3636
}
3737

38+
void setData(SensorLongDataPacket &data) {}
39+
3840
String toString()
3941
{
4042
if (_format == PEVENT && (_value > 0)) {

‎src/sensors/SensorActivity.h

+2
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,8 @@ class SensorActivity : public SensorClass {
2323
DataParser::parseActivity(data, _value);
2424
}
2525

26+
void setData(SensorLongDataPacket &data) {}
27+
2628
String toString()
2729
{
2830
return getActivity();

‎src/sensors/SensorBSEC.h

+2-1
Original file line numberDiff line numberDiff line change
@@ -27,8 +27,9 @@ class SensorBSEC : public SensorClass {
2727
float comp_h() {return _data.comp_h;}
2828
uint32_t comp_g() {return _data.comp_g;}
2929

30+
void setData(SensorDataPacket &data) {}
3031

31-
void setData(SensorDataPacket &data)
32+
void setData(SensorLongDataPacket &data)
3233
{
3334
if (_id == SENSOR_ID_BSEC ) {
3435
DataParser::parseBSEC(data, _data);

‎src/sensors/SensorClass.h

+1
Original file line numberDiff line numberDiff line change
@@ -23,6 +23,7 @@ class SensorClass {
2323
void end();
2424

2525
virtual void setData(SensorDataPacket &data) = 0;
26+
virtual void setData(SensorLongDataPacket &data) = 0;
2627
virtual String toString() = 0;
2728

2829
protected:

‎src/sensors/SensorID.h

+11-2
Original file line numberDiff line numberDiff line change
@@ -1,7 +1,8 @@
11
#ifndef SENSOR_ID_H_
22
#define SENSOR_ID_H_
33

4-
#define NUM_SUPPORTEND_SENSOR 77
4+
#define NUM_SUPPORTEND_SENSOR 79
5+
#define NUM_LONG_SENSOR 2
56

67
enum SensorID {
78
SENSOR_ID_ACC_PASS = 1, /* Accelerometer passthrough */
@@ -99,7 +100,8 @@ enum SensorPayload {
99100
P40BITUNSIGNED = 10,
100101
PEVENT = 11,
101102
ACTIVITY = 12,
102-
DEBUG_DATA = 13
103+
DEBUG_DATA = 13,
104+
BSEC = 14
103105
};
104106

105107
struct SensorStruct
@@ -109,6 +111,11 @@ struct SensorStruct
109111
float scaleFactor;
110112
};
111113

114+
static SensorStruct LongSensorList[2] = {
115+
{SENSOR_ID_BSEC, BSEC, 1.0},
116+
{SENSOR_ID_BSEC_LEGACY, BSEC, 1.0}
117+
};
118+
112119
static SensorStruct SensorList[NUM_SUPPORTEND_SENSOR] = {
113120
{SENSOR_ID_ACC_PASS, VECTOR3D, 1.0},
114121
{SENSOR_ID_ACC_RAW, VECTOR3D, 1.0},
@@ -158,6 +165,7 @@ static SensorStruct SensorList[NUM_SUPPORTEND_SENSOR] = {
158165
{SENSOR_ID_GYRO_BIAS_WU, VECTOR3D, 1.0},
159166
{SENSOR_ID_MAG_BIAS_WU, VECTOR3D, 1.0},
160167
{SENSOR_ID_STD_WU, PEVENT, 1.0},
168+
{SENSOR_ID_BSEC, BSEC, 1.0},
161169
{SENSOR_ID_TEMP, P16BITSIGNED, 0.01},
162170
{SENSOR_ID_BARO, P24BITUNSIGNED, 0.0078},
163171
{SENSOR_ID_HUM, P8BITUNISIGNED, 1.0},
@@ -180,6 +188,7 @@ static SensorStruct SensorList[NUM_SUPPORTEND_SENSOR] = {
180188
{SENSOR_ID_PROX, P8BITUNISIGNED, 1.0},
181189
{SENSOR_ID_LIGHT_WU, P16BITUNSIGNED, 46.296},
182190
{SENSOR_ID_PROX_WU, P8BITUNISIGNED, 1.0},
191+
{SENSOR_ID_BSEC_LEGACY, BSEC, 1.0},
183192
{DEBUG_DATA_EVENT, DEBUG_DATA, 1.0},
184193
{TIMESTAMP_SMALL_DELTA, P8BITUNISIGNED, 0.000015625},
185194
{TIMESTAMP_SMALL_DELTA_WU, P8BITUNISIGNED, 0.000015625},

0 commit comments

Comments
 (0)