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

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

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

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