start the wrapper burrito
Diff: BNO080.cpp
- Revision:
- 3:197ad972fb7c
- Parent:
- 2:2269b723d16a
- Child:
- 5:7e2cd0f351b2
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