Cansat
/
esp32_BNO08x
2
0
Fork 0
Code Issues Pull Requests Packages Projects Releases Wiki Activity

corrected failure to enable_report after all reports disabled

This commit is contained in:
myles-parfeniuk 2024-11-15 16:48:51 -08:00
parent d4edb89033
commit 73589647fc
2 changed files with 55 additions and 25 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
include/BNO08x.hpp
View File

@ -368,7 +368,7 @@ class BNO08x
bool wait_for_data(); bool wait_for_data();
bool receive_packet(); bool receive_packet();
void send_packet(bno08x_tx_packet_t* packet); void send_packet(bno08x_tx_packet_t* packet);
void flush_rx_packets(uint8_t flush_count, TickType_t delay); void flush_rx_packets(uint8_t flush_count);
void enable_report(uint8_t report_ID, uint32_t time_between_reports, const EventBits_t report_evt_grp_bit, uint32_t special_config = 0); void enable_report(uint8_t report_ID, uint32_t time_between_reports, const EventBits_t report_evt_grp_bit, uint32_t special_config = 0);
void disable_report(uint8_t report_ID, const EventBits_t report_evt_grp_bit); void disable_report(uint8_t report_ID, const EventBits_t report_evt_grp_bit);
void queue_packet(uint8_t channel_number, uint8_t data_length, uint8_t* commands); void queue_packet(uint8_t channel_number, uint8_t data_length, uint8_t* commands);
@ -387,6 +387,7 @@ class BNO08x
// for debug // for debug
void print_header(bno08x_rx_packet_t* packet); void print_header(bno08x_rx_packet_t* packet);
void print_packet(bno08x_rx_packet_t* packet); void print_packet(bno08x_rx_packet_t* packet);
bool first_boot = true; ///< true only for first execution of hard_reset(), used to suppress the printing of product ID report.
// spi task // spi task
TaskHandle_t spi_task_hdl; ///<spi_task() handle TaskHandle_t spi_task_hdl; ///<spi_task() handle
@ -474,9 +475,6 @@ class BNO08x
10UL / 10UL /
portTICK_PERIOD_MS; ///<How long to delay after queueing command to allow it to execute (for ex. after sending command to enable report). portTICK_PERIOD_MS; ///<How long to delay after queueing command to allow it to execute (for ex. after sending command to enable report).
static const constexpr TickType_t FLUSH_PKT_DELAY_MS =
20UL / portTICK_PERIOD_MS; ///<How long to delay between wait_for_rx_done() calls when flush_rx_packets() is called.
static const constexpr uint32_t SCLK_MAX_SPEED = 3000000UL; ///<Max SPI SCLK speed BNO08x is capable of static const constexpr uint32_t SCLK_MAX_SPEED = 3000000UL; ///<Max SPI SCLK speed BNO08x is capable of
// evt_grp_spi bits // evt_grp_spi bits

74
source/BNO08x.cpp
View File

@ -513,7 +513,7 @@ bool BNO08x::wait_for_data()
*/ */
bool BNO08x::wait_for_tx_done() bool BNO08x::wait_for_tx_done()
{ {
// if no reports are enabled we can assume interrupts are disabled (see spi_task()) /* if no reports are enabled we can assume interrupts are disabled (see spi_task()) */
if (xEventGroupGetBits(evt_grp_report_en) == 0) if (xEventGroupGetBits(evt_grp_report_en) == 0)
gpio_intr_enable(imu_config.io_int); // re-enable interrupts gpio_intr_enable(imu_config.io_int); // re-enable interrupts
@ -595,7 +595,7 @@ bool BNO08x::soft_reset()
success = wait_for_tx_done(); success = wait_for_tx_done();
// flush any packets received // flush any packets received
flush_rx_packets(3, FLUSH_PKT_DELAY_MS); flush_rx_packets(3);
return success; return success;
} }
@ -642,7 +642,7 @@ bool BNO08x::mode_on()
success = wait_for_tx_done(); success = wait_for_tx_done();
// flush any packets received // flush any packets received
flush_rx_packets(3, FLUSH_PKT_DELAY_MS); flush_rx_packets(3);
return success; return success;
} }
@ -662,7 +662,7 @@ bool BNO08x::mode_sleep()
success = wait_for_tx_done(); success = wait_for_tx_done();
// flush any packets received // flush any packets received
flush_rx_packets(3, FLUSH_PKT_DELAY_MS); flush_rx_packets(3);
return success; return success;
} }
@ -733,6 +733,10 @@ bool BNO08x::receive_packet()
*/ */
void BNO08x::enable_report(uint8_t report_ID, uint32_t time_between_reports, const EventBits_t report_evt_grp_bit, uint32_t special_config) void BNO08x::enable_report(uint8_t report_ID, uint32_t time_between_reports, const EventBits_t report_evt_grp_bit, uint32_t special_config)
{ {
// if no reports have been enabled before this one, we can assume the IMU has gone to sleep, wake it up with a reset
if ((xEventGroupGetBits(evt_grp_report_en) & ~report_evt_grp_bit) == 0)
hard_reset();
queue_feature_command(report_ID, time_between_reports, special_config); queue_feature_command(report_ID, time_between_reports, special_config);
if (wait_for_tx_done()) // wait for transmit operation to complete if (wait_for_tx_done()) // wait for transmit operation to complete
{ {
@ -740,11 +744,13 @@ void BNO08x::enable_report(uint8_t report_ID, uint32_t time_between_reports, con
// if no reports were enabled before this one, we can assume hint interrupt was disabled, re-enable to read reports // 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) if ((xEventGroupGetBits(evt_grp_report_en) & ~report_evt_grp_bit) == 0)
{
gpio_intr_enable(imu_config.io_int); gpio_intr_enable(imu_config.io_int);
}
} }
// flush the first few reports returned to ensure new data // flush the first few reports returned to ensure new data
flush_rx_packets(3, 0); flush_rx_packets(3);
} }
/** /**
@ -764,6 +770,30 @@ void BNO08x::disable_report(uint8_t report_ID, const EventBits_t report_evt_grp_
{ {
xEventGroupClearBits(evt_grp_report_en, report_evt_grp_bit); xEventGroupClearBits(evt_grp_report_en, report_evt_grp_bit);
/*
6.5.5 of SH-2 Ref manual: "Note that SH-2 protocol version 1.0.1 and higher will send Get Feature Response messages
unsolicited if a sensor’s rate changes (e.g. due to change in the rate of a related sensor."
For ex. after calling disable_rotation_vector(), the following response packet will be sent:
I (3497) BNO08x: SHTP Header:
Raw 32 bit word: 0x15000205
Packet Length: 21
Channel Number: 2
Sequence Number: 5
Channel Type: Control
Body:
0xFC 0x05 0x00 0x00 0x00 0x00
0x00 0x00 0x00 0x00 0x00 0x00
0x00 0x00 0x00 0x00 0x00
The 0xFC indicates it is a get feature response, the 0x05 indicates it is for the rotation vector, the rest of the body will be 0.
Lets flush this response as it's currently detected as an invalid packet, we don't care about it.
It might be wise to detect the response for the respective report being disabled, but is probably not necessary.
*/
flush_rx_packets(3);
// no reports enabled, disable hint to avoid wasting processing time // no reports enabled, disable hint to avoid wasting processing time
if ((xEventGroupGetBits(evt_grp_report_en)) == 0) if ((xEventGroupGetBits(evt_grp_report_en)) == 0)
gpio_intr_disable(imu_config.io_int); gpio_intr_disable(imu_config.io_int);
@ -825,13 +855,10 @@ void BNO08x::send_packet(bno08x_tx_packet_t* packet)
xEventGroupSetBits(evt_grp_spi, EVT_GRP_SPI_TX_DONE_BIT); xEventGroupSetBits(evt_grp_spi, EVT_GRP_SPI_TX_DONE_BIT);
} }
void BNO08x::flush_rx_packets(uint8_t flush_count, TickType_t delay) void BNO08x::flush_rx_packets(uint8_t flush_count)
{ {
for (int i = 0; i < flush_count; i++) for (int i = 0; i < flush_count; i++)
{
wait_for_rx_done(); wait_for_rx_done();
vTaskDelay(delay);
}
} }
/** /**
@ -1230,18 +1257,23 @@ uint16_t BNO08x::parse_product_id_report(bno08x_rx_packet_t* packet)
uint32_t sw_build_number = PARSE_PRODUCT_ID_REPORT_SW_BUILD_NO(packet); uint32_t sw_build_number = PARSE_PRODUCT_ID_REPORT_SW_BUILD_NO(packet);
uint32_t sw_version_patch = PARSE_PRODUCT_ID_REPORT_SW_VERSION_PATCH(packet); uint32_t sw_version_patch = PARSE_PRODUCT_ID_REPORT_SW_VERSION_PATCH(packet);
// print product ID info packet if (first_boot)
ESP_LOGI(TAG, {
"Product ID Info: \n\r" first_boot = false;
" ---------------------------\n\r"
" Product ID: 0x%" PRIx32 "\n\r" // print product ID info packet
" SW Version Major: 0x%" PRIx32 "\n\r" ESP_LOGI(TAG,
" SW Version Minor: 0x%" PRIx32 "\n\r" "Product ID Info: \n\r"
" SW Part Number: 0x%" PRIx32 "\n\r" " ---------------------------\n\r"
" SW Build Number: 0x%" PRIx32 "\n\r" " Product ID: 0x%" PRIx32 "\n\r"
" SW Version Patch: 0x%" PRIx32 "\n\r" " SW Version Major: 0x%" PRIx32 "\n\r"
" ---------------------------\n\r", " SW Version Minor: 0x%" PRIx32 "\n\r"
product_id, sw_version_major, sw_version_minor, sw_part_number, sw_build_number, sw_version_patch); " SW Part Number: 0x%" PRIx32 "\n\r"
" SW Build Number: 0x%" PRIx32 "\n\r"
" SW Version Patch: 0x%" PRIx32 "\n\r"
" ---------------------------\n\r",
product_id, sw_version_major, sw_version_minor, sw_part_number, sw_build_number, sw_version_patch);
}
xQueueSend(queue_reset_reason, &reset_reason, 0); xQueueSend(queue_reset_reason, &reset_reason, 0);