Diff: BNO080.cpp

Revision:

5:7e2cd0f351b2

Parent:

3:197ad972fb7c

Child:

6:546e4553cf6d

--- a/BNO080.cpp	Tue Jul 16 16:37:24 2019 +0000
+++ b/BNO080.cpp	Mon Jul 22 18:54:25 2019 +0000
@@ -69,7 +69,7 @@
 
 /// Set to 1 to enable debug printouts.  Should be very useful if the chip is giving you trouble.
 /// When debugging, it is recommended to use the highest possible serial baudrate so as not to interrupt the timing of operations.
-#define BNO_DEBUG 0
+#define BNO_DEBUG 1
 
 BNO080::BNO080(Serial *debugPort, PinName user_SDApin, PinName user_SCLpin, PinName user_INTPin, PinName user_RSTPin,
 			   uint8_t i2cAddress, int i2cPortSpeed) :
@@ -77,7 +77,7 @@
 		_i2cPort(user_SDApin, user_SCLpin),
 		_i2cAddress(i2cAddress),
 		_int(user_INTPin),
-		_rst(user_RSTPin, 1),
+		_rst(user_RSTPin, 1), 
 		commandSequenceNumber(0),
 		stability(UNKNOWN),
 		stepDetected(false),
@@ -142,6 +142,7 @@
 		else
 		{
 			// high and low detected
+			
 			break;
 		}
 	}
@@ -154,7 +155,7 @@
 	// host. It must not send any other data until this step is complete.
 	// We don't actually care what's in it, we're just using it as a signal to indicate that the reset is complete.
 	receivePacket();
-
+	_debugPort->printf("::begin -> exited receivePacket() function \r\n");
 	// now, after startup, the BNO will send an Unsolicited Initialize response (SH-2 section 6.4.5.2), and an Executable Reset command
 	waitForPacket(CHANNEL_EXECUTABLE, EXECUTABLE_REPORTID_RESET);
 
@@ -163,11 +164,12 @@
 	shtpData[3] = 0;
 	sendCommand(COMMAND_INITIALIZE);
 
-
 	if(!waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_COMMAND_RESPONSE) || shtpData[2] != COMMAND_INITIALIZE || shtpData[5] != 0)
 	{
+		_debugPort->printf("%u %u \r\n ", shtpData[2], shtpData[5]);
 		_debugPort->printf("BNO080 reports initialization failed.\n");
 		__enable_irq();
+		_debugPort->printf("returning false \r\n");
 		return false;
 	}
 	else
@@ -177,6 +179,7 @@
 #endif
 	}
 
+	_debugPort->printf("outside if \r\n");
 
 	// Finally, we want to interrogate the device about its model and version.
 	zeroBuffer();
@@ -425,6 +428,7 @@
 //Sends the packet to enable the rotation vector
 void BNO080::enableReport(Report report, uint16_t timeBetweenReports)
 {
+	_debugPort->printf("begin enable/r/n");
 #if BNO_DEBUG
 	// check time is valid
 	float periodSeconds = static_cast<float>(timeBetweenReports / 1000.0);
@@ -832,17 +836,20 @@
 		{
 			if(!receivePacket(timeout))
 			{
+				_debugPort->printf("cannot receive\r\n");
 				return false;
 			}
 
 			if(channel == shtpHeader[2] && reportID == shtpData[0])
 			{
 				// found correct packet!
+				_debugPort->printf("can receive/r/n");
 				return true;
 			}
 			else
 			{
 				// other data packet, send to proper channels
+				_debugPort->printf("other/r/n");
 				processPacket();
 			}
 		}
@@ -891,16 +898,17 @@
 	shtpData[2] = command; //Command
 
 	//Caller must set these
-	/*shtpData[3] = 0; //P0
-		shtpData[4] = 0; //P1
-		shtpData[5] = 0; //P2
-		shtpData[6] = 0;
-		shtpData[7] = 0;
-		shtpData[8] = 0;
-		shtpData[9] = 0;
-		shtpData[10] = 0;
-		shtpData[11] = 0;*/
-
+	/*
+	shtpData[3] = 0; //P0
+	shtpData[4] = 0; //P1
+	shtpData[5] = 0; //P2
+	shtpData[6] = 0;
+	shtpData[7] = 0;
+	shtpData[8] = 0;
+	shtpData[9] = 0;
+	shtpData[10] = 0;
+	shtpData[11] = 0;
+	*/
 	//Transmit packet on channel 2, 12 bytes
 	sendPacket(CHANNEL_CONTROL, 12);
 }
@@ -1246,12 +1254,13 @@
 {
 	Timer waitStartTime;
 	waitStartTime.start();
-
+	_debugPort->printf("::receivePacket -> timer started and entering while after _int.read() \r\n");
 	while(_int.read() != 0)
 	{
+		_debugPort->printf("::receivePacket -> _int.read was non-zero \r\n");
 		if(waitStartTime.read() > timeout)
 		{
-			_debugPort->printf("BNO I2C wait timeout\n");
+			_debugPort->printf("BNO I2C wait timeout\r\n");
 			return false;
 		}
 
@@ -1262,7 +1271,7 @@
 
 	// to indicate an i2c read, shift the 7 bit address up 1 bit and set bit 0 to a 1
 	int writeResult = _i2cPort.write((_i2cAddress << 1) | 0x1);
-
+	_debugPort->printf("::recievePacket -> i2cPort started and writeResult = %d \r\n", writeResult);
 	if(writeResult != 1)
 	{
 		_debugPort->printf("BNO I2C read failed!\n");
@@ -1280,7 +1289,10 @@
 	shtpHeader[1] = packetMSB;
 	shtpHeader[2] = channelNumber;
 	shtpHeader[3] = sequenceNum;
-
+	//---------------------------added code for debugging ------------------------------------------
+	for(int i =0; i<4; i++)
+		_debugPort->printf("::receivPacket -> shtpHeader[%d] = %u \r\n", i, shtpHeader[i]);
+		
 	if(shtpHeader[0] == 0xFF && shtpHeader[1] == 0xFF)
 	{
 		// invalid according to BNO080 datasheet section 1.4.1
@@ -1318,7 +1330,7 @@
 		if (dataSpot < STORED_PACKET_SIZE) //BNO080 can respond with upto 270 bytes, avoid overflow
 			shtpData[dataSpot] = incoming; //Store data into the shtpData array
 	}
-
+	_debugPort->printf("in between \r\n");
 	_i2cPort.stop();
 
 #if BNO_DEBUG