From a372e4cfdd479660ce793bf86260d79f3d41b15f Mon Sep 17 00:00:00 2001 From: myles-parfeniuk Date: Sat, 2 Mar 2024 02:55:33 -0800 Subject: [PATCH] calibration routine fix, formatting changes --- .clang-format | 12 +- BNO08x.cpp | 1193 ++++++++++++++++++++++++++++--------------------- BNO08x.hpp | 610 +++++++++++++------------ 3 files changed, 991 insertions(+), 824 deletions(-) diff --git a/.clang-format b/.clang-format index b502b67..b913b2b 100644 --- a/.clang-format +++ b/.clang-format @@ -1,11 +1,11 @@ --- -BasedOnStyle: Google +BasedOnStyle: LLVM Standard: Cpp11 UseTab: Never IndentWidth: 4 -ConstructorInitializerIndentWidth: 8 +ConstructorInitializerIndentWidth: 4 ContinuationIndentWidth: 8 AccessModifierOffset: -4 @@ -16,7 +16,7 @@ ConstructorInitializerAllOnOneLineOrOnePerLine: false AllowShortBlocksOnASingleLine: false AllowShortCaseLabelsOnASingleLine: false -AllowShortFunctionsOnASingleLine: Inline +AllowShortFunctionsOnASingleLine: None AllowShortIfStatementsOnASingleLine: false AllowShortLoopsOnASingleLine: false @@ -29,4 +29,8 @@ SpaceAfterCStyleCast: true CommentPragmas: '^[/!]<' -ColumnLimit: 120 +ColumnLimit: 150 # Adjusted column limit to prevent line breaks + +BreakBeforeBraces: Allman + +IndentAccessModifiers: true \ No newline at end of file diff --git a/BNO08x.cpp b/BNO08x.cpp index f5147ef..21bd5c6 100644 --- a/BNO08x.cpp +++ b/BNO08x.cpp @@ -13,10 +13,11 @@ bno08x_config_t BNO08x::default_imu_config; * @return void, nothing to return */ BNO08x::BNO08x(bno08x_config_t imu_config) - : tx_packet_queued(0U) - , imu_config(imu_config) - , calibration_status(1) - , int_asserted(false) { + : tx_packet_queued(0U) + , imu_config(imu_config) + , calibration_status(1) + , int_asserted(false) +{ // clear all buffers memset(tx_buffer, 0, sizeof(tx_buffer)); memset(rx_buffer, 0, sizeof(rx_buffer)); @@ -25,36 +26,36 @@ BNO08x::BNO08x(bno08x_config_t imu_config) 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) + 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) + 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 + 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 + 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 + 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); @@ -79,13 +80,14 @@ BNO08x::BNO08x(bno08x_config_t imu_config) 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 + 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 + 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); @@ -99,11 +101,11 @@ BNO08x::BNO08x(bno08x_config_t imu_config) 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_device_polling_transmit(spi_hdl, &spi_transaction); // send data packet tx_semaphore = xSemaphoreCreateRecursiveMutex(); spi_task_hdl = NULL; - xTaskCreate(&spi_task_trampoline, "spi_task", 4096, this, 8, &spi_task_hdl); // launch SPI task + xTaskCreate(&spi_task_trampoline, "spi_task", 4096, this, 8, &spi_task_hdl); // launch SPI task } /** @@ -113,65 +115,69 @@ BNO08x::BNO08x(bno08x_config_t imu_config) * * @return void, nothing to return */ -bool BNO08x::initialize() { +bool BNO08x::initialize() +{ // Receive advertisement message on boot (see SH2 Ref. Manual 5.2 & 5.3) - if (!hard_reset()) { + if (!hard_reset()) + { ESP_LOGE(TAG, "Failed to receive advertisement message on boot."); return false; - } else { + } + 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()) { + if (!wait_for_device_int()) + { ESP_LOGE(TAG, "Failed to receive initialize response on boot."); return false; - } else { + } + 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()) { + 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()) { + 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 + 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]); + 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" + " 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 + (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; @@ -182,23 +188,26 @@ bool BNO08x::initialize() { * * @return void, nothing to return */ -bool BNO08x::wait_for_device_int() { - int64_t start_time = esp_timer_get_time(); // get start time to manager timeout period - gpio_intr_enable(imu_config.io_int); // re-enable interrupts +bool BNO08x::wait_for_device_int() +{ + int64_t start_time = esp_timer_get_time(); // get start time to manager timeout period + gpio_intr_enable(imu_config.io_int); // re-enable interrupts // wait until an interrupt has been asserted or timeout has occured - while (!int_asserted) { + while (!int_asserted) + { vTaskDelay(10 / portTICK_PERIOD_MS); if ((esp_timer_get_time() - start_time) > HOST_INT_TIMEOUT_US) break; } - if (int_asserted) { + if (int_asserted) + { if (imu_config.debug_en) ESP_LOGI(TAG, "int asserted"); - int_asserted = false; // reset HINT ISR flag + int_asserted = false; // reset HINT ISR flag return true; } @@ -213,17 +222,18 @@ bool BNO08x::wait_for_device_int() { * * @return void, nothing to return */ -bool BNO08x::hard_reset() { +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 + 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 + 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; @@ -237,7 +247,8 @@ bool BNO08x::hard_reset() { * * @return True if reset was success. */ -bool BNO08x::soft_reset() { +bool BNO08x::soft_reset() +{ bool success = false; memset(commands, 0, sizeof(commands)); @@ -246,9 +257,9 @@ bool BNO08x::soft_reset() { queue_packet(CHANNEL_EXECUTABLE, 1); success = wait_for_device_int(); vTaskDelay(20 / portTICK_PERIOD_MS); - success = wait_for_device_int(); // receive advertisement message; + success = wait_for_device_int(); // receive advertisement message; vTaskDelay(20 / portTICK_PERIOD_MS); - success = wait_for_device_int(); // receive initialize message + success = wait_for_device_int(); // receive initialize message return success; } @@ -259,16 +270,18 @@ bool BNO08x::soft_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() { +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 + 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 (wait_for_device_int()) + { if (commands[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) return (commands[1]); } @@ -281,7 +294,8 @@ uint8_t BNO08x::get_reset_reason() { * * @return True if exiting sleep mode was success. */ -bool BNO08x::mode_on() { +bool BNO08x::mode_on() +{ bool success = false; memset(commands, 0, sizeof(commands)); @@ -290,9 +304,9 @@ bool BNO08x::mode_on() { queue_packet(CHANNEL_EXECUTABLE, 1); success = wait_for_device_int(); vTaskDelay(20 / portTICK_PERIOD_MS); - success = wait_for_device_int(); // receive advertisement message; + success = wait_for_device_int(); // receive advertisement message; vTaskDelay(20 / portTICK_PERIOD_MS); - success = wait_for_device_int(); // receive initialize message + success = wait_for_device_int(); // receive initialize message return success; } @@ -302,7 +316,8 @@ bool BNO08x::mode_on() { * * @return True if entering sleep mode was success. */ -bool BNO08x::mode_sleep() { +bool BNO08x::mode_sleep() +{ bool success = false; memset(commands, 0, sizeof(commands)); @@ -311,9 +326,9 @@ bool BNO08x::mode_sleep() { queue_packet(CHANNEL_EXECUTABLE, 1); success = wait_for_device_int(); vTaskDelay(20 / portTICK_PERIOD_MS); - success = wait_for_device_int(); // receive advertisement message; + success = wait_for_device_int(); // receive advertisement message; vTaskDelay(20 / portTICK_PERIOD_MS); - success = wait_for_device_int(); // receive initialize message + success = wait_for_device_int(); // receive initialize message return success; } @@ -323,13 +338,14 @@ bool BNO08x::mode_sleep() { * * @return void, nothing to return */ -bool BNO08x::receive_packet() { +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 + if (gpio_get_level(imu_config.io_int)) // ensure INT pin is low return false; // setup transaction to receive first 4 bytes (packet header) @@ -339,12 +355,12 @@ bool BNO08x::receive_packet() { 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) + 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 + packet_length_rx &= ~(1 << 15); // Clear the MSbit if (imu_config.debug_en) ESP_LOGW(TAG, "packet rx length: %d", packet_length_rx); @@ -352,7 +368,7 @@ bool BNO08x::receive_packet() { if (packet_length_rx == 0) return false; - packet_length_rx -= 4; // remove 4 header bytes from packet length (we already read those) + 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; @@ -361,9 +377,9 @@ bool BNO08x::receive_packet() { spi_transaction.rxlength = packet_length_rx * 8; spi_transaction.flags = 0; - spi_device_polling_transmit(spi_hdl, &spi_transaction); // receive rest of packet + spi_device_polling_transmit(spi_hdl, &spi_transaction); // receive rest of packet - gpio_set_level(imu_config.io_cs, 1); // de-assert chip select + gpio_set_level(imu_config.io_cs, 1); // de-assert chip select return true; } @@ -373,21 +389,23 @@ bool BNO08x::receive_packet() { * * @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 +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; xSemaphoreTake(tx_semaphore, portMAX_DELAY); 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) + 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++) { + for (i = 0; i < data_length; i++) + { tx_buffer[i + 4] = commands[i]; } @@ -401,7 +419,8 @@ void BNO08x::queue_packet(uint8_t channel_number, uint8_t data_length) { * * @return void, nothing to return */ -void BNO08x::send_packet() { +void BNO08x::send_packet() +{ // setup transaction to send packet spi_transaction.length = packet_length_tx * 8; spi_transaction.rxlength = 0; @@ -409,10 +428,10 @@ void BNO08x::send_packet() { 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, 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 + gpio_set_level(imu_config.io_cs, 1); // de-assert chip select tx_packet_queued = 0; } @@ -423,10 +442,11 @@ void BNO08x::send_packet() { * @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 +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); } @@ -436,9 +456,10 @@ void BNO08x::queue_command(uint8_t 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 +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); } @@ -447,10 +468,11 @@ void BNO08x::queue_request_product_id_command() { * * @return void, nothing to return */ -void BNO08x::calibrate_all() { +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 + 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 } /** @@ -458,10 +480,11 @@ void BNO08x::calibrate_all() { * * @return void, nothing to return */ -void BNO08x::calibrate_accelerometer() { +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 + 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 } /** @@ -469,10 +492,11 @@ void BNO08x::calibrate_accelerometer() { * * @return void, nothing to return */ -void BNO08x::calibrate_gyro() { +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 + 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 } /** @@ -480,10 +504,11 @@ void BNO08x::calibrate_gyro() { * * @return void, nothing to return */ -void BNO08x::calibrate_magnetometer() { +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 + 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 } /** @@ -491,10 +516,11 @@ void BNO08x::calibrate_magnetometer() { * * @return void, nothing to return */ -void BNO08x::calibrate_planar_accelerometer() { +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 + 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 } /** @@ -503,39 +529,41 @@ void BNO08x::calibrate_planar_accelerometer() { * @param sensor_to_calibrate The sensor to calibrate. * @return void, nothing to return */ -void BNO08x::queue_calibrate_command(uint8_t sensor_to_calibrate) { +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; + switch (sensor_to_calibrate) + { + case CALIBRATE_ACCEL: + commands[3] = 1; + break; - case CALIBRATE_GYRO: - commands[4] = 1; - break; + case CALIBRATE_GYRO: + commands[4] = 1; + break; - case CALIBRATE_MAG: - commands[5] = 1; - break; + case CALIBRATE_MAG: + commands[5] = 1; + break; - case CALIBRATE_PLANAR_ACCEL: - commands[7] = 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_ACCEL_GYRO_MAG: + commands[3] = 1; + commands[4] = 1; + commands[5] = 1; + break; - case CALIBRATE_STOP: - // do nothing, send packet of all 0s - break; + case CALIBRATE_STOP: + // do nothing, send packet of all 0s + break; - default: + default: - break; + break; } calibration_status = 1; @@ -548,15 +576,16 @@ void BNO08x::queue_calibrate_command(uint8_t sensor_to_calibrate) { * * @return void, nothing to return */ -void BNO08x::request_calibration_status() { +void BNO08x::request_calibration_status() +{ memset(commands, 0, sizeof(commands)); - commands[6] = 0x01; // P3 - 0x01 - Subcommand: Get ME Calibration + 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 + 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 } /** @@ -564,7 +593,8 @@ void BNO08x::request_calibration_status() { * * @return void, nothing to return */ -bool BNO08x::calibration_complete() { +bool BNO08x::calibration_complete() +{ if (calibration_status == 0) return true; @@ -576,10 +606,11 @@ bool BNO08x::calibration_complete() { * * @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 +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 } /** @@ -587,13 +618,14 @@ void BNO08x::end_calibration() { * * @return void, nothing to return */ -void BNO08x::save_calibration() { +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 + 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 } /** @@ -605,32 +637,35 @@ void BNO08x::save_calibration() { * * @return void, nothing to return */ -bool BNO08x::run_full_calibration_routine() { +bool BNO08x::run_full_calibration_routine() +{ float magf_x = 0; float magf_y = 0; float magf_z = 0; - uint8_t magnetometer_accuracy = LOW_ACCURACY; + 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 = LOW_ACCURACY; + 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 + 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_game_rotation_vector(100); // Send data update every 100ms // Enable Magnetic Field output - enable_magnetometer(100); // Send data update every 100ms + enable_magnetometer(100); // Send data update every 100ms - while (1) { - if (data_available()) { + while (1) + { + if (data_available()) + { magf_x = get_magf_X(); magf_y = get_magf_Y(); magf_z = get_magf_Z(); @@ -642,41 +677,52 @@ bool BNO08x::run_full_calibration_routine() { 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); + 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); - if ((magnetometer_accuracy >= MED_ACCURACY) && (quat_accuracy == HIGH_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) { + if (high_accuracy > 10) + { save_calibration(); request_calibration_status(); save_calibration_attempt = 0; - while (save_calibration_attempt < 100) { - if (data_available()) { - if (calibration_complete()) { + 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 >= 100) + vTaskDelay(1 / portTICK_PERIOD_MS); + + if (save_calibration_attempt >= 20) ESP_LOGE(TAG, "Calibration data failed to store."); return false; } } - vTaskDelay(40 / portTICK_PERIOD_MS); + vTaskDelay(5 / portTICK_PERIOD_MS); } } @@ -685,7 +731,8 @@ bool BNO08x::run_full_calibration_routine() { * * @return true if new data has been parsed and saved */ -bool BNO08x::data_available() { +bool BNO08x::data_available() +{ return (get_readings() != 0); } @@ -694,30 +741,37 @@ bool BNO08x::data_available() { * * @return void, nothing to return */ -uint16_t BNO08x::get_readings() { - if (wait_for_device_int()) { +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]); + 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 (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) { + 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) { + 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 parse_input_report(); // This will update the rawAccelX, etc variables depending on which feature + // report is found } } @@ -743,23 +797,25 @@ uint16_t BNO08x::get_readings() { * * @return void, nothing to return */ -uint16_t BNO08x::parse_input_report() { +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. + 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)); + 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) { + 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]; @@ -771,7 +827,7 @@ uint16_t BNO08x::parse_input_report() { return SENSOR_REPORTID_GYRO_INTEGRATED_ROTATION_VECTOR; } - status = rx_buffer[5 + 2] & 0x03; // Get status bits + 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]; @@ -779,132 +835,138 @@ uint16_t BNO08x::parse_input_report() { uint16_t data5 = 0; uint16_t data6 = 0; - if (data_length - 5 > 9) { + if (data_length - 5 > 9) + { data4 = (uint16_t) rx_buffer[5 + 11] << 8 | rx_buffer[5 + 10]; } - if (data_length - 5 > 11) { + if (data_length - 5 > 11) + { data5 = (uint16_t) rx_buffer[5 + 13] << 8 | rx_buffer[5 + 12]; } - if (data_length - 5 > 13) { + 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; + 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_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_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_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_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_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_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_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 + 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; + // 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_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_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 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 + 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; + 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; + 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; + 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; - } + // 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; + break; } // TODO additional feature reports may be strung together. Parse them all. @@ -916,18 +978,23 @@ uint16_t BNO08x::parse_input_report() { * * @return The command report ID, 0 if invalid. */ -uint16_t BNO08x::parse_command_report() { +uint16_t BNO08x::parse_command_report() +{ uint8_t command = 0; - if (rx_buffer[0] == SHTP_REPORT_COMMAND_RESPONSE) { + 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 + 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) + if (command == COMMAND_ME_CALIBRATE) + { + calibration_status = rx_buffer[5 + 0]; // R0 - Status (0 = success, non-zero = fail) } return rx_buffer[0]; - } else { + } + else + { // This sensor report ID is unhandled. // See SH2 Ref. Manual to add additional feature reports as needed } @@ -941,10 +1008,11 @@ uint16_t BNO08x::parse_command_report() { * @param time_between_reports Desired time between reports in miliseconds. * @return void, nothing to return */ -void BNO08x::enable_game_rotation_vector(uint16_t time_between_reports) { +void BNO08x::enable_game_rotation_vector(uint16_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 + 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 } /** @@ -953,10 +1021,11 @@ void BNO08x::enable_game_rotation_vector(uint16_t time_between_reports) { * @param time_between_reports Desired time between reports in miliseconds. * @return void, nothing to return */ -void BNO08x::enable_rotation_vector(uint16_t time_between_reports) { +void BNO08x::enable_rotation_vector(uint16_t time_between_reports) +{ queue_feature_command(SENSOR_REPORTID_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 + 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 } /** @@ -965,10 +1034,11 @@ void BNO08x::enable_rotation_vector(uint16_t time_between_reports) { * @param time_between_reports Desired time between reports in miliseconds. * @return void, nothing to return */ -void BNO08x::enable_ARVR_stabilized_rotation_vector(uint16_t time_between_reports) { +void BNO08x::enable_ARVR_stabilized_rotation_vector(uint16_t time_between_reports) +{ queue_feature_command(SENSOR_REPORTID_AR_VR_STABILIZED_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 + 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 } /** @@ -977,10 +1047,11 @@ void BNO08x::enable_ARVR_stabilized_rotation_vector(uint16_t time_between_report * @param time_between_reports Desired time between reports in miliseconds. * @return void, nothing to return */ -void BNO08x::enable_ARVR_stabilized_game_rotation_vector(uint16_t time_between_reports) { +void BNO08x::enable_ARVR_stabilized_game_rotation_vector(uint16_t time_between_reports) +{ queue_feature_command(SENSOR_REPORTID_AR_VR_STABILIZED_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 + 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 } /** @@ -989,10 +1060,11 @@ void BNO08x::enable_ARVR_stabilized_game_rotation_vector(uint16_t time_between_r * @param time_between_reports Desired time between reports in miliseconds. * @return void, nothing to return */ -void BNO08x::enable_gyro_integrated_rotation_vector(uint16_t time_between_reports) { +void BNO08x::enable_gyro_integrated_rotation_vector(uint16_t time_between_reports) +{ queue_feature_command(SENSOR_REPORTID_GYRO_INTEGRATED_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 + 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 } /** @@ -1001,10 +1073,11 @@ void BNO08x::enable_gyro_integrated_rotation_vector(uint16_t time_between_report * @param time_between_reports Desired time between reports in miliseconds. * @return void, nothing to return */ -void BNO08x::enable_accelerometer(uint16_t time_between_reports) { +void BNO08x::enable_accelerometer(uint16_t time_between_reports) +{ queue_feature_command(SENSOR_REPORTID_ACCELEROMETER, 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 + 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 } /** @@ -1013,10 +1086,11 @@ void BNO08x::enable_accelerometer(uint16_t time_between_reports) { * @param time_between_reports Desired time between reports in miliseconds. * @return void, nothing to return */ -void BNO08x::enable_linear_accelerometer(uint16_t time_between_reports) { +void BNO08x::enable_linear_accelerometer(uint16_t time_between_reports) +{ queue_feature_command(SENSOR_REPORTID_LINEAR_ACCELERATION, 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 + 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 } /** @@ -1025,10 +1099,11 @@ void BNO08x::enable_linear_accelerometer(uint16_t time_between_reports) { * @param time_between_reports Desired time between reports in miliseconds. * @return void, nothing to return */ -void BNO08x::enable_gravity(uint16_t time_between_reports) { +void BNO08x::enable_gravity(uint16_t time_between_reports) +{ queue_feature_command(SENSOR_REPORTID_GRAVITY, 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 + 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 } /** @@ -1037,10 +1112,11 @@ void BNO08x::enable_gravity(uint16_t time_between_reports) { * @param time_between_reports Desired time between reports in miliseconds. * @return void, nothing to return */ -void BNO08x::enable_gyro(uint16_t time_between_reports) { +void BNO08x::enable_gyro(uint16_t time_between_reports) +{ queue_feature_command(SENSOR_REPORTID_GYROSCOPE, 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 + 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 } /** @@ -1049,10 +1125,11 @@ void BNO08x::enable_gyro(uint16_t time_between_reports) { * @param time_between_reports Desired time between reports in miliseconds. * @return void, nothing to return */ -void BNO08x::enable_uncalibrated_gyro(uint16_t time_between_reports) { +void BNO08x::enable_uncalibrated_gyro(uint16_t time_between_reports) +{ queue_feature_command(SENSOR_REPORTID_UNCALIBRATED_GYRO, 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 + 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 } /** @@ -1061,10 +1138,11 @@ void BNO08x::enable_uncalibrated_gyro(uint16_t time_between_reports) { * @param time_between_reports Desired time between reports in miliseconds. * @return void, nothing to return */ -void BNO08x::enable_magnetometer(uint16_t time_between_reports) { +void BNO08x::enable_magnetometer(uint16_t time_between_reports) +{ queue_feature_command(SENSOR_REPORTID_MAGNETIC_FIELD, 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 + 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 } /** @@ -1073,10 +1151,11 @@ void BNO08x::enable_magnetometer(uint16_t time_between_reports) { * @param time_between_reports Desired time between reports in miliseconds. * @return void, nothing to return */ -void BNO08x::enable_tap_detector(uint16_t time_between_reports) { +void BNO08x::enable_tap_detector(uint16_t time_between_reports) +{ queue_feature_command(SENSOR_REPORTID_TAP_DETECTOR, 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 + 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 } /** @@ -1085,10 +1164,11 @@ void BNO08x::enable_tap_detector(uint16_t time_between_reports) { * @param time_between_reports Desired time between reports in miliseconds. * @return void, nothing to return */ -void BNO08x::enable_step_counter(uint16_t time_between_reports) { +void BNO08x::enable_step_counter(uint16_t time_between_reports) +{ queue_feature_command(SENSOR_REPORTID_STEP_COUNTER, 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 + 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 } /** @@ -1097,10 +1177,11 @@ void BNO08x::enable_step_counter(uint16_t time_between_reports) { * @param time_between_reports Desired time between reports in miliseconds. * @return void, nothing to return */ -void BNO08x::enable_stability_classifier(uint16_t time_between_reports) { +void BNO08x::enable_stability_classifier(uint16_t time_between_reports) +{ queue_feature_command(SENSOR_REPORTID_STABILITY_CLASSIFIER, 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 + 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 } /** @@ -1111,12 +1192,12 @@ void BNO08x::enable_stability_classifier(uint16_t time_between_reports) { * @param activity_confidence_vals Returned activity level confidences. * @return void, nothing to return */ -void BNO08x::enable_activity_classifier( - uint16_t time_between_reports, uint32_t activities_to_enable, uint8_t (&activity_confidence_vals)[9]) { - activity_confidences = activity_confidence_vals; // Store pointer to array +void BNO08x::enable_activity_classifier(uint16_t time_between_reports, uint32_t activities_to_enable, uint8_t (&activity_confidence_vals)[9]) +{ + activity_confidences = activity_confidence_vals; // Store pointer to array queue_feature_command(SENSOR_REPORTID_PERSONAL_ACTIVITY_CLASSIFIER, time_between_reports, activities_to_enable); - 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 + 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 } /** @@ -1125,10 +1206,11 @@ void BNO08x::enable_activity_classifier( * @param time_between_reports Desired time between reports in miliseconds. * @return void, nothing to return */ -void BNO08x::enable_raw_accelerometer(uint16_t time_between_reports) { +void BNO08x::enable_raw_accelerometer(uint16_t time_between_reports) +{ queue_feature_command(SENSOR_REPORTID_RAW_ACCELEROMETER, 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 + 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 } /** @@ -1137,10 +1219,11 @@ void BNO08x::enable_raw_accelerometer(uint16_t time_between_reports) { * @param time_between_reports Desired time between reports in miliseconds. * @return void, nothing to return */ -void BNO08x::enable_raw_gyro(uint16_t time_between_reports) { +void BNO08x::enable_raw_gyro(uint16_t time_between_reports) +{ queue_feature_command(SENSOR_REPORTID_RAW_GYROSCOPE, 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 + 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 } /** @@ -1149,10 +1232,11 @@ void BNO08x::enable_raw_gyro(uint16_t time_between_reports) { * @param time_between_reports Desired time between reports in miliseconds. * @return void, nothing to return */ -void BNO08x::enable_raw_magnetometer(uint16_t time_between_reports) { +void BNO08x::enable_raw_magnetometer(uint16_t time_between_reports) +{ queue_feature_command(SENSOR_REPORTID_RAW_MAGNETOMETER, 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 + 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 } /** @@ -1163,10 +1247,11 @@ void BNO08x::enable_raw_magnetometer(uint16_t time_between_reports) { * TARE_GAME_ROTATION_VECTOR, TARE_GEOMAGNETIC_ROTATION_VECTOR, etc.. * @return void, nothing to return */ -void BNO08x::tare_now(uint8_t axis_sel, uint8_t rotation_vector_basis) { +void BNO08x::tare_now(uint8_t axis_sel, uint8_t rotation_vector_basis) +{ queue_tare_command(TARE_NOW, axis_sel, rotation_vector_basis); - wait_for_device_int(); // wait for next interrupt such that command is sent - vTaskDelay(12 / portTICK_PERIOD_MS); // allow some time for command to be executed + wait_for_device_int(); // wait for next interrupt such that command is sent + vTaskDelay(12 / portTICK_PERIOD_MS); // allow some time for command to be executed } /** @@ -1174,10 +1259,11 @@ void BNO08x::tare_now(uint8_t axis_sel, uint8_t rotation_vector_basis) { * * @return void, nothing to return */ -void BNO08x::save_tare() { +void BNO08x::save_tare() +{ queue_tare_command(TARE_PERSIST); - wait_for_device_int(); // wait for next interrupt such that command is sent - vTaskDelay(12 / portTICK_PERIOD_MS); // allow some time for command to be executed + wait_for_device_int(); // wait for next interrupt such that command is sent + vTaskDelay(12 / portTICK_PERIOD_MS); // allow some time for command to be executed } /** @@ -1185,10 +1271,11 @@ void BNO08x::save_tare() { * * @return void, nothing to return */ -void BNO08x::clear_tare() { +void BNO08x::clear_tare() +{ queue_tare_command(TARE_SET_REORIENTATION); - wait_for_device_int(); // wait for next interrupt such that command is sent - vTaskDelay(12 / portTICK_PERIOD_MS); // allow some time for command to be executed + wait_for_device_int(); // wait for next interrupt such that command is sent + vTaskDelay(12 / portTICK_PERIOD_MS); // allow some time for command to be executed } /** @@ -1200,7 +1287,8 @@ void BNO08x::clear_tare() { * * @return void, nothing to return */ -float BNO08x::q_to_float(int16_t fixed_point_value, uint8_t q_point) { +float BNO08x::q_to_float(int16_t fixed_point_value, uint8_t q_point) +{ float q_float = fixed_point_value; q_float *= pow(2, q_point * -1); return (q_float); @@ -1211,7 +1299,8 @@ float BNO08x::q_to_float(int16_t fixed_point_value, uint8_t q_point) { * * @return void, nothing to return */ -uint32_t BNO08x::get_time_stamp() { +uint32_t BNO08x::get_time_stamp() +{ return time_stamp; } @@ -1225,7 +1314,8 @@ uint32_t BNO08x::get_time_stamp() { * * @return void, nothing to return */ -void BNO08x::get_magf(float& x, float& y, float& z, uint8_t& accuracy) { +void BNO08x::get_magf(float& x, float& y, float& z, uint8_t& accuracy) +{ x = q_to_float(raw_magf_X, MAGNETOMETER_Q1); y = q_to_float(raw_magf_Y, MAGNETOMETER_Q1); z = q_to_float(raw_magf_Z, MAGNETOMETER_Q1); @@ -1237,7 +1327,8 @@ void BNO08x::get_magf(float& x, float& y, float& z, uint8_t& accuracy) { * * @return The reported X component of magnetic field vector. */ -float BNO08x::get_magf_X() { +float BNO08x::get_magf_X() +{ float mag = q_to_float(raw_magf_X, MAGNETOMETER_Q1); return mag; } @@ -1247,7 +1338,8 @@ float BNO08x::get_magf_X() { * * @return The reported Y component of magnetic field vector. */ -float BNO08x::get_magf_Y() { +float BNO08x::get_magf_Y() +{ float mag = q_to_float(raw_magf_Y, MAGNETOMETER_Q1); return mag; } @@ -1257,7 +1349,8 @@ float BNO08x::get_magf_Y() { * * @return The reported Z component of magnetic field vector. */ -float BNO08x::get_magf_Z() { +float BNO08x::get_magf_Z() +{ float mag = q_to_float(raw_magf_Z, MAGNETOMETER_Q1); return mag; } @@ -1267,7 +1360,8 @@ float BNO08x::get_magf_Z() { * * @return The accuracy of reported magnetic field vector. */ -uint8_t BNO08x::get_magf_accuracy() { +uint8_t BNO08x::get_magf_accuracy() +{ return magf_accuracy; } @@ -1281,7 +1375,8 @@ uint8_t BNO08x::get_magf_accuracy() { * * @return void, nothing to return */ -void BNO08x::get_gravity(float& x, float& y, float& z, uint8_t& accuracy) { +void BNO08x::get_gravity(float& x, float& y, float& z, uint8_t& accuracy) +{ x = q_to_float(gravity_X, GRAVITY_Q1); y = q_to_float(gravity_Y, GRAVITY_Q1); z = q_to_float(gravity_Z, GRAVITY_Q1); @@ -1293,7 +1388,8 @@ void BNO08x::get_gravity(float& x, float& y, float& z, uint8_t& accuracy) { * * @return x axis gravity in m/s^2 */ -float BNO08x::get_gravity_X() { +float BNO08x::get_gravity_X() +{ return q_to_float(gravity_X, GRAVITY_Q1); } @@ -1302,7 +1398,8 @@ float BNO08x::get_gravity_X() { * * @return y axis gravity in m/s^2 */ -float BNO08x::get_gravity_Y() { +float BNO08x::get_gravity_Y() +{ return q_to_float(gravity_Y, GRAVITY_Q1); } @@ -1311,7 +1408,8 @@ float BNO08x::get_gravity_Y() { * * @return z axis gravity in m/s^2 */ -float BNO08x::get_gravity_Z() { +float BNO08x::get_gravity_Z() +{ return q_to_float(gravity_Z, GRAVITY_Q1); } @@ -1320,7 +1418,8 @@ float BNO08x::get_gravity_Z() { * * @return Accuracy of reported gravity. */ -uint8_t BNO08x::get_gravity_accuracy() { +uint8_t BNO08x::get_gravity_accuracy() +{ return gravity_accuracy; } @@ -1329,7 +1428,8 @@ uint8_t BNO08x::get_gravity_accuracy() { * * @return Rotation about the x axis in radians. */ -float BNO08x::get_roll() { +float BNO08x::get_roll() +{ float t_0 = 0.0; float t_1 = 0.0; float dq_w = get_quat_real(); @@ -1355,7 +1455,8 @@ float BNO08x::get_roll() { * * @return Rotation about the y axis in radians. */ -float BNO08x::get_pitch() { +float BNO08x::get_pitch() +{ float t_2 = 0.0; float dq_w = get_quat_real(); float dq_x = get_quat_I(); @@ -1383,7 +1484,8 @@ float BNO08x::get_pitch() { * * @return Rotation about the z axis in radians. */ -float BNO08x::get_yaw() { +float BNO08x::get_yaw() +{ float t_3 = 0.0; float t_4 = 0.0; float dq_w = get_quat_real(); @@ -1409,7 +1511,8 @@ float BNO08x::get_yaw() { * * @return Rotation about the x axis in degrees. */ -float BNO08x::get_roll_deg() { +float BNO08x::get_roll_deg() +{ return get_roll() * (180.0 / M_PI); } @@ -1418,7 +1521,8 @@ float BNO08x::get_roll_deg() { * * @return Rotation about the y axis in degrees. */ -float BNO08x::get_pitch_deg() { +float BNO08x::get_pitch_deg() +{ return get_pitch() * (180.0 / M_PI); } @@ -1427,7 +1531,8 @@ float BNO08x::get_pitch_deg() { * * @return Rotation about the z axis in degrees. */ -float BNO08x::get_yaw_deg() { +float BNO08x::get_yaw_deg() +{ return get_yaw() * (180.0 / M_PI); } @@ -1443,7 +1548,8 @@ float BNO08x::get_yaw_deg() { * * @return void, nothing to return */ -void BNO08x::get_quat(float& i, float& j, float& k, float& real, float& rad_accuracy, uint8_t& accuracy) { +void BNO08x::get_quat(float& i, float& j, float& k, float& real, float& rad_accuracy, uint8_t& accuracy) +{ i = q_to_float(raw_quat_I, ROTATION_VECTOR_Q1); j = q_to_float(raw_quat_J, ROTATION_VECTOR_Q1); k = q_to_float(raw_quat_K, ROTATION_VECTOR_Q1); @@ -1457,7 +1563,8 @@ void BNO08x::get_quat(float& i, float& j, float& k, float& real, float& rad_accu * * @return The I component of reported quaternion. */ -float BNO08x::get_quat_I() { +float BNO08x::get_quat_I() +{ float quat = q_to_float(raw_quat_I, ROTATION_VECTOR_Q1); return quat; } @@ -1467,7 +1574,8 @@ float BNO08x::get_quat_I() { * * @return The J component of reported quaternion. */ -float BNO08x::get_quat_J() { +float BNO08x::get_quat_J() +{ float quat = q_to_float(raw_quat_J, ROTATION_VECTOR_Q1); return quat; } @@ -1477,7 +1585,8 @@ float BNO08x::get_quat_J() { * * @return The K component of reported quaternion. */ -float BNO08x::get_quat_K() { +float BNO08x::get_quat_K() +{ float quat = q_to_float(raw_quat_K, ROTATION_VECTOR_Q1); return quat; } @@ -1487,7 +1596,8 @@ float BNO08x::get_quat_K() { * * @return The real component of reported quaternion. */ -float BNO08x::get_quat_real() { +float BNO08x::get_quat_real() +{ float quat = q_to_float(raw_quat_real, ROTATION_VECTOR_Q1); return quat; } @@ -1497,7 +1607,8 @@ float BNO08x::get_quat_real() { * * @return The radian accuracy of reported quaternion. */ -float BNO08x::get_quat_radian_accuracy() { +float BNO08x::get_quat_radian_accuracy() +{ float quat = q_to_float(raw_quat_radian_accuracy, ROTATION_VECTOR_Q1); return quat; } @@ -1507,7 +1618,8 @@ float BNO08x::get_quat_radian_accuracy() { * * @return The accuracy of reported quaternion. */ -uint8_t BNO08x::get_quat_accuracy() { +uint8_t BNO08x::get_quat_accuracy() +{ return quat_accuracy; } @@ -1521,7 +1633,8 @@ uint8_t BNO08x::get_quat_accuracy() { * * @return void, nothing to return */ -void BNO08x::get_accel(float& x, float& y, float& z, uint8_t& accuracy) { +void BNO08x::get_accel(float& x, float& y, float& z, uint8_t& accuracy) +{ x = q_to_float(raw_accel_X, ACCELEROMETER_Q1); y = q_to_float(raw_accel_Y, ACCELEROMETER_Q1); z = q_to_float(raw_accel_Z, ACCELEROMETER_Q1); @@ -1533,7 +1646,8 @@ void BNO08x::get_accel(float& x, float& y, float& z, uint8_t& accuracy) { * * @return The angular reported x axis acceleration. */ -float BNO08x::get_accel_X() { +float BNO08x::get_accel_X() +{ return q_to_float(raw_accel_X, ACCELEROMETER_Q1); } @@ -1542,7 +1656,8 @@ float BNO08x::get_accel_X() { * * @return The angular reported y axis acceleration. */ -float BNO08x::get_accel_Y() { +float BNO08x::get_accel_Y() +{ return q_to_float(raw_accel_Y, ACCELEROMETER_Q1); } @@ -1551,7 +1666,8 @@ float BNO08x::get_accel_Y() { * * @return The angular reported z axis acceleration. */ -float BNO08x::get_accel_Z() { +float BNO08x::get_accel_Z() +{ return q_to_float(raw_accel_Z, ACCELEROMETER_Q1); } @@ -1560,7 +1676,8 @@ float BNO08x::get_accel_Z() { * * @return Accuracy of linear acceleration. */ -uint8_t BNO08x::get_accel_accuracy() { +uint8_t BNO08x::get_accel_accuracy() +{ return accel_accuracy; } @@ -1574,7 +1691,8 @@ uint8_t BNO08x::get_accel_accuracy() { * * @return void, nothing to return */ -void BNO08x::get_linear_accel(float& x, float& y, float& z, uint8_t& accuracy) { +void BNO08x::get_linear_accel(float& x, float& y, float& z, uint8_t& accuracy) +{ x = q_to_float(raw_lin_accel_X, LINEAR_ACCELEROMETER_Q1); y = q_to_float(raw_lin_accel_Y, LINEAR_ACCELEROMETER_Q1); z = q_to_float(raw_lin_accel_Z, LINEAR_ACCELEROMETER_Q1); @@ -1586,7 +1704,8 @@ void BNO08x::get_linear_accel(float& x, float& y, float& z, uint8_t& accuracy) { * * @return The angular reported x axis linear acceleration. */ -float BNO08x::get_linear_accel_X() { +float BNO08x::get_linear_accel_X() +{ return q_to_float(raw_lin_accel_X, LINEAR_ACCELEROMETER_Q1); } @@ -1595,7 +1714,8 @@ float BNO08x::get_linear_accel_X() { * * @return The angular reported y axis linear acceleration. */ -float BNO08x::get_linear_accel_Y() { +float BNO08x::get_linear_accel_Y() +{ return q_to_float(raw_lin_accel_Y, LINEAR_ACCELEROMETER_Q1); } @@ -1604,7 +1724,8 @@ float BNO08x::get_linear_accel_Y() { * * @return The angular reported z axis linear acceleration. */ -float BNO08x::get_linear_accel_Z() { +float BNO08x::get_linear_accel_Z() +{ return q_to_float(raw_lin_accel_Z, LINEAR_ACCELEROMETER_Q1); } @@ -1613,7 +1734,8 @@ float BNO08x::get_linear_accel_Z() { * * @return Accuracy of linear acceleration. */ -uint8_t BNO08x::get_linear_accel_accuracy() { +uint8_t BNO08x::get_linear_accel_accuracy() +{ return accel_lin_accuracy; } @@ -1622,7 +1744,8 @@ uint8_t BNO08x::get_linear_accel_accuracy() { * * @return Reported raw accelerometer x axis reading from physical MEMs sensor. */ -int16_t BNO08x::get_raw_accel_X() { +int16_t BNO08x::get_raw_accel_X() +{ return mems_raw_accel_X; } @@ -1631,7 +1754,8 @@ int16_t BNO08x::get_raw_accel_X() { * * @return Reported raw accelerometer y axis reading from physical MEMs sensor. */ -int16_t BNO08x::get_raw_accel_Y() { +int16_t BNO08x::get_raw_accel_Y() +{ return mems_raw_accel_Y; } @@ -1640,7 +1764,8 @@ int16_t BNO08x::get_raw_accel_Y() { * * @return Reported raw accelerometer z axis reading from physical MEMs sensor. */ -int16_t BNO08x::get_raw_accel_Z() { +int16_t BNO08x::get_raw_accel_Z() +{ return mems_raw_accel_Z; } @@ -1649,7 +1774,8 @@ int16_t BNO08x::get_raw_accel_Z() { * * @return Reported raw gyroscope x axis reading from physical MEMs sensor. */ -int16_t BNO08x::get_raw_gyro_X() { +int16_t BNO08x::get_raw_gyro_X() +{ return mems_raw_gyro_X; } @@ -1658,7 +1784,8 @@ int16_t BNO08x::get_raw_gyro_X() { * * @return Reported raw gyroscope y axis reading from physical MEMs sensor. */ -int16_t BNO08x::get_raw_gyro_Y() { +int16_t BNO08x::get_raw_gyro_Y() +{ return mems_raw_gyro_Y; } @@ -1667,7 +1794,8 @@ int16_t BNO08x::get_raw_gyro_Y() { * * @return Reported raw gyroscope z axis reading from physical MEMs sensor. */ -int16_t BNO08x::get_raw_gyro_Z() { +int16_t BNO08x::get_raw_gyro_Z() +{ return mems_raw_gyro_Z; } @@ -1676,7 +1804,8 @@ int16_t BNO08x::get_raw_gyro_Z() { * * @return Reported raw magnetometer x axis reading from physical magnetometer sensor. */ -int16_t BNO08x::get_raw_magf_X() { +int16_t BNO08x::get_raw_magf_X() +{ return mems_raw_magf_X; } @@ -1685,7 +1814,8 @@ int16_t BNO08x::get_raw_magf_X() { * * @return Reported raw magnetometer y axis reading from physical magnetometer sensor. */ -int16_t BNO08x::get_raw_magf_Y() { +int16_t BNO08x::get_raw_magf_Y() +{ return mems_raw_magf_Y; } @@ -1694,7 +1824,8 @@ int16_t BNO08x::get_raw_magf_Y() { * * @return Reported raw magnetometer z axis reading from physical magnetometer sensor. */ -int16_t BNO08x::get_raw_magf_Z() { +int16_t BNO08x::get_raw_magf_Z() +{ return mems_raw_magf_Z; } @@ -1708,7 +1839,8 @@ int16_t BNO08x::get_raw_magf_Z() { * * @return void, nothing to return */ -void BNO08x::get_gyro_calibrated_velocity(float& x, float& y, float& z, uint8_t& accuracy) { +void BNO08x::get_gyro_calibrated_velocity(float& x, float& y, float& z, uint8_t& accuracy) +{ x = q_to_float(raw_gyro_X, GYRO_Q1); y = q_to_float(raw_gyro_Y, GYRO_Q1); z = q_to_float(raw_gyro_Z, GYRO_Q1); @@ -1720,7 +1852,8 @@ void BNO08x::get_gyro_calibrated_velocity(float& x, float& y, float& z, uint8_t& * * @return The angular reported x axis angular velocity from calibrated gyro (drift compensation applied). */ -float BNO08x::get_gyro_calibrated_velocity_X() { +float BNO08x::get_gyro_calibrated_velocity_X() +{ return q_to_float(raw_gyro_X, GYRO_Q1); } @@ -1729,7 +1862,8 @@ float BNO08x::get_gyro_calibrated_velocity_X() { * * @return The angular reported y axis angular velocity from calibrated gyro (drift compensation applied). */ -float BNO08x::get_gyro_calibrated_velocity_Y() { +float BNO08x::get_gyro_calibrated_velocity_Y() +{ return q_to_float(raw_gyro_Y, GYRO_Q1); } @@ -1738,7 +1872,8 @@ float BNO08x::get_gyro_calibrated_velocity_Y() { * * @return The angular reported z axis angular velocity from calibrated gyro (drift compensation applied). */ -float BNO08x::get_gyro_calibrated_velocity_Z() { +float BNO08x::get_gyro_calibrated_velocity_Z() +{ return q_to_float(raw_gyro_Z, GYRO_Q1); } @@ -1747,7 +1882,8 @@ float BNO08x::get_gyro_calibrated_velocity_Z() { * * @return Accuracy of calibrated gyro. */ -uint8_t BNO08x::get_gyro_accuracy() { +uint8_t BNO08x::get_gyro_accuracy() +{ return gyro_accuracy; } @@ -1765,8 +1901,8 @@ uint8_t BNO08x::get_gyro_accuracy() { * * @return void, nothing to return */ -void BNO08x::get_uncalibrated_gyro( - float& x, float& y, float& z, float& b_x, float& b_y, float& b_z, uint8_t& accuracy) { +void BNO08x::get_uncalibrated_gyro(float& x, float& y, float& z, float& b_x, float& b_y, float& b_z, uint8_t& accuracy) +{ x = q_to_float(raw_uncalib_gyro_X, GYRO_Q1); y = q_to_float(raw_uncalib_gyro_Y, GYRO_Q1); z = q_to_float(raw_uncalib_gyro_Z, GYRO_Q1); @@ -1781,7 +1917,8 @@ void BNO08x::get_uncalibrated_gyro( * * @return The angular reported x axis angular velocity from uncalibrated gyro. */ -float BNO08x::get_uncalibrated_gyro_X() { +float BNO08x::get_uncalibrated_gyro_X() +{ return q_to_float(raw_uncalib_gyro_X, GYRO_Q1); } @@ -1790,7 +1927,8 @@ float BNO08x::get_uncalibrated_gyro_X() { * * @return The angular reported Y axis angular velocity from uncalibrated gyro. */ -float BNO08x::get_uncalibrated_gyro_Y() { +float BNO08x::get_uncalibrated_gyro_Y() +{ return q_to_float(raw_uncalib_gyro_Y, GYRO_Q1); } @@ -1799,7 +1937,8 @@ float BNO08x::get_uncalibrated_gyro_Y() { * * @return The angular reported Z axis angular velocity from uncalibrated gyro. */ -float BNO08x::get_uncalibrated_gyro_Z() { +float BNO08x::get_uncalibrated_gyro_Z() +{ return q_to_float(raw_uncalib_gyro_Z, GYRO_Q1); } @@ -1808,7 +1947,8 @@ float BNO08x::get_uncalibrated_gyro_Z() { * * @return The angular reported x axis drift estimate. */ -float BNO08x::get_uncalibrated_gyro_bias_X() { +float BNO08x::get_uncalibrated_gyro_bias_X() +{ return q_to_float(raw_bias_X, GYRO_Q1); } @@ -1817,7 +1957,8 @@ float BNO08x::get_uncalibrated_gyro_bias_X() { * * @return The angular reported Y axis drift estimate. */ -float BNO08x::get_uncalibrated_gyro_bias_Y() { +float BNO08x::get_uncalibrated_gyro_bias_Y() +{ return q_to_float(raw_bias_Y, GYRO_Q1); } @@ -1826,7 +1967,8 @@ float BNO08x::get_uncalibrated_gyro_bias_Y() { * * @return The angular reported Z axis drift estimate. */ -float BNO08x::get_uncalibrated_gyro_bias_Z() { +float BNO08x::get_uncalibrated_gyro_bias_Z() +{ return q_to_float(raw_bias_Z, GYRO_Q1); } @@ -1835,7 +1977,8 @@ float BNO08x::get_uncalibrated_gyro_bias_Z() { * * @return Accuracy of uncalibrated gyro. */ -uint8_t BNO08x::get_uncalibrated_gyro_accuracy() { +uint8_t BNO08x::get_uncalibrated_gyro_accuracy() +{ return uncalib_gyro_accuracy; } @@ -1848,7 +1991,8 @@ uint8_t BNO08x::get_uncalibrated_gyro_accuracy() { * * @return void, nothing to return */ -void BNO08x::get_gyro_velocity(float& x, float& y, float& z) { +void BNO08x::get_gyro_velocity(float& x, float& y, float& z) +{ x = q_to_float(raw_velocity_gyro_X, ANGULAR_VELOCITY_Q1); y = q_to_float(raw_velocity_gyro_Y, ANGULAR_VELOCITY_Q1); z = q_to_float(raw_velocity_gyro_Z, ANGULAR_VELOCITY_Q1); @@ -1859,7 +2003,8 @@ void BNO08x::get_gyro_velocity(float& x, float& y, float& z) { * * @return The reported x axis angular velocity. */ -float BNO08x::get_gyro_velocity_X() { +float BNO08x::get_gyro_velocity_X() +{ return q_to_float(raw_velocity_gyro_X, ANGULAR_VELOCITY_Q1); } @@ -1868,7 +2013,8 @@ float BNO08x::get_gyro_velocity_X() { * * @return The reported y axis angular velocity. */ -float BNO08x::get_gyro_velocity_Y() { +float BNO08x::get_gyro_velocity_Y() +{ return q_to_float(raw_velocity_gyro_Y, ANGULAR_VELOCITY_Q1); } @@ -1877,7 +2023,8 @@ float BNO08x::get_gyro_velocity_Y() { * * @return The reported Z axis angular velocity. */ -float BNO08x::get_gyro_velocity_Z() { +float BNO08x::get_gyro_velocity_Z() +{ return q_to_float(raw_velocity_gyro_Z, ANGULAR_VELOCITY_Q1); } @@ -1886,9 +2033,10 @@ float BNO08x::get_gyro_velocity_Z() { * * @return 7 bit tap code indicated which axis taps have occurred. (See Ref. Manual 6.5.27) */ -uint8_t BNO08x::get_tap_detector() { +uint8_t BNO08x::get_tap_detector() +{ uint8_t previous_tap_detector = tap_detector; - tap_detector = 0; // Reset so user code sees exactly one tap + tap_detector = 0; // Reset so user code sees exactly one tap return (previous_tap_detector); } @@ -1897,7 +2045,8 @@ uint8_t BNO08x::get_tap_detector() { * * @return The current amount of counted steps. */ -uint16_t BNO08x::get_step_count() { +uint16_t BNO08x::get_step_count() +{ return step_count; } @@ -1906,7 +2055,8 @@ uint16_t BNO08x::get_step_count() { * * @return The current stability (0 = unknown, 1 = on table, 2 = stationary) */ -int8_t BNO08x::get_stability_classifier() { +int8_t BNO08x::get_stability_classifier() +{ return stability_classifier; } @@ -1924,7 +2074,8 @@ int8_t BNO08x::get_stability_classifier() { * 7 = runnning * 8 = on stairs */ -uint8_t BNO08x::get_activity_classifier() { +uint8_t BNO08x::get_activity_classifier() +{ return activity_classifier; } @@ -1933,7 +2084,8 @@ uint8_t BNO08x::get_activity_classifier() { * * @return void, nothing to return */ -void BNO08x::print_header() { +void BNO08x::print_header() +{ // print most recent header ESP_LOGI(TAG, "SHTP Header:\n\r" @@ -1942,8 +2094,8 @@ void BNO08x::print_header() { " Channel Number: %d\n\r" " Sequence Number: %d\n\r" " Channel Type: %s\n\r", - (int) packet_header_rx[0], (int) packet_header_rx[1], (int) packet_header_rx[2], (int) packet_header_rx[3], - (int) (packet_length_rx + 4), (int) packet_header_rx[2], (int) packet_header_rx[3], + (int) packet_header_rx[0], (int) packet_header_rx[1], (int) packet_header_rx[2], (int) packet_header_rx[3], (int) (packet_length_rx + 4), + (int) packet_header_rx[2], (int) packet_header_rx[3], (packet_header_rx[2] == 0) ? "Command" : (packet_header_rx[2] == 1) ? "Executable" : (packet_header_rx[2] == 2) ? "Control" @@ -1953,7 +2105,8 @@ void BNO08x::print_header() { : "Unknown"); } -void BNO08x::print_packet() { +void BNO08x::print_packet() +{ uint8_t i = 0; uint16_t print_length = 0; char packet_string[600]; @@ -1965,11 +2118,12 @@ void BNO08x::print_packet() { print_length = packet_length_rx; sprintf(packet_string, " Body: \n\r "); - for (i = 0; i < print_length; i++) { + for (i = 0; i < print_length; i++) + { sprintf(byte_string, " 0x%02X ", rx_buffer[i]); strcat(packet_string, byte_string); - if ((i + 1) % 6 == 0) // add a newline every 6 bytes + if ((i + 1) % 6 == 0) // add a newline every 6 bytes strcat(packet_string, "\n\r "); } @@ -1981,8 +2135,8 @@ void BNO08x::print_packet() { " Sequence Number: %d\n\r" " Channel Type: %s\n\r" "%s", - (int) packet_header_rx[0], (int) packet_header_rx[1], (int) packet_header_rx[2], (int) packet_header_rx[3], - (int) (packet_length_rx + 4), (int) packet_header_rx[2], (int) packet_header_rx[3], + (int) packet_header_rx[0], (int) packet_header_rx[1], (int) packet_header_rx[2], (int) packet_header_rx[3], (int) (packet_length_rx + 4), + (int) packet_header_rx[2], (int) packet_header_rx[3], (packet_header_rx[2] == 0) ? "Command" : (packet_header_rx[2] == 1) ? "Executable" : (packet_header_rx[2] == 2) ? "Control" @@ -2002,7 +2156,8 @@ void BNO08x::print_packet() { * * @return Q1 value for requested sensor. */ -int16_t BNO08x::get_Q1(uint16_t record_ID) { +int16_t BNO08x::get_Q1(uint16_t record_ID) +{ // Q1 is lower 16 bits of word 7 return (uint16_t) (FRS_read_word(record_ID, 7) & 0xFFFF); } @@ -2016,7 +2171,8 @@ int16_t BNO08x::get_Q1(uint16_t record_ID) { * * @return Q2 value for requested sensor. */ -int16_t BNO08x::get_Q2(uint16_t record_ID) { +int16_t BNO08x::get_Q2(uint16_t record_ID) +{ // Q2 is upper 16 bits of word 7 return (uint16_t) (FRS_read_word(record_ID, 7) >> 16U); } @@ -2030,7 +2186,8 @@ int16_t BNO08x::get_Q2(uint16_t record_ID) { * * @return Q3 value for requested sensor. */ -int16_t BNO08x::get_Q3(uint16_t record_ID) { +int16_t BNO08x::get_Q3(uint16_t record_ID) +{ // Q3 is upper 16 bits of word 8 return (uint16_t) (FRS_read_word(record_ID, 8) >> 16U); } @@ -2042,13 +2199,14 @@ int16_t BNO08x::get_Q3(uint16_t record_ID) { * * @return The resolution value for the requested sensor. */ -float BNO08x::get_resolution(uint16_t record_ID) { - int16_t Q = get_Q1(record_ID); // use same q as sensor's input report for range calc +float BNO08x::get_resolution(uint16_t record_ID) +{ + int16_t Q = get_Q1(record_ID); // use same q as sensor's input report for range calc // resolution is word 2 uint32_t value = FRS_read_word(record_ID, 2); - return q_to_float(value, Q); // return resolution + return q_to_float(value, Q); // return resolution } /** @@ -2058,13 +2216,14 @@ float BNO08x::get_resolution(uint16_t record_ID) { * * @return The range value for the requested sensor. */ -float BNO08x::get_range(uint16_t record_ID) { - int16_t Q = get_Q1(record_ID); // use same q as sensor's input report for range calc +float BNO08x::get_range(uint16_t record_ID) +{ + int16_t Q = get_Q1(record_ID); // use same q as sensor's input report for range calc // resolution is word 1 uint32_t value = FRS_read_word(record_ID, 1); - return q_to_float(value, Q); // return range + return q_to_float(value, Q); // return range } /** @@ -2078,11 +2237,12 @@ float BNO08x::get_range(uint16_t record_ID) { * * @return Requested meta data word, 0 if failed. */ -uint32_t BNO08x::FRS_read_word(uint16_t record_ID, uint8_t word_number) { - if (FRS_read_data(record_ID, word_number, 1)) // start at desired word and only read one 1 word +uint32_t BNO08x::FRS_read_word(uint16_t record_ID, uint8_t word_number) +{ + if (FRS_read_data(record_ID, word_number, 1)) // start at desired word and only read one 1 word return (meta_data[0]); else - return 0; // FRS read failed + return 0; // FRS read failed } /** @@ -2097,17 +2257,18 @@ uint32_t BNO08x::FRS_read_word(uint16_t record_ID, uint8_t word_number) { * * @return True if read request acknowledged (HINT was asserted) */ -bool BNO08x::FRS_read_request(uint16_t record_ID, uint16_t read_offset, uint16_t block_size) { +bool BNO08x::FRS_read_request(uint16_t record_ID, uint16_t read_offset, uint16_t block_size) +{ memset(commands, 0, sizeof(commands)); - commands[0] = SHTP_REPORT_FRS_READ_REQUEST; // FRS Read Request - commands[1] = 0; // Reserved - commands[2] = (read_offset >> 0) & 0xFF; // Read Offset LSB - commands[3] = (read_offset >> 8) & 0xFF; // Read Offset MSB - commands[4] = (record_ID >> 0) & 0xFF; // FRS Type LSB - commands[5] = (record_ID >> 8) & 0xFF; // FRS Type MSB - commands[6] = (block_size >> 0) & 0xFF; // Block size LSB - commands[7] = (block_size >> 8) & 0xFF; // Block size MSB + commands[0] = SHTP_REPORT_FRS_READ_REQUEST; // FRS Read Request + commands[1] = 0; // Reserved + commands[2] = (read_offset >> 0) & 0xFF; // Read Offset LSB + commands[3] = (read_offset >> 8) & 0xFF; // Read Offset MSB + commands[4] = (record_ID >> 0) & 0xFF; // FRS Type LSB + commands[5] = (record_ID >> 8) & 0xFF; // FRS Type MSB + commands[6] = (block_size >> 0) & 0xFF; // Block size LSB + commands[7] = (block_size >> 8) & 0xFF; // Block size MSB // Transmit packet on channel 2, 8 bytes queue_packet(CHANNEL_CONTROL, 8); @@ -2126,7 +2287,8 @@ bool BNO08x::FRS_read_request(uint16_t record_ID, uint16_t read_offset, uint16_t * * @return True if meta data read successfully. */ -bool BNO08x::FRS_read_data(uint16_t record_ID, uint8_t start_location, uint8_t words_to_read) { +bool BNO08x::FRS_read_data(uint16_t record_ID, uint8_t start_location, uint8_t words_to_read) +{ uint32_t data_0 = 0; uint32_t data_1 = 0; uint8_t data_length = 0; @@ -2134,10 +2296,13 @@ bool BNO08x::FRS_read_data(uint16_t record_ID, uint8_t start_location, uint8_t w uint8_t attempt_count = 0; uint16_t i = 0; - if (FRS_read_request(record_ID, start_location, words_to_read)) { + if (FRS_read_request(record_ID, start_location, words_to_read)) + { vTaskDelay(5 / portTICK_PERIOD_MS); - for (attempt_count = 0; attempt_count < 10; attempt_count++) { - if (wait_for_device_int()) { + for (attempt_count = 0; attempt_count < 10; attempt_count++) + { + if (wait_for_device_int()) + { if (rx_buffer[0] != SHTP_REPORT_FRS_READ_RESPONSE) return false; @@ -2148,10 +2313,8 @@ bool BNO08x::FRS_read_data(uint16_t record_ID, uint8_t start_location, uint8_t w data_length = rx_buffer[1] >> 4; FRS_status = rx_buffer[1] & 0x0F; - data_0 = (uint32_t) rx_buffer[7] << 24 | (uint32_t) rx_buffer[6] << 16 | (uint32_t) rx_buffer[5] << 8 | - (uint32_t) rx_buffer[4]; - data_1 = (uint32_t) rx_buffer[11] << 24 | (uint32_t) rx_buffer[10] << 16 | (uint32_t) rx_buffer[9] << 8 | - (uint32_t) rx_buffer[8]; + data_0 = (uint32_t) rx_buffer[7] << 24 | (uint32_t) rx_buffer[6] << 16 | (uint32_t) rx_buffer[5] << 8 | (uint32_t) rx_buffer[4]; + data_1 = (uint32_t) rx_buffer[11] << 24 | (uint32_t) rx_buffer[10] << 16 | (uint32_t) rx_buffer[9] << 8 | (uint32_t) rx_buffer[8]; // save meta data words to their respective buffer if (data_length > 0) @@ -2160,7 +2323,8 @@ bool BNO08x::FRS_read_data(uint16_t record_ID, uint8_t start_location, uint8_t w if (data_length > 1) meta_data[i++] = data_1; - if (i >= 9) { + if (i >= 9) + { if (imu_config.debug_en) ESP_LOGW(TAG, "meta_data array overrun, returning from FRS_read_request()"); @@ -2185,27 +2349,28 @@ bool BNO08x::FRS_read_data(uint16_t record_ID, uint8_t start_location, uint8_t w * * @return void, nothing to return */ -void BNO08x::queue_feature_command(uint8_t report_ID, uint16_t time_between_reports, uint32_t specific_config) { +void BNO08x::queue_feature_command(uint8_t report_ID, uint16_t time_between_reports, uint32_t specific_config) +{ uint32_t us_between_reports = (uint32_t) time_between_reports * 1000UL; memset(commands, 0, sizeof(commands)); - commands[0] = SHTP_REPORT_SET_FEATURE_COMMAND; // Set feature command (See Ref. Manual 6.5.4) - commands[1] = report_ID; // Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector - commands[2] = 0; // Feature flags - commands[3] = 0; // Change sensitivity (LSB) - commands[4] = 0; // Change sensitivity (MSB) - commands[5] = (us_between_reports >> 0) & 0xFF; // Report interval (LSB) in microseconds. 0x7A120 = 500ms - commands[6] = (us_between_reports >> 8) & 0xFF; // Report interval - commands[7] = (us_between_reports >> 16) & 0xFF; // Report interval - commands[8] = (us_between_reports >> 24) & 0xFF; // Report interval (MSB) - commands[9] = 0; // Batch Interval (LSB) - commands[10] = 0; // Batch Interval - commands[11] = 0; // Batch Interval - commands[12] = 0; // Batch Interval (MSB) - commands[13] = (specific_config >> 0) & 0xFF; // Sensor-specific config (LSB) - commands[14] = (specific_config >> 8) & 0xFF; // Sensor-specific config - commands[15] = (specific_config >> 16) & 0xFF; // Sensor-specific config - commands[16] = (specific_config >> 24) & 0xFF; // Sensor-specific config (MSB) + commands[0] = SHTP_REPORT_SET_FEATURE_COMMAND; // Set feature command (See Ref. Manual 6.5.4) + commands[1] = report_ID; // Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector + commands[2] = 0; // Feature flags + commands[3] = 0; // Change sensitivity (LSB) + commands[4] = 0; // Change sensitivity (MSB) + commands[5] = (us_between_reports >> 0) & 0xFF; // Report interval (LSB) in microseconds. 0x7A120 = 500ms + commands[6] = (us_between_reports >> 8) & 0xFF; // Report interval + commands[7] = (us_between_reports >> 16) & 0xFF; // Report interval + commands[8] = (us_between_reports >> 24) & 0xFF; // Report interval (MSB) + commands[9] = 0; // Batch Interval (LSB) + commands[10] = 0; // Batch Interval + commands[11] = 0; // Batch Interval + commands[12] = 0; // Batch Interval (MSB) + commands[13] = (specific_config >> 0) & 0xFF; // Sensor-specific config (LSB) + commands[14] = (specific_config >> 8) & 0xFF; // Sensor-specific config + commands[15] = (specific_config >> 16) & 0xFF; // Sensor-specific config + commands[16] = (specific_config >> 24) & 0xFF; // Sensor-specific config (MSB) // Transmit packet on channel 2, 17 bytes queue_packet(CHANNEL_CONTROL, 17); @@ -2221,12 +2386,14 @@ void BNO08x::queue_feature_command(uint8_t report_ID, uint16_t time_between_repo * * @return void, nothing to return */ -void BNO08x::queue_tare_command(uint8_t command, uint8_t axis, uint8_t rotation_vector_basis) { +void BNO08x::queue_tare_command(uint8_t command, uint8_t axis, uint8_t rotation_vector_basis) +{ memset(commands, 0, sizeof(commands)); commands[3] = command; - if (command == TARE_NOW) { + if (command == TARE_NOW) + { commands[4] = axis; commands[5] = rotation_vector_basis; } @@ -2243,8 +2410,9 @@ void BNO08x::queue_tare_command(uint8_t command, uint8_t axis, uint8_t rotation_ * * @return void, nothing to return */ -void BNO08x::queue_feature_command(uint8_t report_ID, uint16_t time_between_reports) { - queue_feature_command(report_ID, time_between_reports, 0); // No specific config +void BNO08x::queue_feature_command(uint8_t report_ID, uint16_t time_between_reports) +{ + queue_feature_command(report_ID, time_between_reports, 0); // No specific config } /** @@ -2254,10 +2422,10 @@ void BNO08x::queue_feature_command(uint8_t report_ID, uint16_t time_between_repo * * @return void, nothing to return */ -void BNO08x::spi_task_trampoline(void* arg) { - BNO08x* imu = (BNO08x*) - arg; // cast argument received by xTaskCreate ("this" pointer to imu object created by constructor call) - imu->spi_task(); // launch spi task from object +void BNO08x::spi_task_trampoline(void* arg) +{ + BNO08x* imu = (BNO08x*) arg; // cast argument received by xTaskCreate ("this" pointer to imu object created by constructor call) + imu->spi_task(); // launch spi task from object } /** @@ -2265,19 +2433,21 @@ void BNO08x::spi_task_trampoline(void* arg) { * * @return void, nothing to return */ -void BNO08x::spi_task() { - while (1) { - ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // block until notified by ISR - xSemaphoreTake(tx_semaphore, portMAX_DELAY); // wait if queue_packet is executing +void BNO08x::spi_task() +{ + while (1) + { + ulTaskNotifyTake(pdTRUE, portMAX_DELAY); // block until notified by ISR + xSemaphoreTake(tx_semaphore, portMAX_DELAY); // wait if queue_packet is executing if (tx_packet_queued != 0) - send_packet(); // send packet + send_packet(); // send packet else - receive_packet(); // receive packet + receive_packet(); // receive packet - int_asserted = true; // SPI completed, set interrupt asserted flag as true + int_asserted = true; // SPI completed, set interrupt asserted flag as true - xSemaphoreGive(tx_semaphore); // give back the semaphore such that queue packet be blocked + xSemaphoreGive(tx_semaphore); // give back the semaphore such that queue packet be blocked } } @@ -2288,13 +2458,14 @@ void BNO08x::spi_task() { * * @return void, nothing to return */ -void IRAM_ATTR BNO08x::hint_handler(void* arg) { +void IRAM_ATTR BNO08x::hint_handler(void* arg) +{ BaseType_t xHighPriorityTaskWoken = pdFALSE; - BNO08x* imu = (BNO08x*) arg; // cast argument received by gpio_isr_handler_add ("this" pointer to imu object - // created by constructor call) + BNO08x* imu = (BNO08x*) arg; // cast argument received by gpio_isr_handler_add ("this" pointer to imu object + // created by constructor call) - gpio_intr_disable(imu->imu_config.io_int); // disable interrupts - vTaskNotifyGiveFromISR(imu->spi_task_hdl, &xHighPriorityTaskWoken); // notify SPI task BNO08x is ready for - // servicing - portYIELD_FROM_ISR(xHighPriorityTaskWoken); // perform context switch if necessary + gpio_intr_disable(imu->imu_config.io_int); // disable interrupts + vTaskNotifyGiveFromISR(imu->spi_task_hdl, &xHighPriorityTaskWoken); // notify SPI task BNO08x is ready for + // servicing + portYIELD_FROM_ISR(xHighPriorityTaskWoken); // perform context switch if necessary } diff --git a/BNO08x.hpp b/BNO08x.hpp index 662c600..70340f4 100644 --- a/BNO08x.hpp +++ b/BNO08x.hpp @@ -16,7 +16,8 @@ #include /// @brief SHTP protocol channels -enum channels_t { +enum channels_t +{ CHANNEL_COMMAND, CHANNEL_EXECUTABLE, CHANNEL_CONTROL, @@ -26,25 +27,31 @@ enum channels_t { }; /// @brief Sensor accuracy returned during sensor calibration -enum sensor_accuracy_t { LOW_ACCURACY = 1, MED_ACCURACY, HIGH_ACCURACY }; +enum class IMUAccuracy +{ + LOW = 1, + MED, + HIGH +}; /// @brief IMU configuration settings passed into constructor -typedef struct bno08x_config_t { - spi_host_device_t spi_peripheral; ///