BNO080Async.cpp

00001 //
00002 // Created by jamie on 11/18/2020.
00003 //
00004 
00005 #include "BNO080Async.h"
00006 
00007 #include <cinttypes>
00008 
00009 #define BNO_ASYNC_DEBUG 0
00010 
00011 // event flags constants for signalling the thread
00012 #define EF_INTERRUPT 0b1
00013 #define EF_SHUTDOWN 0b10
00014 #define EF_RESTART 0b100
00015 
00016 void BNO080Async::threadMain()
00017 {
00018     while(commLoop())
00019     {
00020         // loop forever
00021     }
00022 }
00023 
00024 bool BNO080Async::commLoop()
00025 {
00026     _rst = 0; // Reset BNO080
00027     ThisThread::sleep_for(1ms); // Min length not specified in datasheet?
00028     _rst = 1; // Bring out of reset
00029 
00030     // wait for a falling edge (NOT just a low) on the INT pin to denote startup
00031     {
00032         EventFlags edgeWaitFlags;
00033         _int.fall(callback([&](){edgeWaitFlags.set(1);}));
00034 
00035         // have the RTOS wait until an edge is detected or the timeout is hit
00036         uint32_t edgeWaitEvent = edgeWaitFlags.wait_any(1, (BNO080_RESET_TIMEOUT).count());
00037 
00038         if(!edgeWaitEvent)
00039         {
00040             _debugPort->printf("Error: BNO080 reset timed out, chip not detected.\n");
00041         }
00042     }
00043 
00044 #if BNO_ASYNC_DEBUG
00045     _debugPort->printf("BNO080 detected!\r\n");
00046 #endif
00047 
00048     wakeupFlags.set(EF_INTERRUPT);
00049 
00050     // configure interrupt to send the event flag
00051     _int.fall(callback([&]()
00052     {
00053         if(!inTXRX)
00054         {
00055             wakeupFlags.set(EF_INTERRUPT);
00056         }
00057     }));
00058 
00059     while(true)
00060     {
00061         uint32_t wakeupEvent = wakeupFlags.wait_any(EF_INTERRUPT | EF_SHUTDOWN | EF_RESTART);
00062 
00063         if(wakeupEvent & EF_SHUTDOWN)
00064         {
00065             // shutdown thread permanently
00066             return false;
00067         }
00068         else if(wakeupEvent & EF_RESTART)
00069         {
00070             // restart thread
00071             return true;
00072         }
00073 
00074         // lock the mutex to handle remaining cases
00075         bnoDataMutex.lock();
00076 
00077         if(wakeupEvent & EF_INTERRUPT)
00078         {
00079             while(_int == 0)
00080             {
00081                 inTXRX = true;
00082 
00083                 // send data if there is data to send.  This may also receive a packet.
00084                 if (dataToSend)
00085                 {
00086                     BNO080SPI::sendPacket(txPacketChannelNumber, txPacketDataLength);
00087                     dataToSend = false;
00088                 }
00089                 else
00090                 {
00091                     BNO080SPI::receivePacket();
00092                 }
00093 
00094                 inTXRX = false;
00095 
00096                 // clear the wake flag if it was set
00097                 _wakePin = 1;
00098 
00099                 // If the IMU wants to send us an interrupt immediately after a transaction,
00100                 // it will be missed due to the inTXRX block.  So, we need to keep checking _int
00101                 // in a loop.
00102 
00103                 // update received flag
00104                 dataReceived = true;
00105 
00106                 // check if this packet is being waited on.
00107                 if (waitingForPacket)
00108                 {
00109                     if (waitingForReportID == rxShtpData[0] && waitingForChannel == rxShtpHeader[2])
00110                     {
00111                         // unblock main thread so it can process this packet
00112                         waitingPacketArrived = true;
00113                         waitingPacketArrivedCV.notify_all();
00114 
00115                         // unlock mutex and wait until the main thread says it's OK to receive another packet
00116                         clearToRxNextPacket = false;
00117                         waitingPacketProcessedCV.wait([&]() { return clearToRxNextPacket; });
00118                     }
00119                 }
00120                 else
00121                 {
00122                     processPacket();
00123                 }
00124 
00125                 // clear the interrupt flag if it has been set because we're about to check _int anyway.
00126                 // Prevents spurious wakeups.
00127                 wakeupFlags.clear(EF_INTERRUPT);
00128 
00129             }
00130 
00131         }
00132 
00133         // unlock data mutex before waiting for flags
00134         bnoDataMutex.unlock();
00135     }
00136 }
00137 
00138 bool BNO080Async::sendPacket(uint8_t channelNumber, uint8_t dataLength)
00139 {
00140     // first make sure mutex is locked
00141     if(bnoDataMutex.get_owner() != ThisThread::get_id())
00142     {
00143         _debugPort->printf("IMU communication function called without bnoDataMutex locked!\n");
00144         return false;
00145     }
00146 
00147     // now set class variables
00148     txPacketChannelNumber = channelNumber;
00149     txPacketDataLength = dataLength;
00150     dataToSend = true;
00151 
00152     // signal thread to wake up by sending _wake signal (which will cause the IMU to interrupt us once ready)
00153     _wakePin = 0;
00154 
00155     return true;
00156 }
00157 
00158 void BNO080Async::clearSendBuffer()
00159 {
00160     // first make sure mutex is locked
00161     if(bnoDataMutex.get_owner() != ThisThread::get_id())
00162     {
00163         _debugPort->printf("IMU communication function called without bnoDataMutex locked!\n");
00164         return;
00165     }
00166 
00167     // Check if we are trying to send a packet while another packet is still queued.
00168     // Since we don't have an actual queue, just wait until the other packet is sent.
00169     while(dataToSend)
00170     {
00171         dataSentCV.wait();
00172     }
00173 
00174     // now actually erase the buffer
00175     BNO080Base::clearSendBuffer();
00176 }
00177 
00178 bool BNO080Async::waitForPacket(int channel, uint8_t reportID, std::chrono::milliseconds timeout)
00179 {
00180     // first make sure mutex is locked
00181     if(bnoDataMutex.get_owner() != ThisThread::get_id())
00182     {
00183         _debugPort->printf("IMU communication function called without bnoDataMutex locked!\n");
00184         return false;
00185     }
00186 
00187     // send information to thread
00188     waitingForPacket = true;
00189     waitingForChannel = channel;
00190     waitingForReportID = reportID;
00191     waitingPacketArrived = false;
00192 
00193     // now unlock mutex and allow thread to run and receive packets
00194     waitingPacketArrivedCV.wait_for(timeout, [&]() {return waitingPacketArrived;});
00195 
00196     if(!waitingPacketArrived)
00197     {
00198         _debugPort->printf("Packet wait timeout.\n");
00199         return false;
00200     }
00201 
00202     // packet we are waiting for is now in the buffer.
00203     waitingForPacket = false;
00204 
00205     // Now we can unblock the comms thread and allow it to run.
00206     // BUT, it can't actually start until bnoDataMutex is released.
00207     // This means that the packet data is guaranteed to stay in the buffer until that mutex is released
00208     // which will happen either when the main thread is done with the IMU, or on the next sendPacket() or
00209     // waitForPacket() call.
00210     clearToRxNextPacket = true;
00211     waitingPacketProcessedCV.notify_all();
00212 
00213     return true;
00214 }
00215 
00216 BNO080Async::BNO080Async(Stream *debugPort, PinName rstPin, PinName intPin, PinName wakePin, PinName misoPin,
00217                          PinName mosiPin, PinName sclkPin, PinName csPin, int spiSpeed, osPriority_t threadPriority):
00218  BNO080SPI(debugPort, rstPin, intPin, wakePin, misoPin, mosiPin, sclkPin, csPin, spiSpeed),
00219  commThread(threadPriority),
00220  dataSentCV(bnoDataMutex),
00221  waitingPacketArrivedCV(bnoDataMutex),
00222  waitingPacketProcessedCV(bnoDataMutex)
00223 {
00224 
00225 }
00226 
00227 
00228 bool BNO080Async::begin()
00229 {
00230     // shut down thread if it's running
00231     if(commThread.get_state() == Thread::Deleted)
00232     {
00233         // start thread for the first time
00234         commThread.start(callback(this, &BNO080Async::threadMain));
00235     }
00236     else
00237     {
00238         // restart thread
00239         wakeupFlags.set(EF_RESTART);
00240     }
00241 
00242 
00243     {
00244         ScopedLock<Mutex> lock(bnoDataMutex);
00245 
00246         // once the thread starts it, the BNO will send an Unsolicited Initialize response (SH-2 section 6.4.5.2), and an Executable Reset command
00247         if(!waitForPacket(CHANNEL_EXECUTABLE, EXECUTABLE_REPORTID_RESET, 1s))
00248         {
00249             _debugPort->printf("No initialization report from BNO080.\n");
00250             return false;
00251         }
00252         else
00253         {
00254 #if BNO_DEBUG
00255             _debugPort->printf("BNO080 reports initialization successful!\n");
00256 #endif
00257         }
00258 
00259         // Finally, we want to interrogate the device about its model and version.
00260         clearSendBuffer();
00261         txShtpData[0] = SHTP_REPORT_PRODUCT_ID_REQUEST; //Request the product ID and reset info
00262         txShtpData[1] = 0; //Reserved
00263         sendPacket(CHANNEL_CONTROL, 2);
00264 
00265         waitForPacket(CHANNEL_CONTROL, SHTP_REPORT_PRODUCT_ID_RESPONSE);
00266 
00267         if (rxShtpData[0] == SHTP_REPORT_PRODUCT_ID_RESPONSE)
00268         {
00269             majorSoftwareVersion = rxShtpData[2];
00270             minorSoftwareVersion = rxShtpData[3];
00271             patchSoftwareVersion = (rxShtpData[13] << 8) | rxShtpData[12];
00272             partNumber = (rxShtpData[7] << 24) | (rxShtpData[6] << 16) | (rxShtpData[5] << 8) | rxShtpData[4];
00273             buildNumber = (rxShtpData[11] << 24) | (rxShtpData[10] << 16) | (rxShtpData[9] << 8) | rxShtpData[8];
00274 
00275 #if BNO_DEBUG
00276             _debugPort->printf("BNO080 reports as SW version %hhu.%hhu.%hu, build %lu, part no. %lu\n",
00277                            majorSoftwareVersion, minorSoftwareVersion, patchSoftwareVersion,
00278                            buildNumber, partNumber);
00279 #endif
00280 
00281         }
00282         else
00283         {
00284             _debugPort->printf("Bad response from product ID command.\n");
00285             return false;
00286         }
00287     }
00288 
00289 
00290     // successful init
00291     return true;
00292 }
00293 
00294 bool BNO080Async::updateData()
00295 {
00296     bool newData = dataReceived;
00297     dataReceived = false;
00298     return newData;
00299 }
00300 
00301 
00302 
00303 
00304 
00305