Fully featured I2C and SPI driver for CEVA (Hilcrest)'s BNO080 and FSM300 Inertial Measurement Units.
Dependents: BNO080-Examples BNO080-Examples
BNO080 Driver
by Jamie Smith / USC Rocket Propulsion Lab
After lots of development, we are proud to present our driver for the Hilcrest BNO080 IMU! This driver is inspired by SparkFun and Nathan Seidle's Arduino driver for this chip, but has been substantially rewritten and adapted.
It supports the main features of the chip, such as reading rotation and acceleration data, as well as some of its more esoteric functionality, such as counting steps and detecting whether the device is being hand-held.
Features
- Support for 15 different data reports from the IMU, from acceleration to rotation to tap detection
- Support for reading of sensor data, and automatic checking of update rate against allowed values in metadata
BNO_DEBUG
switch enabling verbose, detailed output about communications with the chip for ease of debugging- Ability to tare sensor rotation and set mounting orientation
- Can operate in several execution modes: polling I2C, polling SPI, and threaded SPI (which handles timing-critical functions in a dedicated thread, and automatically activates when the IMU has data available)
- Also has experimental support for using asynchronous SPI transactions, allowing other threads to execute while communication with the BNO is occurring. Note that this functionality requires a patch to Mbed OS source code due to Mbed bug #13941
- Calibration function
- Reasonable code size for what you get: the library uses about 4K of flash and one instance of the object uses about 1700 bytes of RAM.
Documentation
Full Doxygen documentation is available online here
Example Code
Here's a simple example:
BNO080 Rotation Vector and Acceleration
#include <mbed.h> #include <BNO080.h> int main() { Serial pc(USBTX, USBRX); // Create IMU, passing in output stream, pins, I2C address, and I2C frequency // These pin assignments are specific to my dev setup -- you'll need to change them BNO080I2C imu(&pc, p28, p27, p16, p30, 0x4a, 100000); pc.baud(115200); pc.printf("============================================================\n"); // Tell the IMU to report rotation every 100ms and acceleration every 200ms imu.enableReport(BNO080::ROTATION, 100); imu.enableReport(BNO080::TOTAL_ACCELERATION, 200); while (true) { wait(.001f); // poll the IMU for new data -- this returns true if any packets were received if(imu.updateData()) { // now check for the specific type of data that was received (can be multiple at once) if (imu.hasNewData(BNO080::ROTATION)) { // convert quaternion to Euler degrees and print pc.printf("IMU Rotation Euler: "); TVector3 eulerRadians = imu.rotationVector.euler(); TVector3 eulerDegrees = eulerRadians * (180.0 / M_PI); eulerDegrees.print(pc, true); pc.printf("\n"); } if (imu.hasNewData(BNO080::TOTAL_ACCELERATION)) { // print the acceleration vector using its builtin print() method pc.printf("IMU Total Acceleration: "); imu.totalAcceleration.print(pc, true); pc.printf("\n"); } } } }
If you want more, a comprehensive, ready-to-run set of examples is available on my BNO080-Examples repository.
Credits
This driver makes use of a lightweight, public-domain library for vectors and quaternions available here.
Changelog
Version 2.1 (Nov 24 2020)
- Added BNO080Async, which provides a threaded implementation of the SPI driver. This should help get the best performance and remove annoying timing requirements on the code calling the driver
- Added experimental
USE_ASYNC_SPI
option - Fixed bug in v2.0 causing calibrations to fail
Version 2.0 (Nov 18 2020)
- Added SPI support
- Refactored buffer system so that SPI could be implemented as a subclass. Unfortunately this does substantially increase the memory usage of the driver, but I believe that the benefits are worth it.
Version 1.3 (Jul 21 2020)
- Fix deprecation warnings and compile errors in Mbed 6
- Fix compile errors in Arm Compiler (why doesn't it have M_PI????)
Version 1.2 (Jan 30 2020)
- Removed accidental IRQ change
- Fixed hard iron offset reading incorrectly due to missing cast
Version 1.1 (Jun 14 2019)
- Added support for changing permanent orientation
- Add FRS writing functions
- Removed some errant printfs
Version 1.0 (Dec 29 2018)
- Initial Mbed OS release
Diff: BNO080Async.cpp
- Revision:
- 9:430f5302f9e1
diff -r 199c7fad233d -r 430f5302f9e1 BNO080Async.cpp --- /dev/null Thu Jan 01 00:00:00 1970 +0000 +++ b/BNO080Async.cpp Tue Nov 24 15:06:05 2020 -0800 @@ -0,0 +1,305 @@ +// +// Created by jamie on 11/18/2020. +// + +#include "BNO080Async.h" + +#include <cinttypes> + +#define BNO_ASYNC_DEBUG 0 + +// event flags constants for signalling the thread +#define EF_INTERRUPT 0b1 +#define EF_SHUTDOWN 0b10 +#define EF_RESTART 0b100 + +void BNO080Async::threadMain() +{ + while(commLoop()) + { + // loop forever + } +} + +bool BNO080Async::commLoop() +{ + _rst = 0; // Reset BNO080 + ThisThread::sleep_for(1ms); // Min length not specified in datasheet? + _rst = 1; // Bring out of reset + + // wait for a falling edge (NOT just a low) on the INT pin to denote startup + { + EventFlags edgeWaitFlags; + _int.fall(callback([&](){edgeWaitFlags.set(1);})); + + // have the RTOS wait until an edge is detected or the timeout is hit + uint32_t edgeWaitEvent = edgeWaitFlags.wait_any(1, (BNO080_RESET_TIMEOUT).count()); + + if(!edgeWaitEvent) + { + _debugPort->printf("Error: BNO080 reset timed out, chip not detected.\n"); + } + } + +#if BNO_ASYNC_DEBUG + _debugPort->printf("BNO080 detected!\r\n"); +#endif + + wakeupFlags.set(EF_INTERRUPT); + + // configure interrupt to send the event flag + _int.fall(callback([&]() + { + if(!inTXRX) + { + wakeupFlags.set(EF_INTERRUPT); + } + })); + + while(true) + { + uint32_t wakeupEvent = wakeupFlags.wait_any(EF_INTERRUPT | EF_SHUTDOWN | EF_RESTART); + + if(wakeupEvent & EF_SHUTDOWN) + { + // shutdown thread permanently + return false; + } + else if(wakeupEvent & EF_RESTART) + { + // restart thread + return true; + } + + // lock the mutex to handle remaining cases + bnoDataMutex.lock(); + + if(wakeupEvent & EF_INTERRUPT) + { + while(_int == 0) + { + inTXRX = true; + + // send data if there is data to send. This may also receive a packet. + if (dataToSend) + { + BNO080SPI::sendPacket(txPacketChannelNumber, txPacketDataLength); + dataToSend = false; + } + else + { + BNO080SPI::receivePacket(); + } + + inTXRX = false; + + // clear the wake flag if it was set + _wakePin = 1; + + // If the IMU wants to send us an interrupt immediately after a transaction, + // it will be missed due to the inTXRX block. So, we need to keep checking _int + // in a loop. + + // update received flag + dataReceived = true; + + // check if this packet is being waited on. + if (waitingForPacket) + { + if (waitingForReportID == rxShtpData[0] && waitingForChannel == rxShtpHeader[2]) + { + // unblock main thread so it can process this packet + waitingPacketArrived = true; + waitingPacketArrivedCV.notify_all(); + + // unlock mutex and wait until the main thread says it's OK to receive another packet + clearToRxNextPacket = false; + waitingPacketProcessedCV.wait([&]() { return clearToRxNextPacket; }); + } + } + else + { + processPacket(); + } + + // clear the interrupt flag if it has been set because we're about to check _int anyway. + // Prevents spurious wakeups. + wakeupFlags.clear(EF_INTERRUPT); + + } + + } + + // unlock data mutex before waiting for flags + bnoDataMutex.unlock(); + } +} + +bool BNO080Async::sendPacket(uint8_t channelNumber, uint8_t dataLength) +{ + // first make sure mutex is locked + if(bnoDataMutex.get_owner() != ThisThread::get_id()) + { + _debugPort->printf("IMU communication function called without bnoDataMutex locked!\n"); + return false; + } + + // now set class variables + txPacketChannelNumber = channelNumber; + txPacketDataLength = dataLength; + dataToSend = true; + + // signal thread to wake up by sending _wake signal (which will cause the IMU to interrupt us once ready) + _wakePin = 0; + + return true; +} + +void BNO080Async::clearSendBuffer() +{ + // first make sure mutex is locked + if(bnoDataMutex.get_owner() != ThisThread::get_id()) + { + _debugPort->printf("IMU communication function called without bnoDataMutex locked!\n"); + return; + } + + // Check if we are trying to send a packet while another packet is still queued. + // Since we don't have an actual queue, just wait until the other packet is sent. + while(dataToSend) + { + dataSentCV.wait(); + } + + // now actually erase the buffer + BNO080Base::clearSendBuffer(); +} + +bool BNO080Async::waitForPacket(int channel, uint8_t reportID, std::chrono::milliseconds timeout) +{ + // first make sure mutex is locked + if(bnoDataMutex.get_owner() != ThisThread::get_id()) + { + _debugPort->printf("IMU communication function called without bnoDataMutex locked!\n"); + return false; + } + + // send information to thread + waitingForPacket = true; + waitingForChannel = channel; + waitingForReportID = reportID; + waitingPacketArrived = false; + + // now unlock mutex and allow thread to run and receive packets + waitingPacketArrivedCV.wait_for(timeout, [&]() {return waitingPacketArrived;}); + + if(!waitingPacketArrived) + { + _debugPort->printf("Packet wait timeout.\n"); + return false; + } + + // packet we are waiting for is now in the buffer. + waitingForPacket = false; + + // Now we can unblock the comms thread and allow it to run. + // BUT, it can't actually start until bnoDataMutex is released. + // This means that the packet data is guaranteed to stay in the buffer until that mutex is released + // which will happen either when the main thread is done with the IMU, or on the next sendPacket() or + // waitForPacket() call. + clearToRxNextPacket = true; + waitingPacketProcessedCV.notify_all(); + + return true; +} + +BNO080Async::BNO080Async(Stream *debugPort, PinName rstPin, PinName intPin, PinName wakePin, PinName misoPin, + PinName mosiPin, PinName sclkPin, PinName csPin, int spiSpeed, osPriority_t threadPriority): + BNO080SPI(debugPort, rstPin, intPin, wakePin, misoPin, mosiPin, sclkPin, csPin, spiSpeed), + commThread(threadPriority), + dataSentCV(bnoDataMutex), + waitingPacketArrivedCV(bnoDataMutex), + waitingPacketProcessedCV(bnoDataMutex) +{ + +} + + +bool BNO080Async::begin() +{ + // shut down thread if it's running + if(commThread.get_state() == Thread::Deleted) + { + // start thread for the first time + commThread.start(callback(this, &BNO080Async::threadMain)); + } + else + { + // restart thread + wakeupFlags.set(EF_RESTART); + } + + + { + ScopedLock<Mutex> lock(bnoDataMutex); + + // once the thread starts it, the BNO will send an Unsolicited Initialize response (SH-2 section 6.4.5.2), and an Executable Reset command + if(!waitForPacket(CHANNEL_EXECUTABLE, EXECUTABLE_REPORTID_RESET, 1s)) + { + _debugPort->printf("No initialization report from BNO080.\n"); + return false; + } + else + { +#if BNO_DEBUG + _debugPort->printf("BNO080 reports initialization successful!\n"); +#endif + } + + // Finally, we want to interrogate the device about its model and version. + clearSendBuffer(); + txShtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; //Request the product ID and reset info + txShtpData[1] = 0; //Reserved + sendPacket(CHANNEL_CONTROL, 2); + + waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_PRODUCT_ID_RESPONSE); + + if (rxShtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE) + { + majorSoftwareVersion = rxShtpData[2]; + minorSoftwareVersion = rxShtpData[3]; + patchSoftwareVersion = (rxShtpData[13] << 8) | rxShtpData[12]; + partNumber = (rxShtpData[7] << 24) | (rxShtpData[6] << 16) | (rxShtpData[5] << 8) | rxShtpData[4]; + buildNumber = (rxShtpData[11] << 24) | (rxShtpData[10] << 16) | (rxShtpData[9] << 8) | rxShtpData[8]; + +#if BNO_DEBUG + _debugPort->printf("BNO080 reports as SW version %hhu.%hhu.%hu, build %lu, part no. %lu\n", + majorSoftwareVersion, minorSoftwareVersion, patchSoftwareVersion, + buildNumber, partNumber); +#endif + + } + else + { + _debugPort->printf("Bad response from product ID command.\n"); + return false; + } + } + + + // successful init + return true; +} + +bool BNO080Async::updateData() +{ + bool newData = dataReceived; + dataReceived = false; + return newData; +} + + + + + +