forked from myles-parfeniuk/esp32_BNO08x
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathBNO08x.cpp
2675 lines (2363 loc) · 85.1 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, and local task for SPI transactions.
*
* @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)
: tx_semaphore(xSemaphoreCreateBinary())
, int_asserted_semaphore(xSemaphoreCreateBinary())
, imu_config(imu_config)
, calibration_status(1)
{
// clear all buffers
memset(tx_buffer, 0, sizeof(tx_buffer));
memset(rx_buffer, 0, sizeof(rx_buffer));
memset(packet_header_rx, 0, sizeof(packet_header_rx));
memset(commands, 0, sizeof(commands));
memset(sequence_number, 0, sizeof(sequence_number));
// 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 > 3000000) // max sclk speed of 3MHz for BNO08x
{
ESP_LOGE(TAG, "Max clock speed exceeded, %lld overwritten with 3000000Hz", imu_config.sclk_speed);
imu_config.sclk_speed = 3000000;
}
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_ENABLE;
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.)
memset(tx_buffer, 0, sizeof(tx_buffer));
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
spi_task_hdl = NULL;
xTaskCreate(&spi_task_trampoline, "spi_task", 4096, this, 8, &spi_task_hdl); // launch SPI task
}
/**
* @brief Initializes BNO08x sensor
*
* Resets sensor and goes through initializing process outlined in BNO08x datasheet.
*
* @return void, nothing to return
*/
bool BNO08x::initialize()
{
// Receive advertisement message on boot (see SH2 Ref. Manual 5.2 & 5.3)
if (!hard_reset())
{
ESP_LOGE(TAG, "Failed to receive advertisement message on boot.");
return false;
}
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_device_int())
{
ESP_LOGE(TAG, "Failed to receive initialize response on boot.");
return false;
}
else
{
ESP_LOGI(TAG, "Received initialize response.");
}
// queue request for product ID command
queue_request_product_id_command();
// transmit request for product ID
if (!wait_for_device_int())
{
ESP_LOGE(TAG, "Failed to send product ID report request");
return false;
}
// receive product ID report
if (!wait_for_device_int())
{
ESP_LOGE(TAG, "Failed to receive product ID report.");
return false;
}
if (rx_buffer[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) // check to see that product ID matches what it should
{
uint32_t sw_part_number =
((uint32_t) rx_buffer[7] << 24) | ((uint32_t) rx_buffer[6] << 16) | ((uint32_t) rx_buffer[5] << 8) | ((uint32_t) rx_buffer[4]);
uint32_t sw_build_number =
((uint32_t) rx_buffer[11] << 24) | ((uint32_t) rx_buffer[10] << 16) | ((uint32_t) rx_buffer[9] << 8) | ((uint32_t) rx_buffer[8]);
uint16_t sw_version_patch = ((uint16_t) rx_buffer[13] << 8) | ((uint16_t) rx_buffer[12]);
// print product ID info packet
ESP_LOGI(TAG,
"Successfully initialized....\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) rx_buffer[0], (uint32_t) rx_buffer[2], (uint32_t) rx_buffer[3], sw_part_number, sw_build_number,
(uint32_t) sw_version_patch);
}
else
return false;
return true;
}
/**
* @brief Re-enables interrupts and waits for BNO08x to assert HINT pin.
*
* @return void, nothing to return
*/
bool BNO08x::wait_for_device_int()
{
gpio_intr_enable(imu_config.io_int); // re-enable interrupts
// wait until an interrupt has been asserted or timeout has occured
if (xSemaphoreTake(int_asserted_semaphore, HOST_INT_TIMEOUT_MS / portTICK_PERIOD_MS) == pdTRUE)
{
if (imu_config.debug_en)
ESP_LOGI(TAG, "int asserted");
return true;
}
else
{
ESP_LOGE(TAG, "Interrupt to host device never asserted.");
return false;
}
}
/**
* @brief Hard resets BNO08x sensor.
*
* @return void, nothing to return
*/
bool BNO08x::hard_reset()
{
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
if (!wait_for_device_int()) // wait for BNO08x to assert INT pin
{
ESP_LOGE(TAG, "Reset Failed, interrupt to host device never asserted.");
return false;
}
return true;
}
/**
* @brief Soft resets BNO08x sensor using executable channel.
*
* @return True if reset was success.
*/
bool BNO08x::soft_reset()
{
bool success = false;
memset(commands, 0, sizeof(commands));
commands[0] = 1;
queue_packet(CHANNEL_EXECUTABLE, 1);
success = wait_for_device_int();
vTaskDelay(20 / portTICK_PERIOD_MS);
success = wait_for_device_int(); // receive advertisement message;
vTaskDelay(20 / portTICK_PERIOD_MS);
success = wait_for_device_int(); // receive initialize message
return success;
}
/**
* @brief Get the reason for the most recent 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()
{
memset(commands, 0, sizeof(commands));
commands[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; // Request the product ID and reset info
commands[1] = 0; // Reserved
// Transmit packet on channel 2, 2 bytes
queue_packet(CHANNEL_CONTROL, 2);
wait_for_device_int();
if (wait_for_device_int())
{
if (commands[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE)
return (commands[1]);
}
return 0;
}
/**
* @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;
memset(commands, 0, sizeof(commands));
commands[0] = 2;
queue_packet(CHANNEL_EXECUTABLE, 1);
success = wait_for_device_int();
vTaskDelay(20 / portTICK_PERIOD_MS);
success = wait_for_device_int(); // receive advertisement message;
vTaskDelay(20 / portTICK_PERIOD_MS);
success = wait_for_device_int(); // receive initialize message
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;
memset(commands, 0, sizeof(commands));
commands[0] = 3;
queue_packet(CHANNEL_EXECUTABLE, 1);
success = wait_for_device_int();
vTaskDelay(20 / portTICK_PERIOD_MS);
success = wait_for_device_int(); // receive advertisement message;
vTaskDelay(20 / portTICK_PERIOD_MS);
success = wait_for_device_int(); // receive initialize message
return success;
}
/**
* @brief Receives a SHTP packet via SPI.
*
* @return void, nothing to return
*/
bool BNO08x::receive_packet()
{
uint8_t dummy_header_tx[4];
memset(packet_header_rx, 0, sizeof(packet_header_rx));
memset(dummy_header_tx, 0, sizeof(dummy_header_tx));
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_rx;
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_rx = (((uint16_t) packet_header_rx[1]) << 8) | ((uint16_t) packet_header_rx[0]);
packet_length_rx &= ~(1 << 15); // Clear the MSbit
if (imu_config.debug_en)
ESP_LOGW(TAG, "packet rx length: %d", packet_length_rx);
if (packet_length_rx == 0)
return false;
packet_length_rx -= 4; // remove 4 header bytes from packet length (we already read those)
// setup transacton to read the data packet
spi_transaction.rx_buffer = rx_buffer;
spi_transaction.tx_buffer = NULL;
spi_transaction.length = packet_length_rx * 8;
spi_transaction.rxlength = packet_length_rx * 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
return true;
}
/**
* @brief Queues an SHTP packet to be sent via SPI.
*
* @return void, nothing to return
*/
void BNO08x::queue_packet(uint8_t channel_number, uint8_t data_length)
{
packet_length_tx = data_length + 4; // add 4 bytes for header
uint8_t i = 0;
memset(tx_buffer, 0, sizeof(tx_buffer));
tx_buffer[0] = packet_length_tx & 0xFF; // packet length LSB
tx_buffer[1] = packet_length_tx >> 8; // packet length MSB
tx_buffer[2] = channel_number; // channel number to write to
tx_buffer[3] = sequence_number[channel_number]++; // increment and send sequence number (packet counter)
// save commands to send to tx_buffer
for (i = 0; i < data_length; i++)
{
tx_buffer[i + 4] = commands[i];
}
xSemaphoreGive(tx_semaphore);
}
/**
* @brief Sends a queued SHTP packet via SPI.
*
* @return void, nothing to return
*/
void BNO08x::send_packet()
{
// setup transaction to send packet
spi_transaction.length = packet_length_tx * 8;
spi_transaction.rxlength = 0;
spi_transaction.tx_buffer = tx_buffer;
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
}
/**
* @brief Queues a packet containing a command.
*
* @param command The command to be sent.
* @return void, nothing to return
*/
void BNO08x::queue_command(uint8_t command)
{
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);
}
/**
* @brief Queues a packet containing the request product ID command.
*
* @return void, nothing to return
*/
void BNO08x::queue_request_product_id_command()
{
commands[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; // request product ID and reset info
commands[1] = 0; // reserved
queue_packet(CHANNEL_CONTROL, 2);
}
/**
* @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_device_int(); // wait for next interrupt such that command is sent
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_device_int(); // wait for next interrupt such that command is sent
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_device_int(); // wait for next interrupt such that command is sent
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_device_int(); // wait for next interrupt such that command is sent
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_device_int(); // wait for next interrupt such that command is sent
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)
{
memset(commands, 0, sizeof(commands));
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);
}
/**
* @brief Requests ME calibration status from BNO08x (see Ref. Manual 6.4.7.2)
*
* @return void, nothing to return
*/
void BNO08x::request_calibration_status()
{
memset(commands, 0, sizeof(commands));
commands[6] = 0x01; // P3 - 0x01 - Subcommand: Get ME Calibration
// Using this commands packet, send a command
queue_command(COMMAND_ME_CALIBRATE);
wait_for_device_int(); // wait for next interrupt such that command is sent
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_device_int(); // wait for next interrupt such that command is sent
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()
{
memset(commands, 0, sizeof(commands));
// Using this shtpData packet, send a command
queue_command(COMMAND_DCD); // Save DCD command
wait_for_device_int(); // wait for next interrupt such that command is sent
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(100); // Send data update every 100ms
// Enable Magnetic Field output
enable_magnetometer(100); // 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()
{
return (get_readings() != 0);
}
/**
* @brief Waits for BNO08x HINT pin to assert, and parses the received data.
*
* @return void, nothing to return
*/
uint16_t BNO08x::get_readings()
{
if (wait_for_device_int())
{
if (imu_config.debug_en)
ESP_LOGE(
TAG, "SHTP Header RX'd: 0x%X 0x%X 0x%X 0x%X", packet_header_rx[0], packet_header_rx[1], packet_header_rx[2], packet_header_rx[3]);
// Check to see if this packet is a sensor reporting its data to us
if (packet_header_rx[2] == CHANNEL_REPORTS && rx_buffer[0] == SHTP_REPORT_BASE_TIMESTAMP)
{
if (imu_config.debug_en)
ESP_LOGI(TAG, "RX'd packet, channel report");
return parse_input_report(); // This will update the rawAccelX, etc variables depending on which feature
// report is found
}
else if (packet_header_rx[2] == CHANNEL_CONTROL)
{
if (imu_config.debug_en)
ESP_LOGI(TAG, "RX'd packet, channel control");
return parse_command_report(); // This will update responses to commands, calibrationStatus, etc.
}
else if (packet_header_rx[2] == CHANNEL_GYRO)
{
if (imu_config.debug_en)
ESP_LOGI(TAG, "Rx packet, channel gyro");
return parse_input_report(); // This will update the rawAccelX, etc variables depending on which feature
// report is found
}
}
return 0;
}
/**
* @brief Parses received input report sent by BNO08x.
*
* Unit responds with packet that contains the following:
*
* packet_header_rx[0:3]: First, a 4 byte header
* rx_buffer[0:4]: Then a 5 byte timestamp of microsecond ticks since reading was taken
* rx_buffer[5 + 0]: Then a feature report ID (0x01 for Accel, 0x05 for Rotation Vector, etc...)
* rx_buffer[5 + 1]: Sequence number (See Ref.Manual 6.5.8.2)
* rx_buffer[5 + 2]: Status
* rx_buffer[3]: Delay
* rx_buffer[4:5]: i/accel x/gyro x/etc
* rx_buffer[6:7]: j/accel y/gyro y/etc
* rx_buffer[8:9]: k/accel z/gyro z/etc
* rx_buffer[10:11]: real/gyro temp/etc
* rx_buffer[12:13]: Accuracy estimate
*
* @return void, nothing to return
*/
uint16_t BNO08x::parse_input_report()
{
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_rx[1] << 8 | packet_header_rx[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
data_length -= 4; // Remove the header bytes from the data count
time_stamp = ((uint32_t) rx_buffer[4] << (8 * 3)) | ((uint32_t) rx_buffer[3] << (8 * 2)) | ((uint32_t) rx_buffer[2] << (8 * 1)) |
((uint32_t) rx_buffer[1] << (8 * 0));
// The gyro-integrated input reports are sent via the special gyro channel and do no include the usual ID, sequence,
// and status fields
if (packet_header_rx[2] == CHANNEL_GYRO)
{
raw_quat_I = (uint16_t) rx_buffer[1] << 8 | rx_buffer[0];
raw_quat_J = (uint16_t) rx_buffer[3] << 8 | rx_buffer[2];
raw_quat_K = (uint16_t) rx_buffer[5] << 8 | rx_buffer[4];
raw_quat_real = (uint16_t) rx_buffer[7] << 8 | rx_buffer[6];
raw_velocity_gyro_X = (uint16_t) rx_buffer[9] << 8 | rx_buffer[8];
raw_velocity_gyro_Y = (uint16_t) rx_buffer[11] << 8 | rx_buffer[10];
raw_velocity_gyro_Z = (uint16_t) rx_buffer[13] << 8 | rx_buffer[12];
return SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR;
}
status = rx_buffer[5 + 2] & 0x03; // Get status bits
uint16_t data1 = (uint16_t) rx_buffer[5 + 5] << 8 | rx_buffer[5 + 4];
uint16_t data2 = (uint16_t) rx_buffer[5 + 7] << 8 | rx_buffer[5 + 6];
uint16_t data3 = (uint16_t) rx_buffer[5 + 9] << 8 | rx_buffer[5 + 8];
uint16_t data4 = 0;
uint16_t data5 = 0;
uint16_t data6 = 0;
if (data_length - 5 > 9)
{
data4 = (uint16_t) rx_buffer[5 + 11] << 8 | rx_buffer[5 + 10];
}
if (data_length - 5 > 11)
{
data5 = (uint16_t) rx_buffer[5 + 13] << 8 | rx_buffer[5 + 12];
}
if (data_length - 5 > 13)
{
data6 = (uint16_t) rx_buffer[5 + 15] << 8 | rx_buffer[5 + 14];
}
// Store these generic values to their proper global variable
switch (rx_buffer[5])
{
case SENSOR_REPORTID_ACCELEROMETER:
accel_accuracy = status;
raw_accel_X = data1;
raw_accel_Y = data2;
raw_accel_Z = data3;
break;
case SENSOR_REPORTID_LINEAR_ACCELERATION:
accel_lin_accuracy = status;
raw_lin_accel_X = data1;
raw_lin_accel_Y = data2;
raw_lin_accel_Z = data3;
break;
case SENSOR_REPORTID_GYROSCOPE:
gyro_accuracy = status;
raw_gyro_X = data1;
raw_gyro_Y = data2;
raw_gyro_Z = data3;
break;
case SENSOR_REPORTID_UNCALIBRATED_GYRO:
uncalib_gyro_accuracy = status;
raw_uncalib_gyro_X = data1;
raw_uncalib_gyro_Y = data2;
raw_uncalib_gyro_Z = data3;
raw_bias_X = data4;
raw_bias_Y = data5;
raw_bias_Z = data6;
break;
case SENSOR_REPORTID_MAGNETIC_FIELD:
magf_accuracy = status;
raw_magf_X = data1;
raw_magf_Y = data2;
raw_magf_Z = data3;
break;
case SENSOR_REPORTID_TAP_DETECTOR:
tap_detector = rx_buffer[5 + 4]; // Byte 4 only
break;
case SENSOR_REPORTID_STEP_COUNTER:
step_count = data3; // Bytes 8/9
break;
case SENSOR_REPORTID_STABILITY_CLASSIFIER:
stability_classifier = rx_buffer[5 + 4]; // Byte 4 only
break;
case SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER:
activity_classifier = rx_buffer[5 + 5]; // Most likely state
// Load activity classification confidences into the array
for (i = 0; i < 9; i++) // Hardcoded to max of 9. TODO - bring in array size
activity_confidences[i] = rx_buffer[5 + 6 + i]; // 5 bytes of timestamp, byte 6 is first confidence
// byte
break;
case SENSOR_REPORTID_RAW_ACCELEROMETER:
mems_raw_accel_X = data1;
mems_raw_accel_Y = data2;
mems_raw_accel_Z = data3;
break;
case SENSOR_REPORTID_RAW_GYROSCOPE:
mems_raw_gyro_X = data1;
mems_raw_gyro_Y = data2;
mems_raw_gyro_Z = data3;
break;
case SENSOR_REPORTID_RAW_MAGNETOMETER:
mems_raw_magf_X = data1;
mems_raw_magf_Y = data2;
mems_raw_magf_Z = data3;
break;
case SHTP_REPORT_COMMAND_RESPONSE:
// The BNO080 responds with this report to command requests. It's up to use to remember which command we
// issued.
command = rx_buffer[5 + 2]; // This is the Command byte of the response
if (command == COMMAND_ME_CALIBRATE)
calibration_status = rx_buffer[5 + 5]; // R0 - Status (0 = success, non-zero = fail)
break;
case SENSOR_REPORTID_GRAVITY:
gravity_accuracy = status;
gravity_X = data1;
gravity_Y = data2;
gravity_Z = data3;
break;
default:
if (rx_buffer[5] == SENSOR_REPORTID_ROTATION_VECTOR || rx_buffer[5] == SENSOR_REPORTID_GAME_ROTATION_VECTOR ||
rx_buffer[5] == SENSOR_REPORTID_AR_VR_STABILIZED_ROTATION_VECTOR ||
rx_buffer[5] == SENSOR_REPORTID_AR_VR_STABILIZED_GAME_ROTATION_VECTOR)
{
quat_accuracy = status;
raw_quat_I = data1;
raw_quat_J = data2;
raw_quat_K = data3;
raw_quat_real = data4;
// Only available on rotation vector and ar/vr stabilized rotation vector,
// not game rot vector and not ar/vr stabilized rotation vector
raw_quat_radian_accuracy = data5;
}
else
{
// This sensor report ID is unhandled.
// See reference manual to add additional feature reports as needed
return 0;
}
break;
}
// TODO additional feature reports may be strung together. Parse them all.
return rx_buffer[5];
}
/**
* @brief Parses received command report sent by BNO08x (See Ref. Manual 6.3.9)
*
* @return The command report ID, 0 if invalid.
*/
uint16_t BNO08x::parse_command_report()
{
uint8_t command = 0;
if (rx_buffer[0] == SHTP_REPORT_COMMAND_RESPONSE)
{
// The BNO080 responds with this report to command requests. It's up to use to remember which command we issued.
command = rx_buffer[2]; // This is the Command byte of the response
if (command == COMMAND_ME_CALIBRATE)
{
calibration_status = rx_buffer[5 + 0]; // R0 - Status (0 = success, non-zero = fail)
}
return rx_buffer[0];
}
else
{
// This sensor report ID is unhandled.
// See SH2 Ref. Manual to add additional feature reports as needed
}
return 0;
}
/**
* @brief Sends command to enable game rotation vector reports (See Ref. Manual 6.5.19)
*
* @param time_between_reports Desired time between reports in microseconds.
* @return void, nothing to return
*/
void BNO08x::enable_game_rotation_vector(uint32_t time_between_reports)
{
queue_feature_command(SENSOR_REPORTID_GAME_ROTATION_VECTOR, time_between_reports);
wait_for_device_int(); // wait for next interrupt such that command is sent
vTaskDelay(50 / portTICK_PERIOD_MS); // allow some time for command to be executed
}