Diff: BNO080.cpp

Revision:

3:197ad972fb7c

Parent:

2:2269b723d16a

Child:

5:7e2cd0f351b2

--- 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