-
Notifications
You must be signed in to change notification settings - Fork 3
/
BNO08x.cpp
2859 lines (2513 loc) · 88.9 KB
/
BNO08x.cpp
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
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#include "BNO08x.hpp"
bool BNO08x::isr_service_installed = {false};
bno08x_config_t BNO08x::default_imu_config;
/**
* @brief BNO08x imu constructor.
*
* Construct a BNO08x object for managing a BNO08x sensor.
* Initializes required GPIO pins, interrupts, SPI peripheral.
*
* @param imu_config Configuration settings (optional), default settings can be seen in bno08x_config_t
* @return void, nothing to return
*/
BNO08x::BNO08x(bno08x_config_t imu_config)
: evt_grp_spi(xEventGroupCreate())
, evt_grp_report_en(xEventGroupCreate())
, queue_rx_data(xQueueCreate(1, sizeof(bno08x_rx_packet_t)))
, queue_tx_data(xQueueCreate(1, sizeof(bno08x_tx_packet_t)))
, queue_frs_read_data(xQueueCreate(1, RX_DATA_LENGTH * sizeof(uint8_t)))
, queue_reset_reason(xQueueCreate(1, sizeof(uint32_t)))
, imu_config(imu_config)
, calibration_status(1)
{
uint8_t tx_buffer[50] = {0};
// SPI bus config
bus_config.mosi_io_num = imu_config.io_mosi; // assign mosi gpio pin
bus_config.miso_io_num = imu_config.io_miso; // assign miso gpio pin
bus_config.sclk_io_num = imu_config.io_sclk; // assign sclk gpio pin
bus_config.quadhd_io_num = -1; // hold signal gpio (not used)
bus_config.quadwp_io_num = -1; // write protect signal gpio (not used)
// SPI slave device specific config
imu_spi_config.mode = 0x3; // set mode to 3 as per BNO08x datasheet (CPHA second edge, CPOL bus high when idle)
if (imu_config.sclk_speed > 3000000UL) // max sclk speed of 3MHz for BNO08x
{
ESP_LOGE(TAG, "Max clock speed exceeded, %ld overwritten with 3MHz", imu_config.sclk_speed);
imu_config.sclk_speed = 3000000UL;
}
imu_spi_config.clock_source = SPI_CLK_SRC_DEFAULT;
imu_spi_config.clock_speed_hz = imu_config.sclk_speed; // assign SCLK speed
imu_spi_config.address_bits = 0; // 0 address bits, not using this system
imu_spi_config.command_bits = 0; // 0 command bits, not using this system
imu_spi_config.spics_io_num = -1; // due to esp32 silicon issue, chip select cannot be used with full-duplex mode
// driver, it must be handled via calls to gpio pins
imu_spi_config.queue_size = 5; // only allow for 5 queued transactions at a time
// SPI non-driver-controlled GPIO config
// configure outputs
gpio_config_t outputs_config;
if (imu_config.io_wake != GPIO_NUM_NC)
outputs_config.pin_bit_mask =
(1ULL << imu_config.io_cs) | (1ULL << imu_config.io_rst) | (1ULL << imu_config.io_wake); // configure CS, RST, and wake gpio pins
else
outputs_config.pin_bit_mask = (1ULL << imu_config.io_cs) | (1ULL << imu_config.io_rst);
outputs_config.mode = GPIO_MODE_OUTPUT;
outputs_config.pull_down_en = GPIO_PULLDOWN_DISABLE;
outputs_config.pull_up_en = GPIO_PULLUP_DISABLE;
outputs_config.intr_type = GPIO_INTR_DISABLE;
gpio_config(&outputs_config);
gpio_set_level(imu_config.io_cs, 1);
gpio_set_level(imu_config.io_rst, 1);
if (imu_config.io_wake != GPIO_NUM_NC)
gpio_set_level(imu_config.io_wake, 1);
// configure input (HINT pin)
gpio_config_t inputs_config;
inputs_config.pin_bit_mask = (1ULL << imu_config.io_int);
inputs_config.mode = GPIO_MODE_INPUT;
inputs_config.pull_up_en = GPIO_PULLUP_DISABLE;
inputs_config.pull_down_en = GPIO_PULLDOWN_DISABLE;
inputs_config.intr_type = GPIO_INTR_NEGEDGE;
gpio_config(&inputs_config);
// check if GPIO ISR service has been installed (only has to be done once regardless of SPI slaves being used)
if (!isr_service_installed)
{
gpio_install_isr_service(0); // install isr service
isr_service_installed = true;
}
ESP_ERROR_CHECK(gpio_isr_handler_add(imu_config.io_int, hint_handler, (void*) this));
gpio_intr_disable(imu_config.io_int); // disable interrupts initially before reset
// initialize the spi peripheral
spi_bus_initialize(imu_config.spi_peripheral, &bus_config, SPI_DMA_CH_AUTO);
// add the imu device to the bus
spi_bus_add_device(imu_config.spi_peripheral, &imu_spi_config, &spi_hdl);
// do first SPI operation into nowhere before BNO085 reset to let periphiral stabilize (Anton B.)
spi_transaction.length = 8;
spi_transaction.rxlength = 0;
spi_transaction.tx_buffer = tx_buffer;
spi_transaction.rx_buffer = NULL;
spi_transaction.flags = 0;
spi_device_polling_transmit(spi_hdl, &spi_transaction); // send data packet
}
/**
* @brief Initializes BNO08x sensor
*
* Resets sensor and goes through initializing process outlined in BNO08x datasheet.
* Launches two tasks, one to manage SPI transactions, another to process any received data.
*
* @return void, nothing to return
*/
bool BNO08x::initialize()
{
// launch tasks
data_proc_task_hdl = NULL;
spi_task_hdl = NULL;
xTaskCreate(&data_proc_task_trampoline, "bno08x_data_processing_task", CONFIG_ESP32_BNO08X_DATA_PROC_TASK_SZ, this, 7,
&data_proc_task_hdl); // launch data processing task
xTaskCreate(&spi_task_trampoline, "bno08x_spi_task", 4096, this, 8, &spi_task_hdl); // launch SPI task
if (!hard_reset())
return false;
if (get_reset_reason() != 0)
{
ESP_LOGI(TAG, "Successfully initialized....");
return true;
}
return false;
}
/**
* @brief Waits for data to be received over SPI, or HOST_INT_TIMEOUT_MS to elapse.
*
* If no reports are currently enabled the hint pin interrupt will be re-enabled by this function.
* This function is for when the validity of packets is not a concern, it is for flushing packets
* we do not care about.
*
* @return True if data has been received over SPI within HOST_INT_TIMEOUT_MS.
*/
bool BNO08x::wait_for_rx_done()
{
bool success = false;
// if no reports are enabled we can assume interrupts are disabled (see spi_task())
if (xEventGroupGetBits(evt_grp_report_en) == 0)
gpio_intr_enable(imu_config.io_int); // re-enable interrupts
// wait until an interrupt has been asserted and data received or timeout has occured
if (xEventGroupWaitBits(evt_grp_spi, EVT_GRP_SPI_RX_DONE_BIT, pdTRUE, pdTRUE, HOST_INT_TIMEOUT_MS / portTICK_PERIOD_MS))
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "int asserted");
#endif
success = true;
}
else
{
ESP_LOGE(TAG, "Interrupt to host device never asserted.");
success = false;
}
return success;
}
/**
* @brief Waits for a valid or invalid packet to be received or HOST_INT_TIMEOUT_MS to elapse.
*
* If no reports are currently enabled the hint pin interrupt will be re-enabled by this function.
*
* @return True if valid packet has been received within HOST_INT_TIMEOUT_MS, false if otherwise.
*/
bool BNO08x::wait_for_data()
{
bool success = false;
// if no reports are enabled we can assume interrupts are disabled (see spi_task())
if (xEventGroupGetBits(evt_grp_report_en) == 0)
gpio_intr_enable(imu_config.io_int); // re-enable interrupts
// check to see receive operation has finished
if (xEventGroupWaitBits(evt_grp_spi, EVT_GRP_SPI_RX_DONE_BIT, pdTRUE, pdTRUE, HOST_INT_TIMEOUT_MS / portTICK_PERIOD_MS))
{
// wait until processing is done, this should never go to timeout; however, it will be set slightly after EVT_GRP_SPI_RX_DONE_BIT
if (xEventGroupWaitBits(evt_grp_spi, EVT_GRP_SPI_RX_VALID_PACKET | EVT_GRP_SPI_RX_INVALID_PACKET, pdFALSE, pdFALSE,
HOST_INT_TIMEOUT_MS / portTICK_PERIOD_MS))
{
// only return true if packet is valid
if (xEventGroupGetBits(evt_grp_spi) & EVT_GRP_SPI_RX_VALID_PACKET)
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "Valid packet received.");
#endif
success = true;
}
else
{
ESP_LOGE(TAG, "Invalid packet received.");
}
}
}
else
{
ESP_LOGE(TAG, "Interrupt to host device never asserted.");
}
xEventGroupClearBits(evt_grp_spi, EVT_GRP_SPI_RX_VALID_PACKET | EVT_GRP_SPI_RX_INVALID_PACKET);
return success;
}
/**
* @brief Waits for a queued packet to be sent or HOST_INT_TIMEOUT_MS to elapse.
*
* If no reports are currently enabled the hint pin interrupt will be re-enabled by this function.
*
* @return True if packet was sent within HOST_INT_TIMEOUT_MS, false if otherwise.
*/
bool BNO08x::wait_for_tx_done()
{
// if no reports are enabled we can assume interrupts are disabled (see spi_task())
if (xEventGroupGetBits(evt_grp_report_en) == 0)
gpio_intr_enable(imu_config.io_int); // re-enable interrupts
if (xEventGroupWaitBits(evt_grp_spi, EVT_GRP_SPI_TX_DONE, pdTRUE, pdTRUE, HOST_INT_TIMEOUT_MS / portTICK_PERIOD_MS))
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "Packet sent successfully.");
#endif
return true;
}
else
{
ESP_LOGE(TAG, "Packet failed to send.");
}
return false;
}
/**
* @brief Hard resets BNO08x sensor.
*
* @return void, nothing to return
*/
bool BNO08x::hard_reset()
{
bool success = false;
// resetting disables all reports
xEventGroupClearBits(evt_grp_report_en, EVT_GRP_RPT_ALL_BITS);
gpio_set_level(imu_config.io_cs, 1);
if (imu_config.io_wake != GPIO_NUM_NC)
gpio_set_level(imu_config.io_wake, 1);
gpio_set_level(imu_config.io_rst, 0); // set reset pin low
vTaskDelay(50 / portTICK_PERIOD_MS); // 10ns min, set to 50ms to let things stabilize(Anton)
gpio_set_level(imu_config.io_rst, 1); // bring out of reset
// Receive advertisement message on boot (see SH2 Ref. Manual 5.2 & 5.3)
if (!wait_for_rx_done()) // wait for receive operation to complete
{
ESP_LOGE(TAG, "Reset Failed, interrupt to host device never asserted.");
}
else
{
ESP_LOGI(TAG, "Received advertisement message.");
// The BNO080 will then transmit an unsolicited Initialize Response (see SH2 Ref. Manual 6.4.5.2)
if (!wait_for_rx_done())
{
ESP_LOGE(TAG, "Failed to receive initialize response on boot.");
}
else
{
ESP_LOGI(TAG, "Received initialize response.");
success = true;
}
}
return success;
}
/**
* @brief Soft resets BNO08x sensor using executable channel.
*
* @return True if reset was success.
*/
bool BNO08x::soft_reset()
{
bool success = false;
uint8_t commands[20] = {0};
// reseting resets all reports
xEventGroupClearBits(evt_grp_report_en, EVT_GRP_RPT_ALL_BITS);
commands[0] = 1;
queue_packet(CHANNEL_EXECUTABLE, 1, commands);
success = wait_for_tx_done();
// flush any packets received
for (int i = 0; i < 3; i++)
{
wait_for_rx_done();
vTaskDelay(20 / portTICK_PERIOD_MS);
}
return success;
}
/**
* @brief Requests product ID, prints the returned info over serial, and returns the reason for the most resent reset.
*
* @return The reason for the most recent recent reset ( 1 = POR (power on reset), 2 = internal reset, 3 = watchdog
* timer, 4 = external reset 5 = other)
*/
uint8_t BNO08x::get_reset_reason()
{
uint32_t reset_reason = 0;
// queue request for product ID command
queue_request_product_id_command();
// wait for transmit to finish
if (!wait_for_tx_done())
ESP_LOGE(TAG, "Failed to send product ID report request");
else
{
// receive product ID report
if (wait_for_data())
xQueueReceive(queue_reset_reason, &reset_reason, HOST_INT_TIMEOUT_MS / portTICK_PERIOD_MS);
else
ESP_LOGE(TAG, "Failed to receive product ID report.");
}
return reset_reason;
}
/**
* @brief Turns on/ brings BNO08x sensor out of sleep mode using executable channel.
*
* @return True if exiting sleep mode was success.
*/
bool BNO08x::mode_on()
{
bool success = false;
uint8_t commands[20] = {0};
commands[0] = 2;
queue_packet(CHANNEL_EXECUTABLE, 1, commands);
success = wait_for_tx_done();
// flush any packets received
for (int i = 0; i < 3; i++)
{
wait_for_rx_done();
vTaskDelay(10 / portTICK_PERIOD_MS);
}
return success;
}
/**
* @brief Puts BNO08x sensor into sleep/low power mode using executable channel.
*
* @return True if entering sleep mode was success.
*/
bool BNO08x::mode_sleep()
{
bool success = false;
uint8_t commands[20] = {0};
commands[0] = 3;
queue_packet(CHANNEL_EXECUTABLE, 1, commands);
success = wait_for_tx_done();
// flush any packets received
for (int i = 0; i < 3; i++)
{
wait_for_rx_done();
vTaskDelay(10 / portTICK_PERIOD_MS);
}
return success;
}
/**
* @brief Receives a SHTP packet via SPI and sends it to data_proc_task()
*
* @return void, nothing to return
*/
bool BNO08x::receive_packet()
{
bno08x_rx_packet_t packet;
uint8_t dummy_header_tx[4] = {0};
if (gpio_get_level(imu_config.io_int)) // ensure INT pin is low
return false;
// setup transaction to receive first 4 bytes (packet header)
spi_transaction.rx_buffer = packet.header;
spi_transaction.tx_buffer = dummy_header_tx;
spi_transaction.length = 4 * 8;
spi_transaction.rxlength = 4 * 8;
spi_transaction.flags = 0;
gpio_set_level(imu_config.io_cs, 0); // assert chip select
spi_device_polling_transmit(spi_hdl, &spi_transaction); // receive first 4 bytes (packet header)
// calculate length of packet from received header
packet.length = (((uint16_t) packet.header[1]) << 8) | ((uint16_t) packet.header[0]);
packet.length &= ~(1 << 15); // Clear the MSbit
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGW(TAG, "packet rx length: %d", packet.length);
#endif
if (packet.length == 0)
return false;
packet.length -= 4; // remove 4 header bytes from packet length (we already read those)
// setup transacton to read the data packet
spi_transaction.rx_buffer = packet.body;
spi_transaction.tx_buffer = NULL;
spi_transaction.length = packet.length * 8;
spi_transaction.rxlength = packet.length * 8;
spi_transaction.flags = 0;
spi_device_polling_transmit(spi_hdl, &spi_transaction); // receive rest of packet
gpio_set_level(imu_config.io_cs, 1); // de-assert chip select
xQueueSend(queue_rx_data, &packet, 0); // send received data to data_proc_task
xEventGroupSetBits(evt_grp_spi, EVT_GRP_SPI_RX_DONE_BIT);
return true;
}
/**
* @brief Enables a sensor report for a given ID.
*
* @param report_ID The report ID of the sensor, i.e. SENSOR_REPORT_ID_X
* @param time_between_reports The desired time in microseconds between each report. The BNO08x will send reports according to this interval.
* @param report_evt_grp_bit The event group bit for the respective report, to indicate to spi_task() it's enabled, i.e. EVT_GRP_RPT_X
*
* If no reports were enabled prior to call, this function will re-enable interrupts on hint pin.
*
* @return void, nothing to return
*/
void BNO08x::enable_report(uint8_t report_ID, uint32_t time_between_reports, const EventBits_t report_evt_grp_bit, uint32_t special_config)
{
queue_feature_command(report_ID, time_between_reports, special_config);
if (wait_for_tx_done()) // wait for transmit operation to complete
{
xEventGroupSetBits(evt_grp_report_en, report_evt_grp_bit);
// if no reports were enabled before this one, we can assume hint interrupt was disabled, re-enable to read reports
if ((xEventGroupGetBits(evt_grp_report_en) & ~report_evt_grp_bit) == 0)
gpio_intr_enable(imu_config.io_int);
}
// flush the first few reports returned to ensure new data
for (int i = 0; i < 3; i++)
wait_for_rx_done();
}
/**
* @brief Disables a sensor report for a given ID by setting its time interval to 0.
*
* @param report_ID The report ID of the sensor, i.e. SENSOR_REPORT_ID_X
* @param report_evt_grp_bit The event group bit for the respective report, to indicate to spi_task() it's disabled, i.e. EVT_GRP_RPT_X
*
* If no reports are enabled after disabling, this function will disable interrupts on hint pin.
*
* @return void, nothing to return
*/
void BNO08x::disable_report(uint8_t report_ID, const EventBits_t report_evt_grp_bit)
{
queue_feature_command(report_ID, 0);
if (wait_for_tx_done()) // wait for transmit operation to complete
{
xEventGroupClearBits(evt_grp_report_en, report_evt_grp_bit);
// no reports enabled, disable hint to avoid wasting processing time
if ((xEventGroupGetBits(evt_grp_report_en)) == 0)
gpio_intr_disable(imu_config.io_int);
}
}
/**
* @brief Queues an SHTP packet to be sent via SPI.
*
* @param SHTP channel number
* @param data_length data length in bytes
* @param commands array containing data to be sent
*
* @return void, nothing to return
*/
void BNO08x::queue_packet(uint8_t channel_number, uint8_t data_length, uint8_t* commands)
{
static uint8_t sequence_number[6] = {0}; // Sequence num of each com channel, 6 in total
bno08x_tx_packet_t packet;
packet.length = data_length + 4; // add 4 bytes for header
packet.body[0] = packet.length & 0xFF; // packet length LSB
packet.body[1] = packet.length >> 8; // packet length MSB
packet.body[2] = channel_number; // channel number to write to
packet.body[3] = sequence_number[channel_number]++; // increment and send sequence number (packet counter)
// save commands to send to tx_buffer
for (int i = 0; i < data_length; i++)
{
packet.body[i + 4] = commands[i];
}
xQueueSend(queue_tx_data, &packet, 0);
}
/**
* @brief Sends a queued SHTP packet via SPI.
*
* @param packet The packet queued to be sent.
*
* @return void, nothing to return
*/
void BNO08x::send_packet(bno08x_tx_packet_t* packet)
{
// setup transaction to send packet
spi_transaction.length = packet->length * 8;
spi_transaction.rxlength = 0;
spi_transaction.tx_buffer = packet->body;
spi_transaction.rx_buffer = NULL;
spi_transaction.flags = 0;
gpio_set_level(imu_config.io_cs, 0); // assert chip select
spi_device_polling_transmit(spi_hdl, &spi_transaction); // send data packet
gpio_set_level(imu_config.io_cs, 1); // de-assert chip select
xEventGroupSetBits(evt_grp_spi, EVT_GRP_SPI_TX_DONE);
}
/**
* @brief Queues a packet containing a command.
*
* @param command The command to be sent.
* @param commands Command data array, pre-packed with exception of first 3 elements (command info)
*
* @return void, nothing to return
*/
void BNO08x::queue_command(uint8_t command, uint8_t* commands)
{
static uint8_t command_sequence_number = 0; // Sequence num of command, sent within command packet.
commands[0] = SHTP_REPORT_COMMAND_REQUEST; // Command Request
commands[1] = command_sequence_number++; // Increments automatically each function call
commands[2] = command; // Command
queue_packet(CHANNEL_CONTROL, 12, commands);
}
/**
* @brief Queues a packet containing the request product ID command.
*
* @return void, nothing to return
*/
void BNO08x::queue_request_product_id_command()
{
uint8_t commands[20] = {0};
commands[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; // request product ID and reset info
commands[1] = 0; // reserved
queue_packet(CHANNEL_CONTROL, 2, commands);
}
/**
* @brief Sends command to calibrate accelerometer, gyro, and magnetometer.
*
* @return void, nothing to return
*/
void BNO08x::calibrate_all()
{
queue_calibrate_command(CALIBRATE_ACCEL_GYRO_MAG);
wait_for_tx_done(); // wait for transmit operation to complete
vTaskDelay(50 / portTICK_PERIOD_MS); // allow some time for command to be executed
}
/**
* @brief Sends command to calibrate accelerometer.
*
* @return void, nothing to return
*/
void BNO08x::calibrate_accelerometer()
{
queue_calibrate_command(CALIBRATE_ACCEL);
wait_for_tx_done(); // wait for transmit operation to complete
vTaskDelay(50 / portTICK_PERIOD_MS); // allow some time for command to be executed
}
/**
* @brief Sends command to calibrate gyro.
*
* @return void, nothing to return
*/
void BNO08x::calibrate_gyro()
{
queue_calibrate_command(CALIBRATE_GYRO);
wait_for_tx_done(); // wait for transmit operation to complete
vTaskDelay(50 / portTICK_PERIOD_MS); // allow some time for command to be executed
}
/**
* @brief Sends command to calibrate magnetometer.
*
* @return void, nothing to return
*/
void BNO08x::calibrate_magnetometer()
{
queue_calibrate_command(CALIBRATE_MAG);
wait_for_tx_done(); // wait for transmit operation to complete
vTaskDelay(50 / portTICK_PERIOD_MS); // allow some time for command to be executed
}
/**
* @brief Sends command to calibrate planar accelerometer
*
* @return void, nothing to return
*/
void BNO08x::calibrate_planar_accelerometer()
{
queue_calibrate_command(CALIBRATE_PLANAR_ACCEL);
wait_for_tx_done(); // wait for transmit operation to complete
vTaskDelay(50 / portTICK_PERIOD_MS); // allow some time for command to be executed
}
/**
* @brief Queues a packet containing a command to calibrate the specified sensor.
*
* @param sensor_to_calibrate The sensor to calibrate.
* @return void, nothing to return
*/
void BNO08x::queue_calibrate_command(uint8_t sensor_to_calibrate)
{
uint8_t commands[20] = {0};
switch (sensor_to_calibrate)
{
case CALIBRATE_ACCEL:
commands[3] = 1;
break;
case CALIBRATE_GYRO:
commands[4] = 1;
break;
case CALIBRATE_MAG:
commands[5] = 1;
break;
case CALIBRATE_PLANAR_ACCEL:
commands[7] = 1;
break;
case CALIBRATE_ACCEL_GYRO_MAG:
commands[3] = 1;
commands[4] = 1;
commands[5] = 1;
break;
case CALIBRATE_STOP:
// do nothing, send packet of all 0s
break;
default:
break;
}
calibration_status = 1;
queue_command(COMMAND_ME_CALIBRATE, commands);
}
/**
* @brief Requests ME calibration status from BNO08x (see Ref. Manual 6.4.7.2)
*
* @return void, nothing to return
*/
void BNO08x::request_calibration_status()
{
uint8_t commands[20] = {0};
commands[6] = 0x01; // P3 - 0x01 - Subcommand: Get ME Calibration
// Using this commands packet, send a command
queue_command(COMMAND_ME_CALIBRATE, commands);
wait_for_tx_done(); // wait for transmit operation to complete
vTaskDelay(50 / portTICK_PERIOD_MS); // allow some time for command to be executed
}
/**
* @brief Returns true if calibration has completed.
*
* @return void, nothing to return
*/
bool BNO08x::calibration_complete()
{
if (calibration_status == 0)
return true;
return false;
}
/**
* @brief Sends command to end calibration procedure.
*
* @return void, nothing to return
*/
void BNO08x::end_calibration()
{
queue_calibrate_command(CALIBRATE_STOP); // Disables all calibrations
wait_for_tx_done(); // wait for transmit operation to complete
vTaskDelay(50 / portTICK_PERIOD_MS); // allow some time for command to be executed
}
/**
* @brief Sends command to save internal calibration data (See Ref. Manual 6.4.7).
*
* @return void, nothing to return
*/
void BNO08x::save_calibration()
{
uint8_t commands[20] = {0};
// Using this shtpData packet, send a command
queue_command(COMMAND_DCD, commands); // Save DCD command
wait_for_tx_done(); // wait for transmit operation to complete
vTaskDelay(50 / portTICK_PERIOD_MS); // allow some time for command to be executed
}
/**
* @brief Runs full calibration routine.
*
* Enables game rotation vector and magnetometer, starts ME calibration process.
* Waits for accuracy of returned quaternions and magnetic field vectors to be high, then saves calibration data and
* returns.
*
* @return void, nothing to return
*/
bool BNO08x::run_full_calibration_routine()
{
float magf_x = 0;
float magf_y = 0;
float magf_z = 0;
uint8_t magnetometer_accuracy = (uint8_t) IMUAccuracy::LOW;
float quat_I = 0;
float quat_J = 0;
float quat_K = 0;
float quat_real = 0;
uint8_t quat_accuracy = (uint8_t) IMUAccuracy::LOW;
uint16_t high_accuracy = 0;
uint16_t save_calibration_attempt = 0;
// Enable dynamic calibration for accel, gyro, and mag
calibrate_all(); // Turn on cal for Accel, Gyro, and Mag
// Enable Game Rotation Vector output
enable_game_rotation_vector(100000UL); // Send data update every 100ms
// Enable Magnetic Field output
enable_magnetometer(100000UL); // Send data update every 100ms
while (1)
{
if (data_available())
{
magf_x = get_magf_X();
magf_y = get_magf_Y();
magf_z = get_magf_Z();
magnetometer_accuracy = get_magf_accuracy();
quat_I = get_quat_I();
quat_J = get_quat_J();
quat_K = get_quat_K();
quat_real = get_quat_real();
quat_accuracy = get_quat_accuracy();
ESP_LOGI(TAG, "Magnetometer: x: %.3f y: %.3f z: %.3f, accuracy: %d", magf_x, magf_y, magf_z, magnetometer_accuracy);
ESP_LOGI(TAG, "Quaternion Rotation Vector: i: %.3f j: %.3f k: %.3f, real: %.3f, accuracy: %d", quat_I, quat_J, quat_K, quat_real,
quat_accuracy);
vTaskDelay(5 / portTICK_PERIOD_MS);
if ((magnetometer_accuracy >= (uint8_t) IMUAccuracy::MED) && (quat_accuracy == (uint8_t) IMUAccuracy::HIGH))
high_accuracy++;
else
high_accuracy = 0;
if (high_accuracy > 10)
{
save_calibration();
request_calibration_status();
save_calibration_attempt = 0;
while (save_calibration_attempt < 20)
{
if (data_available())
{
if (calibration_complete())
{
ESP_LOGW(TAG, "Calibration data successfully stored.");
return true;
}
else
{
save_calibration();
request_calibration_status();
save_calibration_attempt++;
}
}
}
vTaskDelay(1 / portTICK_PERIOD_MS);
if (save_calibration_attempt >= 20)
ESP_LOGE(TAG, "Calibration data failed to store.");
return false;
}
}
vTaskDelay(5 / portTICK_PERIOD_MS);
}
}
/**
* @brief Checks if BNO08x has asserted interrupt and sent data.
*
* @return true if new data has been parsed and saved
*/
bool BNO08x::data_available()
{
if (xEventGroupGetBits(evt_grp_report_en) == 0)
{
ESP_LOGE(TAG, "No reports enabled.");
return false;
}
return wait_for_data();
}
/**
* @brief Registers a callback to execute when new data from a report is received.
*
* @param cb_fxn Pointer to the call-back function should be of void return type and void input parameters.
* @return void, nothing to return
*/
void BNO08x::register_cb(std::function<void()> cb_fxn)
{
cb_list.push_back(cb_fxn);
}
/**
* @brief Parses a packet received from bno08x, updating any data according to received reports.
*
* @param packet The packet to be parsed.
* @return 0 if invalid packet.
*/
uint16_t BNO08x::parse_packet(bno08x_rx_packet_t* packet)
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGW(TAG, "SHTP Header RX'd: 0x%X 0x%X 0x%X 0x%X", packet->header[0], packet->header[1], packet->header[2], packet->header[3]);
#endif
if (packet->body[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) // check to see that product ID matches what it should
{
return parse_product_id_report(packet);
}
if (packet->body[0] == SHTP_REPORT_FRS_READ_RESPONSE)
{
return parse_frs_read_response_report(packet);
}
// Check to see if this packet is a sensor reporting its data to us
if (packet->header[2] == CHANNEL_REPORTS && packet->body[0] == SHTP_REPORT_BASE_TIMESTAMP)
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "RX'd packet, channel report");
#endif
return parse_input_report(packet); // This will update the rawAccelX, etc variables depending on which feature
// report is found
}
else if (packet->header[2] == CHANNEL_CONTROL)
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "RX'd packet, channel control");
#endif
return parse_command_report(packet); // This will update responses to commands, calibrationStatus, etc.
}
else if (packet->header[2] == CHANNEL_GYRO)
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "Rx packet, channel gyro");
#endif
return parse_input_report(packet); // This will update the rawAccelX, etc variables depending on which feature
// report is found
}
return 0;
}
/**
* @brief Parses product id report and prints device info.
*
* @param packet The packet containing product id report.
* @return 1, always valid.
*/
uint16_t BNO08x::parse_product_id_report(bno08x_rx_packet_t* packet)
{
uint32_t reset_reason = (uint32_t) packet->body[1];
uint32_t sw_part_number = ((uint32_t) packet->body[7] << 24) | ((uint32_t) packet->body[6] << 16) | ((uint32_t) packet->body[5] << 8) |
((uint32_t) packet->body[4]);
uint32_t sw_build_number = ((uint32_t) packet->body[11] << 24) | ((uint32_t) packet->body[10] << 16) | ((uint32_t) packet->body[9] << 8) |
((uint32_t) packet->body[8]);
uint16_t sw_version_patch = ((uint16_t) packet->body[13] << 8) | ((uint16_t) packet->body[12]);
// print product ID info packet
ESP_LOGI(TAG,
"Product ID Info: \n\r"
" ---------------------------\n\r"
" Product ID: 0x%" PRIx32 "\n\r"
" SW Version Major: 0x%" PRIx32 "\n\r"
" SW Version Minor: 0x%" PRIx32 "\n\r"
" SW Part Number: 0x%" PRIx32 "\n\r"
" SW Build Number: 0x%" PRIx32 "\n\r"
" SW Version Patch: 0x%" PRIx32 "\n\r"
" ---------------------------\n\r",
(uint32_t) packet->body[0], (uint32_t) packet->body[2], (uint32_t) packet->body[3], sw_part_number, sw_build_number,
(uint32_t) sw_version_patch);
xQueueSend(queue_reset_reason, &reset_reason, 0);
return 1;
}
/**
* @brief Sends packet to be parsed to meta data function call (frs_read_word()) through queue.
*
* @param packet The packet containing the frs read report.
* @return 1, always valid, parsing for this happens in frs_read_word()
*/
uint16_t BNO08x::parse_frs_read_response_report(bno08x_rx_packet_t* packet)
{
xQueueSend(queue_frs_read_data, &packet->body, 0);
return 1;
}
/**
* @brief Parses received input report sent by BNO08x.
*
* Unit responds with packet that contains the following:
*
* packet->header[0:3]: First, a 4 byte header
* packet->body[0:4]: Then a 5 byte timestamp of microsecond ticks since reading was taken
* packet->body[5 + 0]: Then a feature report ID (0x01 for Accel, 0x05 for Rotation Vector, etc...)
* packet->body[5 + 1]: Sequence number (See Ref.Manual 6.5.8.2)
* packet->body[5 + 2]: Status
* packet->body[3]: Delay
* packet->body[4:5]: i/accel x/gyro x/etc
* packet->body[6:7]: j/accel y/gyro y/etc
* packet->body[8:9]: k/accel z/gyro z/etc
* packet->body[10:11]: real/gyro temp/etc
* packet->body[12:13]: Accuracy estimate
*
* @return The report ID of the respective sensor, for ex. SENSOR_REPORT_ID_GYRO_INTEGRATED_ROTATION_VECTOR, 0 if invalid.
*/
uint16_t BNO08x::parse_input_report(bno08x_rx_packet_t* packet)
{
uint16_t i = 0;
uint8_t status = 0;
uint8_t command = 0;
// Calculate the number of data bytes in this packet
uint16_t data_length = ((uint16_t) packet->header[1] << 8 | packet->header[0]);
data_length &= ~(1 << 15); // Clear the MSbit. This bit indicates if this package is a continuation of the last.
// Ignore it for now. TODO catch this as an error and exit