BNO080 Driver

Diff: BNO080.cpp

Revision:

9:430f5302f9e1

Parent:

8:199c7fad233d

--- a/BNO080.cpp	Wed Nov 18 18:07:27 2020 -0800
+++ b/BNO080.cpp	Tue Nov 24 15:06:05 2020 -0800
@@ -93,7 +93,7 @@
 
 bool BNO080Base::begin()
 {
-	//Configure the BNO080 for I2C communication
+	//Configure the BNO080
 
 	_rst = 0; // Reset BNO080
 	ThisThread::sleep_for(1ms); // Min length not specified in datasheet?
@@ -159,7 +159,7 @@
 	}
 
 	// Finally, we want to interrogate the device about its model and version.
-	zeroBuffer();
+	clearSendBuffer();
 	txShtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; //Request the product ID and reset info
 	txShtpData[1] = 0; //Reserved
 	sendPacket(CHANNEL_CONTROL, 2);
@@ -194,7 +194,7 @@
 
 void BNO080Base::tare(bool zOnly)
 {
-	zeroBuffer();
+	clearSendBuffer();
 
  	// from SH-2 section 6.4.4.1
 	txShtpData[3] = 0; // perform tare now
@@ -216,7 +216,7 @@
 bool BNO080Base::enableCalibration(bool calibrateAccel, bool calibrateGyro, bool calibrateMag)
 {
 	// send the Configure ME Calibration command
-	zeroBuffer();
+	clearSendBuffer();
 
 	txShtpData[3] = static_cast<uint8_t>(calibrateAccel ? 1 : 0);
 	txShtpData[4] = static_cast<uint8_t>(calibrateGyro ? 1 : 0);
@@ -259,7 +259,7 @@
 
 bool BNO080Base::saveCalibration()
 {
-	zeroBuffer();
+	clearSendBuffer();
 
 	// no arguments
 	sendCommand(COMMAND_SAVE_DCD);
@@ -295,7 +295,7 @@
 
 void BNO080Base::setSensorOrientation(Quaternion orientation)
 {
-	zeroBuffer();
+	clearSendBuffer();
 
 	// convert floats to Q
 	int16_t Q_x = floatToQ(orientation.x(), ORIENTATION_QUAT_Q_POINT);
@@ -915,7 +915,7 @@
 bool BNO080Base::readFRSRecord(uint16_t recordID, uint32_t* readBuffer, uint16_t readLength)
 {
 	// send initial read request
-	zeroBuffer();
+	clearSendBuffer();
 
 	txShtpData[0] = SHTP_REPORT_FRS_READ_REQUEST;
 	// read offset of 0 -> start at the start of the record
@@ -1024,7 +1024,7 @@
 bool BNO080Base::writeFRSRecord(uint16_t recordID, uint32_t* buffer, uint16_t length)
 {
 	// send initial write request, which tells the chip where we're writing
-	zeroBuffer();
+	clearSendBuffer();
 
 	txShtpData[0] = SHTP_REPORT_FRS_WRITE_REQUEST;
 	// length to write (must be <= record length)
@@ -1057,7 +1057,7 @@
 	for(uint16_t wordIndex = 0; wordIndex < length; wordIndex += 2)
 	{
 		// send packet containing 2 words
-		zeroBuffer();
+		clearSendBuffer();
 		txShtpData[0] = SHTP_REPORT_FRS_WRITE_DATA;
 
 		// offset to write at
@@ -1214,7 +1214,7 @@
 }
 
 
-void BNO080Base::zeroBuffer()
+void BNO080Base::clearSendBuffer()
 {
 	memset(txPacketBuffer, 0, SHTP_HEADER_SIZE + SHTP_MAX_TX_PACKET_SIZE);
 }
@@ -1548,7 +1548,7 @@
 
 	// send packet to IMU.
 	// This also might receive the first part of another packet, which there is no way to avoid.
-	_spiPort.write(reinterpret_cast<char*>(txPacketBuffer), totalLength, reinterpret_cast<char*>(rxPacketBuffer), totalLength);
+	spiTransferAndWait(txPacketBuffer, totalLength, rxPacketBuffer, totalLength);
 
 	if(rxShtpHeader[0] == 0 && rxShtpHeader[0] == 0)
 	{
@@ -1587,8 +1587,8 @@
 
 	}
 
-	// read the header bytes first
-	_spiPort.write(nullptr, 0, reinterpret_cast<char *>(rxPacketBuffer), SHTP_HEADER_SIZE);
+	// read the header bytes first.
+	spiTransferAndWait(nullptr, 0, rxPacketBuffer, SHTP_HEADER_SIZE);
 
 	// now read the data
 	return receiveCompletePacket(SHTP_HEADER_SIZE, timeout);
@@ -1655,7 +1655,7 @@
 	if(bytesRead == SHTP_HEADER_SIZE)
 	{
 		// just read the entire packet into the buffer
-		_spiPort.write(nullptr, 0, reinterpret_cast<char*>(rxPacketBuffer), totalLength);
+		spiTransferAndWait(nullptr, 0, rxPacketBuffer, totalLength);
 	}
 	else
 	{
@@ -1663,7 +1663,7 @@
 		size_t receiveLength = SHTP_HEADER_SIZE + (totalLength - bytesRead);
 
 		// read remaining bytes into the data buffer starting at the next byte
-		_spiPort.write(nullptr, 0, reinterpret_cast<char*>(rxPacketBuffer) + bytesRead, receiveLength);
+		spiTransferAndWait(nullptr, 0, rxPacketBuffer + bytesRead, receiveLength);
 
 		// erase the new header we just read, leaving only the data as a contiguous block
 		std::memmove(rxPacketBuffer + bytesRead, rxPacketBuffer + bytesRead + SHTP_HEADER_SIZE, receiveLength - SHTP_HEADER_SIZE);
@@ -1678,4 +1678,28 @@
 
 	// done!
 	return true;
-}
\ No newline at end of file
+}
+
+#if USE_ASYNC_SPI
+
+void BNO080SPI::spiTransferAndWait(const uint8_t *tx_buffer, int tx_length, uint8_t *rx_buffer, int rx_length)
+{
+	_spiPort.transfer(tx_buffer, tx_length, rx_buffer, rx_length,
+					  callback(this, &BNO080SPI::onSPITransferComplete),
+					  SPI_EVENT_COMPLETE | SPI_EVENT_ERROR);
+
+	uint32_t waitResult = spiCompleteFlag.wait_any(SPI_EVENT_ALL);
+	if(!(waitResult & SPI_EVENT_COMPLETE))
+	{
+		// at least let the user know the error happened...
+		_debugPort->printf("BNO Async SPI Error %" PRIu32 "\n", waitResult);
+	}
+
+}
+
+void BNO080SPI::onSPITransferComplete(int event)
+{
+	spiCompleteFlag.set(event);
+}
+
+#endif
\ No newline at end of file