The library needs to be tested once we get the IMU
Revision 3:197ad972fb7c, committed 2019-06-14
- Comitter:
- Jamie Smith
- Date:
- Fri Jun 14 20:31:37 2019 -0700
- Parent:
- 2:2269b723d16a
- Commit message:
- Add permanent orientation support, fix a few bugs
Changed in this revision
diff -r 2269b723d16a -r 197ad972fb7c BNO080.cpp --- a/BNO080.cpp Sat Dec 29 04:09:34 2018 -0800 +++ b/BNO080.cpp Fri Jun 14 20:31:37 2019 -0700 @@ -53,7 +53,7 @@ * * 3 -> Sensor Reports * -- Used for sensors to send back data reports. - * -- AFAIK the only report ID on this channel will be 0xFB (Report Base Timestamp); sensor data is send in a series of structures + * -- AFAIK the only report ID on this channel will be 0xFB (Report Base Timestamp); sensor data is sent in a series of structures * following an 0xFB * * 4 -> Wake Sensor Reports @@ -103,7 +103,7 @@ bool BNO080::begin() { - //Configure the BNO080 for SPI communication + //Configure the BNO080 for I2C communication _rst = 0; // Reset BNO080 wait(.002f); // Min length not specified in datasheet? @@ -111,6 +111,7 @@ // wait for a falling edge (NOT just a low) on the INT pin to denote startup Timer timeoutTimer; + timeoutTimer.start(); bool highDetected = false; bool lowDetected = false; @@ -145,7 +146,9 @@ } } - _debugPort->printf("BNO080 detected!\n"); +#if BNO_DEBUG + _debugPort->printf("BNO080 detected!\r\n"); +#endif // At system startup, the hub must send its full advertisement message (see SHTP 5.2 and 5.3) to the // host. It must not send any other data until this step is complete. @@ -314,16 +317,12 @@ { zeroBuffer(); - _debugPort->printf("y: %f", orientation.y()); - // convert floats to Q int16_t Q_x = floatToQ(orientation.x(), ORIENTATION_QUAT_Q_POINT); int16_t Q_y = floatToQ(orientation.y(), ORIENTATION_QUAT_Q_POINT); int16_t Q_z = floatToQ(orientation.z(), ORIENTATION_QUAT_Q_POINT); int16_t Q_w = floatToQ(orientation.w(), ORIENTATION_QUAT_Q_POINT); - _debugPort->printf("Q_y: %hd", Q_y); - shtpData[3] = 2; // set reorientation shtpData[4] = static_cast<uint8_t>(Q_x & 0xFF); //P1 - X component LSB @@ -344,6 +343,20 @@ // NOTE: unlike literally every other command, a sensor orientation command is never acknowledged in any way. } +#define ORIENTATION_RECORD_LEN 4 + +bool BNO080::setPermanentOrientation(Quaternion orientation) +{ + uint32_t orientationRecord[ORIENTATION_RECORD_LEN]; + + // each word is one element of the quaternion + orientationRecord[0] = static_cast<uint32_t>(floatToQ_dword(orientation.x(), FRS_ORIENTATION_Q_POINT)); + orientationRecord[1] = static_cast<uint32_t>(floatToQ_dword(orientation.y(), FRS_ORIENTATION_Q_POINT)); + orientationRecord[2] = static_cast<uint32_t>(floatToQ_dword(orientation.z(), FRS_ORIENTATION_Q_POINT)); + orientationRecord[3] = static_cast<uint32_t>(floatToQ_dword(orientation.w(), FRS_ORIENTATION_Q_POINT)); + + return writeFRSRecord(FRS_RECORDID_SYSTEM_ORIENTATION, orientationRecord, ORIENTATION_RECORD_LEN); +} bool BNO080::updateData() { @@ -412,23 +425,24 @@ //Sends the packet to enable the rotation vector void BNO080::enableReport(Report report, uint16_t timeBetweenReports) { - // check time - float periodSeconds = timeBetweenReports / 1000.0; +#if BNO_DEBUG + // check time is valid + float periodSeconds = static_cast<float>(timeBetweenReports / 1000.0); if(periodSeconds < getMinPeriod(report)) { - _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.\n", + _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", static_cast<uint8_t>(report), periodSeconds, getMinPeriod(report)); return; } - /* - else if(getMaxPeriod(report) > 0 && periodSeconds > getMaxPeriod(report)) - { - _debugPort->printf("Error: attempt made to set report 0x%02hhx to period of %.06f s, which is larger than its max period of %.06f s.\n", - static_cast<uint8_t>(report), periodSeconds, getMaxPeriod(report)); - return; - } - */ + + + + + + + +#endif setFeatureCommand(static_cast<uint8_t>(report), timeBetweenReports); // note: we don't wait for ACKs on these packets because they can take quite a while, like half a second, to come in @@ -591,7 +605,7 @@ { if(currReportOffset >= STORED_PACKET_SIZE) { - _debugPort->printf("Error: sensor report longer than packet buffer!\n"); + _debugPort->printf("Error: sensor report longer than packet buffer! Some data was not read! Increase buffer size or decrease number of reports!\r\n"); return; } @@ -784,6 +798,8 @@ significantMotionDetected = true; currReportOffset += SIZEOF_SIGNIFICANT_MOTION; + + break; case SENSOR_REPORTID_SHAKE_DETECTOR: @@ -795,6 +811,8 @@ currReportOffset += SIZEOF_SHAKE_DETECTOR; + break; + default: _debugPort->printf("Error: unrecognized report ID in sensor report: %hhx. Byte %u, length %hu\n", shtpData[currReportOffset], currReportOffset, packetLength); return; @@ -858,6 +876,11 @@ return qVal; } +int32_t BNO080::floatToQ_dword(float qFloat, uint16_t qPoint) +{ + int32_t qVal = static_cast<int32_t>(qFloat * pow(2, qPoint)); + return qVal; +} //Tell the sensor to do a command //See 6.3.8 page 41, Command request //The caller is expected to set P0 through P8 prior to calling @@ -934,7 +957,8 @@ size_t readOffset = 0; while(readOffset < readLength) { - if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_READ_RESPONSE)) + // it seems like it can take quite a long time for FRS data to be read, so we have to increase the timeout + if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_READ_RESPONSE, .3f)) { #if BNO_DEBUG _debugPort->printf("Error: did not receive FRS read response after sending read request!\n"); @@ -1020,6 +1044,155 @@ } +bool BNO080::writeFRSRecord(uint16_t recordID, uint32_t* buffer, uint16_t length) +{ + // send initial write request, which tells the chip where we're writing + zeroBuffer(); + + shtpData[0] = SHTP_REPORT_FRS_WRITE_REQUEST; + // length to write (must be <= record length) + shtpData[2] = static_cast<uint8_t>(length & 0xFF); + shtpData[3] = static_cast<uint8_t>(length >> 8); + // record ID + shtpData[4] = static_cast<uint8_t>(recordID & 0xFF); + shtpData[5] = static_cast<uint8_t>(recordID >> 8); + + sendPacket(CHANNEL_CONTROL, 6); + + // wait for FRS to become ready + if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_WRITE_RESPONSE, .3f)) + { +#if BNO_DEBUG + _debugPort->printf("Error: did not receive FRS write ready response after sending write request!\r\n"); +#endif + return false; + } + + if(shtpData[1] != 4) + { +#if BNO_DEBUG + _debugPort->printf("Error: FRS reports error initiating write operation: %hhu!\r\n", shtpData[1]); +#endif + return false; + } + + // now, send the actual data + for(uint16_t wordIndex = 0; wordIndex < length; wordIndex += 2) + { + // send packet containing 2 words + zeroBuffer(); + shtpData[0] = SHTP_REPORT_FRS_WRITE_DATA; + + // offset to write at + shtpData[2] = static_cast<uint8_t>(wordIndex & 0xFF); + shtpData[3] = static_cast<uint8_t>(wordIndex >> 8); + + // data 0 + *reinterpret_cast<uint32_t*>(shtpData + 4) = buffer[wordIndex]; + + // data 1, if it exists + if(wordIndex != length - 1) + { + *reinterpret_cast<uint32_t*>(shtpData + 8) = buffer[wordIndex + 1]; + } + + sendPacket(CHANNEL_CONTROL, 12); + + // wait for acknowledge + if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_FRS_WRITE_RESPONSE, .3f)) + { +#if BNO_DEBUG + _debugPort->printf("Error: did not receive FRS write response after sending write data!\r\n"); +#endif + return false; + } + + uint8_t status = shtpData[1]; + + switch(status) + { + case 0: + if(length - wordIndex >= 2) + { + // status OK, write still in progress + } + else + { +#if BNO_DEBUG + _debugPort->printf("Error: FRS reports write in progress when it should be complete!\r\n"); +#endif + return false; + } + break; + case 3: + case 8: + if(length - wordIndex <= 2) + { + // status OK, write complete + } + else + { +#if BNO_DEBUG + _debugPort->printf("Error: FRS reports write complete when it should be still going!\n"); +#endif + return false; + } + break; + case 1: +#if BNO_DEBUG + _debugPort->printf("Error: FRS reports invalid record ID!\n"); +#endif + return false; + case 2: +#if BNO_DEBUG + _debugPort->printf("Error: FRS is busy!\n"); +#endif + return false; + case 5: +#if BNO_DEBUG + _debugPort->printf("Error: FRS reports write failed!\n"); +#endif + return false; + case 6: +#if BNO_DEBUG + _debugPort->printf("Error: FRS reports data received while not in write mode!\n"); +#endif + return false; + case 7: +#if BNO_DEBUG + _debugPort->printf("Error: FRS reports invalid length!\n"); +#endif + return false; + case 9: +#if BNO_DEBUG + _debugPort->printf("Error: FRS reports invalid data for this record!\n"); +#endif + return false; + + case 10: +#if BNO_DEBUG + _debugPort->printf("Error: FRS reports flash device unavailable!\n"); +#endif + return false; + + case 11: +#if BNO_DEBUG + _debugPort->printf("Error: FRS reports record is read-only!\n"); +#endif + return false; + default: +#if BNO_DEBUG + _debugPort->printf("Error: FRS reports unknown result code %hhu!\n", status); +#endif + break; + + } + } + + // write complete + return true; +} + //Given the data packet, send the header then the data //Returns false if sensor does not ACK bool BNO080::sendPacket(uint8_t channelNumber, uint8_t dataLength) @@ -1213,7 +1386,7 @@ bool BNO080::loadReportMetadata(BNO080::Report report) { - uint16_t reportMetaRecord; + uint16_t reportMetaRecord = 0; // first, convert the report into the correct FRS record ID for that report's metadata // data from SH-2 section 5.1
diff -r 2269b723d16a -r 197ad972fb7c BNO080.h --- a/BNO080.h Sat Dec 29 04:09:34 2018 -0800 +++ b/BNO080.h Fri Jun 14 20:31:37 2019 -0700 @@ -27,7 +27,7 @@ #include "BNO080Constants.h" // useful define when working with orientation quaternions -#define SQRT_2 1.414213562 +#define SQRT_2 1.414213562f /** Class to drive the BNO080 9-axis IMU. @@ -63,8 +63,8 @@ #define SHTP_HEADER_SIZE 4 -// Arbitrarily chosen, but should hopefully be large enough for all packets we need. -// If you enable lots of sensor reports and get an error, you might need to increase this. + // Arbitrarily chosen, but should hopefully be large enough for all packets we need. + // If you enable lots of sensor reports and get an error, you might need to increase this. #define STORED_PACKET_SIZE 128 /// Each SHTP packet has a header of 4 uint8_ts @@ -397,7 +397,7 @@ PinName user_INTPin, PinName user_RSTPin, uint8_t i2cAddress=0x4a, - int i2cPortSpeed=400000); + int i2cPortSpeed=100000); /** * Resets and connects to the IMU. Verifies that it's connected, and reads out its version @@ -460,13 +460,25 @@ * NOTE: this driver provides the macro SQRT_2 to help with entering values from that table. * * NOTE 2: this setting does not persist and will have to be re-applied every time the chip is reset. - * It is possible to set an FRS record and cause it to persist, but that method is not currently - * supported by this driver, + * Use setPermanentOrientation() for that. * * @param orientation quaternion mapping from IMU space to world space. */ void setSensorOrientation(Quaternion orientation); + /** + * Sets the orientation quaternion, telling the sensor how it's mounted + * in relation to world space. See page 40 of the BNO080 datasheet. + * + * Unlike setSensorOrientation(), this setting will persist across sensor restarts. + * However, it will also take a few hundred milliseconds to write. + * + * @param orientation quaternion mapping from IMU space to world space. + * + * @return true if the operation succeeded, false if it failed. + */ + bool setPermanentOrientation(Quaternion orientation); + // Report functions //----------------------------------------------------------------------------------------------------------------- @@ -654,6 +666,18 @@ int16_t floatToQ(float qFloat, uint8_t qPoint); /** + * Given a floating point value and a Q point, convert to Q + * See https://en.wikipedia.org/wiki/Q_(number_format) + * + * This version is used for the signed 32-bit values in metadata records. + * + * @param qFloat + * @param qPoint + * @return + */ + int32_t floatToQ_dword(float qFloat, uint16_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 @@ -684,6 +708,20 @@ bool readFRSRecord(uint16_t recordID, uint32_t* readBuffer, uint16_t readLength); /** + * Write a record to the FRS (Flash Record System) on the IMU. FRS records are composed of 32-bit words, + * with the size of each record determined by the record type. + * + * Will block until the entire record has been written. + * @param recordID Record ID to write. See SH-2 figures 28 and 29 for a list of these. Sometimes also called + * the "FRS Type" by the datasheet (???). + * @param buffer Buffer to write data into. + * @param length Amount of words to write to the record. Must be <= the length of the record. + * + * @return whether the request succeeded + */ + bool writeFRSRecord(uint16_t recordID, uint32_t* buffer, uint16_t length); + + /** * 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
diff -r 2269b723d16a -r 197ad972fb7c BNO080Constants.h --- a/BNO080Constants.h Sat Dec 29 04:09:34 2018 -0800 +++ b/BNO080Constants.h Fri Jun 14 20:31:37 2019 -0700 @@ -8,7 +8,7 @@ //-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-=-= -//Registers +// Channels #define CHANNEL_COMMAND 0 #define CHANNEL_EXECUTABLE 1 #define CHANNEL_CONTROL 2 @@ -27,6 +27,9 @@ #define SHTP_REPORT_COMMAND_REQUEST 0xF2 #define SHTP_REPORT_FRS_READ_RESPONSE 0xF3 #define SHTP_REPORT_FRS_READ_REQUEST 0xF4 +#define SHTP_REPORT_FRS_WRITE_RESPONSE 0xF5 +#define SHTP_REPORT_FRS_WRITE_DATA 0xF6 +#define SHTP_REPORT_FRS_WRITE_REQUEST 0xF7 #define SHTP_REPORT_PRODUCT_ID_RESPONSE 0xF8 #define SHTP_REPORT_PRODUCT_ID_REQUEST 0xF9 #define SHTP_REPORT_BASE_TIMESTAMP 0xFB @@ -63,6 +66,7 @@ #define ROTATION_ACCURACY_Q_POINT 12 // for rotation accuracy data #define POWER_Q_POINT 10 // for power information in the metadata #define ORIENTATION_QUAT_Q_POINT 14 // for the set orientation command +#define FRS_ORIENTATION_Q_POINT 30 // for the sensor orientation FRS record // Report IDs on the Executable channel // See Figure 1-27 in the BNO080 datasheet @@ -71,13 +75,7 @@ //Record IDs from SH-2 figure 28 //These are used to read and set various configuration options #define FRS_RECORDID_SERIAL_NUMBER 0x4B4B - -//Record IDs from SH-2 figure 29 -//These are used to read the metadata for each sensor type -#define FRS_RECORDID_ACCELEROMETER 0xE302 -#define FRS_RECORDID_GYROSCOPE_CALIBRATED 0xE306 -#define FRS_RECORDID_MAGNETIC_FIELD_CALIBRATED 0xE309 -#define FRS_RECORDID_ROTATION_VECTOR 0xE30B +#define FRS_RECORDID_SYSTEM_ORIENTATION 0x2D3E //Command IDs from section 6.4, page 42 //These are used to calibrate, initialize, set orientation, tare etc the sensor @@ -101,6 +99,9 @@ // timing for reset // per my measurement, reset takes about 90ms, so let's take twice that +// By the way, I discovered (by accident) that a symptom of brownout is the chip taking +// a long time to reset. So if you had to increase this, check that your Vcc is +// within the allowed range. #define BNO080_RESET_TIMEOUT .18f #endif //HAMSTER_BNO080CONSTANTS_H