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

formating, refactoring

This commit is contained in:
myles-parfeniuk 2024-11-15 17:50:42 -08:00
parent 505d8281bf
commit 93d8c837d2
4 changed files with 140 additions and 77 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

7
.clang-format
View File

@ -3,14 +3,12 @@ BasedOnStyle: LLVM
Standard: Cpp11
UseTab: Never
IndentWidth: 4
ConstructorInitializerIndentWidth: 4
ContinuationIndentWidth: 8
AccessModifierOffset: -4
AlwaysBreakTemplateDeclarations: true
BreakConstructorInitializersBeforeComma: true
ConstructorInitializerAllOnOneLineOrOnePerLine: false
@ -29,8 +27,9 @@ SpaceAfterCStyleCast: true
CommentPragmas: '^[/!]<'
ColumnLimit: 150 # Adjusted column limit to prevent line breaks
ColumnLimit: 150
BreakBeforeBraces: Allman
IndentAccessModifiers: true
IndentAccessModifiers: true
IndentPPDirectives: AfterHash

23
include/BNO08x.hpp
View File

@ -366,7 +366,9 @@ class BNO08x
bool wait_for_rx_done();
bool wait_for_tx_done();
bool wait_for_data();
bool receive_packet();
esp_err_t receive_packet();
esp_err_t receive_packet_header(bno08x_rx_packet_t* packet);
esp_err_t receive_packet_body(bno08x_rx_packet_t* packet);
void send_packet(bno08x_tx_packet_t* packet);
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);
@ -387,7 +389,7 @@ class BNO08x
// for debug
void print_header(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.
bool first_boot = true; ///< true only for first execution of hard_reset(), used to suppress the printing of product ID report.
// spi task
TaskHandle_t spi_task_hdl; ///<spi_task() handle
@ -541,14 +543,15 @@ class BNO08x
static const constexpr uint8_t COMMAND_CLEAR_DCD = 11U; ///<Clear DCD & Reset command (See SH2 Ref. Manual 6.4)
// SHTP channel 2 control report IDs, used in communication with sensor (See Ref. Manual 6.2)
static const constexpr uint8_t SHTP_REPORT_COMMAND_RESPONSE = 0xF1U; ///< See SH2 Ref. Manual 6.3.9
static const constexpr uint8_t SHTP_REPORT_COMMAND_REQUEST = 0xF2U; ///< See SH2 Ref. Manual 6.3.8
static const constexpr uint8_t SHTP_REPORT_FRS_READ_RESPONSE = 0xF3U; ///< See SH2 Ref. Manual 6.3.7
static const constexpr uint8_t SHTP_REPORT_FRS_READ_REQUEST = 0xF4U; ///< See SH2 Ref. Manual 6.3.6
static const constexpr uint8_t SHTP_REPORT_PRODUCT_ID_RESPONSE = 0xF8U; ///< See SH2 Ref. Manual 6.3.2
static const constexpr uint8_t SHTP_REPORT_PRODUCT_ID_REQUEST = 0xF9U; ///< See SH2 Ref. Manual 6.3.1
static const constexpr uint8_t SHTP_REPORT_BASE_TIMESTAMP = 0xFBU; ///< See SH2 Ref. Manual 7.2.1
static const constexpr uint8_t SHTP_REPORT_SET_FEATURE_COMMAND = 0xFDU; ///< See SH2 Ref. Manual 6.5.4
static const constexpr uint8_t SHTP_REPORT_COMMAND_RESPONSE = 0xF1U; ///< See SH2 Ref. Manual 6.3.9
static const constexpr uint8_t SHTP_REPORT_COMMAND_REQUEST = 0xF2U; ///< See SH2 Ref. Manual 6.3.8
static const constexpr uint8_t SHTP_REPORT_FRS_READ_RESPONSE = 0xF3U; ///< See SH2 Ref. Manual 6.3.7
static const constexpr uint8_t SHTP_REPORT_FRS_READ_REQUEST = 0xF4U; ///< See SH2 Ref. Manual 6.3.6
static const constexpr uint8_t SHTP_REPORT_PRODUCT_ID_RESPONSE = 0xF8U; ///< See SH2 Ref. Manual 6.3.2
static const constexpr uint8_t SHTP_REPORT_PRODUCT_ID_REQUEST = 0xF9U; ///< See SH2 Ref. Manual 6.3.1
static const constexpr uint8_t SHTP_REPORT_BASE_TIMESTAMP = 0xFBU; ///< See SH2 Ref. Manual 7.2.1
static const constexpr uint8_t SHTP_REPORT_SET_FEATURE_COMMAND = 0xFDU; ///< See SH2 Ref. Manual 6.5.4
static const constexpr uint8_t SHTP_REPORT_GET_FEATURE_RESPONSE = 0xFCU; ///< See SH2 Ref. Manual 6.5.5
// Sensor report IDs, used when enabling and reading BNO08x reports
static const constexpr uint8_t SENSOR_REPORT_ID_ACCELEROMETER = 0x01U; ///< See SH2 Ref. Manual 6.5.9

2
include/BNO08x_macros.hpp
View File

@ -17,7 +17,7 @@
// parsing universal to any packet
#define PARSE_PACKET_LENGTH(packet) \
(UINT16_CLR_LSB(static_cast<uint16_t>(packet.header[1]) << 8U) | UINT16_CLR_MSB(static_cast<uint16_t>(packet.header[0])))
(UINT16_CLR_LSB(static_cast<uint16_t>(packet->header[1]) << 8U) | UINT16_CLR_MSB(static_cast<uint16_t>(packet->header[0])))
#define PARSE_PACKET_DATA_LENGTH(packet_ptr) \
(UINT16_CLR_LSB(static_cast<uint16_t>(packet->header[1]) << 8U) | UINT16_CLR_MSB(static_cast<uint16_t>(packet->header[0])))

185
source/BNO08x.cpp
View File

@ -443,9 +443,11 @@ bool BNO08x::wait_for_rx_done()
// 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))
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "int asserted");
#endif
// clang-format off
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "int asserted");
#endif
// clang-format on
success = true;
}
@ -483,9 +485,11 @@ bool BNO08x::wait_for_data()
// only return true if packet is valid
if (xEventGroupGetBits(evt_grp_spi) & EVT_GRP_SPI_RX_VALID_PACKET_BIT)
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "Valid packet received.");
#endif
// clang-format off
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "Valid packet received.");
#endif
// clang-format on
success = true;
}
@ -519,9 +523,11 @@ bool BNO08x::wait_for_tx_done()
if (xEventGroupWaitBits(evt_grp_spi, EVT_GRP_SPI_TX_DONE_BIT, pdTRUE, pdTRUE, HOST_INT_TIMEOUT_MS))
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "Packet sent successfully.");
#endif
// clang-format off
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "Packet sent successfully.");
#endif
// clang-format on
return true;
}
@ -673,52 +679,89 @@ bool BNO08x::mode_sleep()
*
* @return void, nothing to return
*/
bool BNO08x::receive_packet()
esp_err_t BNO08x::receive_packet()
{
bno08x_rx_packet_t packet;
uint8_t dummy_header_tx[4] = {0};
esp_err_t ret = ESP_OK;
if (gpio_get_level(imu_config.io_int)) // ensure INT pin is low
return false;
return ESP_ERR_INVALID_STATE;
gpio_set_level(imu_config.io_cs, 0); // assert chip select
// receive packet header
ret = receive_packet_header(&packet);
if (ret != ESP_OK)
{
gpio_set_level(imu_config.io_cs, 1); // de-assert chip select
return ret;
}
// clang-format off
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGW(TAG, "packet rx length: %d", packet.length);
#endif
// clang-format on
if (packet.length == 0)
{
gpio_set_level(imu_config.io_cs, 1); // de-assert chip select
return ESP_ERR_INVALID_RESPONSE;
}
ret = receive_packet_body(&packet);
if (ret == ESP_OK)
{
xQueueSend(queue_rx_data, &packet, 0); // send received data to data_proc_task
xEventGroupSetBits(evt_grp_spi, EVT_GRP_SPI_RX_DONE_BIT);
}
gpio_set_level(imu_config.io_cs, 1); // de-assert chip select
return ret;
}
esp_err_t BNO08x::receive_packet_header(bno08x_rx_packet_t* packet)
{
esp_err_t ret = ESP_OK;
uint8_t dummy_header_tx[4] = {0};
// setup transaction to receive first 4 bytes (packet header)
spi_transaction.rx_buffer = 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)
ret = spi_device_polling_transmit(spi_hdl, &spi_transaction); // receive first 4 bytes (packet header)
// calculate length of packet from received header
packet.length = PARSE_PACKET_LENGTH(packet);
packet.length &= ~(1U << 15U); // Clear the MSbit
if (ret == ESP_OK)
{
// calculate length of packet from received header
packet->length = PARSE_PACKET_LENGTH(packet);
packet->length &= ~(1U << 15U); // clear the MSbit
}
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGW(TAG, "packet rx length: %d", packet.length);
#endif
return ret;
}
if (packet.length == 0)
return false;
esp_err_t BNO08x::receive_packet_body(bno08x_rx_packet_t* packet)
{
esp_err_t ret = ESP_OK;
packet.length -= 4; // remove 4 header bytes from packet length (we already read those)
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.rx_buffer = packet->body;
spi_transaction.tx_buffer = NULL;
spi_transaction.length = packet.length * 8;
spi_transaction.rxlength = packet.length * 8;
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
ret = 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;
return ret;
}
/**
@ -1195,15 +1238,23 @@ void BNO08x::register_cb(std::function<void()> cb_fxn)
*/
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
// clang-format off
#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
// clang-format on
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_GET_FEATURE_RESPONSE)
{
}*/
if (packet->body[0] == SHTP_REPORT_FRS_READ_RESPONSE)
{
return parse_frs_read_response_report(packet);
@ -1212,9 +1263,11 @@ uint16_t BNO08x::parse_packet(bno08x_rx_packet_t* 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
// clang-format off
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "RX'd packet, channel report");
#endif
// clang-format on
return parse_input_report(packet); // This will update the rawAccelX, etc variables depending on which feature
// report is found
@ -1222,18 +1275,22 @@ uint16_t BNO08x::parse_packet(bno08x_rx_packet_t* packet)
if (packet->header[2] == CHANNEL_CONTROL)
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "RX'd packet, channel control");
#endif
// clang-format off
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "RX'd packet, channel control");
#endif
// clang-format on
return parse_command_report(packet); // This will update responses to commands, calibrationStatus, etc.
}
if (packet->header[2] == CHANNEL_GYRO)
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "Rx packet, channel gyro");
#endif
// clang-format off
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
ESP_LOGI(TAG, "Rx packet, channel gyro");
#endif
// clang-format on
return parse_input_report(packet); // This will update the rawAccelX, etc variables depending on which feature
// report is found
@ -1250,13 +1307,13 @@ uint16_t BNO08x::parse_packet(bno08x_rx_packet_t* packet)
*/
uint16_t BNO08x::parse_product_id_report(bno08x_rx_packet_t* packet)
{
uint32_t product_id = PARSE_PRODUCT_ID_REPORT_PRODUCT_ID(packet);
uint32_t reset_reason = PARSE_PRODUCT_ID_REPORT_RESET_REASON(packet);
uint32_t sw_part_number = PARSE_PRODUCT_ID_REPORT_SW_PART_NO(packet);
uint32_t sw_version_major = PARSE_PRODUCT_ID_REPORT_SW_VERSION_MAJOR(packet);
uint32_t sw_version_minor = PARSE_PRODUCT_ID_REPORT_SW_VERSION_MINOR(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);
const uint32_t product_id = PARSE_PRODUCT_ID_REPORT_PRODUCT_ID(packet);
const uint32_t reset_reason = PARSE_PRODUCT_ID_REPORT_RESET_REASON(packet);
const uint32_t sw_part_number = PARSE_PRODUCT_ID_REPORT_SW_PART_NO(packet);
const uint32_t sw_version_major = PARSE_PRODUCT_ID_REPORT_SW_VERSION_MAJOR(packet);
const uint32_t sw_version_minor = PARSE_PRODUCT_ID_REPORT_SW_VERSION_MINOR(packet);
const uint32_t sw_build_number = PARSE_PRODUCT_ID_REPORT_SW_BUILD_NO(packet);
const uint32_t sw_version_patch = PARSE_PRODUCT_ID_REPORT_SW_VERSION_PATCH(packet);
if (first_boot)
{
@ -3174,10 +3231,12 @@ void BNO08x::spi_task_trampoline(void* arg)
*/
void BNO08x::spi_task()
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
static uint64_t prev_time = esp_timer_get_time();
static uint64_t current_time = 0;
#endif
// clang-format off
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
static uint64_t prev_time = esp_timer_get_time();
static uint64_t current_time = 0;
#endif
// clang-format on
bno08x_tx_packet_t tx_packet;
@ -3193,11 +3252,13 @@ void BNO08x::spi_task()
if (CHECK_TASKS_RUNNING(evt_grp_task_flow, EVT_GRP_TSK_FLW_RUNNING_BIT)) // ensure deconstructor has not requested that task be deleted
{
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
current_time = esp_timer_get_time();
ESP_LOGI(TAG, "HINT asserted, time since last assertion: %llu", (current_time - prev_time));
prev_time = current_time;
#endif
// clang-format off
#ifdef CONFIG_ESP32_BNO08x_DEBUG_STATEMENTS
current_time = esp_timer_get_time();
ESP_LOGI(TAG, "HINT asserted, time since last assertion: %llu", (current_time - prev_time));
prev_time = current_time;
#endif
// clang-format on
if (xQueueReceive(queue_tx_data, &tx_packet, 0)) // check for queued packet to be sent, non blocking
send_packet(&tx_packet); // send packet