Crazyflie.cpp
Go to the documentation of this file.
00001 //#include <regex>
00002 #include <mutex>
00003 
00004 #include "Crazyflie.h"
00005 #include "crtp.h"
00006 #include "bootloader.h"
00007 
00008 #include "Crazyradio.h"
00009 #include "CrazyflieUSB.h"
00010 
00011 #include <iostream>
00012 #include <cstring>
00013 #include <stdexcept>
00014 #include <thread>
00015 #include <cmath>
00016 
00017 #define MAX_RADIOS 16
00018 #define MAX_USB     4
00019 
00020 Crazyradio* g_crazyradios[MAX_RADIOS];
00021 std::mutex g_radioMutex[MAX_RADIOS];
00022 
00023 CrazyflieUSB* g_crazyflieUSB[MAX_USB];
00024 std::mutex g_crazyflieusbMutex[MAX_USB];
00025 
00026 
00027 Crazyflie::Crazyflie(
00028   const std::string& link_uri)
00029   : m_radio(nullptr)
00030   , m_transport(nullptr)
00031   , m_devId(0)
00032   , m_channel(0)
00033   , m_address(0)
00034   , m_datarate(Crazyradio::Datarate_250KPS)
00035   , m_logTocEntries()
00036   , m_logBlockCb()
00037   , m_paramTocEntries()
00038   , m_paramValues()
00039   , m_emptyAckCallback(nullptr)
00040   , m_linkQualityCallback(nullptr)
00041   , m_consoleCallback(nullptr)
00042 {
00043   int datarate;
00044   int channel;
00045   char datarateType;
00046   bool success = false;
00047 
00048   success = std::sscanf(link_uri.c_str(), "radio://%d/%d/%d%c/%lx",
00049      &m_devId, &channel, &datarate,
00050      &datarateType, &m_address) == 5;
00051   if (!success) {
00052     success = std::sscanf(link_uri.c_str(), "radio://%d/%d/%d%c",
00053        &m_devId, &channel, &datarate,
00054        &datarateType) == 4;
00055     m_address = 0xE7E7E7E7E7;
00056   }
00057 
00058   if (success)
00059   {
00060     m_channel = channel;
00061     if (datarate == 250 && datarateType == 'K') {
00062       m_datarate = Crazyradio::Datarate_250KPS;
00063     }
00064     else if (datarate == 1 && datarateType == 'M') {
00065       m_datarate = Crazyradio::Datarate_1MPS;
00066     }
00067     else if (datarate == 2 && datarateType == 'M') {
00068       m_datarate = Crazyradio::Datarate_2MPS;
00069     }
00070 
00071     if (m_devId >= MAX_RADIOS) {
00072       throw std::runtime_error("This version does not support that many radios. Adjust MAX_RADIOS and recompile!");
00073     }
00074 
00075     {
00076       std::unique_lock<std::mutex> mlock(g_radioMutex[m_devId]);
00077       if (!g_crazyradios[m_devId]) {
00078         g_crazyradios[m_devId] = new Crazyradio(m_devId);
00079         // g_crazyradios[m_devId]->setAckEnable(false);
00080         g_crazyradios[m_devId]->setAckEnable(true);
00081         g_crazyradios[m_devId]->setArc(0);
00082       }
00083     }
00084 
00085     m_radio = g_crazyradios[m_devId];
00086   }
00087   else {
00088     success = std::sscanf(link_uri.c_str(), "usb://%d",
00089        &m_devId) == 1;
00090 
00091     if (m_devId >= MAX_USB) {
00092       throw std::runtime_error("This version does not support that many CFs over USB. Adjust MAX_USB and recompile!");
00093     }
00094 
00095     {
00096       std::unique_lock<std::mutex> mlock(g_crazyflieusbMutex[m_devId]);
00097       if (!g_crazyflieUSB[m_devId]) {
00098         g_crazyflieUSB[m_devId] = new CrazyflieUSB(m_devId);
00099       }
00100     }
00101 
00102     m_transport = g_crazyflieUSB[m_devId];
00103   }
00104 
00105   if (!success) {
00106     throw std::runtime_error("Uri is not valid!");
00107   }
00108 }
00109 
00110 void Crazyflie::logReset()
00111 {
00112   crtpLogResetRequest request;
00113   startBatchRequest();
00114   addRequest(request, 1);
00115   handleRequests();
00116 }
00117 
00118 void Crazyflie::sendSetpoint(
00119   float roll,
00120   float pitch,
00121   float yawrate,
00122   uint16_t thrust)
00123 {
00124   crtpSetpointRequest request(roll, pitch, yawrate, thrust);
00125   sendPacket((const uint8_t*)&request, sizeof(request));
00126 }
00127 
00128 void Crazyflie::sendExternalPositionUpdate(
00129   float x,
00130   float y,
00131   float z)
00132 {
00133   crtpExternalPositionUpdate position(x, y, z);
00134   sendPacket((const uint8_t*)&position, sizeof(position));
00135 }
00136 
00137 void Crazyflie::sendPing()
00138 {
00139   uint8_t ping = 0xFF;
00140   sendPacket(&ping, sizeof(ping));
00141 }
00142 
00146 void Crazyflie::transmitPackets()
00147 {
00148   if (!m_outgoing_packets.empty())
00149   {
00150     std::vector<crtpPacket_t>::iterator it;
00151     for (it = m_outgoing_packets.begin(); it != m_outgoing_packets.end(); it++)
00152     {
00153       sendPacket(it->raw, it->size+1);
00154     }
00155     m_outgoing_packets.clear();
00156   }
00157 }
00158 
00159 // https://forum.bitcraze.io/viewtopic.php?f=9&t=1488
00160 void Crazyflie::reboot()
00161 {
00162   const uint8_t reboot_init[] = {0xFF, 0xFE, 0xFF};
00163   sendPacketOrTimeout(reboot_init, sizeof(reboot_init));
00164 
00165   const uint8_t reboot_to_firmware[] = {0xFF, 0xFE, 0xF0, 0x01};
00166   sendPacketOrTimeout(reboot_to_firmware, sizeof(reboot_to_firmware));
00167 }
00168 
00169 uint64_t Crazyflie::rebootToBootloader()
00170 {
00171   bootloaderResetInitRequest req(TargetNRF51);
00172   startBatchRequest();
00173   addRequest(req, 3);
00174   handleRequests(/*crtpMode=*/false);
00175   const bootloaderResetInitResponse* response = getRequestResult<bootloaderResetInitResponse>(0);
00176 
00177   uint64_t result =
00178       ((uint64_t)response->addr[0] << 0)
00179     | ((uint64_t)response->addr[1] << 8)
00180     | ((uint64_t)response->addr[2] << 16)
00181     | ((uint64_t)response->addr[3] << 24)
00182     | ((uint64_t)0xb1 << 32);
00183 
00184   const uint8_t reboot_to_bootloader[] = {0xFF, 0xFE, 0xF0, 0x00};
00185   // for (size_t i = 0; i < 10; ++i) {
00186   //   if (sendPacket(reboot_to_bootloader, sizeof(reboot_to_bootloader))) {
00187   //     break;
00188   //   }
00189   // }
00190   sendPacketOrTimeout(reboot_to_bootloader, sizeof(reboot_to_bootloader));
00191 
00192   return result;
00193 }
00194 
00195 void Crazyflie::sysoff()
00196 {
00197   const uint8_t shutdown[] = {0xFF, 0xFE, 0x02};
00198   sendPacketOrTimeout(shutdown, sizeof(shutdown));
00199 }
00200 
00201 void Crazyflie::trySysOff()
00202 {
00203   const uint8_t shutdown[] = {0xFF, 0xFE, 0x02};
00204   for (size_t i = 0; i < 10; ++i) {
00205     if (sendPacket(shutdown, sizeof(shutdown))) {
00206       break;
00207     }
00208   }
00209 }
00210 
00211 void Crazyflie::alloff()
00212 {
00213   const uint8_t shutdown[] = {0xFF, 0xFE, 0x01};
00214   sendPacketOrTimeout(shutdown, sizeof(shutdown));
00215 }
00216 
00217 void Crazyflie::syson()
00218 {
00219   const uint8_t shutdown[] = {0xFF, 0xFE, 0x03};
00220   sendPacketOrTimeout(shutdown, sizeof(shutdown));
00221 }
00222 
00223 float Crazyflie::vbat()
00224 {
00225   struct nrf51vbatResponse
00226   {
00227     uint8_t dummy1;
00228     uint8_t dummy2;
00229     uint8_t dummy3;
00230     float vbat;
00231   } __attribute__((packed));
00232 
00233   const uint8_t shutdown[] = {0xFF, 0xFE, 0x04};
00234   startBatchRequest();
00235   addRequest(shutdown, 2);
00236   handleRequests();
00237   return getRequestResult<nrf51vbatResponse>(0)->vbat;
00238 }
00239 
00240 void Crazyflie::writeFlash(
00241   BootloaderTarget target,
00242   const std::vector<uint8_t>& data)
00243 {
00244   // Get info about the target
00245   bootloaderGetInfoRequest req(target);
00246   startBatchRequest();
00247   addRequest(req, 3);
00248   handleRequests(/*crtpMode=*/false);
00249   const bootloaderGetInfoResponse* response = getRequestResult<bootloaderGetInfoResponse>(0);
00250   uint16_t pageSize = response->pageSize;
00251   uint16_t flashStart = response->flashStart;
00252   uint16_t nBuffPage = response->nBuffPage;
00253   // std::cout << "pageSize: " << pageSize
00254   //           << " nBuffPage: " << nBuffPage
00255   //           << " nFlashPage: " << response->nFlashPage
00256   //           << " flashStart: " << flashStart
00257   //           << " version: " << (int)response->version
00258   //           << std::endl;
00259 
00260   uint16_t numPages = ceil(data.size() / (float)pageSize);
00261   if (numPages + flashStart >= response->nFlashPage) {
00262     std::stringstream sstr;
00263     sstr << "Requested size too large!";
00264     throw std::runtime_error(sstr.str());
00265   }
00266   // std::cout << "numPages: " << numPages << std::endl;
00267 
00268   // write flash
00269   size_t offset = 0;
00270   uint16_t usedBuffers = 0;
00271   // startBatchRequest();
00272   for (uint16_t page = flashStart; page < numPages + flashStart; ++page) {
00273     for (uint16_t address = 0; address < pageSize; address += 25) {
00274       // std::cout << "request: " << page << " " << address << std::endl;
00275       bootloaderLoadBufferRequest req(target, usedBuffers, address);
00276       size_t requestedSize = std::min<size_t>(data.size() - offset, std::min<size_t>(25, pageSize - address));
00277       memcpy(req.data, &data[offset], requestedSize);
00278       // addRequest(req, 0);
00279       // for (size_t i = 0; i < 10; ++i)
00280       // std::cout << "request: " << req.page << " " << req.address << " " << requestedSize << std::endl;
00281       // for (size_t i = 0; i < 10; ++i) {
00282 
00283       auto start = std::chrono::system_clock::now();
00284       // while (true) {
00285         sendPacketOrTimeout((uint8_t*)&req, 7 + requestedSize);
00286       //   startBatchRequest();
00287       //   bootloaderReadBufferRequest req2(target, usedBuffers, address);
00288       //   addRequest(req2, 7);
00289       //   handleRequests(/*crtpMode=*/false);
00290       //   const bootloaderReadBufferResponse* response = getRequestResult<bootloaderReadBufferResponse>(0);
00291       //   if (memcmp(req.data, response->data, requestedSize) == 0) {
00292       //     break;
00293       //   }
00294       //   auto end = std::chrono::system_clock::now();
00295       //   std::chrono::duration<double> elapsedSeconds = end-start;
00296       //   if (elapsedSeconds.count() > 1.0) {
00297       //     throw std::runtime_error("timeout");
00298       //   }
00299       // }
00300       offset += requestedSize;
00301       if (offset >= data.size()) {
00302         break;
00303       }
00304     }
00305     ++usedBuffers;
00306     if (usedBuffers == nBuffPage
00307         || page == numPages + flashStart - 1) {
00308 
00309 
00310       // startBatchRequest();
00311       // for (uint16_t buf = 0; buf < usedBuffers; ++buf) {
00312       //   for (uint16_t address = 0; address < pageSize; address += 25) {
00313       //     // std::cout << "request: " << page << " " << address << std::endl;
00314       //     bootloaderReadBufferRequest req(target, buf, address);
00315       //     addRequest(req, 7);
00316       //   }
00317       // }
00318       // handleRequests(/*crtpMode=*/false);
00319 
00320 
00321       // upload all the buffers now
00322       // std::cout << "try to upload buffers!" << std::endl;
00323       // handleRequests(/*crtpMode=*/false);
00324       // std::cout << "buffers uploaded!" << std::endl;
00325 
00326       // std::this_thread::sleep_for(std::chrono::milliseconds(100));
00327 
00328       // write flash
00329       bootloaderWriteFlashRequest req(target, 0, page - usedBuffers + 1, usedBuffers);
00330       sendPacketOrTimeout((uint8_t*)&req, sizeof(req));
00331 
00332       auto start = std::chrono::system_clock::now();
00333 
00334       size_t tries = 0;
00335       while (true) {
00336         Crazyradio::Ack ack;
00337         bootloaderFlashStatusRequest statReq(target);
00338         sendPacket((const uint8_t*)&statReq, sizeof(statReq), ack);
00339         if (   ack.ack
00340             && ack.size == 5
00341             && memcmp(&req, ack.data, 3) == 0) {
00342           if (ack.data[3] != 1 || ack.data[4] != 0) {
00343             throw std::runtime_error("Error during flashing!");
00344           }
00345           break;
00346         }
00347 
00348         // std::cout << page - usedBuffers + 1 << "," << usedBuffers << std::endl;
00349         // startBatchRequest();
00350         // bootloaderWriteFlashRequest req(target, 0, page - usedBuffers + 1, usedBuffers);
00351         // addRequest(req, 3);
00352         // handleRequests(/*crtpMode=*/false);
00353         // const bootloaderWriteFlashResponse* response = getRequestResult<bootloaderWriteFlashResponse>(0);
00354         // if (response->done == 1 && response->error == 0) {
00355         //   break;
00356         // }
00357         auto end = std::chrono::system_clock::now();
00358         std::chrono::duration<double> elapsedSeconds = end-start;
00359         if (elapsedSeconds.count() > 0.5) {
00360           start = end;
00361           sendPacketOrTimeout((uint8_t*)&req, sizeof(req));
00362           ++tries;
00363           if (tries > 5) {
00364             throw std::runtime_error("timeout");
00365           }
00366         }
00367       }
00368 
00369       // std::cout << "Flashed: " << (page - flashStart) / (float)numPages * 100.0 << " %" << std::endl;
00370 
00371       // get ready to fill more buffers
00372       // if (page != numPages + flashStart - 1) {
00373       //   startBatchRequest();
00374       // }
00375       usedBuffers = 0;
00376     }
00377   }
00378 
00379 
00380 }
00381 
00382 void Crazyflie::readFlash(
00383   BootloaderTarget target,
00384   size_t size,
00385   std::vector<uint8_t>& data)
00386 {
00387   // Get info about the target
00388   bootloaderGetInfoRequest req(target);
00389   startBatchRequest();
00390   addRequest(req, 3);
00391   handleRequests(/*crtpMode=*/false);
00392   const bootloaderGetInfoResponse* response = getRequestResult<bootloaderGetInfoResponse>(0);
00393   uint16_t pageSize = response->pageSize;
00394   uint16_t flashStart = response->flashStart;
00395   // std::cout << "pageSize: " << pageSize
00396   //           << " nBuffPage: " << response->nBuffPage
00397   //           << " nFlashPage: " << response->nFlashPage
00398   //           << " flashStart: " << flashStart
00399   //           << " version: " << (int)response->version
00400   //           << std::endl;
00401 
00402   uint16_t numPages = ceil(size / (float)pageSize);
00403   if (numPages + flashStart >= response->nFlashPage) {
00404     std::stringstream sstr;
00405     sstr << "Requested size too large!";
00406     throw std::runtime_error(sstr.str());
00407   }
00408   // std::cout << "numPages: " << numPages << std::endl;
00409 
00410   // read flash
00411   size_t offset = 0;
00412   startBatchRequest();
00413   for (uint16_t page = flashStart; page < numPages + flashStart; ++page) {
00414     for (uint16_t address = 0; address < pageSize; address += 25) {
00415       // std::cout << "request: " << page << " " << address << std::endl;
00416       bootloaderReadFlashRequest req(target, page, address);
00417       addRequest(req, 7);
00418       size_t requestedSize = std::min(25, pageSize - address);
00419       offset += requestedSize;
00420       if (offset > size) {
00421         break;
00422       }
00423     }
00424   }
00425   handleRequests(/*crtpMode=*/false);
00426 
00427   // update output
00428   data.resize(size);
00429   size_t i = 0;
00430   offset = 0;
00431   for (uint16_t page = flashStart; page < numPages + flashStart; ++page) {
00432     for (uint16_t address = 0; address < pageSize; address += 25) {
00433       const bootloaderReadFlashResponse* response = getRequestResult<bootloaderReadFlashResponse>(i++);
00434       size_t requestedSize = std::min(25, pageSize - address);
00435       // std::cout << "offset: " << offset << " reqS: " << requestedSize;
00436       memcpy(&data[offset], response->data, std::min(size - offset, requestedSize));
00437       offset += requestedSize;
00438       if (offset > size) {
00439         break;
00440       }
00441     }
00442   }
00443 }
00444 
00445 void Crazyflie::requestLogToc()
00446 {
00447   // Find the number of log variables in TOC
00448   crtpLogGetInfoRequest infoRequest;
00449   startBatchRequest();
00450   addRequest(infoRequest, 1);
00451   handleRequests();
00452   size_t len = getRequestResult<crtpLogGetInfoResponse>(0)->log_len;
00453   std::cout << "Log: " << len << std::endl;
00454 
00455   // Request detailed information
00456   startBatchRequest();
00457   for (size_t i = 0; i < len; ++i) {
00458     crtpLogGetItemRequest itemRequest(i);
00459     addRequest(itemRequest, 2);
00460   }
00461   handleRequests();
00462 
00463   // Update internal structure with obtained data
00464   m_logTocEntries.resize(len);
00465   for (size_t i = 0; i < len; ++i) {
00466     auto response = getRequestResult<crtpLogGetItemResponse>(i);
00467     LogTocEntry& entry = m_logTocEntries[i];
00468     entry.id = i;
00469     entry.type = (LogType)response->type;
00470     entry.group = std::string(&response->text[0]);
00471     entry.name = std::string(&response->text[entry.group.size() + 1]);
00472   }
00473 }
00474 
00475 void Crazyflie::requestParamToc()
00476 {
00477   // Find the number of parameters in TOC
00478   crtpParamTocGetInfoRequest infoRequest;
00479   startBatchRequest();
00480   addRequest(infoRequest, 1);
00481   handleRequests();
00482   size_t len = getRequestResult<crtpParamTocGetInfoResponse>(0)->numParam;
00483 
00484   std::cout << "Params: " << len << std::endl;
00485 
00486   // Request detailed information and values
00487   startBatchRequest();
00488   for (size_t i = 0; i < len; ++i) {
00489     crtpParamTocGetItemRequest itemRequest(i);
00490     addRequest(itemRequest, 2);
00491     crtpParamReadRequest readRequest(i);
00492     addRequest(readRequest, 1);
00493   }
00494   handleRequests();
00495 
00496   // Update internal structure with obtained data
00497   m_paramTocEntries.resize(len);
00498   for (size_t i = 0; i < len; ++i) {
00499     auto r = getRequestResult<crtpParamTocGetItemResponse>(i*2+0);
00500     auto val = getRequestResult<crtpParamValueResponse>(i*2+1);
00501 
00502     ParamTocEntry& entry = m_paramTocEntries[i];
00503     entry.id = i;
00504     entry.type = (ParamType)(r->length | r-> type << 2 | r->sign << 3);
00505     entry.readonly = r->readonly;
00506     entry.group = std::string(&r->text[0]);
00507     entry.name = std::string(&r->text[entry.group.size() + 1]);
00508 
00509     ParamValue v;
00510     std::memcpy(&v, &val->valueFloat, 4);
00511     m_paramValues[i] = v;
00512   }
00513 }
00514 
00515 void Crazyflie::setParam(uint8_t id, const ParamValue& value) {
00516 
00517   startBatchRequest();
00518   bool found = false;
00519   for (auto&& entry : m_paramTocEntries) {
00520     if (entry.id == id) {
00521       found = true;
00522       switch (entry.type) {
00523         case ParamTypeUint8:
00524           {
00525             crtpParamWriteRequest<uint8_t> request(id, value.valueUint8);
00526             addRequest(request, 1);
00527             break;
00528           }
00529         case ParamTypeInt8:
00530           {
00531             crtpParamWriteRequest<int8_t> request(id, value.valueInt8);
00532             addRequest(request, 1);
00533             break;
00534           }
00535         case ParamTypeUint16:
00536           {
00537             crtpParamWriteRequest<uint16_t> request(id, value.valueUint16);
00538             addRequest(request, 1);
00539             break;
00540           }
00541         case ParamTypeInt16:
00542           {
00543             crtpParamWriteRequest<int16_t> request(id, value.valueInt16);
00544             addRequest(request, 1);
00545             break;
00546           }
00547         case ParamTypeUint32:
00548           {
00549             crtpParamWriteRequest<uint32_t> request(id, value.valueUint32);
00550             addRequest(request, 1);
00551             break;
00552           }
00553         case ParamTypeInt32:
00554           {
00555             crtpParamWriteRequest<int32_t> request(id, value.valueInt32);
00556             addRequest(request, 1);
00557             break;
00558           }
00559         case ParamTypeFloat:
00560           {
00561             crtpParamWriteRequest<float> request(id, value.valueFloat);
00562             addRequest(request, 1);
00563             break;
00564           }
00565       }
00566     }
00567   }
00568 
00569   if (!found) {
00570     std::stringstream sstr;
00571     sstr << "Could not find parameter with id " << id;
00572     throw std::runtime_error(sstr.str());
00573   }
00574   handleRequests();
00575 
00576   m_paramValues[id] = value;
00577 }
00578 
00579 bool Crazyflie::sendPacket(
00580   const uint8_t* data,
00581   uint32_t length)
00582 {
00583   Crazyradio::Ack ack;
00584   sendPacket(data, length, ack);
00585   return ack.ack;
00586 }
00587 
00588  void Crazyflie::sendPacketOrTimeout(
00589    const uint8_t* data,
00590    uint32_t length,
00591    float timeout)
00592 {
00593   auto start = std::chrono::system_clock::now();
00594   while (!sendPacket(data, length)) {
00595     auto end = std::chrono::system_clock::now();
00596     std::chrono::duration<double> elapsedSeconds = end-start;
00597     if (elapsedSeconds.count() > timeout) {
00598       throw std::runtime_error("timeout");
00599     }
00600   }
00601 }
00602 
00603 void Crazyflie::sendPacket(
00604   const uint8_t* data,
00605   uint32_t length,
00606   Crazyradio::Ack& ack)
00607 {
00608   static uint32_t numPackets = 0;
00609   static uint32_t numAcks = 0;
00610 
00611   numPackets++;
00612 
00613   if (m_radio) {
00614     std::unique_lock<std::mutex> mlock(g_radioMutex[m_devId]);
00615     if (m_radio->getAddress() != m_address) {
00616       m_radio->setAddress(m_address);
00617     }
00618     if (m_radio->getChannel() != m_channel) {
00619       m_radio->setChannel(m_channel);
00620     }
00621     if (m_radio->getDatarate() != m_datarate) {
00622       m_radio->setDatarate(m_datarate);
00623     }
00624     m_radio->sendPacket(data, length, ack);
00625   } else {
00626     std::unique_lock<std::mutex> mlock(g_crazyflieusbMutex[m_devId]);
00627     m_transport->sendPacket(data, length, ack);
00628   }
00629   ack.data[ack.size] = 0;
00630   if (ack.ack) {
00631     handleAck(ack);
00632     numAcks++;
00633   }
00634   if (numPackets == 100) {
00635     if (m_linkQualityCallback) {
00636       // We just take the ratio of sent vs. acked packets here
00637       // for a sliding window of 100 packets
00638       float linkQuality = numAcks / (float)numPackets;
00639       m_linkQualityCallback(linkQuality);
00640     }
00641     numPackets = 0;
00642     numAcks = 0;
00643   }
00644 }
00645 
00646 void Crazyflie::handleAck(
00647   const Crazyradio::Ack& result)
00648 {
00649   if (crtpConsoleResponse::match(result)) {
00650     if (result.size > 0) {
00651       crtpConsoleResponse* r = (crtpConsoleResponse*)result.data;
00652       if (m_consoleCallback) {
00653         m_consoleCallback(r->text);
00654       }
00655     }
00656     // ROS_INFO("Console: %s", r->text);
00657   }
00658   else if (crtpLogGetInfoResponse::match(result)) {
00659     // handled in batch system
00660   }
00661   else if (crtpLogGetItemResponse::match(result)) {
00662     // handled in batch system
00663   }
00664   else if (crtpLogControlResponse::match(result)) {
00665     // handled in batch system
00666   }
00667   else if (crtpLogDataResponse::match(result)) {
00668     crtpLogDataResponse* r = (crtpLogDataResponse*)result.data;
00669     auto iter = m_logBlockCb.find(r->blockId);
00670     if (iter != m_logBlockCb.end()) {
00671       iter->second(r, result.size - 5);
00672     }
00673     else {
00674       std::cout << "Received unrequested data for block: " << (int)r->blockId << std::endl;
00675     }
00676   }
00677   else if (crtpParamTocGetInfoResponse::match(result)) {
00678     // handled in batch system
00679   }
00680   else if (crtpParamTocGetItemResponse::match(result)) {
00681     // handled in batch system
00682   }
00683   else if (crtpParamValueResponse::match(result)) {
00684     // handled in batch system
00685   }
00686   else if (crtpPlatformRSSIAck::match(result)) {
00687     crtpPlatformRSSIAck* r = (crtpPlatformRSSIAck*)result.data;
00688     if (m_emptyAckCallback) {
00689       m_emptyAckCallback(r);
00690     }
00691   }
00692   else {
00693     crtp* header = (crtp*)result.data;
00694     std::cout << "Don't know ack: Port: " << (int)header->port << " Channel: " << (int)header->channel << " Len: " << (int)result.size << std::endl;
00695     // for (size_t i = 1; i < result.size; ++i) {
00696     //   std::cout << "    " << (int)result.data[i] << std::endl;
00697     // }
00698     queueGenericPacket(result);
00699   }
00700 }
00701 
00702 const Crazyflie::LogTocEntry* Crazyflie::getLogTocEntry(
00703   const std::string& group,
00704   const std::string& name) const
00705 {
00706   for (auto&& entry : m_logTocEntries) {
00707     if (entry.group == group && entry.name == name) {
00708       return &entry;
00709     }
00710   }
00711   return nullptr;
00712 }
00713 
00714 const Crazyflie::ParamTocEntry* Crazyflie::getParamTocEntry(
00715   const std::string& group,
00716   const std::string& name) const
00717 {
00718   for (auto&& entry : m_paramTocEntries) {
00719     if (entry.group == group && entry.name == name) {
00720       return &entry;
00721     }
00722   }
00723   return nullptr;
00724 }
00725 
00726 uint8_t Crazyflie::registerLogBlock(
00727   std::function<void(crtpLogDataResponse*, uint8_t)> cb)
00728 {
00729   for (uint8_t id = 0; id < 255; ++id) {
00730     if (m_logBlockCb.find(id) == m_logBlockCb.end()) {
00731       m_logBlockCb[id] = cb;
00732       return id;
00733     }
00734   }
00735 }
00736 
00737 bool Crazyflie::unregisterLogBlock(
00738   uint8_t id)
00739 {
00740   m_logBlockCb.erase(m_logBlockCb.find(id));
00741 }
00742 
00743 // Batch system
00744 
00745 void Crazyflie::startBatchRequest()
00746 {
00747   m_batchRequests.clear();
00748 }
00749 
00750 void Crazyflie::addRequest(
00751   const uint8_t* data,
00752   size_t numBytes,
00753   size_t numBytesToMatch)
00754 {
00755   m_batchRequests.resize(m_batchRequests.size() + 1);
00756   m_batchRequests.back().request.resize(numBytes);
00757   memcpy(m_batchRequests.back().request.data(), data, numBytes);
00758   m_batchRequests.back().numBytesToMatch = numBytesToMatch;
00759   m_batchRequests.back().finished = false;
00760 }
00761 
00762 void Crazyflie::handleRequests(
00763   float baseTime,
00764   float timePerRequest)
00765 {
00766   auto start = std::chrono::system_clock::now();
00767   Crazyradio::Ack ack;
00768   m_numRequestsFinished = 0;
00769   bool sendPing = false;
00770 
00771   float timeout = baseTime + timePerRequest * m_batchRequests.size();
00772 
00773   while (true) {
00774     if (!sendPing) {
00775       for (const auto& request : m_batchRequests) {
00776         if (!request.finished) {
00777           // std::cout << "sendReq" << std::endl;
00778           sendPacket(request.request.data(), request.request.size(), ack);
00779           handleBatchAck(ack);
00780 
00781           auto end = std::chrono::system_clock::now();
00782           std::chrono::duration<double> elapsedSeconds = end-start;
00783           if (elapsedSeconds.count() > timeout) {
00784             throw std::runtime_error("timeout");
00785           }
00786         }
00787       }
00788       sendPing = true;
00789     } else {
00790       for (size_t i = 0; i < 10; ++i) {
00791         uint8_t ping = 0xFF;
00792         sendPacket(&ping, sizeof(ping), ack);
00793         handleBatchAck(ack);
00794         // if (ack.ack && crtpPlatformRSSIAck::match(ack)) {
00795         //   sendPing = false;
00796         // }
00797 
00798         auto end = std::chrono::system_clock::now();
00799         std::chrono::duration<double> elapsedSeconds = end-start;
00800         if (elapsedSeconds.count() > timeout) {
00801           throw std::runtime_error("timeout");
00802         }
00803       }
00804 
00805       sendPing = false;
00806     }
00807     if (m_numRequestsFinished == m_batchRequests.size()) {
00808       break;
00809     }
00810   }
00811 }
00812 
00813 void Crazyflie::handleBatchAck(
00814   const Crazyradio::Ack& ack)
00815 {
00816   if (ack.ack) {
00817     for (auto& request : m_batchRequests) {
00818       if (crtp(ack.data[0]) == crtp(request.request[0])
00819           && memcmp(&ack.data[1], &request.request[1], request.numBytesToMatch) == 0
00820           && !request.finished) {
00821         request.ack = ack;
00822         request.finished = true;
00823         ++m_numRequestsFinished;
00824         // std::cout << "gotack" <<std::endl;
00825         return;
00826       }
00827     }
00828     // handle generic ack
00829     handleAck(ack);
00830     // crtp c(ack.data[0]);
00831     //std::cout << "didnt handle ack " << (int) c.port << " " << (int) c.channel << " " << (int) ack.data[1] << " " << (int) ack.data[2] << std::endl;
00832     // TODO: generic handle ack here?
00833   }
00834 }
00835 


crazyflie_cpp
Author(s): Wolfgang Hoenig
autogenerated on Sun Oct 8 2017 02:47:59