Skip to content

Commit 2799823

Browse files
giulcioffigiulcioffi
committed
Start adding double sensor payload length on Host side
1 parent 607644f commit 2799823

18 files changed

+160
-45
lines changed

‎src/Arduino_BHY2Host.cpp

+2-2
Original file line numberDiff line numberDiff line change
@@ -49,7 +49,7 @@ void Arduino_BHY2Host::update()
4949
} else {
5050
uint8_t available = availableSensorData();
5151
for (int i = 0; i < available; i++) {
52-
SensorDataPacket data;
52+
SensorLongDataPacket data;
5353
readSensorData(data);
5454
sensorManager.process(data);
5555
}
@@ -99,7 +99,7 @@ uint8_t Arduino_BHY2Host::availableSensorData()
9999
return eslovHandler.requestAvailableData();
100100
}
101101

102-
bool Arduino_BHY2Host::readSensorData(SensorDataPacket &data)
102+
bool Arduino_BHY2Host::readSensorData(SensorLongDataPacket &data)
103103
{
104104
return eslovHandler.requestSensorData(data);
105105
}

‎src/Arduino_BHY2Host.h

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

4646
void parse(SensorDataPacket& data, DataXYZ& vector);
4747
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

+12-3
Original file line numberDiff line numberDiff line change
@@ -90,12 +90,16 @@ void EslovHandler::update()
9090
uint8_t availableData = requestAvailableData();
9191
Serial.write(availableData);
9292

93-
SensorDataPacket sensorData;
93+
SensorLongDataPacket sensorData;
9494
while (availableData) {
9595
//delay(ESLOV_DELAY);
9696
requestSensorData(sensorData);
9797
delay(ESLOV_DELAY);
98-
Serial.write((uint8_t*)&sensorData, sizeof(sensorData));
98+
if (sensorData.sensorId == 115 || sensorData.sensorId == 171) {
99+
Serial.write((uint8_t*)&sensorData, sizeof(SensorLongDataPacket));
100+
} else {
101+
Serial.write((uint8_t*)&sensorData, sizeof(SensorDataPacket));
102+
}
99103
availableData--;
100104
}
101105

@@ -214,7 +218,7 @@ uint8_t EslovHandler::requestAvailableData()
214218
delay(ESLOV_DELAY);
215219
}
216220

217-
bool EslovHandler::requestSensorData(SensorDataPacket &sData)
221+
bool EslovHandler::requestSensorData(SensorLongDataPacket &sData)
218222
{
219223
if (_eslovState != ESLOV_READ_SENSOR_STATE) {
220224
writeStateChange(ESLOV_READ_SENSOR_STATE);
@@ -227,6 +231,11 @@ bool EslovHandler::requestSensorData(SensorDataPacket &sData)
227231
for (uint8_t i = 0; i < sizeof(SensorDataPacket); i++) {
228232
data[i] = Wire.read();
229233
}
234+
if (data[0] == 115 || data[0] == 171) {
235+
for (uint8_t i = sizeof(SensorDataPacket); i < (sizeof(SensorLongDataPacket) - sizeof(SensorDataPacket)); i++) {
236+
data[i] = Wire.read();
237+
}
238+
}
230239
return true;
231240
}
232241

‎src/EslovHandler.h

+1-1
Original file line numberDiff line numberDiff line change
@@ -48,7 +48,7 @@ class EslovHandler {
4848
void writeConfigPacket(SensorConfigurationPacket& config);
4949
uint8_t requestPacketAck();
5050
uint8_t requestAvailableData() ;
51-
bool requestSensorData(SensorDataPacket &sData);
51+
bool requestSensorData(SensorLongDataPacket &sData);
5252
void toggleEslovIntPin();
5353

5454
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/SensorManager.cpp

+8-2
Original file line numberDiff line numberDiff line change
@@ -6,11 +6,17 @@ 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);
13+
if (data.sensorId == 115 || data.sensorId == 171) {
14+
_sensors[i]->setData(data);
15+
} else {
16+
SensorDataPacket shortData;
17+
memcpy(&shortData, &data, sizeof(SensorDataPacket));
18+
_sensors[i]->setData(shortData);
19+
}
1420
return; // can more sensor objects use the same sensor id?
1521
}
1622
}

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

‎src/sensors/SensorOrientation.h

+2
Original file line numberDiff line numberDiff line change
@@ -42,6 +42,8 @@ class SensorOrientation : public SensorClass {
4242
DataParser::parseEuler(data, _data, _factor);
4343
}
4444

45+
void setData(SensorLongDataPacket &data) {}
46+
4547
String toString()
4648
{
4749
return _data.toString();

‎src/sensors/SensorQuaternion.h

+2
Original file line numberDiff line numberDiff line change
@@ -39,6 +39,8 @@ class SensorQuaternion : public SensorClass {
3939
DataParser::parseQuaternion(data, _data, _factor);
4040
}
4141

42+
void setData(SensorLongDataPacket &data) {}
43+
4244
String toString()
4345
{
4446
return _data.toString();

‎src/sensors/SensorTypes.h

+89-1
Original file line numberDiff line numberDiff line change
@@ -3,7 +3,9 @@
33

44
#include "stdint.h"
55

6-
#define SENSOR_DATA_FIXED_LENGTH (18)
6+
#define SENSOR_DATA_FIXED_LENGTH (10)
7+
8+
#define SENSOR_LONG_DATA_FIXED_LENGTH (18)
79

810
struct __attribute__((packed)) SensorConfigurationPacket {
911
uint8_t sensorId;
@@ -102,4 +104,90 @@ struct __attribute__((packed)) SensorDataPacket {
102104
}
103105
};
104106

107+
struct __attribute__((packed)) SensorLongDataPacket {
108+
uint8_t sensorId;
109+
uint8_t size;
110+
uint8_t data[SENSOR_LONG_DATA_FIXED_LENGTH];
111+
112+
float getFloat(uint8_t index) {
113+
float result = 0;
114+
uint8_t length = sizeof(result);
115+
if (index + length > SENSOR_LONG_DATA_FIXED_LENGTH) {
116+
length = SENSOR_LONG_DATA_FIXED_LENGTH > index ? SENSOR_LONG_DATA_FIXED_LENGTH - index : 0;
117+
}
118+
if (length > 0)
119+
memcpy(&result, &data[index], length);
120+
return result;
121+
}
122+
123+
uint8_t getUint8(uint8_t index) {
124+
if (index >= SENSOR_LONG_DATA_FIXED_LENGTH) {
125+
return 0;
126+
}
127+
return data[index];
128+
}
129+
130+
uint16_t getUint16(uint8_t index) {
131+
uint16_t result = 0;
132+
uint8_t length = sizeof(result);
133+
if (index + length > SENSOR_LONG_DATA_FIXED_LENGTH) {
134+
length = SENSOR_LONG_DATA_FIXED_LENGTH > index ? SENSOR_LONG_DATA_FIXED_LENGTH - index : 0;
135+
}
136+
if (length > 0)
137+
memcpy(&result, &data[index], length);
138+
return result;
139+
}
140+
141+
uint32_t getUint24(uint8_t index) {
142+
uint32_t result = 0;
143+
uint8_t length = 3;
144+
if (index + length > SENSOR_LONG_DATA_FIXED_LENGTH) {
145+
length = SENSOR_LONG_DATA_FIXED_LENGTH > index ? SENSOR_LONG_DATA_FIXED_LENGTH - index : 0;
146+
}
147+
if (length > 0)
148+
memcpy(&result, &data[index], length);
149+
return result;
150+
}
151+
152+
uint32_t getUint32(uint8_t index) {
153+
uint32_t result = 0;
154+
uint8_t length = sizeof(result);
155+
if (index + length > SENSOR_LONG_DATA_FIXED_LENGTH) {
156+
length = SENSOR_LONG_DATA_FIXED_LENGTH > index ? SENSOR_LONG_DATA_FIXED_LENGTH - index : 0;
157+
}
158+
if (length > 0)
159+
memcpy(&result, &data[index], length);
160+
return result;
161+
}
162+
163+
int8_t getInt8(uint8_t index) {
164+
if (index >= SENSOR_LONG_DATA_FIXED_LENGTH) {
165+
return 0;
166+
}
167+
return data[index];
168+
}
169+
170+
int16_t getInt16(uint8_t index) {
171+
int16_t result = 0;
172+
uint8_t length = sizeof(result);
173+
if (index + length > SENSOR_LONG_DATA_FIXED_LENGTH) {
174+
length = SENSOR_LONG_DATA_FIXED_LENGTH > index ? SENSOR_LONG_DATA_FIXED_LENGTH - index : 0;
175+
}
176+
if (length > 0)
177+
memcpy(&result, &data[index], length);
178+
return result;
179+
}
180+
181+
int32_t getInt32(uint8_t index) {
182+
int32_t result = 0;
183+
uint8_t length = sizeof(result);
184+
if (index + length > SENSOR_LONG_DATA_FIXED_LENGTH) {
185+
length = SENSOR_LONG_DATA_FIXED_LENGTH > index ? SENSOR_LONG_DATA_FIXED_LENGTH - index : 0;
186+
}
187+
if (length > 0)
188+
memcpy(&result, &data[index], length);
189+
return result;
190+
}
191+
};
192+
105193
#endif

0 commit comments

Comments
 (0)