Sheila Pham
start the wrapper burrito
Diff: BNO080.h
- Revision:
- 0:f677e13975d0
- Child:
- 1:aac28ffd63ed
--- /dev/null Thu Jan 01 00:00:00 1970 +0000
+++ b/BNO080.h Sun Dec 23 05:23:21 2018 +0000
@@ -0,0 +1,453 @@
+/*
+ * This is USC RPL's ARM MBed BNO080 IMU driver, by Jamie Smith.
+ *
+ * It is based on SparkFun and Nathan Seidle's Arduino driver for this chip, but is substantially rewritten and adapted.
+ * It also supports some extra features, such as setting the mounting orientation and
+ * enabling some additional data reports.
+ *
+ * This driver uses no dynamic allocation, but does allocate a couple hundred bytes of class variables as buffers.
+ * This should allow you to monitor its memory usage using MBed's size printout.
+ *
+ * The BNO080 is a very complex chip; it's capable of monitoring and controlling other sensors and making
+ * intelligent decisions and calculations using its data. Accordingly, the protocol for communicating with it
+ * is quite complex, and it took me quite a while to wrap my head around it. If you need to modify or debug
+ * this driver, look at the CPP file for an overview of the chip's communication protocol.
+ *
+ * Note: this driver only supports I2C. I attempted to create an SPI version, but as far as I can tell,
+ * the BNO's SPI interface has a bug that causes you to be unable to wake the chip from sleep in some conditions.
+ * Until this is fixed, SPI on it is virtually unusable.
+ */
+
+#ifndef HAMSTER_BNO080_H
+#define HAMSTER_BNO080_H
+
+#include <mbed.h>
+#include <quaternion.h>
+
+#include "BNO080Constants.h"
+
+class BNO080
+{
+ /**
+ * Serial stream to print debug info to. Used for errors, and debugging output if debugging is enabled.
+ */
+ Serial * _debugPort;
+
+ /**
+ * I2C port object
+ */
+ I2C _i2cPort;
+
+ /// user defined port speed
+ int _i2cPortSpeed;
+
+ /// i2c address of IMU (7 bits)
+ uint8_t _i2cAddress;
+
+ DigitalIn _int;
+ DigitalOut _rst;
+
+ DigitalOut _scope;
+
+ // packet storage
+ //-----------------------------------------------------------------------------------------------------------------
+
+#define SHTP_HEADER_SIZE 4
+#define STORED_PACKET_SIZE 128
+
+ /// Each packet has a header of 4 uint8_ts
+ uint8_t shtpHeader[SHTP_HEADER_SIZE];
+
+ /// Stores data contained in each packet. Packets can contain an arbitrary amount of data, but we shouldn't need to read more than a few hundred bytes of them.
+ /// The only long packets we actually care about are batched sensor data packets, and with how this driver handles batching, we *should* only have to deal
+ /// with at most 9 reports at a time = ~90 bytes + a few bytes of padding
+ uint8_t shtpData[STORED_PACKET_SIZE];
+
+ /// Length of packet that was received into buffer. Does NOT include header bytes.
+ uint16_t packetLength;
+
+ /// There are 6 com channels. Each channel has its own seqnum
+ uint8_t sequenceNumber[6] = {0, 0, 0, 0, 0, 0};
+
+ /// Commands have a seqNum as well. These are inside command packet, the header uses its own seqNum per channel
+ uint8_t commandSequenceNumber = 0;
+
+ // data storage
+ //-----------------------------------------------------------------------------------------------------------------
+
+ // 1 larger than the largest sensor report ID
+#define STATUS_ARRAY_LEN 0x1A
+
+ /// stores status of each sensor, indexed by report ID
+ uint8_t reportStatus[STATUS_ARRAY_LEN] = {};
+
+public:
+
+ /// List of all sensor reports that the IMU supports.
+ enum class Report : uint8_t
+ {
+ /**
+ * Total acceleration of the IMU in world space.
+ * See BNO datasheet section 2.1.1
+ */
+ TOTAL_ACCELERATION = SENSOR_REPORTID_ACCELEROMETER,
+
+ /**
+ * Acceleration of the IMU not including the acceleration of gravity.
+ * See BNO datasheet section 2.1.1
+ */
+ LINEAR_ACCELERATION = SENSOR_REPORTID_LINEAR_ACCELERATION,
+
+ /**
+ * Acceleration of gravity felt by the IMU.
+ * See BNO datasheet section 2.1.1
+ */
+ GRAVITY_ACCELERATION = SENSOR_REPORTID_GRAVITY,
+
+ /**
+ * (calibrated) gyroscope reading of the rotational speed of the IMU.
+ * See BNO datasheet section 2.1.2
+ */
+ GYROSCOPE = SENSOR_REPORTID_GYROSCOPE_CALIBRATED,
+
+ /**
+ * (calibrated) reading of magnetic field levels.
+ * See BNO datasheet section 2.1.3
+ */
+ MAG_FIELD = SENSOR_REPORTID_MAGNETIC_FIELD_CALIBRATED,
+
+ /**
+ * Fused reading of the IMU's rotation in space using all three sensors. This is the most accurate reading
+ * of absolute orientation that the IMU can provide.
+ * See BNO datasheet section 2.2.4
+ */
+ ROTATION = SENSOR_REPORTID_ROTATION_VECTOR,
+
+ /**
+ * Fused reading of rotation from accelerometer and magnetometer readings. This report is designed to decrease
+ * power consumption (by turning off the gyroscope) in exchange for reduced responsiveness.
+ */
+ GEOMAGNETIC_ROTATION = SENSOR_REPORTID_GEOMAGNETIC_ROTATION_VECTOR,
+
+ /**
+ * Fused reading of the IMU's rotation in space. Unlike the regular rotation vector, the Game Rotation Vector
+ * is not referenced against the magnetic field and the "zero yaw" point is arbitrary.
+ * See BNO datasheet section 2.2.2
+ */
+ GAME_ROTATION = SENSOR_REPORTID_GAME_ROTATION_VECTOR,
+
+ /**
+ * Detects a user tapping on the device containing the IMU.
+ * See BNO datasheet section 2.4.2
+ */
+ TAP_DETECTOR = SENSOR_REPORTID_TAP_DETECTOR,
+
+ /**
+ * Detects whether the device is on a table, being held stably, or being moved.
+ * See BNO datasheet section 2.4.1
+ */
+ STABILITY_CLASSIFIER = SENSOR_REPORTID_STABILITY_CLASSIFIER,
+
+ /**
+ * Detects a user taking a step with the IMU worn on their person.
+ * See BNO datasheet section 2.4.3
+ */
+ STEP_DETECTOR = SENSOR_REPORTID_STEP_DETECTOR,
+
+ /**
+ * Detects how many steps a user has taken.
+ * See BNO datasheet section 2.4.4
+ */
+ STEP_COUNTER = SENSOR_REPORTID_STEP_COUNTER,
+
+ /**
+ * Detects when the IMU has made a "significant" motion, defined as moving a few steps and/or accelerating significantly.
+ * See BNO datasheet section 2.4.6
+ */
+ SIGNIFICANT_MOTION = SENSOR_REPORTID_SIGNIFICANT_MOTION,
+
+ /**
+ * Detects when the IMU is being shaken.
+ * See BNO datasheet section 2.4.7
+ */
+ SHAKE_DETECTOR = SENSOR_REPORTID_SHAKE_DETECTOR
+ };
+
+ // data variables to read reports from
+ //-----------------------------------------------------------------------------------------------------------------
+
+ // @{
+ /// Version info read from the IMU when it starts up
+ uint8_t majorSoftwareVersion;
+ uint8_t minorSoftwareVersion;
+ uint16_t patchSoftwareVersion;
+ uint32_t partNumber;
+ uint32_t buildNumber;
+ // @}
+
+
+ /**
+ * Readout from Accleration report.
+ * Represents total acceleration in m/s^2 felt by the BNO's accelerometer.
+ */
+ TVector3 totalAcceleration;
+
+ /**
+ * Readout from Linear Acceleration report.
+ * Represents acceleration felt in m/s^2 by the BNO's accelerometer not including the force of gravity.
+ */
+ TVector3 linearAcceleration;
+
+ /**
+ * Readout from Gravity report.
+ * Represents the force of gravity in m/s^2 felt by the BNO's accelerometer.
+ */
+ TVector3 gravityAcceleration;
+
+ /**
+ * Readout from Calibrated Gyroscope report
+ * Represents the angular velocities of the chip in rad/s in the X, Y, and Z axes
+ */
+ TVector3 gyroRotation;
+
+ /**
+ * Readout from the Magnetic Field Calibrated report.
+ * Represents the magnetic field read by the chip in uT in the X, Y, and Z axes
+ */
+ TVector3 magField;
+
+ /**
+ * Readout from the Rotation Vector report.
+ * Represents the rotation of the IMU (relative to magnetic north) in radians.
+ */
+ Quaternion rotationVector;
+
+ /**
+ * Auxillary accuracy readout from the Rotation Vector report.
+ * Represents the estimated accuracy of the rotation vector in radians.
+ */
+ float rotationAccuracy;
+
+ /**
+ * Readout from the Game Rotation Vector report.
+ * Represents the rotation of the IMU in radians. Unlike the regular rotation vector, the Game Rotation Vector
+ * is not referenced against the magnetic field and the "zero yaw" point is arbitrary.
+ */
+ Quaternion gameRotationVector;
+
+ /**
+ * Readout from the Geomagnetic Rotation Vector report.
+ * Represents the geomagnetic rotation of the IMU (relative to magnetic north) in radians.
+ */
+ Quaternion geomagneticRotationVector;
+
+ /**
+ * Auxillary accuracy readout from the Geomagnetic Rotation Vector report.
+ * Represents the estimated accuracy of the rotation vector in radians.
+ */
+ float geomagneticRotationAccuracy;
+
+ /**
+ * Tap readout from the Tap Detector report. This flag is set to true whenever a tap is detected, and you should
+ * manually clear it when you have processed the tap.
+ */
+ bool tapDetected;
+
+ /**
+ * Whether the last tap detected was a single or double tap.
+ */
+ bool doubleTap;
+
+
+ // Management functions
+ //-----------------------------------------------------------------------------------------------------------------
+
+ /**
+ * Construct a BNO080, providing pins and parameters.
+ *
+ * NOTE: while some schematics tell you to connect the BOOTN and WAKEN pins to the processor, this driver does not use or require them.
+ * Just tie them both to VCC per the datasheet.
+ *
+ * @param debugPort Serial port to write output to. Cannot be nullptr.
+ * @param user_SDApin Hardware SPI MOSI pin
+ * @param user_SCLpin Hardware SPI MISO pin
+ * @param user_SCLKPin Hardware SPI SCLK pin
+ * @param user_CSPin SPI CS pin. Can be any IO pin, no restrictions.
+ * @param user_INTPin Input pin connected to HINTN
+ * @param user_RSTPin Output pin connected to NRST
+ * @param i2cPortSpeed SPI frequency. The BNO's max is 3Mhz, we default to 300Khz for safety.
+ */
+ BNO080(Serial *debugPort, PinName user_SDApin, PinName user_SCLpin, PinName user_INTPin, PinName user_RSTPin,
+ uint8_t i2cAddress=0x4a, int i2cPortSpeed=400000);
+
+ /**
+ * Resets and connects to the IMU.
+ *
+ * If this function is failing, it would be a good idea to turn on BNO_DEBUG in the cpp file to get detailed output
+ *
+ * @return whether or not initialization was successful
+ */
+ bool begin();
+
+ /**
+ * Tells the IMU to use its current rotation vector as the "zero" rotation vector and to reorient
+ * all outputs accordingly.
+ *
+ * @param zOnly If true, only the rotation about the Z axis (the heading) will be tared.
+ */
+ void tare(bool zOnly = false);
+
+ /**
+ * Tells the IMU to begin a dynamic sensor calibration. To calibrate the IMU, call this function and move
+ * the IMU according to the instructions in the "BNO080 Sensor Calibration Procedure" app note
+ * (http://www.hillcrestlabs.com/download/59de9014566d0727bd002ae7).
+ *
+ * To tell when the calibration is complete, look at the status bits for Game Rotation Vector (for accel and gyro)
+ * and Magnetic Field (for the magnetometer).
+ *
+ * The gyro and accelerometer should only need to be calibrated once, but the magnetometer will need to be recalibrated
+ * every time the orientation of ferrous metals around the IMU changes (e.g. when it is put into a new enclosure).
+ *
+ * The new calibration will not be saved in flash until you call saveCalibration().
+ *
+ * @param calibrateAccel Whether to calibrate the accelerometer.
+ * @param calibrateGyro Whether to calibrate the gyro.
+ * @param calibrateMag Whether to calibrate the magnetometer.
+ */
+ void startCalibration(bool calibrateAccel, bool calibrateGyro, bool calibrateMag);
+
+ /**
+ * Saves the calibration started with startCalibration() and ends the calibration.
+ * You will want to call this once the status bits read as "accuracy high".
+ *
+ * WARNING: if you paid for a factory calibrated IMU, then this WILL OVERWRITE THE FACTORY CALIBRATION in whatever sensors
+ * are being calibrated. Use with caution!
+ */
+ void saveCalibration();
+
+ // Report functions
+ //-----------------------------------------------------------------------------------------------------------------
+
+ /**
+ * Checks for new data packets queued on the IMU.
+ * If there are packets queued, receives all of them and updates
+ * the class variables with the results.
+ *
+ * @return true iff new data was received
+ */
+ bool updateData();
+
+
+ /**
+ * Gets the status of a report as a 2 bit number.
+ * per SH-2 section 6.5.1, this is interpreted as: <br>
+ * 0 - unreliable <br>
+ * 1 - accuracy low <br>
+ * 2 - accuracy medium <br>
+ * 3 - accuracy high <br>
+ * of course, these are only updated if a given report is enabled.
+ * @param report
+ * @return
+ */
+ uint8_t getReportStatus(Report report);
+
+
+ /**
+ * Enable a data report from the IMU. Look at the comments above to see what the reports do.
+ *
+ * @param timeBetweenReports time in milliseconds between data updates.
+ */
+ void enableReport(Report report, uint16_t timeBetweenReports);
+
+private:
+
+ // internal utility functions
+ //-----------------------------------------------------------------------------------------------------------------
+
+ /**
+ * Processes the packet currently stored in the buffer, and updates class variables to reflect the data it contains
+ */
+ void processPacket();
+
+ /**
+ * Processes the sensor data packet currently stored in the buffer.
+ * Only called from processPacket()
+ */
+ void parseSensorDataPacket();
+
+ /**
+ * Call to wait for a packet with the given parameters to come in.
+ *
+ * @param channel Channel of the packet
+ * @param reportID Report ID (first data byte) of the packet
+ * @param timeout how long to wait for the packet
+ * @return true if the packet has been received, false if it timed out
+ */
+ bool waitForPacket(int channel, uint8_t reportID, float timeout = .125f);
+
+ /**
+ * Given a Q value, converts fixed point floating to regular floating point number.
+ * @param fixedPointValue
+ * @param qPoint
+ * @return
+ */
+ float qToFloat(int16_t fixedPointValue, uint8_t qPoint);
+
+ /**
+ * Given a floating point value and a Q point, convert to Q
+ * See https://en.wikipedia.org/wiki/Q_(number_format)
+ * @param qFloat
+ * @param qPoint
+ * @return
+ */
+ int16_t floatToQ(float qFloat, uint8_t qPoint);
+
+ /**
+ * Tell the sensor to do a command.
+ * See SH-2 Reference Manual section 6.3.8 page 42, Command request
+ * The caller is expected to set shtpData 3 though 11 prior to calling
+ */
+ void sendCommand(uint8_t command);
+
+ /**
+ * Given a sensor's report ID, this tells the BNO080 to begin reporting the values.
+ *
+ * @param reportID
+ * @param timeBetweenReports
+ * @param specificConfig the specific config word. Useful for personal activity classifier.
+ */
+ void setFeatureCommand(uint8_t reportID, uint16_t timeBetweenReports, uint32_t specificConfig = 0);
+
+ /**
+ * Reads a packet from the IMU and stores it in the class variables.
+ *
+ * @param timeout how long to wait for there to be a packet
+ *
+ * @return whether a packet was recieved.
+ */
+ bool receivePacket(float timeout=.2f);
+
+ /**
+ * Sends the current shtpData contents to the BNO. It's a good idea to disable interrupts before you call this.
+ *
+ * @param channelNumber the channel to send on
+ * @param dataLength How many bits of shtpData to send
+ * @return
+ */
+ bool sendPacket(uint8_t channelNumber, uint8_t dataLength);
+
+ /**
+ * Prints the current shtp packet stored in the buffer.
+ * @param length
+ */
+ void printPacket();
+
+ /**
+ * Erases the current SHTP packet buffer so new data can be written
+ */
+ void zeroBuffer();
+
+};
+
+
+#endif //HAMSTER_BNO080_H