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
Committer:
Jamie Smith
Date:
Tue Nov 24 15:06:05 2020 -0800
Revision:
9:430f5302f9e1
Parent:
8:199c7fad233d
Implement BNO080Async

Who changed what in which revision?

UserRevisionLine numberNew contents of line
Jamie Smith 1:aac28ffd63ed 1 //
Jamie Smith 1:aac28ffd63ed 2 // USC RPL BNO080 driver.
Jamie Smith 1:aac28ffd63ed 3 //
Jamie Smith 1:aac28ffd63ed 4
Jamie Smith 1:aac28ffd63ed 5 /*
Jamie Smith 1:aac28ffd63ed 6 * Overview of BNO080 Communications
Jamie Smith 1:aac28ffd63ed 7 * ===============================================
Jamie Smith 1:aac28ffd63ed 8 *
Jamie Smith 1:aac28ffd63ed 9 * Hilcrest has developed a protocol called SHTP (Sensor Hub Transport Protocol) for binary communications with
Jamie Smith 1:aac28ffd63ed 10 * the BNO080 and the other IMUs it sells. Over this protocol, SH-2 (Sensor Hub 2) messages are sent to configure
Jamie Smith 1:aac28ffd63ed 11 * the chip and read data back.
Jamie Smith 1:aac28ffd63ed 12 *
Jamie Smith 1:aac28ffd63ed 13 * SHTP messages are divided at two hierarchical levels: first the channel, then the report ID. Each category
Jamie Smith 1:aac28ffd63ed 14 * of messages (system commands, sensor data reports, etc.) has its own channel, and the individual messages
Jamie Smith 1:aac28ffd63ed 15 * in each channel are identified by their report id, which is the first byte of the message payload (note that the
Jamie Smith 1:aac28ffd63ed 16 * datasheets don't *always* call the first byte the report ID, but that byte does identify the report, so I'm going
Jamie Smith 1:aac28ffd63ed 17 * with it).
Jamie Smith 1:aac28ffd63ed 18 *
Jamie Smith 1:aac28ffd63ed 19 * ===============================================
Jamie Smith 1:aac28ffd63ed 20 *
Jamie Smith 1:aac28ffd63ed 21 * Information about the BNO080 is split into three datasheets. Here's the download links and what they cover:
Jamie Smith 1:aac28ffd63ed 22 *
Jamie Smith 1:aac28ffd63ed 23 * - the BNO080 datasheet: http://www.hillcrestlabs.com/download/5a05f340566d07c196001ec1
Jamie Smith 1:aac28ffd63ed 24 * -- Chip pinouts
Jamie Smith 1:aac28ffd63ed 25 * -- Example circuits
Jamie Smith 1:aac28ffd63ed 26 * -- Physical specifications
Jamie Smith 1:aac28ffd63ed 27 * -- Supported reports and configuration settings (at a high level)
Jamie Smith 1:aac28ffd63ed 28 * -- List of packets on the SHTP executable channel
Jamie Smith 1:aac28ffd63ed 29 *
Jamie Smith 1:aac28ffd63ed 30 * - the SHTP protocol: http://www.hillcrestlabs.com/download/59de8f99cd829e94dc0029d7
Jamie Smith 1:aac28ffd63ed 31 * -- SHTP transmit and receive protcols (for SPI, I2C, and UART)
Jamie Smith 1:aac28ffd63ed 32 * -- SHTP binary format
Jamie Smith 1:aac28ffd63ed 33 * -- packet types on the SHTP command channel
Jamie Smith 1:aac28ffd63ed 34 *
Jamie Smith 1:aac28ffd63ed 35 * - the SH-2 reference: http://www.hillcrestlabs.com/download/59de8f398934bf6faa00293f
Jamie Smith 1:aac28ffd63ed 36 * -- list of packets and their formats for all channels other than command and executable
Jamie Smith 1:aac28ffd63ed 37 * -- list of FRS (Flash Record System) entries and their formats
Jamie Smith 1:aac28ffd63ed 38 *
Jamie Smith 1:aac28ffd63ed 39 * ===============================================
Jamie Smith 1:aac28ffd63ed 40 *
Jamie Smith 1:aac28ffd63ed 41 * Overview of SHTP channels:
Jamie Smith 1:aac28ffd63ed 42 *
Jamie Smith 1:aac28ffd63ed 43 * 0 -> Command
Jamie Smith 1:aac28ffd63ed 44 * -- Used for protocol-global packets, currently only the advertisement packet (which lists all the channels) and error reports
Jamie Smith 1:aac28ffd63ed 45 *
Jamie Smith 1:aac28ffd63ed 46 * 1 -> Executable
Jamie Smith 1:aac28ffd63ed 47 * -- Used for things that control the software on the chip: commands to reset and sleep
Jamie Smith 1:aac28ffd63ed 48 * -- Also used by the chip to report when it's done booting up
Jamie Smith 1:aac28ffd63ed 49 *
Jamie Smith 1:aac28ffd63ed 50 * 2 -> Control
Jamie Smith 1:aac28ffd63ed 51 * -- Used to send configuration commands to the IMU and for it to send back responses.
Jamie Smith 1:aac28ffd63ed 52 * -- Common report IDs: Command Request (0xF2), Set Feature (0xFD)
Jamie Smith 1:aac28ffd63ed 53 *
Jamie Smith 1:aac28ffd63ed 54 * 3 -> Sensor Reports
Jamie Smith 1:aac28ffd63ed 55 * -- Used for sensors to send back data reports.
Jamie Smith 3:197ad972fb7c 56 * -- AFAIK the only report ID on this channel will be 0xFB (Report Base Timestamp); sensor data is sent in a series of structures
Jamie Smith 1:aac28ffd63ed 57 * following an 0xFB
Jamie Smith 1:aac28ffd63ed 58 *
Jamie Smith 1:aac28ffd63ed 59 * 4 -> Wake Sensor Reports
Jamie Smith 1:aac28ffd63ed 60 * -- same as above, but for sensors configured to wake the device
Jamie Smith 1:aac28ffd63ed 61 *
Jamie Smith 1:aac28ffd63ed 62 * 5 -> Gyro Rotation Vector
Jamie Smith 1:aac28ffd63ed 63 * -- a dedicated channel for the Gyro Rotation Vector sensor report
Jamie Smith 1:aac28ffd63ed 64 * -- Why does this get its own channel? I don't know!!!
Jamie Smith 1:aac28ffd63ed 65 */
Jamie Smith 1:aac28ffd63ed 66
Jamie Smith 1:aac28ffd63ed 67 #include "BNO080.h"
Jamie Smith 1:aac28ffd63ed 68 #include "BNO080Constants.h"
Jamie Smith 8:199c7fad233d 69 #include <cinttypes>
Jamie Smith 8:199c7fad233d 70 #include <algorithm>
Jamie Smith 1:aac28ffd63ed 71
Jamie Smith 1:aac28ffd63ed 72 /// Set to 1 to enable debug printouts. Should be very useful if the chip is giving you trouble.
Jamie Smith 1:aac28ffd63ed 73 /// When debugging, it is recommended to use the highest possible serial baudrate so as not to interrupt the timing of operations.
Jamie Smith 2:2269b723d16a 74 #define BNO_DEBUG 0
Jamie Smith 1:aac28ffd63ed 75
Jamie Smith 8:199c7fad233d 76 BNO080Base::BNO080Base(Stream *debugPort, PinName user_INTPin, PinName user_RSTPin) :
Jamie Smith 1:aac28ffd63ed 77 _debugPort(debugPort),
Jamie Smith 1:aac28ffd63ed 78 _int(user_INTPin),
Jamie Smith 2:2269b723d16a 79 _rst(user_RSTPin, 1),
Jamie Smith 2:2269b723d16a 80 commandSequenceNumber(0),
Jamie Smith 2:2269b723d16a 81 stability(UNKNOWN),
Jamie Smith 2:2269b723d16a 82 stepDetected(false),
Jamie Smith 2:2269b723d16a 83 stepCount(0),
Jamie Smith 2:2269b723d16a 84 significantMotionDetected(false),
Jamie Smith 2:2269b723d16a 85 shakeDetected(false),
Jamie Smith 2:2269b723d16a 86 xAxisShake(false),
Jamie Smith 2:2269b723d16a 87 yAxisShake(false),
Jamie Smith 2:2269b723d16a 88 zAxisShake(false)
Jamie Smith 1:aac28ffd63ed 89 {
Jamie Smith 2:2269b723d16a 90 // zero sequence numbers
Jamie Smith 2:2269b723d16a 91 memset(sequenceNumber, 0, sizeof(sequenceNumber));
Jamie Smith 1:aac28ffd63ed 92 }
Jamie Smith 1:aac28ffd63ed 93
Jamie Smith 8:199c7fad233d 94 bool BNO080Base::begin()
Jamie Smith 1:aac28ffd63ed 95 {
Jamie Smith 9:430f5302f9e1 96 //Configure the BNO080
Jamie Smith 1:aac28ffd63ed 97
Jamie Smith 1:aac28ffd63ed 98 _rst = 0; // Reset BNO080
Jamie Smith 6:5ba996be5312 99 ThisThread::sleep_for(1ms); // Min length not specified in datasheet?
Jamie Smith 1:aac28ffd63ed 100 _rst = 1; // Bring out of reset
Jamie Smith 1:aac28ffd63ed 101
Jamie Smith 1:aac28ffd63ed 102 // wait for a falling edge (NOT just a low) on the INT pin to denote startup
Jamie Smith 1:aac28ffd63ed 103 Timer timeoutTimer;
Jamie Smith 3:197ad972fb7c 104 timeoutTimer.start();
Jamie Smith 1:aac28ffd63ed 105
Jamie Smith 1:aac28ffd63ed 106 bool highDetected = false;
Jamie Smith 1:aac28ffd63ed 107 bool lowDetected = false;
Jamie Smith 1:aac28ffd63ed 108
Jamie Smith 1:aac28ffd63ed 109 while(true)
Jamie Smith 1:aac28ffd63ed 110 {
Jamie Smith 8:199c7fad233d 111 if(timeoutTimer.elapsed_time() > BNO080_RESET_TIMEOUT)
Jamie Smith 1:aac28ffd63ed 112 {
Jamie Smith 1:aac28ffd63ed 113 _debugPort->printf("Error: BNO080 reset timed out, chip not detected.\n");
Jamie Smith 1:aac28ffd63ed 114 return false;
Jamie Smith 1:aac28ffd63ed 115 }
Jamie Smith 1:aac28ffd63ed 116
Jamie Smith 1:aac28ffd63ed 117 // simple edge detector
Jamie Smith 1:aac28ffd63ed 118 if(!highDetected)
Jamie Smith 1:aac28ffd63ed 119 {
Jamie Smith 1:aac28ffd63ed 120 if(_int == 1)
Jamie Smith 1:aac28ffd63ed 121 {
Jamie Smith 1:aac28ffd63ed 122 highDetected = true;
Jamie Smith 1:aac28ffd63ed 123 }
Jamie Smith 1:aac28ffd63ed 124 }
Jamie Smith 1:aac28ffd63ed 125 else if(!lowDetected)
Jamie Smith 1:aac28ffd63ed 126 {
Jamie Smith 1:aac28ffd63ed 127 if(_int == 0)
Jamie Smith 1:aac28ffd63ed 128 {
Jamie Smith 1:aac28ffd63ed 129 lowDetected = true;
Jamie Smith 1:aac28ffd63ed 130 }
Jamie Smith 1:aac28ffd63ed 131 }
Jamie Smith 1:aac28ffd63ed 132 else
Jamie Smith 1:aac28ffd63ed 133 {
Jamie Smith 1:aac28ffd63ed 134 // high and low detected
Jamie Smith 1:aac28ffd63ed 135 break;
Jamie Smith 1:aac28ffd63ed 136 }
Jamie Smith 1:aac28ffd63ed 137 }
Jamie Smith 1:aac28ffd63ed 138
Jamie Smith 3:197ad972fb7c 139 #if BNO_DEBUG
Jamie Smith 3:197ad972fb7c 140 _debugPort->printf("BNO080 detected!\r\n");
Jamie Smith 3:197ad972fb7c 141 #endif
Jamie Smith 1:aac28ffd63ed 142
Jamie Smith 1:aac28ffd63ed 143 // At system startup, the hub must send its full advertisement message (see SHTP 5.2 and 5.3) to the
Jamie Smith 1:aac28ffd63ed 144 // host. It must not send any other data until this step is complete.
Jamie Smith 1:aac28ffd63ed 145 // We don't actually care what's in it, we're just using it as a signal to indicate that the reset is complete.
Jamie Smith 1:aac28ffd63ed 146 receivePacket();
Jamie Smith 1:aac28ffd63ed 147
Jamie Smith 1:aac28ffd63ed 148 // now, after startup, the BNO will send an Unsolicited Initialize response (SH-2 section 6.4.5.2), and an Executable Reset command
Jamie Smith 8:199c7fad233d 149 if(!waitForPacket(CHANNEL_EXECUTABLE, EXECUTABLE_REPORTID_RESET))
Jamie Smith 1:aac28ffd63ed 150 {
Jamie Smith 8:199c7fad233d 151 _debugPort->printf("No initialization report from BNO080.\n");
Jamie Smith 1:aac28ffd63ed 152 return false;
Jamie Smith 1:aac28ffd63ed 153 }
Jamie Smith 1:aac28ffd63ed 154 else
Jamie Smith 1:aac28ffd63ed 155 {
Jamie Smith 1:aac28ffd63ed 156 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 157 _debugPort->printf("BNO080 reports initialization successful!\n");
Jamie Smith 1:aac28ffd63ed 158 #endif
Jamie Smith 1:aac28ffd63ed 159 }
Jamie Smith 1:aac28ffd63ed 160
Jamie Smith 1:aac28ffd63ed 161 // Finally, we want to interrogate the device about its model and version.
Jamie Smith 9:430f5302f9e1 162 clearSendBuffer();
Jamie Smith 8:199c7fad233d 163 txShtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; //Request the product ID and reset info
Jamie Smith 8:199c7fad233d 164 txShtpData[1] = 0; //Reserved
Jamie Smith 1:aac28ffd63ed 165 sendPacket(CHANNEL_CONTROL, 2);
Jamie Smith 1:aac28ffd63ed 166
Jamie Smith 8:199c7fad233d 167 waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_PRODUCT_ID_RESPONSE, 5ms);
Jamie Smith 1:aac28ffd63ed 168
Jamie Smith 8:199c7fad233d 169 if (rxShtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE)
Jamie Smith 1:aac28ffd63ed 170 {
Jamie Smith 8:199c7fad233d 171 majorSoftwareVersion = rxShtpData[2];
Jamie Smith 8:199c7fad233d 172 minorSoftwareVersion = rxShtpData[3];
Jamie Smith 8:199c7fad233d 173 patchSoftwareVersion = (rxShtpData[13] << 8) | rxShtpData[12];
Jamie Smith 8:199c7fad233d 174 partNumber = (rxShtpData[7] << 24) | (rxShtpData[6] << 16) | (rxShtpData[5] << 8) | rxShtpData[4];
Jamie Smith 8:199c7fad233d 175 buildNumber = (rxShtpData[11] << 24) | (rxShtpData[10] << 16) | (rxShtpData[9] << 8) | rxShtpData[8];
Jamie Smith 1:aac28ffd63ed 176
Jamie Smith 1:aac28ffd63ed 177 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 178 _debugPort->printf("BNO080 reports as SW version %hhu.%hhu.%hu, build %lu, part no. %lu\n",
Jamie Smith 1:aac28ffd63ed 179 majorSoftwareVersion, minorSoftwareVersion, patchSoftwareVersion,
Jamie Smith 1:aac28ffd63ed 180 buildNumber, partNumber);
Jamie Smith 1:aac28ffd63ed 181 #endif
Jamie Smith 1:aac28ffd63ed 182
Jamie Smith 1:aac28ffd63ed 183 }
Jamie Smith 1:aac28ffd63ed 184 else
Jamie Smith 1:aac28ffd63ed 185 {
Jamie Smith 1:aac28ffd63ed 186 _debugPort->printf("Bad response from product ID command.\n");
Jamie Smith 1:aac28ffd63ed 187 return false;
Jamie Smith 1:aac28ffd63ed 188 }
Jamie Smith 1:aac28ffd63ed 189
Jamie Smith 1:aac28ffd63ed 190 // successful init
Jamie Smith 1:aac28ffd63ed 191 return true;
Jamie Smith 1:aac28ffd63ed 192
Jamie Smith 1:aac28ffd63ed 193 }
Jamie Smith 1:aac28ffd63ed 194
Jamie Smith 8:199c7fad233d 195 void BNO080Base::tare(bool zOnly)
Jamie Smith 1:aac28ffd63ed 196 {
Jamie Smith 9:430f5302f9e1 197 clearSendBuffer();
Jamie Smith 1:aac28ffd63ed 198
Jamie Smith 1:aac28ffd63ed 199 // from SH-2 section 6.4.4.1
Jamie Smith 8:199c7fad233d 200 txShtpData[3] = 0; // perform tare now
Jamie Smith 1:aac28ffd63ed 201
Jamie Smith 1:aac28ffd63ed 202 if(zOnly)
Jamie Smith 1:aac28ffd63ed 203 {
Jamie Smith 8:199c7fad233d 204 txShtpData[4] = 0b100; // tare Z axis
Jamie Smith 1:aac28ffd63ed 205 }
Jamie Smith 1:aac28ffd63ed 206 else
Jamie Smith 1:aac28ffd63ed 207 {
Jamie Smith 8:199c7fad233d 208 txShtpData[4] = 0b111; // tare X, Y, and Z axes
Jamie Smith 1:aac28ffd63ed 209 }
Jamie Smith 1:aac28ffd63ed 210
Jamie Smith 8:199c7fad233d 211 txShtpData[5] = 0; // reorient all motion outputs
Jamie Smith 1:aac28ffd63ed 212
Jamie Smith 1:aac28ffd63ed 213 sendCommand(COMMAND_TARE);
Jamie Smith 1:aac28ffd63ed 214 }
Jamie Smith 1:aac28ffd63ed 215
Jamie Smith 8:199c7fad233d 216 bool BNO080Base::enableCalibration(bool calibrateAccel, bool calibrateGyro, bool calibrateMag)
Jamie Smith 1:aac28ffd63ed 217 {
Jamie Smith 1:aac28ffd63ed 218 // send the Configure ME Calibration command
Jamie Smith 9:430f5302f9e1 219 clearSendBuffer();
Jamie Smith 1:aac28ffd63ed 220
Jamie Smith 8:199c7fad233d 221 txShtpData[3] = static_cast<uint8_t>(calibrateAccel ? 1 : 0);
Jamie Smith 8:199c7fad233d 222 txShtpData[4] = static_cast<uint8_t>(calibrateGyro ? 1 : 0);
Jamie Smith 8:199c7fad233d 223 txShtpData[5] = static_cast<uint8_t>(calibrateMag ? 1 : 0);
Jamie Smith 1:aac28ffd63ed 224
Jamie Smith 8:199c7fad233d 225 txShtpData[6] = 0; // Configure ME Calibration command
Jamie Smith 1:aac28ffd63ed 226
Jamie Smith 8:199c7fad233d 227 txShtpData[7] = 0; // planar accelerometer calibration always disabled
Jamie Smith 1:aac28ffd63ed 228
Jamie Smith 1:aac28ffd63ed 229 sendCommand(COMMAND_ME_CALIBRATE);
Jamie Smith 1:aac28ffd63ed 230
Jamie Smith 1:aac28ffd63ed 231 // now, wait for the response
Jamie Smith 1:aac28ffd63ed 232 if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE))
Jamie Smith 1:aac28ffd63ed 233 {
Jamie Smith 1:aac28ffd63ed 234 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 235 _debugPort->printf("Timeout waiting for calibration response!\n");
Jamie Smith 1:aac28ffd63ed 236 #endif
Jamie Smith 1:aac28ffd63ed 237 return false;
Jamie Smith 1:aac28ffd63ed 238 }
Jamie Smith 1:aac28ffd63ed 239
Jamie Smith 8:199c7fad233d 240 if(rxShtpData[2] != COMMAND_ME_CALIBRATE)
Jamie Smith 1:aac28ffd63ed 241 {
Jamie Smith 1:aac28ffd63ed 242 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 243 _debugPort->printf("Received wrong response to calibration command!\n");
Jamie Smith 1:aac28ffd63ed 244 #endif
Jamie Smith 1:aac28ffd63ed 245 return false;
Jamie Smith 1:aac28ffd63ed 246 }
Jamie Smith 1:aac28ffd63ed 247
Jamie Smith 8:199c7fad233d 248 if(rxShtpData[5] != 0)
Jamie Smith 1:aac28ffd63ed 249 {
Jamie Smith 1:aac28ffd63ed 250 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 251 _debugPort->printf("IMU reports calibrate command failed!\n");
Jamie Smith 1:aac28ffd63ed 252 #endif
Jamie Smith 1:aac28ffd63ed 253 return false;
Jamie Smith 1:aac28ffd63ed 254 }
Jamie Smith 1:aac28ffd63ed 255
Jamie Smith 1:aac28ffd63ed 256 // acknowledge checks out!
Jamie Smith 1:aac28ffd63ed 257 return true;
Jamie Smith 1:aac28ffd63ed 258 }
Jamie Smith 1:aac28ffd63ed 259
Jamie Smith 8:199c7fad233d 260 bool BNO080Base::saveCalibration()
Jamie Smith 1:aac28ffd63ed 261 {
Jamie Smith 9:430f5302f9e1 262 clearSendBuffer();
Jamie Smith 1:aac28ffd63ed 263
Jamie Smith 1:aac28ffd63ed 264 // no arguments
Jamie Smith 1:aac28ffd63ed 265 sendCommand(COMMAND_SAVE_DCD);
Jamie Smith 1:aac28ffd63ed 266
Jamie Smith 1:aac28ffd63ed 267 // now, wait for the response
Jamie Smith 1:aac28ffd63ed 268 if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE))
Jamie Smith 1:aac28ffd63ed 269 {
Jamie Smith 1:aac28ffd63ed 270 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 271 _debugPort->printf("Timeout waiting for calibration response!\n");
Jamie Smith 1:aac28ffd63ed 272 #endif
Jamie Smith 1:aac28ffd63ed 273 return false;
Jamie Smith 1:aac28ffd63ed 274 }
Jamie Smith 1:aac28ffd63ed 275
Jamie Smith 8:199c7fad233d 276 if(rxShtpData[2] != COMMAND_SAVE_DCD)
Jamie Smith 1:aac28ffd63ed 277 {
Jamie Smith 1:aac28ffd63ed 278 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 279 _debugPort->printf("Received wrong response to calibration command!\n");
Jamie Smith 1:aac28ffd63ed 280 #endif
Jamie Smith 1:aac28ffd63ed 281 return false;
Jamie Smith 1:aac28ffd63ed 282 }
Jamie Smith 1:aac28ffd63ed 283
Jamie Smith 8:199c7fad233d 284 if(rxShtpData[5] != 0)
Jamie Smith 1:aac28ffd63ed 285 {
Jamie Smith 1:aac28ffd63ed 286 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 287 _debugPort->printf("IMU reports calibrate command failed!\n");
Jamie Smith 1:aac28ffd63ed 288 #endif
Jamie Smith 1:aac28ffd63ed 289 return false;
Jamie Smith 1:aac28ffd63ed 290 }
Jamie Smith 1:aac28ffd63ed 291
Jamie Smith 1:aac28ffd63ed 292 // acknowledge checks out!
Jamie Smith 1:aac28ffd63ed 293 return true;
Jamie Smith 1:aac28ffd63ed 294 }
Jamie Smith 1:aac28ffd63ed 295
Jamie Smith 8:199c7fad233d 296 void BNO080Base::setSensorOrientation(Quaternion orientation)
Jamie Smith 1:aac28ffd63ed 297 {
Jamie Smith 9:430f5302f9e1 298 clearSendBuffer();
Jamie Smith 1:aac28ffd63ed 299
Jamie Smith 1:aac28ffd63ed 300 // convert floats to Q
Jamie Smith 1:aac28ffd63ed 301 int16_t Q_x = floatToQ(orientation.x(), ORIENTATION_QUAT_Q_POINT);
Jamie Smith 1:aac28ffd63ed 302 int16_t Q_y = floatToQ(orientation.y(), ORIENTATION_QUAT_Q_POINT);
Jamie Smith 1:aac28ffd63ed 303 int16_t Q_z = floatToQ(orientation.z(), ORIENTATION_QUAT_Q_POINT);
Jamie Smith 1:aac28ffd63ed 304 int16_t Q_w = floatToQ(orientation.w(), ORIENTATION_QUAT_Q_POINT);
Jamie Smith 1:aac28ffd63ed 305
Jamie Smith 8:199c7fad233d 306 txShtpData[3] = 2; // set reorientation
Jamie Smith 1:aac28ffd63ed 307
Jamie Smith 8:199c7fad233d 308 txShtpData[4] = static_cast<uint8_t>(Q_x & 0xFF); //P1 - X component LSB
Jamie Smith 8:199c7fad233d 309 txShtpData[5] = static_cast<uint8_t>(Q_x >> 8); //P2 - X component MSB
Jamie Smith 1:aac28ffd63ed 310
Jamie Smith 8:199c7fad233d 311 txShtpData[6] = static_cast<uint8_t>(Q_y & 0xFF); //P3 - Y component LSB
Jamie Smith 8:199c7fad233d 312 txShtpData[7] = static_cast<uint8_t>(Q_y >> 8); //P4 - Y component MSB
Jamie Smith 1:aac28ffd63ed 313
Jamie Smith 8:199c7fad233d 314 txShtpData[8] = static_cast<uint8_t>(Q_z & 0xFF); //P5 - Z component LSB
Jamie Smith 8:199c7fad233d 315 txShtpData[9] = static_cast<uint8_t>(Q_z >> 8); //P6 - Z component MSB
Jamie Smith 1:aac28ffd63ed 316
Jamie Smith 8:199c7fad233d 317 txShtpData[10] = static_cast<uint8_t>(Q_w & 0xFF); //P7 - W component LSB
Jamie Smith 8:199c7fad233d 318 txShtpData[11] = static_cast<uint8_t>(Q_w >> 8); //P8 - W component MSB
Jamie Smith 1:aac28ffd63ed 319
Jamie Smith 1:aac28ffd63ed 320 //Using this shtpData packet, send a command
Jamie Smith 1:aac28ffd63ed 321 sendCommand(COMMAND_TARE); // Send tare command
Jamie Smith 1:aac28ffd63ed 322
Jamie Smith 1:aac28ffd63ed 323 // NOTE: unlike literally every other command, a sensor orientation command is never acknowledged in any way.
Jamie Smith 1:aac28ffd63ed 324 }
Jamie Smith 1:aac28ffd63ed 325
Jamie Smith 3:197ad972fb7c 326 #define ORIENTATION_RECORD_LEN 4
Jamie Smith 3:197ad972fb7c 327
Jamie Smith 8:199c7fad233d 328 bool BNO080Base::setPermanentOrientation(Quaternion orientation)
Jamie Smith 3:197ad972fb7c 329 {
Jamie Smith 3:197ad972fb7c 330 uint32_t orientationRecord[ORIENTATION_RECORD_LEN];
Jamie Smith 3:197ad972fb7c 331
Jamie Smith 3:197ad972fb7c 332 // each word is one element of the quaternion
Jamie Smith 3:197ad972fb7c 333 orientationRecord[0] = static_cast<uint32_t>(floatToQ_dword(orientation.x(), FRS_ORIENTATION_Q_POINT));
Jamie Smith 3:197ad972fb7c 334 orientationRecord[1] = static_cast<uint32_t>(floatToQ_dword(orientation.y(), FRS_ORIENTATION_Q_POINT));
Jamie Smith 3:197ad972fb7c 335 orientationRecord[2] = static_cast<uint32_t>(floatToQ_dword(orientation.z(), FRS_ORIENTATION_Q_POINT));
Jamie Smith 3:197ad972fb7c 336 orientationRecord[3] = static_cast<uint32_t>(floatToQ_dword(orientation.w(), FRS_ORIENTATION_Q_POINT));
Jamie Smith 3:197ad972fb7c 337
Jamie Smith 3:197ad972fb7c 338 return writeFRSRecord(FRS_RECORDID_SYSTEM_ORIENTATION, orientationRecord, ORIENTATION_RECORD_LEN);
Jamie Smith 3:197ad972fb7c 339 }
Jamie Smith 1:aac28ffd63ed 340
Jamie Smith 8:199c7fad233d 341 bool BNO080Base::updateData()
Jamie Smith 1:aac28ffd63ed 342 {
Jamie Smith 1:aac28ffd63ed 343 if(_int.read() != 0)
Jamie Smith 1:aac28ffd63ed 344 {
Jamie Smith 1:aac28ffd63ed 345 // no waiting packets
Jamie Smith 1:aac28ffd63ed 346 return false;
Jamie Smith 1:aac28ffd63ed 347 }
Jamie Smith 1:aac28ffd63ed 348
Jamie Smith 1:aac28ffd63ed 349 while(_int.read() == 0)
Jamie Smith 1:aac28ffd63ed 350 {
Jamie Smith 1:aac28ffd63ed 351 if(!receivePacket())
Jamie Smith 1:aac28ffd63ed 352 {
Jamie Smith 1:aac28ffd63ed 353 // comms error
Jamie Smith 1:aac28ffd63ed 354 return false;
Jamie Smith 1:aac28ffd63ed 355 }
Jamie Smith 1:aac28ffd63ed 356
Jamie Smith 1:aac28ffd63ed 357 processPacket();
Jamie Smith 8:199c7fad233d 358
Jamie Smith 8:199c7fad233d 359 // Allow time for the IMU to ready another packet if it has one
Jamie Smith 8:199c7fad233d 360 wait_us(150); // time between packets measured with a logic analyzer
Jamie Smith 1:aac28ffd63ed 361 }
Jamie Smith 1:aac28ffd63ed 362
Jamie Smith 1:aac28ffd63ed 363 // packets were received, so data may have changed
Jamie Smith 1:aac28ffd63ed 364 return true;
Jamie Smith 1:aac28ffd63ed 365 }
Jamie Smith 1:aac28ffd63ed 366
Jamie Smith 8:199c7fad233d 367 uint8_t BNO080Base::getReportStatus(Report report)
Jamie Smith 1:aac28ffd63ed 368 {
Jamie Smith 1:aac28ffd63ed 369 uint8_t reportNum = static_cast<uint8_t>(report);
Jamie Smith 1:aac28ffd63ed 370 if(reportNum > STATUS_ARRAY_LEN)
Jamie Smith 1:aac28ffd63ed 371 {
Jamie Smith 1:aac28ffd63ed 372 return 0;
Jamie Smith 1:aac28ffd63ed 373 }
Jamie Smith 1:aac28ffd63ed 374
Jamie Smith 1:aac28ffd63ed 375 return reportStatus[reportNum];
Jamie Smith 1:aac28ffd63ed 376 }
Jamie Smith 1:aac28ffd63ed 377
Jamie Smith 8:199c7fad233d 378 const char* BNO080Base::getReportStatusString(Report report)
Jamie Smith 1:aac28ffd63ed 379 {
Jamie Smith 1:aac28ffd63ed 380 switch(getReportStatus(report))
Jamie Smith 1:aac28ffd63ed 381 {
Jamie Smith 1:aac28ffd63ed 382 case 0:
Jamie Smith 1:aac28ffd63ed 383 return "Unreliable";
Jamie Smith 1:aac28ffd63ed 384 case 1:
Jamie Smith 1:aac28ffd63ed 385 return "Accuracy Low";
Jamie Smith 1:aac28ffd63ed 386 case 2:
Jamie Smith 1:aac28ffd63ed 387 return "Accuracy Medium";
Jamie Smith 1:aac28ffd63ed 388 case 3:
Jamie Smith 1:aac28ffd63ed 389 return "Accuracy High";
Jamie Smith 1:aac28ffd63ed 390 default:
Jamie Smith 1:aac28ffd63ed 391 return "Error";
Jamie Smith 1:aac28ffd63ed 392 }
Jamie Smith 1:aac28ffd63ed 393 }
Jamie Smith 1:aac28ffd63ed 394
Jamie Smith 8:199c7fad233d 395 bool BNO080Base::hasNewData(Report report)
Jamie Smith 1:aac28ffd63ed 396 {
Jamie Smith 1:aac28ffd63ed 397 uint8_t reportNum = static_cast<uint8_t>(report);
Jamie Smith 1:aac28ffd63ed 398 if(reportNum > STATUS_ARRAY_LEN)
Jamie Smith 1:aac28ffd63ed 399 {
Jamie Smith 1:aac28ffd63ed 400 return false;
Jamie Smith 1:aac28ffd63ed 401 }
Jamie Smith 1:aac28ffd63ed 402
Jamie Smith 1:aac28ffd63ed 403 bool newData = reportHasBeenUpdated[reportNum];
Jamie Smith 1:aac28ffd63ed 404 reportHasBeenUpdated[reportNum] = false; // clear flag
Jamie Smith 1:aac28ffd63ed 405 return newData;
Jamie Smith 1:aac28ffd63ed 406 }
Jamie Smith 1:aac28ffd63ed 407
Jamie Smith 1:aac28ffd63ed 408 //Sends the packet to enable the rotation vector
Jamie Smith 8:199c7fad233d 409 void BNO080Base::enableReport(Report report, uint16_t timeBetweenReports)
Jamie Smith 1:aac28ffd63ed 410 {
Jamie Smith 3:197ad972fb7c 411 #if BNO_DEBUG
Jamie Smith 3:197ad972fb7c 412 // check time is valid
Jamie Smith 3:197ad972fb7c 413 float periodSeconds = static_cast<float>(timeBetweenReports / 1000.0);
Jamie Smith 1:aac28ffd63ed 414
Jamie Smith 1:aac28ffd63ed 415 if(periodSeconds < getMinPeriod(report))
Jamie Smith 1:aac28ffd63ed 416 {
Jamie Smith 3:197ad972fb7c 417 _debugPort->printf("Error: attempt made to set report 0x%02hhx to period of %.06f s, which is smaller than its min period of %.06f s.\r\n",
Jamie Smith 1:aac28ffd63ed 418 static_cast<uint8_t>(report), periodSeconds, getMinPeriod(report));
Jamie Smith 1:aac28ffd63ed 419 return;
Jamie Smith 1:aac28ffd63ed 420 }
Jamie Smith 8:199c7fad233d 421
Jamie Smith 3:197ad972fb7c 422 #endif
Jamie Smith 1:aac28ffd63ed 423 setFeatureCommand(static_cast<uint8_t>(report), timeBetweenReports);
Jamie Smith 1:aac28ffd63ed 424
Jamie Smith 1:aac28ffd63ed 425 // note: we don't wait for ACKs on these packets because they can take quite a while, like half a second, to come in
Jamie Smith 1:aac28ffd63ed 426 }
Jamie Smith 1:aac28ffd63ed 427
Jamie Smith 8:199c7fad233d 428 void BNO080Base::disableReport(Report report)
Jamie Smith 1:aac28ffd63ed 429 {
Jamie Smith 1:aac28ffd63ed 430 // set the report's polling period to zero to disable it
Jamie Smith 1:aac28ffd63ed 431 setFeatureCommand(static_cast<uint8_t>(report), 0);
Jamie Smith 1:aac28ffd63ed 432 }
Jamie Smith 1:aac28ffd63ed 433
Jamie Smith 8:199c7fad233d 434 uint32_t BNO080Base::getSerialNumber()
Jamie Smith 1:aac28ffd63ed 435 {
Jamie Smith 1:aac28ffd63ed 436 uint32_t serNoBuffer;
Jamie Smith 1:aac28ffd63ed 437
Jamie Smith 1:aac28ffd63ed 438 if(!readFRSRecord(FRS_RECORDID_SERIAL_NUMBER, &serNoBuffer, 1))
Jamie Smith 1:aac28ffd63ed 439 {
Jamie Smith 1:aac28ffd63ed 440 return 0;
Jamie Smith 1:aac28ffd63ed 441 }
Jamie Smith 1:aac28ffd63ed 442
Jamie Smith 1:aac28ffd63ed 443 return serNoBuffer;
Jamie Smith 1:aac28ffd63ed 444 }
Jamie Smith 1:aac28ffd63ed 445
Jamie Smith 8:199c7fad233d 446 float BNO080Base::getRange(Report report)
Jamie Smith 1:aac28ffd63ed 447 {
Jamie Smith 1:aac28ffd63ed 448 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 449
Jamie Smith 1:aac28ffd63ed 450 return qToFloat_dword(metadataRecord[1], getQ1(report));
Jamie Smith 1:aac28ffd63ed 451 }
Jamie Smith 1:aac28ffd63ed 452
Jamie Smith 1:aac28ffd63ed 453
Jamie Smith 8:199c7fad233d 454 float BNO080Base::getResolution(Report report)
Jamie Smith 1:aac28ffd63ed 455 {
Jamie Smith 1:aac28ffd63ed 456 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 457
Jamie Smith 1:aac28ffd63ed 458 return qToFloat_dword(metadataRecord[2], getQ1(report));
Jamie Smith 1:aac28ffd63ed 459 }
Jamie Smith 1:aac28ffd63ed 460
Jamie Smith 8:199c7fad233d 461 float BNO080Base::getPower(Report report)
Jamie Smith 1:aac28ffd63ed 462 {
Jamie Smith 1:aac28ffd63ed 463 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 464
Jamie Smith 1:aac28ffd63ed 465 uint16_t powerQ = static_cast<uint16_t>(metadataRecord[3] & 0xFFFF);
Jamie Smith 1:aac28ffd63ed 466
Jamie Smith 1:aac28ffd63ed 467 return qToFloat_dword(powerQ, POWER_Q_POINT);
Jamie Smith 1:aac28ffd63ed 468 }
Jamie Smith 1:aac28ffd63ed 469
Jamie Smith 8:199c7fad233d 470 float BNO080Base::getMinPeriod(Report report)
Jamie Smith 1:aac28ffd63ed 471 {
Jamie Smith 1:aac28ffd63ed 472 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 473
Jamie Smith 1:aac28ffd63ed 474 return metadataRecord[4] / 1e6f; // convert from microseconds to seconds
Jamie Smith 1:aac28ffd63ed 475 }
Jamie Smith 1:aac28ffd63ed 476
Jamie Smith 8:199c7fad233d 477 float BNO080Base::getMaxPeriod(Report report)
Jamie Smith 1:aac28ffd63ed 478 {
Jamie Smith 1:aac28ffd63ed 479 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 480
Jamie Smith 1:aac28ffd63ed 481 if(getMetaVersion() == 3)
Jamie Smith 1:aac28ffd63ed 482 {
Jamie Smith 1:aac28ffd63ed 483 // no max period entry in this record format
Jamie Smith 1:aac28ffd63ed 484 return -1.0f;
Jamie Smith 1:aac28ffd63ed 485 }
Jamie Smith 1:aac28ffd63ed 486
Jamie Smith 1:aac28ffd63ed 487 return metadataRecord[9] / 1e6f; // convert from microseconds to seconds
Jamie Smith 1:aac28ffd63ed 488 }
Jamie Smith 1:aac28ffd63ed 489
Jamie Smith 8:199c7fad233d 490 void BNO080Base::printMetadataSummary(Report report)
Jamie Smith 1:aac28ffd63ed 491 {
Jamie Smith 1:aac28ffd63ed 492 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 493 if(!loadReportMetadata(report))
Jamie Smith 1:aac28ffd63ed 494 {
Jamie Smith 1:aac28ffd63ed 495 _debugPort->printf("Failed to load report metadata!\n");
Jamie Smith 1:aac28ffd63ed 496 }
Jamie Smith 1:aac28ffd63ed 497
Jamie Smith 1:aac28ffd63ed 498 _debugPort->printf("======= Metadata for report 0x%02hhx =======\n", static_cast<uint8_t>(report));
Jamie Smith 1:aac28ffd63ed 499
Jamie Smith 1:aac28ffd63ed 500 _debugPort->printf("Range: +- %.04f units\n", getRange(report));
Jamie Smith 1:aac28ffd63ed 501 _debugPort->printf("Resolution: %.04f units\n", getResolution(report));
Jamie Smith 1:aac28ffd63ed 502 _debugPort->printf("Power Used: %.03f mA\n", getPower(report));
Jamie Smith 1:aac28ffd63ed 503 _debugPort->printf("Min Period: %.06f s\n", getMinPeriod(report));
Jamie Smith 1:aac28ffd63ed 504 _debugPort->printf("Max Period: %.06f s\n\n", getMaxPeriod(report));
Jamie Smith 1:aac28ffd63ed 505
Jamie Smith 1:aac28ffd63ed 506 #endif
Jamie Smith 1:aac28ffd63ed 507 }
Jamie Smith 1:aac28ffd63ed 508
Jamie Smith 8:199c7fad233d 509 int16_t BNO080Base::getQ1(Report report)
Jamie Smith 1:aac28ffd63ed 510 {
Jamie Smith 1:aac28ffd63ed 511 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 512
Jamie Smith 1:aac28ffd63ed 513 return static_cast<int16_t>(metadataRecord[7] & 0xFFFF);
Jamie Smith 1:aac28ffd63ed 514 }
Jamie Smith 1:aac28ffd63ed 515
Jamie Smith 8:199c7fad233d 516 int16_t BNO080Base::getQ2(Report report)
Jamie Smith 1:aac28ffd63ed 517 {
Jamie Smith 1:aac28ffd63ed 518 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 519
Jamie Smith 1:aac28ffd63ed 520 return static_cast<int16_t>(metadataRecord[7] >> 16);
Jamie Smith 1:aac28ffd63ed 521 }
Jamie Smith 1:aac28ffd63ed 522
Jamie Smith 8:199c7fad233d 523 int16_t BNO080Base::getQ3(Report report)
Jamie Smith 1:aac28ffd63ed 524 {
Jamie Smith 1:aac28ffd63ed 525 loadReportMetadata(report);
Jamie Smith 1:aac28ffd63ed 526
Jamie Smith 1:aac28ffd63ed 527 return static_cast<int16_t>(metadataRecord[8] >> 16);
Jamie Smith 1:aac28ffd63ed 528 }
Jamie Smith 1:aac28ffd63ed 529
Jamie Smith 8:199c7fad233d 530 void BNO080Base::processPacket()
Jamie Smith 1:aac28ffd63ed 531 {
Jamie Smith 8:199c7fad233d 532 if(rxShtpHeader[2] == CHANNEL_CONTROL)
Jamie Smith 1:aac28ffd63ed 533 {
Jamie Smith 1:aac28ffd63ed 534 // currently no command reports are read
Jamie Smith 1:aac28ffd63ed 535 }
Jamie Smith 8:199c7fad233d 536 else if(rxShtpHeader[2] == CHANNEL_EXECUTABLE)
Jamie Smith 1:aac28ffd63ed 537 {
Jamie Smith 1:aac28ffd63ed 538 // currently no executable reports are read
Jamie Smith 1:aac28ffd63ed 539 }
Jamie Smith 8:199c7fad233d 540 else if(rxShtpHeader[2] == CHANNEL_COMMAND)
Jamie Smith 1:aac28ffd63ed 541 {
Jamie Smith 1:aac28ffd63ed 542
Jamie Smith 1:aac28ffd63ed 543 }
Jamie Smith 8:199c7fad233d 544 else if(rxShtpHeader[2] == CHANNEL_REPORTS || rxShtpHeader[2] == CHANNEL_WAKE_REPORTS)
Jamie Smith 1:aac28ffd63ed 545 {
Jamie Smith 8:199c7fad233d 546 if(rxShtpData[0] == SHTP_REPORT_BASE_TIMESTAMP)
Jamie Smith 1:aac28ffd63ed 547 {
Jamie Smith 1:aac28ffd63ed 548 // sensor data packet
Jamie Smith 1:aac28ffd63ed 549 parseSensorDataPacket();
Jamie Smith 1:aac28ffd63ed 550 }
Jamie Smith 1:aac28ffd63ed 551 }
Jamie Smith 1:aac28ffd63ed 552 }
Jamie Smith 1:aac28ffd63ed 553
Jamie Smith 1:aac28ffd63ed 554 // sizes of various sensor data packet elements
Jamie Smith 1:aac28ffd63ed 555 #define SIZEOF_BASE_TIMESTAMP 5
Jamie Smith 1:aac28ffd63ed 556 #define SIZEOF_TIMESTAMP_REBASE 5
Jamie Smith 1:aac28ffd63ed 557 #define SIZEOF_ACCELEROMETER 10
Jamie Smith 1:aac28ffd63ed 558 #define SIZEOF_LINEAR_ACCELERATION 10
Jamie Smith 1:aac28ffd63ed 559 #define SIZEOF_GYROSCOPE_CALIBRATED 10
Jamie Smith 1:aac28ffd63ed 560 #define SIZEOF_MAGNETIC_FIELD_CALIBRATED 10
Jamie Smith 1:aac28ffd63ed 561 #define SIZEOF_MAGNETIC_FIELD_UNCALIBRATED 16
Jamie Smith 1:aac28ffd63ed 562 #define SIZEOF_ROTATION_VECTOR 14
Jamie Smith 1:aac28ffd63ed 563 #define SIZEOF_GAME_ROTATION_VECTOR 12
Jamie Smith 1:aac28ffd63ed 564 #define SIZEOF_GEOMAGNETIC_ROTATION_VECTOR 14
Jamie Smith 1:aac28ffd63ed 565 #define SIZEOF_TAP_DETECTOR 5
Jamie Smith 1:aac28ffd63ed 566 #define SIZEOF_STABILITY_REPORT 6
Jamie Smith 1:aac28ffd63ed 567 #define SIZEOF_STEP_DETECTOR 8
Jamie Smith 1:aac28ffd63ed 568 #define SIZEOF_STEP_COUNTER 12
Jamie Smith 1:aac28ffd63ed 569 #define SIZEOF_SIGNIFICANT_MOTION 6
Jamie Smith 1:aac28ffd63ed 570 #define SIZEOF_SHAKE_DETECTOR 6
Jamie Smith 1:aac28ffd63ed 571
Jamie Smith 8:199c7fad233d 572 void BNO080Base::parseSensorDataPacket()
Jamie Smith 1:aac28ffd63ed 573 {
Jamie Smith 1:aac28ffd63ed 574 size_t currReportOffset = 0;
Jamie Smith 1:aac28ffd63ed 575
Jamie Smith 1:aac28ffd63ed 576 // every sensor data report first contains a timestamp offset to show how long it has been between when
Jamie Smith 1:aac28ffd63ed 577 // the host interrupt was sent and when the packet was transmitted.
Jamie Smith 1:aac28ffd63ed 578 // We don't use interrupts and don't care about times, so we can throw this out.
Jamie Smith 1:aac28ffd63ed 579 currReportOffset += SIZEOF_BASE_TIMESTAMP;
Jamie Smith 1:aac28ffd63ed 580
Jamie Smith 8:199c7fad233d 581 while(currReportOffset < rxPacketLength)
Jamie Smith 1:aac28ffd63ed 582 {
Jamie Smith 1:aac28ffd63ed 583 // lots of sensor reports use 3 16-bit numbers stored in bytes 4 through 9
Jamie Smith 1:aac28ffd63ed 584 // we can save some time by parsing those out here.
Jamie Smith 8:199c7fad233d 585 uint16_t data1 = (uint16_t)rxShtpData[currReportOffset + 5] << 8 | rxShtpData[currReportOffset + 4];
Jamie Smith 8:199c7fad233d 586 uint16_t data2 = (uint16_t)rxShtpData[currReportOffset + 7] << 8 | rxShtpData[currReportOffset + 6];
Jamie Smith 8:199c7fad233d 587 uint16_t data3 = (uint16_t)rxShtpData[currReportOffset + 9] << 8 | rxShtpData[currReportOffset + 8];
Jamie Smith 1:aac28ffd63ed 588
Jamie Smith 8:199c7fad233d 589 uint8_t reportNum = rxShtpData[currReportOffset];
Jamie Smith 1:aac28ffd63ed 590
Jamie Smith 1:aac28ffd63ed 591 if(reportNum != SENSOR_REPORTID_TIMESTAMP_REBASE)
Jamie Smith 1:aac28ffd63ed 592 {
Jamie Smith 1:aac28ffd63ed 593 // set status from byte 2
Jamie Smith 8:199c7fad233d 594 reportStatus[reportNum] = static_cast<uint8_t>(rxShtpData[currReportOffset + 2] & 0b11);
Jamie Smith 1:aac28ffd63ed 595
Jamie Smith 1:aac28ffd63ed 596 // set updated flag
Jamie Smith 1:aac28ffd63ed 597 reportHasBeenUpdated[reportNum] = true;
Jamie Smith 1:aac28ffd63ed 598 }
Jamie Smith 1:aac28ffd63ed 599
Jamie Smith 8:199c7fad233d 600 switch(rxShtpData[currReportOffset])
Jamie Smith 1:aac28ffd63ed 601 {
Jamie Smith 1:aac28ffd63ed 602 case SENSOR_REPORTID_TIMESTAMP_REBASE:
Jamie Smith 1:aac28ffd63ed 603 currReportOffset += SIZEOF_TIMESTAMP_REBASE;
Jamie Smith 1:aac28ffd63ed 604 break;
Jamie Smith 1:aac28ffd63ed 605
Jamie Smith 1:aac28ffd63ed 606 case SENSOR_REPORTID_ACCELEROMETER:
Jamie Smith 1:aac28ffd63ed 607
Jamie Smith 1:aac28ffd63ed 608 totalAcceleration = TVector3(
Jamie Smith 1:aac28ffd63ed 609 qToFloat(data1, ACCELEROMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 610 qToFloat(data2, ACCELEROMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 611 qToFloat(data3, ACCELEROMETER_Q_POINT));
Jamie Smith 1:aac28ffd63ed 612
Jamie Smith 1:aac28ffd63ed 613 currReportOffset += SIZEOF_ACCELEROMETER;
Jamie Smith 1:aac28ffd63ed 614 break;
Jamie Smith 1:aac28ffd63ed 615
Jamie Smith 1:aac28ffd63ed 616 case SENSOR_REPORTID_LINEAR_ACCELERATION:
Jamie Smith 1:aac28ffd63ed 617
Jamie Smith 1:aac28ffd63ed 618 linearAcceleration = TVector3(
Jamie Smith 1:aac28ffd63ed 619 qToFloat(data1, ACCELEROMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 620 qToFloat(data2, ACCELEROMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 621 qToFloat(data3, ACCELEROMETER_Q_POINT));
Jamie Smith 1:aac28ffd63ed 622
Jamie Smith 1:aac28ffd63ed 623 currReportOffset += SIZEOF_LINEAR_ACCELERATION;
Jamie Smith 1:aac28ffd63ed 624 break;
Jamie Smith 1:aac28ffd63ed 625
Jamie Smith 1:aac28ffd63ed 626 case SENSOR_REPORTID_GRAVITY:
Jamie Smith 1:aac28ffd63ed 627
Jamie Smith 1:aac28ffd63ed 628 gravityAcceleration = TVector3(
Jamie Smith 1:aac28ffd63ed 629 qToFloat(data1, ACCELEROMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 630 qToFloat(data2, ACCELEROMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 631 qToFloat(data3, ACCELEROMETER_Q_POINT));
Jamie Smith 1:aac28ffd63ed 632
Jamie Smith 1:aac28ffd63ed 633 currReportOffset += SIZEOF_LINEAR_ACCELERATION;
Jamie Smith 1:aac28ffd63ed 634 break;
Jamie Smith 1:aac28ffd63ed 635
Jamie Smith 1:aac28ffd63ed 636 case SENSOR_REPORTID_GYROSCOPE_CALIBRATED:
Jamie Smith 1:aac28ffd63ed 637
Jamie Smith 1:aac28ffd63ed 638 gyroRotation = TVector3(
Jamie Smith 1:aac28ffd63ed 639 qToFloat(data1, GYRO_Q_POINT),
Jamie Smith 1:aac28ffd63ed 640 qToFloat(data2, GYRO_Q_POINT),
Jamie Smith 1:aac28ffd63ed 641 qToFloat(data3, GYRO_Q_POINT));
Jamie Smith 1:aac28ffd63ed 642
Jamie Smith 1:aac28ffd63ed 643 currReportOffset += SIZEOF_GYROSCOPE_CALIBRATED;
Jamie Smith 1:aac28ffd63ed 644 break;
Jamie Smith 1:aac28ffd63ed 645
Jamie Smith 1:aac28ffd63ed 646 case SENSOR_REPORTID_MAGNETIC_FIELD_CALIBRATED:
Jamie Smith 1:aac28ffd63ed 647
Jamie Smith 1:aac28ffd63ed 648 magField = TVector3(
Jamie Smith 1:aac28ffd63ed 649 qToFloat(data1, MAGNETOMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 650 qToFloat(data2, MAGNETOMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 651 qToFloat(data3, MAGNETOMETER_Q_POINT));
Jamie Smith 1:aac28ffd63ed 652
Jamie Smith 1:aac28ffd63ed 653 currReportOffset += SIZEOF_MAGNETIC_FIELD_CALIBRATED;
Jamie Smith 1:aac28ffd63ed 654 break;
Jamie Smith 1:aac28ffd63ed 655
Jamie Smith 1:aac28ffd63ed 656 case SENSOR_REPORTID_MAGNETIC_FIELD_UNCALIBRATED:
Jamie Smith 1:aac28ffd63ed 657 {
Jamie Smith 1:aac28ffd63ed 658 magFieldUncalibrated = TVector3(
Jamie Smith 1:aac28ffd63ed 659 qToFloat(data1, MAGNETOMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 660 qToFloat(data2, MAGNETOMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 661 qToFloat(data3, MAGNETOMETER_Q_POINT));
Jamie Smith 1:aac28ffd63ed 662
Jamie Smith 8:199c7fad233d 663 uint16_t ironOffsetXQ = (uint16_t) rxShtpData[currReportOffset + 11] << 8 | rxShtpData[currReportOffset + 10];
Jamie Smith 8:199c7fad233d 664 uint16_t ironOffsetYQ = (uint16_t) rxShtpData[currReportOffset + 13] << 8 | rxShtpData[currReportOffset + 12];
Jamie Smith 8:199c7fad233d 665 uint16_t ironOffsetZQ = (uint16_t) rxShtpData[currReportOffset + 15] << 8 | rxShtpData[currReportOffset + 14];
Jamie Smith 1:aac28ffd63ed 666
Jamie Smith 1:aac28ffd63ed 667 hardIronOffset = TVector3(
Jamie Smith 1:aac28ffd63ed 668 qToFloat(ironOffsetXQ, MAGNETOMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 669 qToFloat(ironOffsetYQ, MAGNETOMETER_Q_POINT),
Jamie Smith 1:aac28ffd63ed 670 qToFloat(ironOffsetZQ, MAGNETOMETER_Q_POINT));
Jamie Smith 1:aac28ffd63ed 671
Jamie Smith 1:aac28ffd63ed 672 currReportOffset += SIZEOF_MAGNETIC_FIELD_UNCALIBRATED;
Jamie Smith 1:aac28ffd63ed 673 }
Jamie Smith 1:aac28ffd63ed 674 break;
Jamie Smith 1:aac28ffd63ed 675
Jamie Smith 1:aac28ffd63ed 676 case SENSOR_REPORTID_ROTATION_VECTOR:
Jamie Smith 1:aac28ffd63ed 677 {
Jamie Smith 8:199c7fad233d 678 uint16_t realPartQ = (uint16_t) rxShtpData[currReportOffset + 11] << 8 | rxShtpData[currReportOffset + 10];
Jamie Smith 8:199c7fad233d 679 uint16_t accuracyQ = (uint16_t) rxShtpData[currReportOffset + 13] << 8 | rxShtpData[currReportOffset + 12];
Jamie Smith 1:aac28ffd63ed 680
Jamie Smith 1:aac28ffd63ed 681 rotationVector = TVector4(
Jamie Smith 1:aac28ffd63ed 682 qToFloat(data1, ROTATION_Q_POINT),
Jamie Smith 1:aac28ffd63ed 683 qToFloat(data2, ROTATION_Q_POINT),
Jamie Smith 1:aac28ffd63ed 684 qToFloat(data3, ROTATION_Q_POINT),
Jamie Smith 1:aac28ffd63ed 685 qToFloat(realPartQ, ROTATION_Q_POINT));
Jamie Smith 1:aac28ffd63ed 686
Jamie Smith 1:aac28ffd63ed 687 rotationAccuracy = qToFloat(accuracyQ, ROTATION_ACCURACY_Q_POINT);
Jamie Smith 1:aac28ffd63ed 688
Jamie Smith 1:aac28ffd63ed 689 currReportOffset += SIZEOF_ROTATION_VECTOR;
Jamie Smith 1:aac28ffd63ed 690 }
Jamie Smith 1:aac28ffd63ed 691 break;
Jamie Smith 1:aac28ffd63ed 692
Jamie Smith 1:aac28ffd63ed 693 case SENSOR_REPORTID_GAME_ROTATION_VECTOR:
Jamie Smith 1:aac28ffd63ed 694 {
Jamie Smith 8:199c7fad233d 695 uint16_t realPartQ = (uint16_t) rxShtpData[currReportOffset + 11] << 8 | rxShtpData[currReportOffset + 10];
Jamie Smith 1:aac28ffd63ed 696
Jamie Smith 1:aac28ffd63ed 697 gameRotationVector = TVector4(
Jamie Smith 1:aac28ffd63ed 698 qToFloat(data1, ROTATION_Q_POINT),
Jamie Smith 1:aac28ffd63ed 699 qToFloat(data2, ROTATION_Q_POINT),
Jamie Smith 1:aac28ffd63ed 700 qToFloat(data3, ROTATION_Q_POINT),
Jamie Smith 1:aac28ffd63ed 701 qToFloat(realPartQ, ROTATION_Q_POINT));
Jamie Smith 1:aac28ffd63ed 702
Jamie Smith 1:aac28ffd63ed 703 currReportOffset += SIZEOF_GAME_ROTATION_VECTOR;
Jamie Smith 1:aac28ffd63ed 704 }
Jamie Smith 1:aac28ffd63ed 705 break;
Jamie Smith 1:aac28ffd63ed 706
Jamie Smith 1:aac28ffd63ed 707 case SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR:
Jamie Smith 1:aac28ffd63ed 708 {
Jamie Smith 8:199c7fad233d 709 uint16_t realPartQ = (uint16_t) rxShtpData[currReportOffset + 11] << 8 | rxShtpData[currReportOffset + 10];
Jamie Smith 8:199c7fad233d 710 uint16_t accuracyQ = (uint16_t) rxShtpData[currReportOffset + 13] << 8 | rxShtpData[currReportOffset + 12];
Jamie Smith 1:aac28ffd63ed 711
Jamie Smith 1:aac28ffd63ed 712 geomagneticRotationVector = TVector4(
Jamie Smith 1:aac28ffd63ed 713 qToFloat(data1, ROTATION_Q_POINT),
Jamie Smith 1:aac28ffd63ed 714 qToFloat(data2, ROTATION_Q_POINT),
Jamie Smith 1:aac28ffd63ed 715 qToFloat(data3, ROTATION_Q_POINT),
Jamie Smith 1:aac28ffd63ed 716 qToFloat(realPartQ, ROTATION_Q_POINT));
Jamie Smith 1:aac28ffd63ed 717
Jamie Smith 1:aac28ffd63ed 718 geomagneticRotationAccuracy = qToFloat(accuracyQ, ROTATION_ACCURACY_Q_POINT);
Jamie Smith 1:aac28ffd63ed 719
Jamie Smith 1:aac28ffd63ed 720 currReportOffset += SIZEOF_GEOMAGNETIC_ROTATION_VECTOR;
Jamie Smith 1:aac28ffd63ed 721 }
Jamie Smith 1:aac28ffd63ed 722 break;
Jamie Smith 1:aac28ffd63ed 723
Jamie Smith 1:aac28ffd63ed 724 case SENSOR_REPORTID_TAP_DETECTOR:
Jamie Smith 1:aac28ffd63ed 725
Jamie Smith 1:aac28ffd63ed 726 // since we got the report, a tap was detected
Jamie Smith 1:aac28ffd63ed 727 tapDetected = true;
Jamie Smith 1:aac28ffd63ed 728
Jamie Smith 8:199c7fad233d 729 doubleTap = (rxShtpData[currReportOffset + 4] & (1 << 6)) != 0;
Jamie Smith 1:aac28ffd63ed 730
Jamie Smith 1:aac28ffd63ed 731 currReportOffset += SIZEOF_TAP_DETECTOR;
Jamie Smith 1:aac28ffd63ed 732 break;
Jamie Smith 1:aac28ffd63ed 733
Jamie Smith 1:aac28ffd63ed 734 case SENSOR_REPORTID_STABILITY_CLASSIFIER:
Jamie Smith 1:aac28ffd63ed 735 {
Jamie Smith 8:199c7fad233d 736 uint8_t classificationNumber = rxShtpData[currReportOffset + 4];
Jamie Smith 1:aac28ffd63ed 737
Jamie Smith 1:aac28ffd63ed 738 if(classificationNumber > 4)
Jamie Smith 1:aac28ffd63ed 739 {
Jamie Smith 1:aac28ffd63ed 740 classificationNumber = 0;
Jamie Smith 1:aac28ffd63ed 741 }
Jamie Smith 1:aac28ffd63ed 742
Jamie Smith 1:aac28ffd63ed 743 stability = static_cast<Stability>(classificationNumber);
Jamie Smith 1:aac28ffd63ed 744
Jamie Smith 1:aac28ffd63ed 745 currReportOffset += SIZEOF_STABILITY_REPORT;
Jamie Smith 1:aac28ffd63ed 746 }
Jamie Smith 1:aac28ffd63ed 747 break;
Jamie Smith 1:aac28ffd63ed 748
Jamie Smith 1:aac28ffd63ed 749 case SENSOR_REPORTID_STEP_DETECTOR:
Jamie Smith 1:aac28ffd63ed 750
Jamie Smith 1:aac28ffd63ed 751 // the fact that we got the report means that a step was detected
Jamie Smith 1:aac28ffd63ed 752 stepDetected = true;
Jamie Smith 1:aac28ffd63ed 753
Jamie Smith 1:aac28ffd63ed 754 currReportOffset += SIZEOF_STEP_DETECTOR;
Jamie Smith 1:aac28ffd63ed 755
Jamie Smith 1:aac28ffd63ed 756 break;
Jamie Smith 1:aac28ffd63ed 757
Jamie Smith 1:aac28ffd63ed 758 case SENSOR_REPORTID_STEP_COUNTER:
Jamie Smith 1:aac28ffd63ed 759
Jamie Smith 8:199c7fad233d 760 stepCount = rxShtpData[currReportOffset + 9] << 8 | rxShtpData[currReportOffset + 8];
Jamie Smith 1:aac28ffd63ed 761
Jamie Smith 1:aac28ffd63ed 762 currReportOffset += SIZEOF_STEP_COUNTER;
Jamie Smith 1:aac28ffd63ed 763
Jamie Smith 1:aac28ffd63ed 764 break;
Jamie Smith 1:aac28ffd63ed 765
Jamie Smith 1:aac28ffd63ed 766 case SENSOR_REPORTID_SIGNIFICANT_MOTION:
Jamie Smith 1:aac28ffd63ed 767
Jamie Smith 1:aac28ffd63ed 768 // the fact that we got the report means that significant motion was detected
Jamie Smith 1:aac28ffd63ed 769 significantMotionDetected = true;
Jamie Smith 1:aac28ffd63ed 770
Jamie Smith 1:aac28ffd63ed 771 currReportOffset += SIZEOF_SIGNIFICANT_MOTION;
Jamie Smith 3:197ad972fb7c 772
Jamie Smith 3:197ad972fb7c 773 break;
Jamie Smith 1:aac28ffd63ed 774
Jamie Smith 1:aac28ffd63ed 775 case SENSOR_REPORTID_SHAKE_DETECTOR:
Jamie Smith 1:aac28ffd63ed 776
Jamie Smith 1:aac28ffd63ed 777 shakeDetected = true;
Jamie Smith 1:aac28ffd63ed 778
Jamie Smith 8:199c7fad233d 779 xAxisShake = (rxShtpData[currReportOffset + 4] & 1) != 0;
Jamie Smith 8:199c7fad233d 780 yAxisShake = (rxShtpData[currReportOffset + 4] & (1 << 1)) != 0;
Jamie Smith 8:199c7fad233d 781 zAxisShake = (rxShtpData[currReportOffset + 4] & (1 << 2)) != 0;
Jamie Smith 1:aac28ffd63ed 782
Jamie Smith 1:aac28ffd63ed 783 currReportOffset += SIZEOF_SHAKE_DETECTOR;
Jamie Smith 1:aac28ffd63ed 784
Jamie Smith 3:197ad972fb7c 785 break;
Jamie Smith 3:197ad972fb7c 786
Jamie Smith 1:aac28ffd63ed 787 default:
Jamie Smith 8:199c7fad233d 788 _debugPort->printf("Error: unrecognized report ID in sensor report: %hhx. Byte %u, length %hu\n", rxShtpData[currReportOffset], currReportOffset, rxPacketLength);
Jamie Smith 1:aac28ffd63ed 789 return;
Jamie Smith 1:aac28ffd63ed 790 }
Jamie Smith 8:199c7fad233d 791
Jamie Smith 8:199c7fad233d 792 if(currReportOffset >= SHTP_RX_PACKET_SIZE)
Jamie Smith 8:199c7fad233d 793 {
Jamie Smith 8:199c7fad233d 794 _debugPort->printf("Error: sensor report longer than packet buffer! Some data was not read! Increase buffer size or decrease number of reports!\r\n");
Jamie Smith 8:199c7fad233d 795 return;
Jamie Smith 8:199c7fad233d 796 }
Jamie Smith 1:aac28ffd63ed 797 }
Jamie Smith 1:aac28ffd63ed 798
Jamie Smith 1:aac28ffd63ed 799 }
Jamie Smith 1:aac28ffd63ed 800
Jamie Smith 8:199c7fad233d 801 bool BNO080Base::waitForPacket(int channel, uint8_t reportID, std::chrono::milliseconds timeout)
Jamie Smith 1:aac28ffd63ed 802 {
Jamie Smith 1:aac28ffd63ed 803 Timer timeoutTimer;
Jamie Smith 1:aac28ffd63ed 804 timeoutTimer.start();
Jamie Smith 1:aac28ffd63ed 805
Jamie Smith 8:199c7fad233d 806 while(timeoutTimer.elapsed_time() <= timeout)
Jamie Smith 1:aac28ffd63ed 807 {
Jamie Smith 1:aac28ffd63ed 808 if(_int.read() == 0)
Jamie Smith 1:aac28ffd63ed 809 {
Jamie Smith 1:aac28ffd63ed 810 if(!receivePacket(timeout))
Jamie Smith 1:aac28ffd63ed 811 {
Jamie Smith 1:aac28ffd63ed 812 return false;
Jamie Smith 1:aac28ffd63ed 813 }
Jamie Smith 1:aac28ffd63ed 814
Jamie Smith 8:199c7fad233d 815 if(channel == rxShtpHeader[2] && reportID == rxShtpData[0])
Jamie Smith 1:aac28ffd63ed 816 {
Jamie Smith 1:aac28ffd63ed 817 // found correct packet!
Jamie Smith 1:aac28ffd63ed 818 return true;
Jamie Smith 1:aac28ffd63ed 819 }
Jamie Smith 1:aac28ffd63ed 820 else
Jamie Smith 1:aac28ffd63ed 821 {
Jamie Smith 1:aac28ffd63ed 822 // other data packet, send to proper channels
Jamie Smith 1:aac28ffd63ed 823 processPacket();
Jamie Smith 1:aac28ffd63ed 824 }
Jamie Smith 1:aac28ffd63ed 825 }
Jamie Smith 1:aac28ffd63ed 826 }
Jamie Smith 1:aac28ffd63ed 827
Jamie Smith 1:aac28ffd63ed 828 _debugPort->printf("Packet wait timeout.\n");
Jamie Smith 1:aac28ffd63ed 829 return false;
Jamie Smith 1:aac28ffd63ed 830 }
Jamie Smith 1:aac28ffd63ed 831
Jamie Smith 1:aac28ffd63ed 832 //Given a register value and a Q point, convert to float
Jamie Smith 1:aac28ffd63ed 833 //See https://en.wikipedia.org/wiki/Q_(number_format)
Jamie Smith 8:199c7fad233d 834 float BNO080Base::qToFloat(int16_t fixedPointValue, uint8_t qPoint)
Jamie Smith 1:aac28ffd63ed 835 {
Jamie Smith 1:aac28ffd63ed 836 float qFloat = fixedPointValue;
Jamie Smith 5:c75be9000d75 837 qFloat *= pow(2.0f, qPoint * -1);
Jamie Smith 1:aac28ffd63ed 838 return (qFloat);
Jamie Smith 1:aac28ffd63ed 839 }
Jamie Smith 1:aac28ffd63ed 840
Jamie Smith 8:199c7fad233d 841 float BNO080Base::qToFloat_dword(uint32_t fixedPointValue, int16_t qPoint)
Jamie Smith 1:aac28ffd63ed 842 {
Jamie Smith 1:aac28ffd63ed 843 float qFloat = fixedPointValue;
Jamie Smith 5:c75be9000d75 844 qFloat *= pow(2.0f, qPoint * -1);
Jamie Smith 1:aac28ffd63ed 845 return (qFloat);
Jamie Smith 1:aac28ffd63ed 846 }
Jamie Smith 1:aac28ffd63ed 847
Jamie Smith 1:aac28ffd63ed 848 //Given a floating point value and a Q point, convert to Q
Jamie Smith 1:aac28ffd63ed 849 //See https://en.wikipedia.org/wiki/Q_(number_format)
Jamie Smith 8:199c7fad233d 850 int16_t BNO080Base::floatToQ(float qFloat, uint8_t qPoint)
Jamie Smith 1:aac28ffd63ed 851 {
Jamie Smith 5:c75be9000d75 852 int16_t qVal = static_cast<int16_t>(qFloat * pow(2.0f, qPoint));
Jamie Smith 1:aac28ffd63ed 853 return qVal;
Jamie Smith 1:aac28ffd63ed 854 }
Jamie Smith 1:aac28ffd63ed 855
Jamie Smith 8:199c7fad233d 856 int32_t BNO080Base::floatToQ_dword(float qFloat, uint16_t qPoint)
Jamie Smith 3:197ad972fb7c 857 {
Jamie Smith 5:c75be9000d75 858 int32_t qVal = static_cast<int32_t>(qFloat * pow(2.0f, qPoint));
Jamie Smith 3:197ad972fb7c 859 return qVal;
Jamie Smith 3:197ad972fb7c 860 }
Jamie Smith 1:aac28ffd63ed 861 //Tell the sensor to do a command
Jamie Smith 1:aac28ffd63ed 862 //See 6.3.8 page 41, Command request
Jamie Smith 1:aac28ffd63ed 863 //The caller is expected to set P0 through P8 prior to calling
Jamie Smith 8:199c7fad233d 864 void BNO080Base::sendCommand(uint8_t command)
Jamie Smith 1:aac28ffd63ed 865 {
Jamie Smith 8:199c7fad233d 866 txShtpData[0] = SHTP_REPORT_COMMAND_REQUEST; //Command Request
Jamie Smith 8:199c7fad233d 867 txShtpData[1] = commandSequenceNumber++; //Increments automatically each function call
Jamie Smith 8:199c7fad233d 868 txShtpData[2] = command; //Command
Jamie Smith 1:aac28ffd63ed 869
Jamie Smith 1:aac28ffd63ed 870 //Caller must set these
Jamie Smith 1:aac28ffd63ed 871 /*shtpData[3] = 0; //P0
Jamie Smith 1:aac28ffd63ed 872 shtpData[4] = 0; //P1
Jamie Smith 1:aac28ffd63ed 873 shtpData[5] = 0; //P2
Jamie Smith 1:aac28ffd63ed 874 shtpData[6] = 0;
Jamie Smith 1:aac28ffd63ed 875 shtpData[7] = 0;
Jamie Smith 1:aac28ffd63ed 876 shtpData[8] = 0;
Jamie Smith 1:aac28ffd63ed 877 shtpData[9] = 0;
Jamie Smith 1:aac28ffd63ed 878 shtpData[10] = 0;
Jamie Smith 1:aac28ffd63ed 879 shtpData[11] = 0;*/
Jamie Smith 1:aac28ffd63ed 880
Jamie Smith 1:aac28ffd63ed 881 //Transmit packet on channel 2, 12 bytes
Jamie Smith 1:aac28ffd63ed 882 sendPacket(CHANNEL_CONTROL, 12);
Jamie Smith 1:aac28ffd63ed 883 }
Jamie Smith 1:aac28ffd63ed 884
Jamie Smith 1:aac28ffd63ed 885 //Given a sensor's report ID, this tells the BNO080 to begin reporting the values
Jamie Smith 1:aac28ffd63ed 886 //Also sets the specific config word. Useful for personal activity classifier
Jamie Smith 8:199c7fad233d 887 void BNO080Base::setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig)
Jamie Smith 1:aac28ffd63ed 888 {
Jamie Smith 1:aac28ffd63ed 889 uint32_t microsBetweenReports = static_cast<uint32_t>(timeBetweenReports * 1000);
Jamie Smith 1:aac28ffd63ed 890
Jamie Smith 1:aac28ffd63ed 891 const uint32_t batchMicros = 0;
Jamie Smith 1:aac28ffd63ed 892
Jamie Smith 8:199c7fad233d 893 txShtpData[0] = SHTP_REPORT_SET_FEATURE_COMMAND; //Set feature command. Reference page 55
Jamie Smith 8:199c7fad233d 894 txShtpData[1] = reportID; //Feature Report ID. 0x01 = Accelerometer, 0x05 = Rotation vector
Jamie Smith 8:199c7fad233d 895 txShtpData[2] = 0; //Feature flags
Jamie Smith 8:199c7fad233d 896 txShtpData[3] = 0; //Change sensitivity (LSB)
Jamie Smith 8:199c7fad233d 897 txShtpData[4] = 0; //Change sensitivity (MSB)
Jamie Smith 8:199c7fad233d 898 txShtpData[5] = (microsBetweenReports >> 0) & 0xFF; //Report interval (LSB) in microseconds. 0x7A120 = 500ms
Jamie Smith 8:199c7fad233d 899 txShtpData[6] = (microsBetweenReports >> 8) & 0xFF; //Report interval
Jamie Smith 8:199c7fad233d 900 txShtpData[7] = (microsBetweenReports >> 16) & 0xFF; //Report interval
Jamie Smith 8:199c7fad233d 901 txShtpData[8] = (microsBetweenReports >> 24) & 0xFF; //Report interval (MSB)
Jamie Smith 8:199c7fad233d 902 txShtpData[9] = (batchMicros >> 0) & 0xFF; //Batch Interval (LSB)
Jamie Smith 8:199c7fad233d 903 txShtpData[10] = (batchMicros >> 8) & 0xFF; //Batch Interval
Jamie Smith 8:199c7fad233d 904 txShtpData[11] = (batchMicros >> 16) & 0xFF;//Batch Interval
Jamie Smith 8:199c7fad233d 905 txShtpData[12] = (batchMicros >> 24) & 0xFF;//Batch Interval (MSB)
Jamie Smith 8:199c7fad233d 906 txShtpData[13] = (specificConfig >> 0) & 0xFF; //Sensor-specific config (LSB)
Jamie Smith 8:199c7fad233d 907 txShtpData[14] = (specificConfig >> 8) & 0xFF; //Sensor-specific config
Jamie Smith 8:199c7fad233d 908 txShtpData[15] = (specificConfig >> 16) & 0xFF; //Sensor-specific config
Jamie Smith 8:199c7fad233d 909 txShtpData[16] = (specificConfig >> 24) & 0xFF; //Sensor-specific config (MSB)
Jamie Smith 1:aac28ffd63ed 910
Jamie Smith 1:aac28ffd63ed 911 //Transmit packet on channel 2, 17 bytes
Jamie Smith 1:aac28ffd63ed 912 sendPacket(CHANNEL_CONTROL, 17);
Jamie Smith 1:aac28ffd63ed 913 }
Jamie Smith 1:aac28ffd63ed 914
Jamie Smith 8:199c7fad233d 915 bool BNO080Base::readFRSRecord(uint16_t recordID, uint32_t* readBuffer, uint16_t readLength)
Jamie Smith 1:aac28ffd63ed 916 {
Jamie Smith 1:aac28ffd63ed 917 // send initial read request
Jamie Smith 9:430f5302f9e1 918 clearSendBuffer();
Jamie Smith 1:aac28ffd63ed 919
Jamie Smith 8:199c7fad233d 920 txShtpData[0] = SHTP_REPORT_FRS_READ_REQUEST;
Jamie Smith 1:aac28ffd63ed 921 // read offset of 0 -> start at the start of the record
Jamie Smith 8:199c7fad233d 922 txShtpData[2] = 0;
Jamie Smith 8:199c7fad233d 923 txShtpData[3] = 0;
Jamie Smith 1:aac28ffd63ed 924 // record ID
Jamie Smith 8:199c7fad233d 925 txShtpData[4] = static_cast<uint8_t>(recordID & 0xFF);
Jamie Smith 8:199c7fad233d 926 txShtpData[5] = static_cast<uint8_t>(recordID >> 8);
Jamie Smith 1:aac28ffd63ed 927 // block size
Jamie Smith 8:199c7fad233d 928 txShtpData[6] = static_cast<uint8_t>(readLength & 0xFF);
Jamie Smith 8:199c7fad233d 929 txShtpData[7] = static_cast<uint8_t>(readLength >> 8);
Jamie Smith 1:aac28ffd63ed 930
Jamie Smith 1:aac28ffd63ed 931 sendPacket(CHANNEL_CONTROL, 8);
Jamie Smith 1:aac28ffd63ed 932
Jamie Smith 1:aac28ffd63ed 933 // now, read back the responses
Jamie Smith 1:aac28ffd63ed 934 size_t readOffset = 0;
Jamie Smith 1:aac28ffd63ed 935 while(readOffset < readLength)
Jamie Smith 1:aac28ffd63ed 936 {
Jamie Smith 3:197ad972fb7c 937 // it seems like it can take quite a long time for FRS data to be read, so we have to increase the timeout
Jamie Smith 8:199c7fad233d 938 if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_READ_RESPONSE, 300ms))
Jamie Smith 1:aac28ffd63ed 939 {
Jamie Smith 1:aac28ffd63ed 940 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 941 _debugPort->printf("Error: did not receive FRS read response after sending read request!\n");
Jamie Smith 1:aac28ffd63ed 942 #endif
Jamie Smith 1:aac28ffd63ed 943 return false;
Jamie Smith 1:aac28ffd63ed 944 }
Jamie Smith 1:aac28ffd63ed 945
Jamie Smith 8:199c7fad233d 946 uint8_t status = static_cast<uint8_t>(rxShtpData[1] & 0b1111);
Jamie Smith 8:199c7fad233d 947 uint8_t dataLength = rxShtpData[1] >> 4;
Jamie Smith 1:aac28ffd63ed 948
Jamie Smith 1:aac28ffd63ed 949 // check status
Jamie Smith 1:aac28ffd63ed 950 if(status == 1)
Jamie Smith 1:aac28ffd63ed 951 {
Jamie Smith 1:aac28ffd63ed 952 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 953 _debugPort->printf("Error: FRS reports invalid record ID!\n");
Jamie Smith 1:aac28ffd63ed 954 #endif
Jamie Smith 1:aac28ffd63ed 955 return false;
Jamie Smith 1:aac28ffd63ed 956 }
Jamie Smith 1:aac28ffd63ed 957 else if(status == 2)
Jamie Smith 1:aac28ffd63ed 958 {
Jamie Smith 1:aac28ffd63ed 959 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 960 _debugPort->printf("Error: FRS is busy!\n");
Jamie Smith 1:aac28ffd63ed 961 #endif
Jamie Smith 1:aac28ffd63ed 962 return false;
Jamie Smith 1:aac28ffd63ed 963 }
Jamie Smith 1:aac28ffd63ed 964 else if(status == 4)
Jamie Smith 1:aac28ffd63ed 965 {
Jamie Smith 1:aac28ffd63ed 966 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 967 _debugPort->printf("Error: FRS reports offset is out of range!\n");
Jamie Smith 1:aac28ffd63ed 968 #endif
Jamie Smith 1:aac28ffd63ed 969 return false;
Jamie Smith 1:aac28ffd63ed 970 }
Jamie Smith 1:aac28ffd63ed 971 else if(status == 5)
Jamie Smith 1:aac28ffd63ed 972 {
Jamie Smith 1:aac28ffd63ed 973 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 974 _debugPort->printf("Error: FRS reports record %hx is empty!\n", recordID);
Jamie Smith 1:aac28ffd63ed 975 #endif
Jamie Smith 1:aac28ffd63ed 976 return false;
Jamie Smith 1:aac28ffd63ed 977 }
Jamie Smith 1:aac28ffd63ed 978 else if(status == 8)
Jamie Smith 1:aac28ffd63ed 979 {
Jamie Smith 1:aac28ffd63ed 980 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 981 _debugPort->printf("Error: FRS reports flash memory device unavailable!\n");
Jamie Smith 1:aac28ffd63ed 982 #endif
Jamie Smith 1:aac28ffd63ed 983 return false;
Jamie Smith 1:aac28ffd63ed 984 }
Jamie Smith 1:aac28ffd63ed 985
Jamie Smith 1:aac28ffd63ed 986 // check data length
Jamie Smith 1:aac28ffd63ed 987 if(dataLength == 0)
Jamie Smith 1:aac28ffd63ed 988 {
Jamie Smith 1:aac28ffd63ed 989 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 990 _debugPort->printf("Error: Received FRS packet with 0 data length!\n");
Jamie Smith 1:aac28ffd63ed 991 #endif
Jamie Smith 1:aac28ffd63ed 992 return false;
Jamie Smith 1:aac28ffd63ed 993 }
Jamie Smith 1:aac28ffd63ed 994 else if(dataLength == 1)
Jamie Smith 1:aac28ffd63ed 995 {
Jamie Smith 1:aac28ffd63ed 996 if(readOffset + 1 != readLength)
Jamie Smith 1:aac28ffd63ed 997 {
Jamie Smith 1:aac28ffd63ed 998 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 999 _debugPort->printf("Error: Received 1 length packet but more than 1 byte remains to be be read!\n");
Jamie Smith 1:aac28ffd63ed 1000 #endif
Jamie Smith 1:aac28ffd63ed 1001 return false;
Jamie Smith 1:aac28ffd63ed 1002 }
Jamie Smith 1:aac28ffd63ed 1003 }
Jamie Smith 1:aac28ffd63ed 1004
Jamie Smith 1:aac28ffd63ed 1005 // now, _finally_, read the dang words
Jamie Smith 8:199c7fad233d 1006 readBuffer[readOffset] = (rxShtpData[7] << 24) | (rxShtpData[6] << 16) | (rxShtpData[5] << 8) | (rxShtpData[4]);
Jamie Smith 1:aac28ffd63ed 1007
Jamie Smith 1:aac28ffd63ed 1008 // check if we only wanted the first word
Jamie Smith 1:aac28ffd63ed 1009 ++readOffset;
Jamie Smith 1:aac28ffd63ed 1010 if(readOffset == readLength)
Jamie Smith 1:aac28ffd63ed 1011 {
Jamie Smith 1:aac28ffd63ed 1012 break;
Jamie Smith 1:aac28ffd63ed 1013 }
Jamie Smith 1:aac28ffd63ed 1014
Jamie Smith 8:199c7fad233d 1015 readBuffer[readOffset] = (rxShtpData[11] << 24) | (rxShtpData[10] << 16) | (rxShtpData[9] << 8) | (rxShtpData[8]);
Jamie Smith 1:aac28ffd63ed 1016 readOffset++;
Jamie Smith 1:aac28ffd63ed 1017 }
Jamie Smith 1:aac28ffd63ed 1018
Jamie Smith 1:aac28ffd63ed 1019 // read successful
Jamie Smith 1:aac28ffd63ed 1020 return true;
Jamie Smith 1:aac28ffd63ed 1021
Jamie Smith 1:aac28ffd63ed 1022 }
Jamie Smith 1:aac28ffd63ed 1023
Jamie Smith 8:199c7fad233d 1024 bool BNO080Base::writeFRSRecord(uint16_t recordID, uint32_t* buffer, uint16_t length)
Jamie Smith 3:197ad972fb7c 1025 {
Jamie Smith 3:197ad972fb7c 1026 // send initial write request, which tells the chip where we're writing
Jamie Smith 9:430f5302f9e1 1027 clearSendBuffer();
Jamie Smith 3:197ad972fb7c 1028
Jamie Smith 8:199c7fad233d 1029 txShtpData[0] = SHTP_REPORT_FRS_WRITE_REQUEST;
Jamie Smith 3:197ad972fb7c 1030 // length to write (must be <= record length)
Jamie Smith 8:199c7fad233d 1031 txShtpData[2] = static_cast<uint8_t>(length & 0xFF);
Jamie Smith 8:199c7fad233d 1032 txShtpData[3] = static_cast<uint8_t>(length >> 8);
Jamie Smith 3:197ad972fb7c 1033 // record ID
Jamie Smith 8:199c7fad233d 1034 txShtpData[4] = static_cast<uint8_t>(recordID & 0xFF);
Jamie Smith 8:199c7fad233d 1035 txShtpData[5] = static_cast<uint8_t>(recordID >> 8);
Jamie Smith 3:197ad972fb7c 1036
Jamie Smith 3:197ad972fb7c 1037 sendPacket(CHANNEL_CONTROL, 6);
Jamie Smith 3:197ad972fb7c 1038
Jamie Smith 3:197ad972fb7c 1039 // wait for FRS to become ready
Jamie Smith 8:199c7fad233d 1040 if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_WRITE_RESPONSE, 300ms))
Jamie Smith 3:197ad972fb7c 1041 {
Jamie Smith 3:197ad972fb7c 1042 #if BNO_DEBUG
Jamie Smith 3:197ad972fb7c 1043 _debugPort->printf("Error: did not receive FRS write ready response after sending write request!\r\n");
Jamie Smith 3:197ad972fb7c 1044 #endif
Jamie Smith 3:197ad972fb7c 1045 return false;
Jamie Smith 3:197ad972fb7c 1046 }
Jamie Smith 3:197ad972fb7c 1047
Jamie Smith 8:199c7fad233d 1048 if(rxShtpData[1] != 4)
Jamie Smith 3:197ad972fb7c 1049 {
Jamie Smith 3:197ad972fb7c 1050 #if BNO_DEBUG
Jamie Smith 8:199c7fad233d 1051 _debugPort->printf("Error: FRS reports error initiating write operation: %hhu!\r\n", rxShtpData[1]);
Jamie Smith 3:197ad972fb7c 1052 #endif
Jamie Smith 3:197ad972fb7c 1053 return false;
Jamie Smith 3:197ad972fb7c 1054 }
Jamie Smith 3:197ad972fb7c 1055
Jamie Smith 3:197ad972fb7c 1056 // now, send the actual data
Jamie Smith 3:197ad972fb7c 1057 for(uint16_t wordIndex = 0; wordIndex < length; wordIndex += 2)
Jamie Smith 3:197ad972fb7c 1058 {
Jamie Smith 3:197ad972fb7c 1059 // send packet containing 2 words
Jamie Smith 9:430f5302f9e1 1060 clearSendBuffer();
Jamie Smith 8:199c7fad233d 1061 txShtpData[0] = SHTP_REPORT_FRS_WRITE_DATA;
Jamie Smith 3:197ad972fb7c 1062
Jamie Smith 3:197ad972fb7c 1063 // offset to write at
Jamie Smith 8:199c7fad233d 1064 txShtpData[2] = static_cast<uint8_t>(wordIndex & 0xFF);
Jamie Smith 8:199c7fad233d 1065 txShtpData[3] = static_cast<uint8_t>(wordIndex >> 8);
Jamie Smith 3:197ad972fb7c 1066
Jamie Smith 3:197ad972fb7c 1067 // data 0
Jamie Smith 8:199c7fad233d 1068 *reinterpret_cast<uint32_t*>(txShtpData + 4) = buffer[wordIndex];
Jamie Smith 3:197ad972fb7c 1069
Jamie Smith 3:197ad972fb7c 1070 // data 1, if it exists
Jamie Smith 3:197ad972fb7c 1071 if(wordIndex != length - 1)
Jamie Smith 3:197ad972fb7c 1072 {
Jamie Smith 8:199c7fad233d 1073 *reinterpret_cast<uint32_t*>(txShtpData + 8) = buffer[wordIndex + 1];
Jamie Smith 3:197ad972fb7c 1074 }
Jamie Smith 3:197ad972fb7c 1075
Jamie Smith 3:197ad972fb7c 1076 sendPacket(CHANNEL_CONTROL, 12);
Jamie Smith 3:197ad972fb7c 1077
Jamie Smith 3:197ad972fb7c 1078 // wait for acknowledge
Jamie Smith 8:199c7fad233d 1079 if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_WRITE_RESPONSE, 300ms))
Jamie Smith 3:197ad972fb7c 1080 {
Jamie Smith 3:197ad972fb7c 1081 #if BNO_DEBUG
Jamie Smith 3:197ad972fb7c 1082 _debugPort->printf("Error: did not receive FRS write response after sending write data!\r\n");
Jamie Smith 3:197ad972fb7c 1083 #endif
Jamie Smith 3:197ad972fb7c 1084 return false;
Jamie Smith 3:197ad972fb7c 1085 }
Jamie Smith 3:197ad972fb7c 1086
Jamie Smith 8:199c7fad233d 1087 uint8_t status = rxShtpData[1];
Jamie Smith 3:197ad972fb7c 1088
Jamie Smith 3:197ad972fb7c 1089 switch(status)
Jamie Smith 3:197ad972fb7c 1090 {
Jamie Smith 3:197ad972fb7c 1091 case 0:
Jamie Smith 3:197ad972fb7c 1092 if(length - wordIndex >= 2)
Jamie Smith 3:197ad972fb7c 1093 {
Jamie Smith 3:197ad972fb7c 1094 // status OK, write still in progress
Jamie Smith 3:197ad972fb7c 1095 }
Jamie Smith 3:197ad972fb7c 1096 else
Jamie Smith 3:197ad972fb7c 1097 {
Jamie Smith 3:197ad972fb7c 1098 #if BNO_DEBUG
Jamie Smith 3:197ad972fb7c 1099 _debugPort->printf("Error: FRS reports write in progress when it should be complete!\r\n");
Jamie Smith 3:197ad972fb7c 1100 #endif
Jamie Smith 3:197ad972fb7c 1101 return false;
Jamie Smith 3:197ad972fb7c 1102 }
Jamie Smith 3:197ad972fb7c 1103 break;
Jamie Smith 3:197ad972fb7c 1104 case 3:
Jamie Smith 3:197ad972fb7c 1105 case 8:
Jamie Smith 3:197ad972fb7c 1106 if(length - wordIndex <= 2)
Jamie Smith 3:197ad972fb7c 1107 {
Jamie Smith 3:197ad972fb7c 1108 // status OK, write complete
Jamie Smith 3:197ad972fb7c 1109 }
Jamie Smith 3:197ad972fb7c 1110 else
Jamie Smith 3:197ad972fb7c 1111 {
Jamie Smith 3:197ad972fb7c 1112 #if BNO_DEBUG
Jamie Smith 3:197ad972fb7c 1113 _debugPort->printf("Error: FRS reports write complete when it should be still going!\n");
Jamie Smith 3:197ad972fb7c 1114 #endif
Jamie Smith 3:197ad972fb7c 1115 return false;
Jamie Smith 3:197ad972fb7c 1116 }
Jamie Smith 3:197ad972fb7c 1117 break;
Jamie Smith 3:197ad972fb7c 1118 case 1:
Jamie Smith 3:197ad972fb7c 1119 #if BNO_DEBUG
Jamie Smith 3:197ad972fb7c 1120 _debugPort->printf("Error: FRS reports invalid record ID!\n");
Jamie Smith 3:197ad972fb7c 1121 #endif
Jamie Smith 3:197ad972fb7c 1122 return false;
Jamie Smith 3:197ad972fb7c 1123 case 2:
Jamie Smith 3:197ad972fb7c 1124 #if BNO_DEBUG
Jamie Smith 3:197ad972fb7c 1125 _debugPort->printf("Error: FRS is busy!\n");
Jamie Smith 3:197ad972fb7c 1126 #endif
Jamie Smith 3:197ad972fb7c 1127 return false;
Jamie Smith 3:197ad972fb7c 1128 case 5:
Jamie Smith 3:197ad972fb7c 1129 #if BNO_DEBUG
Jamie Smith 3:197ad972fb7c 1130 _debugPort->printf("Error: FRS reports write failed!\n");
Jamie Smith 3:197ad972fb7c 1131 #endif
Jamie Smith 3:197ad972fb7c 1132 return false;
Jamie Smith 3:197ad972fb7c 1133 case 6:
Jamie Smith 3:197ad972fb7c 1134 #if BNO_DEBUG
Jamie Smith 3:197ad972fb7c 1135 _debugPort->printf("Error: FRS reports data received while not in write mode!\n");
Jamie Smith 3:197ad972fb7c 1136 #endif
Jamie Smith 3:197ad972fb7c 1137 return false;
Jamie Smith 3:197ad972fb7c 1138 case 7:
Jamie Smith 3:197ad972fb7c 1139 #if BNO_DEBUG
Jamie Smith 3:197ad972fb7c 1140 _debugPort->printf("Error: FRS reports invalid length!\n");
Jamie Smith 3:197ad972fb7c 1141 #endif
Jamie Smith 3:197ad972fb7c 1142 return false;
Jamie Smith 3:197ad972fb7c 1143 case 9:
Jamie Smith 3:197ad972fb7c 1144 #if BNO_DEBUG
Jamie Smith 3:197ad972fb7c 1145 _debugPort->printf("Error: FRS reports invalid data for this record!\n");
Jamie Smith 3:197ad972fb7c 1146 #endif
Jamie Smith 3:197ad972fb7c 1147 return false;
Jamie Smith 3:197ad972fb7c 1148
Jamie Smith 3:197ad972fb7c 1149 case 10:
Jamie Smith 3:197ad972fb7c 1150 #if BNO_DEBUG
Jamie Smith 3:197ad972fb7c 1151 _debugPort->printf("Error: FRS reports flash device unavailable!\n");
Jamie Smith 3:197ad972fb7c 1152 #endif
Jamie Smith 3:197ad972fb7c 1153 return false;
Jamie Smith 3:197ad972fb7c 1154
Jamie Smith 3:197ad972fb7c 1155 case 11:
Jamie Smith 3:197ad972fb7c 1156 #if BNO_DEBUG
Jamie Smith 3:197ad972fb7c 1157 _debugPort->printf("Error: FRS reports record is read-only!\n");
Jamie Smith 3:197ad972fb7c 1158 #endif
Jamie Smith 3:197ad972fb7c 1159 return false;
Jamie Smith 3:197ad972fb7c 1160 default:
Jamie Smith 3:197ad972fb7c 1161 #if BNO_DEBUG
Jamie Smith 3:197ad972fb7c 1162 _debugPort->printf("Error: FRS reports unknown result code %hhu!\n", status);
Jamie Smith 3:197ad972fb7c 1163 #endif
Jamie Smith 3:197ad972fb7c 1164 break;
Jamie Smith 3:197ad972fb7c 1165
Jamie Smith 3:197ad972fb7c 1166 }
Jamie Smith 3:197ad972fb7c 1167 }
Jamie Smith 3:197ad972fb7c 1168
Jamie Smith 3:197ad972fb7c 1169 // write complete
Jamie Smith 3:197ad972fb7c 1170 return true;
Jamie Smith 3:197ad972fb7c 1171 }
Jamie Smith 3:197ad972fb7c 1172
Jamie Smith 8:199c7fad233d 1173 //Pretty prints the contents of a packet in the given buffer
Jamie Smith 8:199c7fad233d 1174 void BNO080Base::printPacket(uint8_t * buffer)
Jamie Smith 1:aac28ffd63ed 1175 {
Jamie Smith 1:aac28ffd63ed 1176 #if BNO_DEBUG
Jamie Smith 1:aac28ffd63ed 1177 //Print the four byte header
Jamie Smith 1:aac28ffd63ed 1178 _debugPort->printf("Header:");
Jamie Smith 1:aac28ffd63ed 1179 for (uint8_t x = 0 ; x < 4 ; x++)
Jamie Smith 1:aac28ffd63ed 1180 {
Jamie Smith 1:aac28ffd63ed 1181 _debugPort->printf(" ");
Jamie Smith 8:199c7fad233d 1182 _debugPort->printf("%02hhx", buffer[x]);
Jamie Smith 1:aac28ffd63ed 1183 }
Jamie Smith 1:aac28ffd63ed 1184
Jamie Smith 8:199c7fad233d 1185 //Calculate the number of data bytes in this packet
Jamie Smith 8:199c7fad233d 1186 uint16_t totalLength = (static_cast<uint16_t>(buffer[1]) << 8 | buffer[0]);
Jamie Smith 8:199c7fad233d 1187 totalLength &= ~(1 << 15);
Jamie Smith 8:199c7fad233d 1188 uint16_t packetLength = totalLength - 4; //Remove the header bytes from the data count
Jamie Smith 8:199c7fad233d 1189
Jamie Smith 8:199c7fad233d 1190 uint16_t printLength = std::min(packetLength, static_cast<uint16_t>(40)); //Artificial limit. We don't want the phone book.
Jamie Smith 1:aac28ffd63ed 1191
Jamie Smith 1:aac28ffd63ed 1192 _debugPort->printf(" Body:");
Jamie Smith 1:aac28ffd63ed 1193 for (uint16_t x = 0 ; x < printLength ; x++)
Jamie Smith 1:aac28ffd63ed 1194 {
Jamie Smith 1:aac28ffd63ed 1195 _debugPort->printf(" ");
Jamie Smith 8:199c7fad233d 1196 _debugPort->printf("%02hhx", buffer[x + SHTP_HEADER_SIZE]);
Jamie Smith 1:aac28ffd63ed 1197 }
Jamie Smith 1:aac28ffd63ed 1198
Jamie Smith 1:aac28ffd63ed 1199 _debugPort->printf(", Length:");
Jamie Smith 8:199c7fad233d 1200 _debugPort->printf("%d", packetLength + SHTP_HEADER_SIZE);
Jamie Smith 8:199c7fad233d 1201 _debugPort->printf(", SeqNum: %hhu", buffer[3]);
Jamie Smith 1:aac28ffd63ed 1202
Jamie Smith 1:aac28ffd63ed 1203 _debugPort->printf(", Channel:");
Jamie Smith 8:199c7fad233d 1204 if (buffer[2] == 0) _debugPort->printf("Command");
Jamie Smith 8:199c7fad233d 1205 else if (buffer[2] == 1) _debugPort->printf("Executable");
Jamie Smith 8:199c7fad233d 1206 else if (buffer[2] == 2) _debugPort->printf("Control");
Jamie Smith 8:199c7fad233d 1207 else if (buffer[2] == 3) _debugPort->printf("Sensor-report");
Jamie Smith 8:199c7fad233d 1208 else if (buffer[2] == 4) _debugPort->printf("Wake-report");
Jamie Smith 8:199c7fad233d 1209 else if (buffer[2] == 5) _debugPort->printf("Gyro-vector");
Jamie Smith 8:199c7fad233d 1210 else _debugPort->printf("%hhu", buffer[2]);
Jamie Smith 1:aac28ffd63ed 1211
Jamie Smith 1:aac28ffd63ed 1212 _debugPort->printf("\n");
Jamie Smith 1:aac28ffd63ed 1213 #endif
Jamie Smith 1:aac28ffd63ed 1214 }
Jamie Smith 1:aac28ffd63ed 1215
Jamie Smith 1:aac28ffd63ed 1216
Jamie Smith 9:430f5302f9e1 1217 void BNO080Base::clearSendBuffer()
Jamie Smith 1:aac28ffd63ed 1218 {
Jamie Smith 8:199c7fad233d 1219 memset(txPacketBuffer, 0, SHTP_HEADER_SIZE + SHTP_MAX_TX_PACKET_SIZE);
Jamie Smith 1:aac28ffd63ed 1220 }
Jamie Smith 1:aac28ffd63ed 1221
Jamie Smith 8:199c7fad233d 1222 bool BNO080Base::loadReportMetadata(BNO080Base::Report report)
Jamie Smith 1:aac28ffd63ed 1223 {
Jamie Smith 3:197ad972fb7c 1224 uint16_t reportMetaRecord = 0;
Jamie Smith 1:aac28ffd63ed 1225
Jamie Smith 1:aac28ffd63ed 1226 // first, convert the report into the correct FRS record ID for that report's metadata
Jamie Smith 1:aac28ffd63ed 1227 // data from SH-2 section 5.1
Jamie Smith 1:aac28ffd63ed 1228 switch(report)
Jamie Smith 1:aac28ffd63ed 1229 {
Jamie Smith 2:2269b723d16a 1230 case TOTAL_ACCELERATION:
Jamie Smith 1:aac28ffd63ed 1231 reportMetaRecord = 0xE301;
Jamie Smith 1:aac28ffd63ed 1232 break;
Jamie Smith 2:2269b723d16a 1233 case LINEAR_ACCELERATION:
Jamie Smith 1:aac28ffd63ed 1234 reportMetaRecord = 0xE303;
Jamie Smith 1:aac28ffd63ed 1235 break;
Jamie Smith 2:2269b723d16a 1236 case GRAVITY_ACCELERATION:
Jamie Smith 1:aac28ffd63ed 1237 reportMetaRecord = 0xE304;
Jamie Smith 1:aac28ffd63ed 1238 break;
Jamie Smith 2:2269b723d16a 1239 case GYROSCOPE:
Jamie Smith 1:aac28ffd63ed 1240 reportMetaRecord = 0xE306;
Jamie Smith 1:aac28ffd63ed 1241 break;
Jamie Smith 2:2269b723d16a 1242 case MAG_FIELD:
Jamie Smith 1:aac28ffd63ed 1243 reportMetaRecord = 0xE309;
Jamie Smith 1:aac28ffd63ed 1244 break;
Jamie Smith 2:2269b723d16a 1245 case MAG_FIELD_UNCALIBRATED:
Jamie Smith 1:aac28ffd63ed 1246 reportMetaRecord = 0xE30A;
Jamie Smith 1:aac28ffd63ed 1247 break;
Jamie Smith 2:2269b723d16a 1248 case ROTATION:
Jamie Smith 1:aac28ffd63ed 1249 reportMetaRecord = 0xE30B;
Jamie Smith 1:aac28ffd63ed 1250 break;
Jamie Smith 2:2269b723d16a 1251 case GEOMAGNETIC_ROTATION:
Jamie Smith 1:aac28ffd63ed 1252 reportMetaRecord = 0xE30D;
Jamie Smith 1:aac28ffd63ed 1253 break;
Jamie Smith 2:2269b723d16a 1254 case GAME_ROTATION:
Jamie Smith 1:aac28ffd63ed 1255 reportMetaRecord = 0xE30C;
Jamie Smith 1:aac28ffd63ed 1256 break;
Jamie Smith 2:2269b723d16a 1257 case TAP_DETECTOR:
Jamie Smith 1:aac28ffd63ed 1258 reportMetaRecord = 0xE313;
Jamie Smith 1:aac28ffd63ed 1259 break;
Jamie Smith 2:2269b723d16a 1260 case STABILITY_CLASSIFIER:
Jamie Smith 1:aac28ffd63ed 1261 reportMetaRecord = 0xE317;
Jamie Smith 1:aac28ffd63ed 1262 break;
Jamie Smith 2:2269b723d16a 1263 case STEP_DETECTOR:
Jamie Smith 1:aac28ffd63ed 1264 reportMetaRecord = 0xE314;
Jamie Smith 1:aac28ffd63ed 1265 break;
Jamie Smith 2:2269b723d16a 1266 case STEP_COUNTER:
Jamie Smith 1:aac28ffd63ed 1267 reportMetaRecord = 0xE315;
Jamie Smith 1:aac28ffd63ed 1268 break;
Jamie Smith 2:2269b723d16a 1269 case SIGNIFICANT_MOTION:
Jamie Smith 1:aac28ffd63ed 1270 reportMetaRecord = 0xE316;
Jamie Smith 1:aac28ffd63ed 1271 break;
Jamie Smith 2:2269b723d16a 1272 case SHAKE_DETECTOR:
Jamie Smith 1:aac28ffd63ed 1273 reportMetaRecord = 0xE318;
Jamie Smith 1:aac28ffd63ed 1274 break;
Jamie Smith 1:aac28ffd63ed 1275 }
Jamie Smith 1:aac28ffd63ed 1276
Jamie Smith 1:aac28ffd63ed 1277 // if we already have that data stored, everything's OK
Jamie Smith 1:aac28ffd63ed 1278 if(bufferMetadataRecord == reportMetaRecord)
Jamie Smith 1:aac28ffd63ed 1279 {
Jamie Smith 1:aac28ffd63ed 1280 return true;
Jamie Smith 1:aac28ffd63ed 1281 }
Jamie Smith 1:aac28ffd63ed 1282
Jamie Smith 1:aac28ffd63ed 1283 // now, load the metadata into the buffer
Jamie Smith 1:aac28ffd63ed 1284 if(!readFRSRecord(reportMetaRecord, metadataRecord, METADATA_BUFFER_LEN))
Jamie Smith 1:aac28ffd63ed 1285 {
Jamie Smith 1:aac28ffd63ed 1286 // clear this so future calls won't try to use the cached version
Jamie Smith 1:aac28ffd63ed 1287 bufferMetadataRecord = 0;
Jamie Smith 1:aac28ffd63ed 1288
Jamie Smith 1:aac28ffd63ed 1289 return false;
Jamie Smith 1:aac28ffd63ed 1290 }
Jamie Smith 1:aac28ffd63ed 1291
Jamie Smith 1:aac28ffd63ed 1292 bufferMetadataRecord = reportMetaRecord;
Jamie Smith 1:aac28ffd63ed 1293
Jamie Smith 1:aac28ffd63ed 1294 return true;
Jamie Smith 1:aac28ffd63ed 1295 }
Jamie Smith 8:199c7fad233d 1296
Jamie Smith 8:199c7fad233d 1297 BNO080I2C::BNO080I2C(Stream *debugPort, PinName user_SDApin, PinName user_SCLpin, PinName user_INTPin, PinName user_RSTPin,
Jamie Smith 8:199c7fad233d 1298 uint8_t i2cAddress, int i2cPortSpeed) :
Jamie Smith 8:199c7fad233d 1299 BNO080Base(debugPort, user_INTPin, user_RSTPin),
Jamie Smith 8:199c7fad233d 1300 _i2cPort(user_SDApin, user_SCLpin),
Jamie Smith 8:199c7fad233d 1301 _i2cAddress(i2cAddress)
Jamie Smith 8:199c7fad233d 1302 {
Jamie Smith 8:199c7fad233d 1303
Jamie Smith 8:199c7fad233d 1304 //Get user settings
Jamie Smith 8:199c7fad233d 1305 _i2cPortSpeed = i2cPortSpeed;
Jamie Smith 8:199c7fad233d 1306 if(_i2cPortSpeed > 4000000)
Jamie Smith 8:199c7fad233d 1307 {
Jamie Smith 8:199c7fad233d 1308 _i2cPortSpeed = 4000000; //BNO080 max is 400Khz
Jamie Smith 8:199c7fad233d 1309 }
Jamie Smith 8:199c7fad233d 1310 _i2cPort.frequency(_i2cPortSpeed);
Jamie Smith 8:199c7fad233d 1311 }
Jamie Smith 8:199c7fad233d 1312
Jamie Smith 8:199c7fad233d 1313
Jamie Smith 8:199c7fad233d 1314 //Given the data packet, send the header then the data
Jamie Smith 8:199c7fad233d 1315 //Returns false if sensor does not ACK
Jamie Smith 8:199c7fad233d 1316 bool BNO080I2C::sendPacket(uint8_t channelNumber, uint8_t dataLength)
Jamie Smith 8:199c7fad233d 1317 {
Jamie Smith 8:199c7fad233d 1318
Jamie Smith 8:199c7fad233d 1319 uint16_t totalLength = dataLength + 4; //Add four bytes for the header
Jamie Smith 8:199c7fad233d 1320
Jamie Smith 8:199c7fad233d 1321 txShtpHeader[0] = totalLength & 0xFF;
Jamie Smith 8:199c7fad233d 1322 txShtpHeader[1] = totalLength >> 8;
Jamie Smith 8:199c7fad233d 1323 txShtpHeader[2] = channelNumber;
Jamie Smith 8:199c7fad233d 1324 txShtpHeader[3] = sequenceNumber[channelNumber];
Jamie Smith 8:199c7fad233d 1325
Jamie Smith 8:199c7fad233d 1326 #if BNO_DEBUG
Jamie Smith 8:199c7fad233d 1327 _debugPort->printf("Transmitting packet: ----------------\n");
Jamie Smith 8:199c7fad233d 1328 printPacket(txPacketBuffer);
Jamie Smith 8:199c7fad233d 1329 #endif
Jamie Smith 8:199c7fad233d 1330
Jamie Smith 8:199c7fad233d 1331 // send packet to IMU
Jamie Smith 8:199c7fad233d 1332 int writeResult = _i2cPort.write(_i2cAddress << 1, reinterpret_cast<char*>(txPacketBuffer), totalLength);
Jamie Smith 8:199c7fad233d 1333
Jamie Smith 8:199c7fad233d 1334 if(writeResult != 0)
Jamie Smith 8:199c7fad233d 1335 {
Jamie Smith 8:199c7fad233d 1336 _debugPort->printf("BNO I2C write failed!\n");
Jamie Smith 8:199c7fad233d 1337 return false;
Jamie Smith 8:199c7fad233d 1338 }
Jamie Smith 8:199c7fad233d 1339 return true;
Jamie Smith 8:199c7fad233d 1340 }
Jamie Smith 8:199c7fad233d 1341
Jamie Smith 8:199c7fad233d 1342 //Check to see if there is any new data available
Jamie Smith 8:199c7fad233d 1343 //Read the contents of the incoming packet into the shtpData array
Jamie Smith 8:199c7fad233d 1344 bool BNO080I2C::receivePacket(std::chrono::milliseconds timeout)
Jamie Smith 8:199c7fad233d 1345 {
Jamie Smith 8:199c7fad233d 1346 Timer waitStartTime;
Jamie Smith 8:199c7fad233d 1347 waitStartTime.start();
Jamie Smith 8:199c7fad233d 1348
Jamie Smith 8:199c7fad233d 1349 while(_int.read() != 0)
Jamie Smith 8:199c7fad233d 1350 {
Jamie Smith 8:199c7fad233d 1351 if(waitStartTime.elapsed_time() > timeout)
Jamie Smith 8:199c7fad233d 1352 {
Jamie Smith 8:199c7fad233d 1353 _debugPort->printf("BNO I2C header wait timeout\n");
Jamie Smith 8:199c7fad233d 1354 return false;
Jamie Smith 8:199c7fad233d 1355 }
Jamie Smith 8:199c7fad233d 1356
Jamie Smith 8:199c7fad233d 1357 }
Jamie Smith 8:199c7fad233d 1358
Jamie Smith 8:199c7fad233d 1359 // start the transaction and contact the IMU
Jamie Smith 8:199c7fad233d 1360 _i2cPort.start();
Jamie Smith 8:199c7fad233d 1361
Jamie Smith 8:199c7fad233d 1362 // to indicate an i2c read, shift the 7 bit address up 1 bit and set bit 0 to a 1
Jamie Smith 8:199c7fad233d 1363 int writeResult = _i2cPort.write((_i2cAddress << 1) | 0x1);
Jamie Smith 8:199c7fad233d 1364
Jamie Smith 8:199c7fad233d 1365 if(writeResult != 1)
Jamie Smith 8:199c7fad233d 1366 {
Jamie Smith 8:199c7fad233d 1367 _debugPort->printf("BNO I2C read failed!\n");
Jamie Smith 8:199c7fad233d 1368 return false;
Jamie Smith 8:199c7fad233d 1369 }
Jamie Smith 8:199c7fad233d 1370
Jamie Smith 8:199c7fad233d 1371 //Get the first four bytes, aka the packet header
Jamie Smith 8:199c7fad233d 1372 uint8_t packetLSB = static_cast<uint8_t>(_i2cPort.read(true));
Jamie Smith 8:199c7fad233d 1373 uint8_t packetMSB = static_cast<uint8_t>(_i2cPort.read(true));
Jamie Smith 8:199c7fad233d 1374 uint8_t channelNumber = static_cast<uint8_t>(_i2cPort.read(true));
Jamie Smith 8:199c7fad233d 1375 uint8_t sequenceNum = static_cast<uint8_t>(_i2cPort.read(true)); //Not sure if we need to store this or not
Jamie Smith 8:199c7fad233d 1376
Jamie Smith 8:199c7fad233d 1377 //Store the header info
Jamie Smith 8:199c7fad233d 1378 rxShtpHeader[0] = packetLSB;
Jamie Smith 8:199c7fad233d 1379 rxShtpHeader[1] = packetMSB;
Jamie Smith 8:199c7fad233d 1380 rxShtpHeader[2] = channelNumber;
Jamie Smith 8:199c7fad233d 1381 rxShtpHeader[3] = sequenceNum;
Jamie Smith 8:199c7fad233d 1382
Jamie Smith 8:199c7fad233d 1383 if(rxShtpHeader[0] == 0xFF && rxShtpHeader[1] == 0xFF)
Jamie Smith 8:199c7fad233d 1384 {
Jamie Smith 8:199c7fad233d 1385 // invalid according to BNO080 datasheet section 1.4.1
Jamie Smith 8:199c7fad233d 1386
Jamie Smith 8:199c7fad233d 1387 #if BNO_DEBUG
Jamie Smith 8:199c7fad233d 1388 _debugPort->printf("Recieved 0xFFFF packet length, protocol error!\n");
Jamie Smith 8:199c7fad233d 1389 #endif
Jamie Smith 8:199c7fad233d 1390 return false;
Jamie Smith 8:199c7fad233d 1391 }
Jamie Smith 8:199c7fad233d 1392
Jamie Smith 8:199c7fad233d 1393 //Calculate the number of data bytes in this packet
Jamie Smith 8:199c7fad233d 1394 rxPacketLength = (static_cast<uint16_t>(packetMSB) << 8 | packetLSB);
Jamie Smith 8:199c7fad233d 1395
Jamie Smith 8:199c7fad233d 1396 // Clear the MSbit.
Jamie Smith 8:199c7fad233d 1397 // This bit indicates if this package is a continuation of the last. TBH, I don't really know what this means (it's not really explained in the datasheet)
Jamie Smith 8:199c7fad233d 1398 // but we don't actually care about any of the advertisement packets
Jamie Smith 8:199c7fad233d 1399 // that use this, so we can just cut off the rest of the packet by releasing chip select.
Jamie Smith 8:199c7fad233d 1400 rxPacketLength &= ~(1 << 15);
Jamie Smith 8:199c7fad233d 1401
Jamie Smith 8:199c7fad233d 1402 if (rxPacketLength == 0)
Jamie Smith 8:199c7fad233d 1403 {
Jamie Smith 8:199c7fad233d 1404 // Packet is empty
Jamie Smith 8:199c7fad233d 1405 return (false); //All done
Jamie Smith 8:199c7fad233d 1406 }
Jamie Smith 8:199c7fad233d 1407
Jamie Smith 8:199c7fad233d 1408 rxPacketLength -= 4; //Remove the header bytes from the data count
Jamie Smith 8:199c7fad233d 1409
Jamie Smith 8:199c7fad233d 1410 //Read incoming data into the shtpData array
Jamie Smith 8:199c7fad233d 1411 for (uint16_t dataSpot = 0 ; dataSpot < rxPacketLength ; dataSpot++)
Jamie Smith 8:199c7fad233d 1412 {
Jamie Smith 8:199c7fad233d 1413 bool sendACK = dataSpot < rxPacketLength - 1;
Jamie Smith 8:199c7fad233d 1414
Jamie Smith 8:199c7fad233d 1415 // per the datasheet, 0xFF is used as filler for the receiver to transmit back
Jamie Smith 8:199c7fad233d 1416 uint8_t incoming = static_cast<uint8_t>(_i2cPort.read(sendACK));
Jamie Smith 8:199c7fad233d 1417 if (dataSpot < (sizeof(rxPacketBuffer) - SHTP_HEADER_SIZE)) //BNO080 can respond with upto 270 bytes, avoid overflow
Jamie Smith 8:199c7fad233d 1418 rxShtpData[dataSpot] = incoming; //Store data into the shtpData array
Jamie Smith 8:199c7fad233d 1419 }
Jamie Smith 8:199c7fad233d 1420
Jamie Smith 8:199c7fad233d 1421 _i2cPort.stop();
Jamie Smith 8:199c7fad233d 1422
Jamie Smith 8:199c7fad233d 1423 /*
Jamie Smith 8:199c7fad233d 1424 // first read the header to get the length of the packet
Jamie Smith 8:199c7fad233d 1425 int i2cResult;
Jamie Smith 8:199c7fad233d 1426 i2cResult = _i2cPort.read(_i2cAddress << 1, reinterpret_cast<char*>(packetBuffer), SHTP_HEADER_SIZE, false);
Jamie Smith 8:199c7fad233d 1427 if(i2cResult != 0)
Jamie Smith 8:199c7fad233d 1428 {
Jamie Smith 8:199c7fad233d 1429 _debugPort->printf("BNO I2C length read failed!\n");
Jamie Smith 8:199c7fad233d 1430 return false;
Jamie Smith 8:199c7fad233d 1431 }
Jamie Smith 8:199c7fad233d 1432
Jamie Smith 8:199c7fad233d 1433 if(shtpHeader[0] == 0xFF && shtpHeader[1] == 0xFF)
Jamie Smith 8:199c7fad233d 1434 {
Jamie Smith 8:199c7fad233d 1435 // invalid according to BNO080 datasheet section 1.4.1
Jamie Smith 8:199c7fad233d 1436 #if BNO_DEBUG
Jamie Smith 8:199c7fad233d 1437 _debugPort->printf("Recieved 0xFFFF packet length, protocol error!\n");
Jamie Smith 8:199c7fad233d 1438 #endif
Jamie Smith 8:199c7fad233d 1439 return false;
Jamie Smith 8:199c7fad233d 1440 }
Jamie Smith 8:199c7fad233d 1441
Jamie Smith 8:199c7fad233d 1442 //Calculate the number of data bytes in this packet
Jamie Smith 8:199c7fad233d 1443 uint16_t totalLength = (static_cast<uint16_t>(shtpHeader[1]) << 8 | shtpHeader[0]);
Jamie Smith 8:199c7fad233d 1444
Jamie Smith 8:199c7fad233d 1445 // Clear the MSbit.
Jamie Smith 8:199c7fad233d 1446 // This bit indicates if this package is a continuation of the last. TBH, I don't really know what this means (it's not really explained in the datasheet)
Jamie Smith 8:199c7fad233d 1447 // but we don't actually care about any of the advertisement packets
Jamie Smith 8:199c7fad233d 1448 // that use this, so we can just cut off the rest of the packet by releasing chip select.
Jamie Smith 8:199c7fad233d 1449 totalLength &= ~(1 << 15);
Jamie Smith 8:199c7fad233d 1450
Jamie Smith 8:199c7fad233d 1451 if (totalLength == 0)
Jamie Smith 8:199c7fad233d 1452 {
Jamie Smith 8:199c7fad233d 1453 // Packet is empty
Jamie Smith 8:199c7fad233d 1454 return (false); //All done
Jamie Smith 8:199c7fad233d 1455 }
Jamie Smith 8:199c7fad233d 1456
Jamie Smith 8:199c7fad233d 1457 // only receive as many bytes as we can fit
Jamie Smith 8:199c7fad233d 1458 if(totalLength > sizeof(packetBuffer))
Jamie Smith 8:199c7fad233d 1459 {
Jamie Smith 8:199c7fad233d 1460 totalLength = sizeof(packetBuffer);
Jamie Smith 8:199c7fad233d 1461 }
Jamie Smith 8:199c7fad233d 1462
Jamie Smith 8:199c7fad233d 1463 packetLength = totalLength - 4; //Remove the header bytes from the data count
Jamie Smith 8:199c7fad233d 1464
Jamie Smith 8:199c7fad233d 1465 waitStartTime.reset();
Jamie Smith 8:199c7fad233d 1466
Jamie Smith 8:199c7fad233d 1467 while(_int.read() != 0)
Jamie Smith 8:199c7fad233d 1468 {
Jamie Smith 8:199c7fad233d 1469 if(waitStartTime.elapsed_time() > timeout)
Jamie Smith 8:199c7fad233d 1470 {
Jamie Smith 8:199c7fad233d 1471 _debugPort->printf("BNO I2C wait timeout\n");
Jamie Smith 8:199c7fad233d 1472 return false;
Jamie Smith 8:199c7fad233d 1473 }
Jamie Smith 8:199c7fad233d 1474
Jamie Smith 8:199c7fad233d 1475 }
Jamie Smith 8:199c7fad233d 1476
Jamie Smith 8:199c7fad233d 1477 //Read the actual packet bytes
Jamie Smith 8:199c7fad233d 1478 _debugPort->printf("Attempting to read %" PRIu16 " bytes\n", totalLength);
Jamie Smith 8:199c7fad233d 1479 i2cResult = _i2cPort.read(_i2cAddress << 1, reinterpret_cast<char*>(packetBuffer), totalLength);
Jamie Smith 8:199c7fad233d 1480 if(i2cResult != 0)
Jamie Smith 8:199c7fad233d 1481 {
Jamie Smith 8:199c7fad233d 1482 _debugPort->printf("BNO I2C read failed!\n");
Jamie Smith 8:199c7fad233d 1483 return false;
Jamie Smith 8:199c7fad233d 1484 }
Jamie Smith 8:199c7fad233d 1485 */
Jamie Smith 8:199c7fad233d 1486
Jamie Smith 8:199c7fad233d 1487 #if BNO_DEBUG
Jamie Smith 8:199c7fad233d 1488 _debugPort->printf("Recieved packet: ----------------\n");
Jamie Smith 8:199c7fad233d 1489 printPacket(rxPacketBuffer); // note: add 4 for the header length
Jamie Smith 8:199c7fad233d 1490 #endif
Jamie Smith 8:199c7fad233d 1491
Jamie Smith 8:199c7fad233d 1492 return (true); //We're done!
Jamie Smith 8:199c7fad233d 1493 }
Jamie Smith 8:199c7fad233d 1494
Jamie Smith 8:199c7fad233d 1495 BNO080SPI::BNO080SPI(Stream *debugPort, PinName rstPin, PinName intPin, PinName wakePin, PinName misoPin,
Jamie Smith 8:199c7fad233d 1496 PinName mosiPin, PinName sclkPin, PinName csPin, int spiSpeed):
Jamie Smith 8:199c7fad233d 1497 BNO080Base(debugPort, intPin, rstPin),
Jamie Smith 8:199c7fad233d 1498 _spiPort(mosiPin, misoPin, sclkPin, csPin, use_gpio_ssel),
Jamie Smith 8:199c7fad233d 1499 _wakePin(wakePin, 1), // wake pin needs to be 1 on reset so that the BNO boots up into SPI mode
Jamie Smith 8:199c7fad233d 1500 _spiSpeed(spiSpeed)
Jamie Smith 8:199c7fad233d 1501 {
Jamie Smith 8:199c7fad233d 1502 _spiPort.frequency(spiSpeed);
Jamie Smith 8:199c7fad233d 1503 _spiPort.format(8, 3);
Jamie Smith 8:199c7fad233d 1504 _spiPort.set_default_write_value(0x0);
Jamie Smith 8:199c7fad233d 1505 }
Jamie Smith 8:199c7fad233d 1506
Jamie Smith 8:199c7fad233d 1507 bool BNO080SPI::sendPacket(uint8_t channelNumber, uint8_t dataLength)
Jamie Smith 8:199c7fad233d 1508 {
Jamie Smith 8:199c7fad233d 1509 if(_int.read() == 0)
Jamie Smith 8:199c7fad233d 1510 {
Jamie Smith 8:199c7fad233d 1511 // The BNO is already awake because it has a packet it wants to send to us
Jamie Smith 8:199c7fad233d 1512 }
Jamie Smith 8:199c7fad233d 1513 else
Jamie Smith 8:199c7fad233d 1514 {
Jamie Smith 8:199c7fad233d 1515 // assert WAKE to tell the BNO to prepare for a transfer
Jamie Smith 8:199c7fad233d 1516 _wakePin = 0;
Jamie Smith 8:199c7fad233d 1517
Jamie Smith 8:199c7fad233d 1518 Timer waitStartTime;
Jamie Smith 8:199c7fad233d 1519 waitStartTime.start();
Jamie Smith 8:199c7fad233d 1520 const std::chrono::milliseconds timeout = 10ms;
Jamie Smith 8:199c7fad233d 1521
Jamie Smith 8:199c7fad233d 1522 while(_int.read() != 0)
Jamie Smith 8:199c7fad233d 1523 {
Jamie Smith 8:199c7fad233d 1524 if(waitStartTime.elapsed_time() > timeout)
Jamie Smith 8:199c7fad233d 1525 {
Jamie Smith 8:199c7fad233d 1526 _debugPort->printf("BNO SPI wake wait timeout\n");
Jamie Smith 8:199c7fad233d 1527 _wakePin = 1;
Jamie Smith 8:199c7fad233d 1528 return false;
Jamie Smith 8:199c7fad233d 1529 }
Jamie Smith 8:199c7fad233d 1530 }
Jamie Smith 8:199c7fad233d 1531 _wakePin = 1;
Jamie Smith 8:199c7fad233d 1532 }
Jamie Smith 8:199c7fad233d 1533
Jamie Smith 8:199c7fad233d 1534 uint16_t totalLength = dataLength + 4; //Add four bytes for the header
Jamie Smith 8:199c7fad233d 1535
Jamie Smith 8:199c7fad233d 1536 txShtpHeader[0] = totalLength & 0xFF;
Jamie Smith 8:199c7fad233d 1537 txShtpHeader[1] = totalLength >> 8;
Jamie Smith 8:199c7fad233d 1538 txShtpHeader[2] = channelNumber;
Jamie Smith 8:199c7fad233d 1539 txShtpHeader[3] = sequenceNumber[channelNumber];
Jamie Smith 8:199c7fad233d 1540
Jamie Smith 8:199c7fad233d 1541 #if BNO_DEBUG
Jamie Smith 8:199c7fad233d 1542 _debugPort->printf("Transmitting packet: ----------------\n");
Jamie Smith 8:199c7fad233d 1543 printPacket(txPacketBuffer);
Jamie Smith 8:199c7fad233d 1544 #endif
Jamie Smith 8:199c7fad233d 1545
Jamie Smith 8:199c7fad233d 1546 // wipe out any existing RX packet header so we know if we received a packet
Jamie Smith 8:199c7fad233d 1547 memset(rxPacketBuffer, 0, SHTP_HEADER_SIZE);
Jamie Smith 8:199c7fad233d 1548
Jamie Smith 8:199c7fad233d 1549 // send packet to IMU.
Jamie Smith 8:199c7fad233d 1550 // This also might receive the first part of another packet, which there is no way to avoid.
Jamie Smith 9:430f5302f9e1 1551 spiTransferAndWait(txPacketBuffer, totalLength, rxPacketBuffer, totalLength);
Jamie Smith 8:199c7fad233d 1552
Jamie Smith 8:199c7fad233d 1553 if(rxShtpHeader[0] == 0 && rxShtpHeader[0] == 0)
Jamie Smith 8:199c7fad233d 1554 {
Jamie Smith 8:199c7fad233d 1555 // no header data so no packet received
Jamie Smith 8:199c7fad233d 1556 return true;
Jamie Smith 8:199c7fad233d 1557 }
Jamie Smith 8:199c7fad233d 1558 else
Jamie Smith 8:199c7fad233d 1559 {
Jamie Smith 8:199c7fad233d 1560 // received first part of data packet while writing
Jamie Smith 8:199c7fad233d 1561 if(receiveCompletePacket(totalLength))
Jamie Smith 8:199c7fad233d 1562 {
Jamie Smith 8:199c7fad233d 1563 // received data packet, send to proper channels
Jamie Smith 8:199c7fad233d 1564 processPacket();
Jamie Smith 8:199c7fad233d 1565 return true;
Jamie Smith 8:199c7fad233d 1566 }
Jamie Smith 8:199c7fad233d 1567
Jamie Smith 8:199c7fad233d 1568 // receive failed
Jamie Smith 8:199c7fad233d 1569 return false;
Jamie Smith 8:199c7fad233d 1570 }
Jamie Smith 8:199c7fad233d 1571 }
Jamie Smith 8:199c7fad233d 1572
Jamie Smith 8:199c7fad233d 1573 //Check to see if there is any new data available
Jamie Smith 8:199c7fad233d 1574 //Read the contents of the incoming packet into the shtpData array
Jamie Smith 8:199c7fad233d 1575 bool BNO080SPI::receivePacket(std::chrono::milliseconds timeout)
Jamie Smith 8:199c7fad233d 1576 {
Jamie Smith 8:199c7fad233d 1577 Timer waitStartTime;
Jamie Smith 8:199c7fad233d 1578 waitStartTime.start();
Jamie Smith 8:199c7fad233d 1579
Jamie Smith 8:199c7fad233d 1580 while (_int.read() != 0)
Jamie Smith 8:199c7fad233d 1581 {
Jamie Smith 8:199c7fad233d 1582 if (waitStartTime.elapsed_time() > timeout)
Jamie Smith 8:199c7fad233d 1583 {
Jamie Smith 8:199c7fad233d 1584 _debugPort->printf("BNO SPI header wait timeout\n");
Jamie Smith 8:199c7fad233d 1585 return false;
Jamie Smith 8:199c7fad233d 1586 }
Jamie Smith 8:199c7fad233d 1587
Jamie Smith 8:199c7fad233d 1588 }
Jamie Smith 8:199c7fad233d 1589
Jamie Smith 9:430f5302f9e1 1590 // read the header bytes first.
Jamie Smith 9:430f5302f9e1 1591 spiTransferAndWait(nullptr, 0, rxPacketBuffer, SHTP_HEADER_SIZE);
Jamie Smith 8:199c7fad233d 1592
Jamie Smith 8:199c7fad233d 1593 // now read the data
Jamie Smith 8:199c7fad233d 1594 return receiveCompletePacket(SHTP_HEADER_SIZE, timeout);
Jamie Smith 8:199c7fad233d 1595 }
Jamie Smith 8:199c7fad233d 1596
Jamie Smith 8:199c7fad233d 1597 bool BNO080SPI::receiveCompletePacket(size_t bytesRead, std::chrono::milliseconds timeout)
Jamie Smith 8:199c7fad233d 1598 {
Jamie Smith 8:199c7fad233d 1599 // Process header bytes
Jamie Smith 8:199c7fad233d 1600 // ------------------------------------------------------------------------
Jamie Smith 8:199c7fad233d 1601 if (rxShtpHeader[0] == 0xFF && rxShtpHeader[1] == 0xFF)
Jamie Smith 8:199c7fad233d 1602 {
Jamie Smith 8:199c7fad233d 1603 // invalid according to BNO080 datasheet section 1.4.1
Jamie Smith 8:199c7fad233d 1604
Jamie Smith 8:199c7fad233d 1605 #if BNO_DEBUG
Jamie Smith 8:199c7fad233d 1606 _debugPort->printf("Recieved 0xFFFF packet length, protocol error!\n");
Jamie Smith 8:199c7fad233d 1607 #endif
Jamie Smith 8:199c7fad233d 1608 return false;
Jamie Smith 8:199c7fad233d 1609 }
Jamie Smith 8:199c7fad233d 1610
Jamie Smith 8:199c7fad233d 1611 //Calculate the number of data bytes in this packet
Jamie Smith 8:199c7fad233d 1612 uint16_t totalLength = (static_cast<uint16_t>(rxShtpHeader[1]) << 8 | rxShtpHeader[0]);
Jamie Smith 8:199c7fad233d 1613
Jamie Smith 8:199c7fad233d 1614 // Clear the MSbit.
Jamie Smith 8:199c7fad233d 1615 totalLength &= ~(1 << 15);
Jamie Smith 8:199c7fad233d 1616
Jamie Smith 8:199c7fad233d 1617 if (totalLength == 0)
Jamie Smith 8:199c7fad233d 1618 {
Jamie Smith 8:199c7fad233d 1619 // Packet is empty
Jamie Smith 8:199c7fad233d 1620 return (false); //All done
Jamie Smith 8:199c7fad233d 1621 }
Jamie Smith 8:199c7fad233d 1622
Jamie Smith 8:199c7fad233d 1623 rxPacketLength = totalLength - SHTP_HEADER_SIZE;
Jamie Smith 8:199c7fad233d 1624
Jamie Smith 8:199c7fad233d 1625 if(totalLength <= bytesRead)
Jamie Smith 8:199c7fad233d 1626 {
Jamie Smith 8:199c7fad233d 1627 // the original transaction already read the completed packet! We're done.
Jamie Smith 8:199c7fad233d 1628 return true;
Jamie Smith 8:199c7fad233d 1629 }
Jamie Smith 8:199c7fad233d 1630
Jamie Smith 8:199c7fad233d 1631 // Receive data
Jamie Smith 8:199c7fad233d 1632 // ------------------------------------------------------------------------
Jamie Smith 8:199c7fad233d 1633
Jamie Smith 8:199c7fad233d 1634 // Wait for it to be ready to talk to us again.
Jamie Smith 8:199c7fad233d 1635 // Note: in my testing this takes about 200ms
Jamie Smith 8:199c7fad233d 1636 Timer waitStartTime;
Jamie Smith 8:199c7fad233d 1637 waitStartTime.start();
Jamie Smith 8:199c7fad233d 1638 while (_int.read() != 0)
Jamie Smith 8:199c7fad233d 1639 {
Jamie Smith 8:199c7fad233d 1640 if (waitStartTime.elapsed_time() > timeout)
Jamie Smith 8:199c7fad233d 1641 {
Jamie Smith 8:199c7fad233d 1642 _debugPort->printf("BNO SPI continued packet header wait timeout\n");
Jamie Smith 8:199c7fad233d 1643 return false;
Jamie Smith 8:199c7fad233d 1644 }
Jamie Smith 8:199c7fad233d 1645
Jamie Smith 8:199c7fad233d 1646 }
Jamie Smith 8:199c7fad233d 1647
Jamie Smith 8:199c7fad233d 1648 if(rxPacketLength > SHTP_RX_PACKET_SIZE)
Jamie Smith 8:199c7fad233d 1649 {
Jamie Smith 8:199c7fad233d 1650 _debugPort->printf("Packet too long (%" PRIu16 " bytes), increase SHTP_RX_PACKET_SIZE\n", rxPacketLength);
Jamie Smith 8:199c7fad233d 1651 _debugPort->printf("Packet dropped, expect subsequent driver errors.\n");
Jamie Smith 8:199c7fad233d 1652 return false;
Jamie Smith 8:199c7fad233d 1653 }
Jamie Smith 8:199c7fad233d 1654
Jamie Smith 8:199c7fad233d 1655 if(bytesRead == SHTP_HEADER_SIZE)
Jamie Smith 8:199c7fad233d 1656 {
Jamie Smith 8:199c7fad233d 1657 // just read the entire packet into the buffer
Jamie Smith 9:430f5302f9e1 1658 spiTransferAndWait(nullptr, 0, rxPacketBuffer, totalLength);
Jamie Smith 8:199c7fad233d 1659 }
Jamie Smith 8:199c7fad233d 1660 else
Jamie Smith 8:199c7fad233d 1661 {
Jamie Smith 8:199c7fad233d 1662 // we want to receive a new header, plus the remaining data bytes that haven't been read.
Jamie Smith 8:199c7fad233d 1663 size_t receiveLength = SHTP_HEADER_SIZE + (totalLength - bytesRead);
Jamie Smith 8:199c7fad233d 1664
Jamie Smith 8:199c7fad233d 1665 // read remaining bytes into the data buffer starting at the next byte
Jamie Smith 9:430f5302f9e1 1666 spiTransferAndWait(nullptr, 0, rxPacketBuffer + bytesRead, receiveLength);
Jamie Smith 8:199c7fad233d 1667
Jamie Smith 8:199c7fad233d 1668 // erase the new header we just read, leaving only the data as a contiguous block
Jamie Smith 8:199c7fad233d 1669 std::memmove(rxPacketBuffer + bytesRead, rxPacketBuffer + bytesRead + SHTP_HEADER_SIZE, receiveLength - SHTP_HEADER_SIZE);
Jamie Smith 8:199c7fad233d 1670 }
Jamie Smith 8:199c7fad233d 1671
Jamie Smith 8:199c7fad233d 1672 #if BNO_DEBUG
Jamie Smith 8:199c7fad233d 1673 _debugPort->printf("Recieved packet: ----------------\n");
Jamie Smith 8:199c7fad233d 1674 printPacket(rxPacketBuffer);
Jamie Smith 8:199c7fad233d 1675 #endif
Jamie Smith 8:199c7fad233d 1676
Jamie Smith 8:199c7fad233d 1677 //ThisThread::sleep_for(4ms);
Jamie Smith 8:199c7fad233d 1678
Jamie Smith 8:199c7fad233d 1679 // done!
Jamie Smith 8:199c7fad233d 1680 return true;
Jamie Smith 9:430f5302f9e1 1681 }
Jamie Smith 9:430f5302f9e1 1682
Jamie Smith 9:430f5302f9e1 1683 #if USE_ASYNC_SPI
Jamie Smith 9:430f5302f9e1 1684
Jamie Smith 9:430f5302f9e1 1685 void BNO080SPI::spiTransferAndWait(const uint8_t *tx_buffer, int tx_length, uint8_t *rx_buffer, int rx_length)
Jamie Smith 9:430f5302f9e1 1686 {
Jamie Smith 9:430f5302f9e1 1687 _spiPort.transfer(tx_buffer, tx_length, rx_buffer, rx_length,
Jamie Smith 9:430f5302f9e1 1688 callback(this, &BNO080SPI::onSPITransferComplete),
Jamie Smith 9:430f5302f9e1 1689 SPI_EVENT_COMPLETE | SPI_EVENT_ERROR);
Jamie Smith 9:430f5302f9e1 1690
Jamie Smith 9:430f5302f9e1 1691 uint32_t waitResult = spiCompleteFlag.wait_any(SPI_EVENT_ALL);
Jamie Smith 9:430f5302f9e1 1692 if(!(waitResult & SPI_EVENT_COMPLETE))
Jamie Smith 9:430f5302f9e1 1693 {
Jamie Smith 9:430f5302f9e1 1694 // at least let the user know the error happened...
Jamie Smith 9:430f5302f9e1 1695 _debugPort->printf("BNO Async SPI Error %" PRIu32 "\n", waitResult);
Jamie Smith 9:430f5302f9e1 1696 }
Jamie Smith 9:430f5302f9e1 1697
Jamie Smith 9:430f5302f9e1 1698 }
Jamie Smith 9:430f5302f9e1 1699
Jamie Smith 9:430f5302f9e1 1700 void BNO080SPI::onSPITransferComplete(int event)
Jamie Smith 9:430f5302f9e1 1701 {
Jamie Smith 9:430f5302f9e1 1702 spiCompleteFlag.set(event);
Jamie Smith 9:430f5302f9e1 1703 }
Jamie Smith 9:430f5302f9e1 1704
Jamie Smith 9:430f5302f9e1 1705 #endif