Communication SHTP avec capteur inertiel IMU BNO080 sparkfun

**Zelwyn** · 02/05/2025, 14h24

Bonjour j'ai du mal à communiquer en SHTP avec le capteur inertiel IMU BNO080.

L'idée c'est de récupérer les données des axe X,Y,Z du capteur afin de l'envoyer à un lidar pour corriger la position du robot quand il tourne.

Mais j'ai du mal à récupérer les données des axes en C, le code est juste censé récupérer les "quaternions" donc X,Y,Z en i2c avec la communication SHTP.

Le problème majeur que je rencontre c'est lors de l'envois de la requête: sendPacket(CHANNEL_CONTROL, shtpData, 2); qui permet d'envoyer au capteur 0xF9 sur le canal de contrôle (le 2) sauf qu'il ne me renvoi jamais la réponse avec 0xF8 sur le bon cannal. j'ai testé un code en arduino IDE qui fonctionne donc je suis sur que mon cablage et que le capteur fonctionne.

Mais je dois le faire en C avec le protocole SHTP, l'i2c lui fonctionne.

Voici le code, et il est aussi en pièce jointe donc le main avec la libraire pour le port série si besoin.

Merci d'avance pour votre aide

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
#define F_CPU 16000000UL
#define F_SCL 400000UL // 400kHz
#include <avr/io.h>
#include <util/delay.h>
#include <stdbool.h>
#include <string.h>
#include <stdio.h>
#include <math.h>
#include "SerialEvdm.h" //pour la communication série et afficher les données
 
// === I2C ===
 
#define TWBR_VAL ((F_CPU/F_SCL - 16)/2)
#define BNO080_ADDRESS 0x4B
#define MAX_PACKET_SIZE 128
#define MAX_METADATA_SIZE 9
bool sendPacket(uint8_t channelNumber, uint8_t* data, uint8_t dataLength);
bool receivePacket();
 
void I2C_Init() {
	TWSR = 0;
	TWBR = (uint8_t)TWBR_VAL;
}
 
bool I2C_Start(uint8_t address, bool read) {
	TWCR = (1 << TWINT) | (1 << TWSTA) | (1 << TWEN);
	while (!(TWCR & (1 << TWINT)));
	if ((TWSR & 0xF8) != 0x08 && (TWSR & 0xF8) != 0x10)
	return false;
 
	TWDR = (address << 1) | (read ? 1 : 0);
	TWCR = (1 << TWINT) | (1 << TWEN);
	while (!(TWCR & (1 << TWINT)));
 
	uint8_t status = TWSR & 0xF8;
	return (read ? status == 0x40 : status == 0x18);
}
 
void I2C_Stop() {
	TWCR = (1 << TWINT) | (1 << TWSTO) | (1 << TWEN);
	while (TWCR & (1 << TWSTO));
}
 
bool I2C_WriteByte(uint8_t data) {
	TWDR = data;
	TWCR = (1 << TWINT) | (1 << TWEN);
	while (!(TWCR & (1 << TWINT)));
	return (TWSR & 0xF8) == 0x28;
}
 
uint8_t I2C_ReadByte(bool ack) {
	TWCR = (1 << TWINT) | (1 << TWEN) | (ack ? (1 << TWEA) : 0);
	while (!(TWCR & (1 << TWINT)));
	return TWDR;
}
 
bool I2C_Write(uint8_t address, uint8_t* data, uint8_t length) {
	if (!I2C_Start(address, false)) return false;
	for (uint8_t i = 0; i < length; i++) {
		if (!I2C_WriteByte(data[i])) {
			I2C_Stop();
			return false;
		}
	}
	I2C_Stop();
	return true;
}
 
bool I2C_Read(uint8_t address, uint8_t* data, uint8_t length) {
	if (!I2C_Start(address, true)) return false;
	for (uint8_t i = 0; i < length; i++) {
		data[i] = I2C_ReadByte(i < (length - 1));
	}
	I2C_Stop();
	return true;
}
 
// === BNO080 / SHTP ===
#define CHANNEL_COMMAND  0
#define CHANNEL_EXECUTABLE 1
#define CHANNEL_CONTROL 2
#define CHANNEL_REPORTS 3
#define CHANNEL_WAKE_REPORTS 4
#define CHANNEL_GYRO 5
 
#define SHTP_REPORT_COMMAND_RESPONSE 0xF1
#define SHTP_REPORT_COMMAND_REQUEST 0xF2
#define SHTP_REPORT_FRS_READ_RESPONSE 0xF3
#define SHTP_REPORT_FRS_READ_REQUEST 0xF4
#define SHTP_REPORT_PRODUCT_ID_RESPONSE 0xF8
#define SHTP_REPORT_PRODUCT_ID_REQUEST 0xF9
#define SHTP_REPORT_BASE_TIMESTAMP 0xFB
#define SHTP_REPORT_SET_FEATURE_COMMAND 0xFD
 
#define SENSOR_REPORTID_GYROSCOPE 0x02
#define SENSOR_REPORTID_ROTATION_VECTOR 0x05
#define SENSOR_REPORTID_GAME_ROTATION_VECTOR 0x08
 
#define FRS_RECORDID_ROTATION_VECTOR 0xE30B
 
#define SHTP_REPORT_SET_FEATURE_COMMAND 0xFD
 
#define FRS_RECORDID_ROTATION_VECTOR 0xE30B
 
#define EXECUTABLE_RESET_COMPLETE 0x1
 
typedef struct {
	uint16_t packetLength;
	uint8_t channelNumber;
	uint8_t sequenceNumber;
} SHTP_Header;
 
uint8_t shtpData[128];
SHTP_Header shtpHeader;
uint8_t sequenceNumber[6] = {0};
 
float quatI = 0, quatJ = 0, quatK = 0, quatReal = 0;
 
float qToFloat(int16_t fixed, uint8_t q) {
	return fixed * powf(2, -q);
}
 
void softReset (void) {
 
	shtpData[0] = 1;
	sendPacket(CHANNEL_CONTROL, shtpData, 1);
 
	// Read all incoming data and flush it
	_delay_ms(50);
	while (receivePacket() == true)
	; //delay(1);
	_delay_ms(50);
	while (receivePacket() == true)
	; //delay(1);
 
}
 
bool begin() {
 
	// BNO080 reset
	softReset();
	_delay_ms(1000);
 
	I2C_Init();
	MyInitSerial();
	_delay_ms(1000);
 
	PrintString("Init...\n");
 
	if (!I2C_Start(BNO080_ADDRESS, false)) {
		PrintString("BNO080 not found!\n");
		while (1);
	}
 
	I2C_Stop();
	PrintString("BNO080 detected\n");
 
	// Clear junk packets
	for (uint8_t i = 0; i < 3; i++) {
		receivePacket();
		_delay_ms(100);
	}
 
	// Request product ID
	shtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST;
	shtpData[1] = 0; // Reserved
	sendPacket(CHANNEL_CONTROL, shtpData, 2);
 
	PrintString("Waiting for Product ID Response...\n");
	for (uint8_t i = 0; i < 20; i++) {
		if (receivePacket()) {
			char debugMsg[64];
			snprintf(debugMsg, sizeof(debugMsg), "Channel: %d, ReportID: 0x%02X\n", shtpHeader.channelNumber, shtpData[0]);
			PrintString(debugMsg);
 
			 // Afficher le contenu du paquet reçu
			 PrintString("Received Data: ");
			 PrintHex(shtpData, shtpHeader.packetLength - 4); // Afficher les données en hexadécimal
 
			// Check for correct response
			if (shtpHeader.channelNumber == CHANNEL_CONTROL && shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) {
				PrintString("Received Product ID Response!\n");
				// Parse and print product info
				uint32_t partNumber = (shtpData[4]) | (shtpData[5] << 8) | (shtpData[6] << 16) | (shtpData[7] << 24);
				PrintString("SW Part Number: ");
				PrintHex((uint8_t*)&partNumber, 4);
				break;
			}
		}
		_delay_ms(200);
	}
 
 
 
 
	// Wait for response
	if ((receivePacket()) == true) {
		if (shtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) {
			// Display product info (optional, for debugging)
			PrintString("Product ID Response:\n");
			uint32_t SW_Part_Number = ((uint32_t)shtpData[7] << 24) | ((uint32_t)shtpData[6] << 16) |
			((uint32_t)shtpData[5] << 8) | ((uint32_t)shtpData[4]);
			PrintString("SW Part Number: ");
			PrintHex((uint8_t*)&SW_Part_Number, sizeof(SW_Part_Number));
			return true;
		}
	}
	PrintString("not received\n");
	return false; // If the response was not received or is incorrect
}
 
bool sendPacket(uint8_t channelNumber, uint8_t* data, uint8_t dataLength) {
    uint8_t packetLength = dataLength + 4; // 4 bytes for header
 
    if (packetLength > MAX_PACKET_SIZE) {
        PrintString("Erreur: packet trop grand\n");
        return false;
    }
 
    // Démarrer la transmission I2C
    if (!I2C_Start(BNO080_ADDRESS, false)) {
        PrintString("Erreur: I2C_Start a échoué\n");
        return false;
    }
 
    // Écriture de l'en-tête du paquet
    if (!I2C_WriteByte(packetLength & 0xFF)) return false;           // LSB de la longueur
    if (!I2C_WriteByte((packetLength >> 8) & 0xFF)) return false;    // MSB de la longueur
    if (!I2C_WriteByte(channelNumber)) return false;                 // Numéro de canal
    if (!I2C_WriteByte(sequenceNumber[channelNumber]++)) return false; // Numéro de séquence
 
    // Écriture des données utilisateur (depuis data[])
    for (uint8_t i = 0; i < dataLength; i++) {
        if (!I2C_WriteByte(data[i])) {
            I2C_Stop();
            PrintString("Erreur: I2C_WriteByte a échoué\n");
            return false;
        }
    }
 
    I2C_Stop();
    return true;
}
 
 
bool receivePacket() {
	uint8_t header[4];
 
	// Lire l'en-tête (4 octets)
	if (!I2C_Read(BNO080_ADDRESS, header, 4)) return false;
	// Stocker les informations d'en-tête
	shtpHeader.packetLength = ((uint16_t)header[1] << 8) | header[0];
	shtpHeader.channelNumber = header[2];
	shtpHeader.sequenceNumber = header[3];
 
	// Supprimer le bit MSB (continuation bit) s’il est présent
	shtpHeader.packetLength &= ~(1 << 15);
	// Vérifier si le paquet est vide
	if (shtpHeader.packetLength <= 4) return false;
	uint16_t dataLength = shtpHeader.packetLength - 4;
 
	// Sécurité : ne pas dépasser la taille du buffer
	if (dataLength > sizeof(shtpData)) dataLength = sizeof(shtpData);
	// Lire les données du paquet
	if (!I2C_Read(BNO080_ADDRESS, shtpData, dataLength)) return false;
	// (Optionnel) Vérifier si c'est un "reset complete"
	if (shtpHeader.channelNumber == CHANNEL_EXECUTABLE && shtpData[0] == EXECUTABLE_RESET_COMPLETE) {
		// Tu pourrais ajouter un flag ici
	}
	return true;
}
 
 
 
void enableRotationVector(uint16_t interval_ms) {
	uint32_t interval_us = interval_ms * 1000UL;
	uint8_t command[17] = {
		SHTP_REPORT_SET_FEATURE_COMMAND,
		SENSOR_REPORTID_ROTATION_VECTOR,
		0x00, 0x00, 0x00,
		interval_us & 0xFF,
		(interval_us >> 8) & 0xFF,
		(interval_us >> 16) & 0xFF,
		(interval_us >> 24) & 0xFF,
		0x00, 0x00, 0x00, 0x00,
		0x00, 0x00, 0x00, 0x00
	};
	sendPacket(CHANNEL_CONTROL, command, sizeof(command));
}
 
void parseRotationVector() {
	if (shtpHeader.channelNumber == CHANNEL_REPORTS &&
	shtpData[0] == SENSOR_REPORTID_ROTATION_VECTOR) {
 
		// décalage de 5 octets après le reportID
		uint8_t offset = 5;
 
		int16_t i = (int16_t)(shtpData[offset + 0] | (shtpData[offset + 1] << 8));
		int16_t j = (int16_t)(shtpData[offset + 2] | (shtpData[offset + 3] << 8));
		int16_t k = (int16_t)(shtpData[offset + 4] | (shtpData[offset + 5] << 8));
		int16_t real = (int16_t)(shtpData[offset + 6] | (shtpData[offset + 7] << 8));
 
		quatI = qToFloat(i, 14);
		quatJ = qToFloat(j, 14);
		quatK = qToFloat(k, 14);
		quatReal = qToFloat(real, 14);
	}
}
 
 
void printQuaternion() {
	char buf[64];
	snprintf(buf, sizeof(buf), "Quat: i=%.2f j=%.2f k=%.2f real=%.2f\n", quatI, quatJ, quatK, quatReal);
	PrintString(buf);
}
 
int main(void) {
	int count = 0;
 
 
	begin();
 
	enableRotationVector(50); // 50ms = 20Hz
 
	while (count <10) {
		if (receivePacket()) {
			// Display Channel and Report ID
			char debugMsg[64];
			snprintf(debugMsg, sizeof(debugMsg), "Channel: %d, ReportID: 0x%02X\n", shtpHeader.channelNumber, shtpData[0]);
			PrintString(debugMsg); // Uses your PrintString function to display
 
			// Display raw data
			PrintString("Data: ");
			PrintHex(shtpData, shtpHeader.packetLength - 4); // Displays the received data in hex
 
			// If it's a quaternion report, parse it
			parseRotationVector();
			printQuaternion(); // Displays the quaternion values
			count++;
		}
		_delay_ms(10); // Small delay for more responsive reading
	}
}

et voici ce que je reçois sur le port série:

Code :

Sélectionner tout - Visualiser dans une fenêtre à part

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
Init...
BNO080 detected
Waiting for Product ID Response...
Channel: 0, ReportID: 0x08
Received Data: 08 80 00 3F 
Channel: 0, ReportID: 0x08
Received Data: 08 80 00 41 
Channel: 0, ReportID: 0x08
Received Data: 08 80 00 43 
Channel: 0, ReportID: 0x08
Received Data: 08 80 00 45 
Channel: 0, ReportID: 0x08
Received Data: 08 80 00 47 
Channel: 0, ReportID: 0x08
Received Data: 08 80 00 49 
Channel: 0, ReportID: 0x08
Received Data: 08 80 00 4B 
Channel: 0, ReportID: 0x08
Received Data: 08 80 00 4D 
Channel: 0, ReportID: 0x08
Received Data: 08 80 00 4F 
Channel: 0, ReportID: 0x08
Received Data: 08 80 00 51 
Channel: 0, ReportID: 0x08
Received Data: 08 80 00 53 
Channel: 0, ReportID: 0x08
Received Data: 08 80 00 55 
Channel: 0, ReportID: 0x08
Received Data: 08 80 00 57 
Channel: 0, ReportID: 0x08
Received Data: 08 80 00 59 
Channel: 0, ReportID: 0x08
Received Data: 08 80 00 5B 
Channel: 0, ReportID: 0x08
Received Data: 08 80 00 5D 
Channel: 0, ReportID: 0x08
Received Data: 08 80 00 5F 
Channel: 0, ReportID: 0x08
Received Data: 08 80 00 61 
Channel: 0, ReportID: 0x08
Received Data: 08 80 00 63 
Channel: 0, ReportID: 0x08
Received Data: 08 80 00 65 
not received
Channel: 0, ReportID: 0x08
Data: 08 80 00 69 
Quat: i=? j=? k=? real=?
Channel: 0, ReportID: 0x08
Data: 08 80 00 6B 
Quat: i=? j=? k=? real=?
Channel: 0, ReportID: 0x08
Data: 08 80 00 6D 
Quat: i=? j=? k=? real=?
Channel: 0, ReportID: 0x08
Data: 08 80 00 6F 
Quat: i=? j=? k=? real=?
Channel: 0, ReportID: 0x08
Data: 08 80 00 71 
Quat: i=? j=? k=? real=?
Channel: 0, ReportID: 0x08
Data: 08 80 00 73
Quat: i=? j=? k=? real=?
Channel: 0, ReportID: 0x08
Data: 08 80 00 75 
Quat: i=? j=? k=? real=?
Channel: 0, ReportID: 0x08
Data: 08 80 00 77 
Quat: i=? j=? k=? real=?
Channel: 0, ReportID: 0x08
Data: 08 80 00 79 
Quat: i=? j=? k=? real=?
Channel: 0, ReportID: 0x08
Data: 08 80 00 7B 
Quat: i=? j=? k=? real=?

**gabi7756** · 02/05/2025, 15h46

Bonjour,
Shtp je connais pas mais la balise code ca je connais

**Zelwyn** · 02/05/2025, 17h00

Ah merci je ne la voyais pas désolé je pensais que c'était pour le style du texte

**Zelwyn** · 09/05/2025, 13h29

ok c'est bon j'ai résolu cela tout seul, vu que j'ai pas eu d'aide je ne mets pas la solution

**LittleWhite** · 10/05/2025, 09h36

Bonjour,

C'est original comme réflexion...
Du coup, si une autre personne rencontre le même problème (ou un problème proche), il va se faire cuire un œuf ?
Ou, plus ironiquement, et si vous oubliez la solution dans quelques mois ? (et que votre recherche Google vous mène à cette discussion)

**Zelwyn** · Hier, 13h49

Envoyé par LittleWhite

Bonjour,

C'est original comme réflexion...
Du coup, si une autre personne rencontre le même problème (ou un problème proche), il va se faire cuire un œuf ?
Ou, plus ironiquement, et si vous oubliez la solution dans quelques mois ? (et que votre recherche Google vous mène à cette discussion)

J'ai bien trouvé la solution par moi même.
Donc si moi j'ai pu la trouver par moi même je pense que n'importe qui sera capable de la trouver aussi par soit même... Non ?

Ah oui et c'est vraiment gentil de vous inquiéter pour moi mais ne vous en faites pas car j'ai bien enregistré cela dans un dossier

Communication SHTP avec capteur inertiel IMU BNO080 sparkfun

C

Discussions similaires

Partager

Partager