Skip to content

Commit d497d9c

Browse files
giulcioffigiulcioffi
authored
Merge pull request #44 from arduino/handle_multiple_sensor_payloads
Handle short and long sensor payloads
2 parents 49fcdff + e610372 commit d497d9c

21 files changed

+239
-18
lines changed

‎src/Arduino_BHY2.cpp

+15
Original file line numberDiff line numberDiff line change
@@ -189,16 +189,31 @@ void Arduino_BHY2::addSensorData(SensorDataPacket &sensorData)
189189
sensortec.addSensorData(sensorData);
190190
}
191191

192+
void Arduino_BHY2::addLongSensorData(SensorLongDataPacket &sensorData)
193+
{
194+
sensortec.addLongSensorData(sensorData);
195+
}
196+
192197
uint8_t Arduino_BHY2::availableSensorData()
193198
{
194199
return sensortec.availableSensorData();
195200
}
196201

202+
uint8_t Arduino_BHY2::availableLongSensorData()
203+
{
204+
return sensortec.availableLongSensorData();
205+
}
206+
197207
bool Arduino_BHY2::readSensorData(SensorDataPacket &data)
198208
{
199209
return sensortec.readSensorData(data);
200210
}
201211

212+
bool Arduino_BHY2::readLongSensorData(SensorLongDataPacket &data)
213+
{
214+
return sensortec.readLongSensorData(data);
215+
}
216+
202217
bool Arduino_BHY2::hasSensor(uint8_t sensorId)
203218
{
204219
return sensortec.hasSensor(sensorId);

‎src/Arduino_BHY2.h

+3
Original file line numberDiff line numberDiff line change
@@ -55,8 +55,11 @@ class Arduino_BHY2 {
5555
void configureSensor(SensorConfigurationPacket& config);
5656
void configureSensor(uint8_t sensorId, float sampleRate, uint32_t latency);
5757
void addSensorData(SensorDataPacket &sensorData);
58+
void addLongSensorData(SensorLongDataPacket &sensorData);
5859
uint8_t availableSensorData();
60+
uint8_t availableLongSensorData();
5961
bool readSensorData(SensorDataPacket &data);
62+
bool readLongSensorData(SensorLongDataPacket &data);
6063
bool hasSensor(uint8_t sensorId);
6164

6265
void parse(SensorDataPacket& data, DataXYZ& vector);

‎src/BLEHandler.cpp

+15
Original file line numberDiff line numberDiff line change
@@ -12,8 +12,10 @@ BLECharacteristic dfuExternalCharacteristic(dfuExternalUuid, BLEWrite, sizeof(DF
1212
// Sensor Data channels
1313
BLEService sensorService("34c2e3bb-34aa-11eb-adc1-0242ac120002");
1414
auto sensorDataUuid = "34c2e3bc-34aa-11eb-adc1-0242ac120002";
15+
auto sensorLongDataUuid = "34c2e3be-34aa-11eb-adc1-0242ac120002";
1516
auto sensorConfigUuid = "34c2e3bd-34aa-11eb-adc1-0242ac120002";
1617
BLECharacteristic sensorDataCharacteristic(sensorDataUuid, (BLERead | BLENotify), sizeof(SensorDataPacket));
18+
BLECharacteristic sensorLongDataCharacteristic(sensorLongDataUuid, (BLERead | BLENotify), sizeof(SensorLongDataPacket));
1719
BLECharacteristic sensorConfigCharacteristic(sensorConfigUuid, BLEWrite, sizeof(SensorConfigurationPacket));
1820

1921
Stream* BLEHandler::_debug = NULL;
@@ -92,6 +94,7 @@ bool BLEHandler::begin()
9294
BLE.setAdvertisedService(sensorService);
9395
sensorService.addCharacteristic(sensorConfigCharacteristic);
9496
sensorService.addCharacteristic(sensorDataCharacteristic);
97+
sensorService.addCharacteristic(sensorLongDataCharacteristic);
9598
BLE.addService(sensorService);
9699
sensorConfigCharacteristic.setEventHandler(BLEWritten, receivedSensorConfig);
97100

@@ -118,6 +121,18 @@ void BLEHandler::update()
118121

119122
}
120123

124+
if (sensorLongDataCharacteristic.subscribed()) {
125+
126+
uint8_t availableLongData = sensortec.availableLongSensorData();
127+
while (availableLongData) {
128+
SensorLongDataPacket data;
129+
sensortec.readLongSensorData(data);
130+
sensorLongDataCharacteristic.writeValue(&data, sizeof(SensorLongDataPacket));
131+
--availableLongData;
132+
}
133+
134+
}
135+
121136
}
122137

123138
void BLEHandler::poll(unsigned long timeout)

‎src/BoschParser.cpp

+21-7
Original file line numberDiff line numberDiff line change
@@ -1,5 +1,6 @@
11
#include "BoschParser.h"
22
#include "BoschSensortec.h"
3+
#include "sensors/SensorID.h"
34

45
Stream* BoschParser::_debug = NULL;
56

@@ -19,28 +20,41 @@ void BoschParser::convertTime(uint64_t time_ticks, uint32_t *s, uint32_t *ns)
1920

2021
void BoschParser::parseData(const struct bhy2_fifo_parse_data_info *fifoData, void *arg)
2122
{
22-
SensorDataPacket sensorData;
23+
SensorLongDataPacket sensorData;
2324
sensorData.sensorId = fifoData->sensor_id;
2425
sensorData.size = (fifoData->data_size > sizeof(sensorData.data)) ? sizeof(sensorData.data) : fifoData->data_size;
2526
memcpy(&sensorData.data, fifoData->data_ptr, sensorData.size);
2627

2728
if (_debug) {
2829
_debug->print("Sensor: ");
2930
_debug->print(sensorData.sensorId);
31+
_debug->print(" size: ");
32+
_debug->print(sensorData.size);
3033
_debug->print(" value: ");
31-
for (uint8_t i = 0; i < (fifoData->data_size - 1); i++)
34+
for (uint8_t i = 0; i < (sensorData.size - 1); i++)
3235
{
3336
_debug->print(sensorData.data[i], HEX);
37+
_debug->print(" ");
3438
}
3539
_debug->print(" ");
36-
for (uint8_t i = 0; i < (fifoData->data_size - 1); i++)
37-
{
38-
_debug->print(fifoData->data_ptr[i], HEX);
40+
}
41+
42+
bool longSensor = false;
43+
44+
for (int i = 0; i < NUM_LONG_SENSOR; i++) {
45+
if (LongSensorList[i].id == sensorData.sensorId) {
46+
longSensor = true;
47+
break;
3948
}
40-
_debug->println("");
4149
}
4250

43-
sensortec.addSensorData(sensorData);
51+
if (longSensor) {
52+
sensortec.addLongSensorData(sensorData);
53+
} else {
54+
SensorDataPacket shortData;
55+
memcpy(&shortData, &sensorData, sizeof(SensorDataPacket));
56+
sensortec.addSensorData(shortData);
57+
}
4458
}
4559

4660
void BoschParser::parseMetaEvent(const struct bhy2_fifo_parse_data_info *callback_info, void *callback_ref)

‎src/BoschSensortec.cpp

+20
Original file line numberDiff line numberDiff line change
@@ -121,16 +121,36 @@ uint8_t BoschSensortec::availableSensorData()
121121
return _sensorQueue.size();
122122
}
123123

124+
uint8_t BoschSensortec::availableLongSensorData()
125+
{
126+
return _longSensorQueue.size();
127+
}
128+
124129
bool BoschSensortec::readSensorData(SensorDataPacket &data)
125130
{
126131
return _sensorQueue.pop(data);
127132
}
128133

134+
bool BoschSensortec::readLongSensorData(SensorLongDataPacket &data)
135+
{
136+
return _longSensorQueue.pop(data);
137+
}
138+
129139
void BoschSensortec::addSensorData(SensorDataPacket &sensorData)
130140
{
131141
// Overwrites oldest data when fifo is full
132142
_sensorQueue.push(sensorData);
133143
// Alternative: handle the full queue by storing it in flash
144+
SensorLongDataPacket longData;
145+
memcpy(&longData, &sensorData, sizeof(SensorDataPacket));
146+
sensorManager.process(longData);
147+
}
148+
149+
void BoschSensortec::addLongSensorData(SensorLongDataPacket &sensorData)
150+
{
151+
// Overwrites oldest data when fifo is full
152+
_longSensorQueue.push(sensorData);
153+
// Alternative: handle the full queue by storing it in flash
134154
sensorManager.process(sensorData);
135155
}
136156

‎src/BoschSensortec.h

+4
Original file line numberDiff line numberDiff line change
@@ -40,15 +40,19 @@ class BoschSensortec {
4040
bool hasSensor(uint8_t sensorId);
4141

4242
uint8_t availableSensorData();
43+
uint8_t availableLongSensorData();
4344
bool readSensorData(SensorDataPacket &data);
45+
bool readLongSensorData(SensorLongDataPacket &data);
4446

4547
// ANNA <-> BOSCH interface
4648
void addSensorData(SensorDataPacket &sensorData);
49+
void addLongSensorData(SensorLongDataPacket &sensorData);
4750

4851
uint8_t acknowledgment();
4952

5053
private:
5154
mbed::CircularBuffer<SensorDataPacket, SENSOR_QUEUE_SIZE, uint8_t> _sensorQueue;
55+
mbed::CircularBuffer<SensorLongDataPacket, SENSOR_QUEUE_SIZE, uint8_t> _longSensorQueue;
5256

5357
uint8_t _workBuffer[WORK_BUFFER_SIZE];
5458
uint8_t _acknowledgment;

‎src/EslovHandler.cpp

+16
Original file line numberDiff line numberDiff line change
@@ -63,16 +63,32 @@ void EslovHandler::requestEvent()
6363
uint8_t availableData = sensortec.availableSensorData();
6464
Wire.write(availableData);
6565

66+
} else if (_state == ESLOV_AVAILABLE_LONG_SENSOR_STATE) {
67+
uint8_t availableData = sensortec.availableLongSensorData();
68+
Wire.write(availableData);
69+
6670
} else if (_state == ESLOV_READ_SENSOR_STATE) {
6771
SensorDataPacket data;
6872
sensortec.readSensorData(data);
73+
6974
Wire.write((uint8_t*)&data, sizeof(SensorDataPacket));
7075
if (_debug) {
7176
_debug->print("data: ");
7277
_debug->println(data.sensorId);
7378
_debug->println(data.size);
7479
}
7580

81+
} else if (_state == ESLOV_READ_LONG_SENSOR_STATE) {
82+
SensorLongDataPacket data;
83+
sensortec.readLongSensorData(data);
84+
85+
Wire.write((uint8_t*)&data, sizeof(SensorLongDataPacket));
86+
if (_debug) {
87+
_debug->print("data: ");
88+
_debug->println(data.sensorId);
89+
_debug->println(data.size);
90+
}
91+
7692
} else if (_state == ESLOV_SENSOR_ACK_STATE) {
7793
uint8_t ack = sensortec.acknowledgment();
7894
if (_debug) {

‎src/EslovHandler.h

+3-1
Original file line numberDiff line numberDiff line change
@@ -20,7 +20,9 @@ enum EslovState {
2020
ESLOV_AVAILABLE_SENSOR_STATE = 0x00,
2121
ESLOV_READ_SENSOR_STATE = 0x01,
2222
ESLOV_DFU_ACK_STATE = 0x02,
23-
ESLOV_SENSOR_ACK_STATE = 0x03
23+
ESLOV_SENSOR_ACK_STATE = 0x03,
24+
ESLOV_AVAILABLE_LONG_SENSOR_STATE = 0x04,
25+
ESLOV_READ_LONG_SENSOR_STATE = 0x05
2426
};
2527

2628
class EslovHandler {

‎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
@@ -76,8 +76,8 @@ class DataParser {
7676
static void parseEuler(SensorDataPacket& data, DataOrientation& vector);
7777
static void parseEuler(SensorDataPacket& data, DataOrientation& vector, float scaleFactor);
7878
static void parseQuaternion(SensorDataPacket& data, DataQuaternion& vector, float scaleFactor);
79-
static void parseBSEC(SensorDataPacket& data, DataBSEC& vector);
80-
static void parseBSECLegacy(SensorDataPacket& data, DataBSEC& vector);
79+
static void parseBSEC(SensorLongDataPacket& data, DataBSEC& vector);
80+
static void parseBSECLegacy(SensorLongDataPacket& data, DataBSEC& vector);
8181
static void parseData(SensorDataPacket& data, float& value, float scaleFactor, SensorPayload format);
8282
static void parseActivity(SensorDataPacket& data, uint16_t& value);
8383
};

‎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) {

‎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

+5
Original file line numberDiff line numberDiff line change
@@ -29,6 +29,11 @@ class SensorBSEC : public SensorClass {
2929

3030

3131
void setData(SensorDataPacket &data)
32+
{
33+
34+
}
35+
36+
void setData(SensorLongDataPacket &data)
3237
{
3338
if (_id == SENSOR_ID_BSEC ) {
3439
DataParser::parseBSEC(data, _data);

‎src/sensors/SensorClass.h

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

2626
virtual void setData(SensorDataPacket &data) = 0;
27+
virtual void setData(SensorLongDataPacket &data) = 0;
2728
virtual String toString() = 0;
2829

2930
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},

‎src/sensors/SensorManager.cpp

+21-3
Original file line numberDiff line numberDiff line change
@@ -6,12 +6,30 @@ SensorManager::SensorManager() :
66
{
77
}
88

9-
void SensorManager::process(SensorDataPacket &data)
9+
void SensorManager::process(SensorLongDataPacket &data)
1010
{
1111
for (int i = 0; i < _sensorsLen; i++) {
1212
if (data.sensorId == _sensors[i]->id()) {
13-
_sensors[i]->setData(data);
14-
return; // can more sensor objects use the same sensor id?
13+
14+
bool longSensor = false;
15+
16+
for (int i = 0; i < NUM_LONG_SENSOR; i++) {
17+
if (LongSensorList[i].id == data.sensorId) {
18+
longSensor = true;
19+
break;
20+
}
21+
}
22+
23+
if (longSensor) {
24+
_sensors[i]->setData(data);
25+
} else {
26+
// All the other sensors have short payloads
27+
SensorDataPacket shortData;
28+
memcpy(&shortData, &data, sizeof(SensorDataPacket));
29+
_sensors[i]->setData(shortData);
30+
}
31+
32+
return;
1533
}
1634
}
1735
}

‎src/sensors/SensorManager.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -6,7 +6,7 @@
66
class SensorManager {
77
public:
88
SensorManager();
9-
void process(SensorDataPacket &data);
9+
void process(SensorLongDataPacket &data);
1010

1111
void subscribe(SensorClass *sensor);
1212
void unsubscribe(SensorClass *sensor);

0 commit comments

Comments
 (0)