Added BNO080Wheelchair.h
Dependents: BNO080_program wheelchaircontrol8 Version1-9 BNO080_program
Diff: BNO080.cpp
- Revision:
- 5:7e2cd0f351b2
- Parent:
- 3:197ad972fb7c
- Child:
- 6:546e4553cf6d
diff -r 8e016f874c42 -r 7e2cd0f351b2 BNO080.cpp --- 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