Crazyflie.cpp
Go to the documentation of this file.
1 //#include <regex>
2 #include <mutex>
3 
4 #include "Crazyflie.h"
5 #include "crtp.h"
6 #include "crtpBootloader.h"
7 #include "crtpNRF51.h"
8 
9 #include "Crazyradio.h"
10 #include "CrazyflieUSB.h"
11 
12 #include <fstream>
13 #include <cstring>
14 #include <stdexcept>
15 #include <thread>
16 #include <cmath>
17 #include <inttypes.h>
18 
19 const static int MAX_RADIOS = 16;
20 const static int MAX_USB = 4;
21 const static bool LOG_COMMUNICATION = 0;
22 
25 
28 
30 
31 
33  const std::string& link_uri,
34  Logger& logger,
35  std::function<void(const char*)> consoleCb)
36  : m_radio(nullptr)
37  , m_transport(nullptr)
38  , m_devId(0)
39  , m_channel(0)
40  , m_address(0)
41  , m_datarate(Crazyradio::Datarate_250KPS)
42  , m_logTocEntries()
43  , m_logBlockCb()
44  , m_paramTocEntries()
45  , m_paramValues()
46  , m_emptyAckCallback(nullptr)
47  , m_linkQualityCallback(nullptr)
48  , m_consoleCallback(consoleCb)
49  , m_log_use_V2(false)
50  , m_param_use_V2(false)
51  , m_logger(logger)
52 {
53  int datarate;
54  int channel;
55  char datarateType;
56  bool success = false;
57 
58  success = std::sscanf(link_uri.c_str(), "radio://%d/%d/%d%c/%" SCNx64,
59  &m_devId, &channel, &datarate,
60  &datarateType, &m_address) == 5;
61  if (!success) {
62  success = std::sscanf(link_uri.c_str(), "radio://%d/%d/%d%c",
63  &m_devId, &channel, &datarate,
64  &datarateType) == 4;
65  m_address = 0xE7E7E7E7E7;
66  }
67 
68  if (success)
69  {
71  if (datarate == 250 && datarateType == 'K') {
73  }
74  else if (datarate == 1 && datarateType == 'M') {
76  }
77  else if (datarate == 2 && datarateType == 'M') {
79  }
80 
81  if (m_devId >= MAX_RADIOS) {
82  throw std::runtime_error("This version does not support that many radios. Adjust MAX_RADIOS and recompile!");
83  }
84 
85  {
86  std::unique_lock<std::mutex> mlock(g_radioMutex[m_devId]);
87  if (!g_crazyradios[m_devId]) {
88  g_crazyradios[m_devId] = new Crazyradio(m_devId);
89  g_crazyradios[m_devId]->enableLogging(LOG_COMMUNICATION);
90  // g_crazyradios[m_devId]->setAckEnable(false);
91  g_crazyradios[m_devId]->setAckEnable(true);
92  g_crazyradios[m_devId]->setArc(0);
93  }
94  }
95 
96  m_radio = g_crazyradios[m_devId];
97  }
98  else {
99  success = std::sscanf(link_uri.c_str(), "usb://%d",
100  &m_devId) == 1;
101 
102  if (m_devId >= MAX_USB) {
103  throw std::runtime_error("This version does not support that many CFs over USB. Adjust MAX_USB and recompile!");
104  }
105 
106  {
107  std::unique_lock<std::mutex> mlock(g_crazyflieusbMutex[m_devId]);
108  if (!g_crazyflieUSB[m_devId]) {
109  g_crazyflieUSB[m_devId] = new CrazyflieUSB(m_devId);
110  g_crazyflieUSB[m_devId]->enableLogging(LOG_COMMUNICATION);
111  }
112  }
113 
114  m_transport = g_crazyflieUSB[m_devId];
115  }
116 
117  if (!success) {
118  throw std::runtime_error("Uri is not valid!");
119  }
120 
121  // enable safelink
122  if (m_radio) {
124  sendPacketOrTimeout(request, /*useSafeLink*/false);
125  }
126 
127  m_curr_up = 0;
128  m_curr_down = 0;
129 
130  m_protocolVersion = -1;
131 
132 }
133 
135 {
138  addRequest(req, 1);
139  handleRequests();
140  return getRequestResult<crtpGetProtocolVersionResponse>(0)->version;
141 }
142 
144 {
147  addRequest(req, 1);
148  handleRequests();
149  return std::string(getRequestResult<crtpGetFirmwareVersionResponse>(0)->version);
150 }
151 
153 {
156  addRequest(req, 1);
157  handleRequests();
158  return std::string(getRequestResult<crtpGetDeviceTypeNameResponse>(0)->name);
159 }
160 
162 {
165  addRequest(request, 1);
166  handleRequests();
167 }
168 
170  float roll,
171  float pitch,
172  float yawrate,
173  uint16_t thrust)
174 {
175  crtpSetpointRequest request(roll, pitch, yawrate, thrust);
176  sendPacket(request);
177 }
178 
180 {
182  sendPacket(request);
183 }
184 
186 {
188  sendPacketOrTimeout(request);
189 }
190 
192 {
194  sendPacketOrTimeout(request);
195 }
196 
198  float x,
199  float y,
200  float z,
201  float yaw)
202 {
203  crtpPositionSetpointRequest request(x, y, z, yaw);
204  sendPacket(request);
205 }
206 
208  float vx,
209  float vy,
210  float yawrate,
211  float zDistance)
212 {
213  crtpHoverSetpointRequest request(vx, vy, yawrate, zDistance);
214  sendPacket(request);
215 }
216 
218  float x, float y, float z,
219  float vx, float vy, float vz,
220  float ax, float ay, float az,
221  float qx, float qy, float qz, float qw,
222  float rollRate, float pitchRate, float yawRate)
223 {
225  x, y, z,
226  vx, vy, vz,
227  ax, ay, az,
228  qx, qy, qz, qw,
229  rollRate, pitchRate, yawRate);
230  sendPacket(request);
231 }
232 
234  float x, float y, float z, float yawRate)
235 {
237  x, y, z, yawRate);
238  sendPacket(request);
239 }
240 
242 {
243  crtpNotifySetpointsStopRequest request(remainValidMillisecs);
244  sendPacketOrTimeout(request);
245 }
246 
248  float x,
249  float y,
250  float z)
251 {
252  crtpExternalPositionUpdate position(x, y, z);
253  sendPacket(position);
254 }
255 
257  float x, float y, float z,
258  float qx, float qy, float qz, float qw)
259 {
260  crtpExternalPoseUpdate pose(x, y, z, qx, qy, qz, qw);
261  sendPacket(pose);
262 }
263 
265 {
266  crtpEmpty req;
267  sendPacket(req);
268 }
269 
274 {
275  if (!m_outgoing_packets.empty())
276  {
277  std::vector<crtpPacket_t>::iterator it;
278  for (it = m_outgoing_packets.begin(); it != m_outgoing_packets.end(); it++)
279  {
280  sendPacketInternal(it->raw, it->size+1);
281  }
282  m_outgoing_packets.clear();
283  }
284 }
285 
286 // https://forum.bitcraze.io/viewtopic.php?f=9&t=1488
288 {
289  if (m_radio) {
291  sendPacketOrTimeout(req1);
292 
293  crtpNrf51ResetRequest req2(/*bootToFirmware*/ 1);
294  sendPacketOrTimeout(req2);
295  }
296 }
297 
299 {
300  if (m_radio) {
303  addRequest(req, 2);
304  handleRequests();
305  const crtpNrf51ResetInitResponse* response = getRequestResult<crtpNrf51ResetInitResponse>(0);
306 
307  uint64_t result =
308  ((uint64_t)response->addr[0] << 0)
309  | ((uint64_t)response->addr[1] << 8)
310  | ((uint64_t)response->addr[2] << 16)
311  | ((uint64_t)response->addr[3] << 24)
312  | ((uint64_t)0xb1 << 32);
313 
314  crtpNrf51ResetRequest req2(/*bootToFirmware*/ 0);
315  sendPacketOrTimeout(req2);
316 
317  // switch to new address
318  m_address = result;
319  m_channel = 0;
321 
322 
323  return result;
324  } else {
325  return -1;
326  }
327 }
328 
330 {
331  if (m_radio) {
332  bootloaderResetRequest req(/*bootToFirmware*/ 1);
333  sendPacketOrTimeout(req, /*useSafeLink*/ false);
334  }
335 }
336 
338 {
339  if (m_radio) {
341  sendPacketOrTimeout(req);
342  }
343 }
344 
346 {
347  if (m_radio) {
349  sendPacketOrTimeout(req);
350  }
351 }
352 
354 {
355  if (m_radio) {
357  sendPacketOrTimeout(req);
358  }
359 }
360 
362 {
363  if (m_radio) {
366  addRequest(req, 2);
367  handleRequests();
368  return getRequestResult<crtpNrf51GetVBatResponse>(0)->vbat;
369  } else {
370  return nan("");
371  }
372 }
373 
376  const std::vector<uint8_t>& data)
377 {
378  // Get info about the target
379  bootloaderGetInfoRequest req(target);
381  addRequest(req, 3);
382  handleRequests(/*crtpMode=*/false, /*useSafeLink*/ false);
383  const bootloaderGetInfoResponse* response = getRequestResult<bootloaderGetInfoResponse>(0);
384  uint16_t pageSize = response->pageSize;
385  uint16_t flashStart = response->flashStart;
386  uint16_t nBuffPage = response->nBuffPage;
387 
388  uint16_t numPages = ceil(data.size() / (float)pageSize);
389  if (numPages + flashStart >= response->nFlashPage) {
390  std::stringstream sstr;
391  sstr << "Requested size too large!";
392  throw std::runtime_error(sstr.str());
393  }
394 
395  std::stringstream sstr;
396  sstr << "pageSize: " << pageSize
397  << " nBuffPage: " << nBuffPage
398  << " nFlashPage: " << response->nFlashPage
399  << " flashStart: " << flashStart
400  << " version: " << (int)response->version
401  << " numPages: " << numPages;
402  m_logger.info(sstr.str());
403 
404  // write flash
405  size_t offset = 0;
406  uint16_t usedBuffers = 0;
407  // startBatchRequest();
408  for (uint16_t page = flashStart; page < numPages + flashStart; ++page) {
409  std::stringstream sstr;
410  sstr << "page: " << page - flashStart + 1 << " / " << numPages;
411  m_logger.info(sstr.str());
412  for (uint16_t address = 0; address < pageSize; address += 25) {
413 
414  // std::cout << "request: " << page << " " << address << std::endl;
415  bootloaderLoadBufferRequest req(target, usedBuffers, address);
416  size_t requestedSize = std::min<size_t>(data.size() - offset, std::min<size_t>(25, pageSize - address));
417  memcpy(req.data, &data[offset], requestedSize);
418  // addRequest(req, 0);
419  // for (size_t i = 0; i < 10; ++i)
420  // std::cout << "request: " << req.page << " " << req.address << " " << requestedSize << std::endl;
421  // for (size_t i = 0; i < 10; ++i) {
422 
423  // auto start = std::chrono::system_clock::now();
424  // while (true) {
425  sendPacketOrTimeoutInternal((uint8_t*)&req, 7 + requestedSize, false);
426  // startBatchRequest();
427  // bootloaderReadBufferRequest req2(target, usedBuffers, address);
428  // addRequest(req2, 7);
429  // handleRequests(/*crtpMode=*/false);
430  // const bootloaderReadBufferResponse* response = getRequestResult<bootloaderReadBufferResponse>(0);
431  // if (memcmp(req.data, response->data, requestedSize) == 0) {
432  // break;
433  // }
434  // auto end = std::chrono::system_clock::now();
435  // std::chrono::duration<double> elapsedSeconds = end-start;
436  // if (elapsedSeconds.count() > 1.0) {
437  // throw std::runtime_error("timeout");
438  // }
439  // }
440  offset += requestedSize;
441  if (offset >= data.size()) {
442  break;
443  }
444  }
445  ++usedBuffers;
446  if (usedBuffers == nBuffPage
447  || page == numPages + flashStart - 1) {
448 
449 
450  // startBatchRequest();
451  // for (uint16_t buf = 0; buf < usedBuffers; ++buf) {
452  // for (uint16_t address = 0; address < pageSize; address += 25) {
453  // // std::cout << "request: " << page << " " << address << std::endl;
454  // bootloaderReadBufferRequest req(target, buf, address);
455  // addRequest(req, 7);
456  // }
457  // }
458  // handleRequests(/*crtpMode=*/false);
459 
460 
461  // upload all the buffers now
462  // std::cout << "try to upload buffers!" << std::endl;
463  // handleRequests(/*crtpMode=*/false);
464  // std::cout << "buffers uploaded!" << std::endl;
465 
466  // std::this_thread::sleep_for(std::chrono::milliseconds(100));
467 
468  // write flash
469  bootloaderWriteFlashRequest req(target, 0, page - usedBuffers + 1, usedBuffers);
470  sendPacketOrTimeoutInternal((uint8_t*)&req, sizeof(req), false);
471 
472  auto start = std::chrono::system_clock::now();
473 
474  size_t tries = 0;
475  while (true) {
477  bootloaderFlashStatusRequest statReq(target);
478  sendPacket(statReq, ack, false);
479  if ( ack.ack
480  && ack.size == 5
481  && memcmp(&req, ack.data, 3) == 0) {
482  if (ack.data[3] != 1 || ack.data[4] != 0) {
483  throw std::runtime_error("Error during flashing!");
484  }
485  break;
486  }
487 
488  // std::cout << page - usedBuffers + 1 << "," << usedBuffers << std::endl;
489  // startBatchRequest();
490  // bootloaderWriteFlashRequest req(target, 0, page - usedBuffers + 1, usedBuffers);
491  // addRequest(req, 3);
492  // handleRequests(/*crtpMode=*/false);
493  // const bootloaderWriteFlashResponse* response = getRequestResult<bootloaderWriteFlashResponse>(0);
494  // if (response->done == 1 && response->error == 0) {
495  // break;
496  // }
497  auto end = std::chrono::system_clock::now();
498  std::chrono::duration<double> elapsedSeconds = end-start;
499  if (elapsedSeconds.count() > 0.5) {
500  start = end;
501  sendPacketOrTimeout(req, false);
502  ++tries;
503  if (tries > 5) {
504  throw std::runtime_error("timeout");
505  }
506  }
507  }
508 
509  // std::cout << "Flashed: " << (page - flashStart) / (float)numPages * 100.0 << " %" << std::endl;
510 
511  // get ready to fill more buffers
512  // if (page != numPages + flashStart - 1) {
513  // startBatchRequest();
514  // }
515  usedBuffers = 0;
516  }
517  }
518 
519 
520 }
521 
524  size_t size,
525  std::vector<uint8_t>& data)
526 {
527  // Get info about the target
528  bootloaderGetInfoRequest req(target);
530  addRequest(req, 3);
531  handleRequests(/*crtpMode=*/false, /*useSafelink*/false);
532  const bootloaderGetInfoResponse* response = getRequestResult<bootloaderGetInfoResponse>(0);
533  uint16_t pageSize = response->pageSize;
534  uint16_t flashStart = response->flashStart;
535 
536  uint16_t numPages = ceil(size / (float)pageSize);
537  if (numPages + flashStart >= response->nFlashPage) {
538  std::stringstream sstr;
539  sstr << "Requested size too large!";
540  throw std::runtime_error(sstr.str());
541  }
542 
543  std::stringstream sstr;
544  sstr << "pageSize: " << pageSize
545  << " nFlashPage: " << response->nFlashPage
546  << " flashStart: " << flashStart
547  << " version: " << (int)response->version
548  << " numPages: " << numPages;
549  m_logger.info(sstr.str());
550 
551  // read flash
552  size_t offset = 0;
554  for (uint16_t page = flashStart; page < numPages + flashStart; ++page) {
555  for (uint16_t address = 0; address < pageSize; address += 25) {
556  // std::cout << "request: " << page << " " << address << std::endl;
558  addRequest(req, 7);
559  size_t requestedSize = std::min(25, pageSize - address);
560  offset += requestedSize;
561  if (offset > size) {
562  break;
563  }
564  }
565  }
566  handleRequests(/*crtpMode=*/false, /*useSafelink*/false);
567 
568  // update output
569  data.resize(size);
570  size_t i = 0;
571  offset = 0;
572  for (uint16_t page = flashStart; page < numPages + flashStart; ++page) {
573  for (uint16_t address = 0; address < pageSize; address += 25) {
574  const bootloaderReadFlashResponse* response = getRequestResult<bootloaderReadFlashResponse>(i++);
575  size_t requestedSize = std::min(25, pageSize - address);
576  // std::cout << "offset: " << offset << " reqS: " << requestedSize;
577  memcpy(&data[offset], response->data, std::min(size - offset, requestedSize));
578  offset += requestedSize;
579  if (offset > size) {
580  break;
581  }
582  }
583  }
584 }
585 
586 void Crazyflie::requestLogToc(bool forceNoCache)
587 {
588  m_log_use_V2 = true;
589  uint16_t len;
590  uint32_t crc;
591 
592  // Lazily initialize protocol version
593  if (m_protocolVersion < 0) {
595  }
596 
597  crtpLogGetInfoV2Request infoRequest;
599  addRequest(infoRequest, 1);
600  if (m_protocolVersion >= 4) {
601  handleRequests();
602  len = getRequestResult<crtpLogGetInfoV2Response>(0)->log_len;
603  crc = getRequestResult<crtpLogGetInfoV2Response>(0)->log_crc;
604  } else {
605  // std::cout << "Fall back to V1 param API" << std::endl;
606  m_log_use_V2 = false;
607 
608  crtpLogGetInfoRequest infoRequest;
610  addRequest(infoRequest, 1);
611  handleRequests();
612  len = getRequestResult<crtpLogGetInfoResponse>(0)->log_len;
613  crc = getRequestResult<crtpLogGetInfoResponse>(0)->log_crc;
614  // std::cout << len << std::endl;
615  }
616 
617  // check if it is in the cache
618  std::string fileName = "log" + std::to_string(crc) + ".csv";
619  std::ifstream infile(fileName);
620 
621  if (forceNoCache || !infile.good()) {
622  m_logger.info("Log: " + std::to_string(len));
623 
624  // Request detailed information
626  if (m_log_use_V2) {
627  for (size_t i = 0; i < len; ++i) {
628  crtpLogGetItemV2Request itemRequest(i);
629  addRequest(itemRequest, 2);
630  }
631  } else {
632  for (size_t i = 0; i < len; ++i) {
633  crtpLogGetItemRequest itemRequest(i);
634  addRequest(itemRequest, 2);
635  }
636  }
637  handleRequests();
638 
639  // Update internal structure with obtained data
640  m_logTocEntries.resize(len);
641  if (m_log_use_V2) {
642  for (size_t i = 0; i < len; ++i) {
643  auto response = getRequestResult<crtpLogGetItemV2Response>(i);
644  LogTocEntry& entry = m_logTocEntries[i];
645  entry.id = i;
646  entry.type = (LogType)response->type;
647  entry.group = std::string(&response->text[0]);
648  entry.name = std::string(&response->text[entry.group.size() + 1]);
649  }
650  } else {
651  for (size_t i = 0; i < len; ++i) {
652  auto response = getRequestResult<crtpLogGetItemResponse>(i);
653  LogTocEntry& entry = m_logTocEntries[i];
654  entry.id = i;
655  entry.type = (LogType)response->type;
656  entry.group = std::string(&response->text[0]);
657  entry.name = std::string(&response->text[entry.group.size() + 1]);
658  }
659  }
660 
661  // Write a cache file
662  {
663  // Atomic file write: write in temporary file first to avoid race conditions
664  std::string fileNameTemp = fileName + ".tmp";
665  std::ofstream output(fileNameTemp);
666  output << "id,type,group,name" << std::endl;
667  for (const auto& entry : m_logTocEntries) {
668  output << std::to_string(entry.id) << ","
669  << std::to_string(entry.type) << ","
670  << entry.group << ","
671  << entry.name << std::endl;
672  }
673  // change the filename
674  rename(fileNameTemp.c_str(), fileName.c_str());
675  }
676  } else {
677  m_logger.info("Found variables in cache.");
678  m_logTocEntries.clear();
679  std::string line, cell;
680  std::getline(infile, line); // ignore header
681  while (std::getline(infile, line)) {
682  std::stringstream lineStream(line);
683  m_logTocEntries.resize(m_logTocEntries.size() + 1);
684  std::getline(lineStream, cell, ',');
685  m_logTocEntries.back().id = std::stoi(cell);
686  std::getline(lineStream, cell, ',');
687  m_logTocEntries.back().type = (LogType)std::stoi(cell);
688  std::getline(lineStream, cell, ',');
689  m_logTocEntries.back().group = cell;
690  std::getline(lineStream, cell, ',');
691  m_logTocEntries.back().name = cell;
692  }
693  }
694 }
695 
696 void Crazyflie::requestParamToc(bool forceNoCache)
697 {
698  m_param_use_V2 = true;
699  uint16_t numParam;
700  uint32_t crc;
701 
702  // Lazily initialize protocol version
703  if (m_protocolVersion < 0) {
705  }
706 
707  // Find the number of parameters in TOC
708  crtpParamTocGetInfoV2Request infoRequest;
710  // std::cout << "infoReq" << std::endl;
711  addRequest(infoRequest, 1);
712  if (m_protocolVersion >= 4) {
713  handleRequests();
714  numParam = getRequestResult<crtpParamTocGetInfoV2Response>(0)->numParam;
715  crc = getRequestResult<crtpParamTocGetInfoV2Response>(0)->crc;
716  } else {
717  // std::cout << "Fall back to V1 param API" << std::endl;
718  m_param_use_V2 = false;
719 
720  crtpParamTocGetInfoRequest infoRequest;
722  addRequest(infoRequest, 1);
723  handleRequests();
724  numParam = getRequestResult<crtpParamTocGetInfoResponse>(0)->numParam;
725  crc = getRequestResult<crtpParamTocGetInfoResponse>(0)->crc;
726  }
727 
728  // check if it is in the cache
729  std::string fileName = "params" + std::to_string(crc) + ".csv";
730  std::ifstream infile(fileName);
731 
732  if (forceNoCache || !infile.good()) {
733  m_logger.info("Params: " + std::to_string(numParam));
734 
735  // Request detailed information and values
737  if (!m_param_use_V2) {
738  for (uint16_t i = 0; i < numParam; ++i) {
739  crtpParamTocGetItemRequest itemRequest(i);
740  addRequest(itemRequest, 2);
741  crtpParamReadRequest readRequest(i);
742  addRequest(readRequest, 1);
743  }
744  } else {
745  for (uint16_t i = 0; i < numParam; ++i) {
746  crtpParamTocGetItemV2Request itemRequest(i);
747  addRequest(itemRequest, 2);
748  crtpParamReadV2Request readRequest(i);
749  addRequest(readRequest, 1);
750  }
751  }
752  handleRequests();
753  // Update internal structure with obtained data
754  m_paramTocEntries.resize(numParam);
755 
756  if (!m_param_use_V2) {
757  for (uint16_t i = 0; i < numParam; ++i) {
758  auto r = getRequestResult<crtpParamTocGetItemResponse>(i*2+0);
759  auto val = getRequestResult<crtpParamValueResponse>(i*2+1);
760 
761  ParamTocEntry& entry = m_paramTocEntries[i];
762  entry.id = i;
763  entry.type = (ParamType)(r->length | r-> type << 2 | r->sign << 3);
764  entry.readonly = r->readonly;
765  entry.group = std::string(&r->text[0]);
766  entry.name = std::string(&r->text[entry.group.size() + 1]);
767 
768  ParamValue v;
769  std::memcpy(&v, &val->valueFloat, 4);
770  m_paramValues[i] = v;
771  }
772  } else {
773  for (uint16_t i = 0; i < numParam; ++i) {
774  auto r = getRequestResult<crtpParamTocGetItemV2Response>(i*2+0);
775  auto val = getRequestResult<crtpParamValueV2Response>(i*2+1);
776 
777  ParamTocEntry& entry = m_paramTocEntries[i];
778  entry.id = i;
779  entry.type = (ParamType)(r->length | r-> type << 2 | r->sign << 3);
780  entry.readonly = r->readonly;
781  entry.group = std::string(&r->text[0]);
782  entry.name = std::string(&r->text[entry.group.size() + 1]);
783 
784  ParamValue v;
785  std::memcpy(&v, &val->valueFloat, 4);
786  m_paramValues[i] = v;
787  }
788  }
789 
790  // Write a cache file
791  {
792  // Atomic file write: write in temporary file first to avoid race conditions
793  std::string fileNameTemp = fileName + ".tmp";
794  std::ofstream output(fileNameTemp);
795  output << "id,type,readonly,group,name" << std::endl;
796  for (const auto& entry : m_paramTocEntries) {
797  output << std::to_string(entry.id) << ","
798  << std::to_string(entry.type) << ","
799  << std::to_string(entry.readonly) << ","
800  << entry.group << ","
801  << entry.name << std::endl;
802  }
803  // change the filename
804  rename(fileNameTemp.c_str(), fileName.c_str());
805  }
806  } else {
807  m_logger.info("Found variables in cache.");
808  m_paramTocEntries.clear();
809  std::string line, cell;
810  std::getline(infile, line); // ignore header
811  while (std::getline(infile, line)) {
812  std::stringstream lineStream(line);
813  m_paramTocEntries.resize(m_paramTocEntries.size() + 1);
814  std::getline(lineStream, cell, ',');
815  m_paramTocEntries.back().id = std::stoi(cell);
816  std::getline(lineStream, cell, ',');
817  m_paramTocEntries.back().type = (ParamType)std::stoi(cell);
818  std::getline(lineStream, cell, ',');
819  m_paramTocEntries.back().readonly = std::stoi(cell);
820  std::getline(lineStream, cell, ',');
821  m_paramTocEntries.back().group = cell;
822  std::getline(lineStream, cell, ',');
823  m_paramTocEntries.back().name = cell;
824  }
825 
826  // Request values
827  if (!m_param_use_V2) {
829  for (size_t i = 0; i < numParam; ++i) {
830  crtpParamReadRequest readRequest(i);
831  addRequest(readRequest, 1);
832  }
833  handleRequests();
834  for (size_t i = 0; i < numParam; ++i) {
835  auto val = getRequestResult<crtpParamValueResponse>(i);
836  ParamValue v;
837  std::memcpy(&v, &val->valueFloat, 4);
838  m_paramValues[i] = v;
839  }
840  } else {
842  for (size_t i = 0; i < numParam; ++i) {
843  crtpParamReadV2Request readRequest(i);
844  addRequest(readRequest, 1);
845  }
846  handleRequests();
847  for (size_t i = 0; i < numParam; ++i) {
848  auto val = getRequestResult<crtpParamValueV2Response>(i);
849  ParamValue v;
850  std::memcpy(&v, &val->valueFloat, 4);
851  m_paramValues[i] = v;
852  }
853  }
854  }
855 }
856 
858 {
859  // Find the number of parameters in TOC
860  crtpMemoryGetNumberRequest infoRequest;
862  addRequest(infoRequest, 1);
863  handleRequests();
864  uint8_t len = getRequestResult<crtpMemoryGetNumberResponse>(0)->numberOfMemories;
865 
866  m_logger.info("Memories: " + std::to_string(len));
867 
868  // Request detailed information and values
870  for (uint8_t i = 0; i < len; ++i) {
871  crtpMemoryGetInfoRequest itemRequest(i);
872  addRequest(itemRequest, 2);
873  }
874  handleRequests();
875 
876  // Update internal structure with obtained data
877  m_memoryTocEntries.resize(len);
878  for (uint8_t i = 0; i < len; ++i) {
879  auto info = getRequestResult<crtpMemoryGetInfoResponse>(i);
880 
882  entry.id = i;
883  entry.type = (MemoryType)info->memType;
884  entry.size = info->memSize;
885  entry.addr = info->memAddr;
886  }
887 }
888 
890 {
892 }
893 
894 void Crazyflie::addSetParam(uint16_t id, const ParamValue& value)
895 {
896  bool found = false;
897  for (auto&& entry : m_paramTocEntries) {
898  if (entry.id == id) {
899  found = true;
900  if (!m_param_use_V2) {
901  switch (entry.type) {
902  case ParamTypeUint8:
903  {
905  addRequest(request, 1);
906  break;
907  }
908  case ParamTypeInt8:
909  {
911  addRequest(request, 1);
912  break;
913  }
914  case ParamTypeUint16:
915  {
917  addRequest(request, 1);
918  break;
919  }
920  case ParamTypeInt16:
921  {
923  addRequest(request, 1);
924  break;
925  }
926  case ParamTypeUint32:
927  {
929  addRequest(request, 1);
930  break;
931  }
932  case ParamTypeInt32:
933  {
935  addRequest(request, 1);
936  break;
937  }
938  case ParamTypeFloat:
939  {
941  addRequest(request, 1);
942  break;
943  }
944  }
945  } else {
946  switch (entry.type) {
947  case ParamTypeUint8:
948  {
950  addRequest(request, 2);
951  break;
952  }
953  case ParamTypeInt8:
954  {
956  addRequest(request, 2);
957  break;
958  }
959  case ParamTypeUint16:
960  {
962  addRequest(request, 2);
963  break;
964  }
965  case ParamTypeInt16:
966  {
968  addRequest(request, 2);
969  break;
970  }
971  case ParamTypeUint32:
972  {
974  addRequest(request, 2);
975  break;
976  }
977  case ParamTypeInt32:
978  {
980  addRequest(request, 2);
981  break;
982  }
983  case ParamTypeFloat:
984  {
986  addRequest(request, 2);
987  break;
988  }
989  }
990  }
991  }
992  }
993 
994  if (!found) {
995  std::stringstream sstr;
996  sstr << "Could not find parameter with id " << id;
997  throw std::runtime_error(sstr.str());
998  }
999 
1000  m_paramValues[id] = value;
1001 }
1002 
1004 {
1005  handleRequests();
1006 }
1007 
1008 void Crazyflie::setParam(uint16_t id, const ParamValue& value)
1009 {
1011  addSetParam(id, value);
1013 }
1014 
1016  const uint8_t* data,
1017  uint32_t length,
1018  bool useSafeLink)
1019 {
1021  sendPacketInternal(data, length, ack, useSafeLink);
1022  return ack.ack;
1023 }
1024 
1026  const uint8_t* data,
1027  uint32_t length,
1028  bool useSafeLink,
1029  float timeout)
1030 {
1031  auto start = std::chrono::system_clock::now();
1032  while (!sendPacketInternal(data, length, useSafeLink)) {
1033  auto end = std::chrono::system_clock::now();
1034  std::chrono::duration<double> elapsedSeconds = end-start;
1035  if (elapsedSeconds.count() > timeout) {
1036  throw std::runtime_error("timeout");
1037  }
1038  }
1039 }
1040 
1042  const uint8_t* data,
1043  uint32_t length,
1045  bool useSafeLink)
1046 {
1047  static uint32_t numPackets = 0;
1048  static uint32_t numAcks = 0;
1049 
1050  numPackets++;
1051 
1052  if (m_radio) {
1053  std::unique_lock<std::mutex> mlock(g_radioMutex[m_devId]);
1054  if (m_radio->getAddress() != m_address) {
1056  }
1057  if (m_radio->getChannel() != m_channel) {
1059  }
1060  if (m_radio->getDatarate() != m_datarate) {
1062  }
1063  if (!m_radio->getAckEnable()) {
1064  m_radio->setAckEnable(true);
1065  }
1066  // consider safelink here:
1067  // Adds 1bit counter to CRTP header to guarantee that no ack (downlink)
1068  // payload are lost and no uplink packet are duplicated.
1069  // The caller should resend packet if not acked (ie. same as with a
1070  // direct call to crazyradio.send_packet)
1071  if (useSafeLink) {
1072  std::vector<uint8_t> dataCopy(data, data + length);
1073  dataCopy[0] &= 0xF3;
1074  dataCopy[0] |= m_curr_up << 3 | m_curr_down << 2;
1075  m_radio->sendPacket(dataCopy.data(), length, ack);
1076  if (ack.ack && ack.size > 0 && (ack.data[0] & 0x04) == (m_curr_down << 2)) {
1077  m_curr_down = 1 - m_curr_down;
1078  }
1079  if (ack.ack) {
1080  m_curr_up = 1 - m_curr_up;
1081  }
1082  } else {
1083  m_radio->sendPacket(data, length, ack);
1084  }
1085 
1086  } else {
1087  std::unique_lock<std::mutex> mlock(g_crazyflieusbMutex[m_devId]);
1088  m_transport->sendPacket(data, length, ack);
1089  }
1090  ack.data[ack.size] = 0;
1091  if (ack.ack) {
1092  handleAck(ack);
1093  numAcks++;
1094  }
1095  if (numPackets == 100) {
1096  if (m_linkQualityCallback) {
1097  // We just take the ratio of sent vs. acked packets here
1098  // for a sliding window of 100 packets
1099  float linkQuality = numAcks / (float)numPackets;
1100  m_linkQualityCallback(linkQuality);
1101  }
1102  numPackets = 0;
1103  numAcks = 0;
1104  }
1105 }
1106 
1108  const ITransport::Ack& result)
1109 {
1110  if (crtpConsoleResponse::match(result)) {
1111  if (result.size > 0) {
1113  if (m_consoleCallback) {
1114  m_consoleCallback(r->text);
1115  }
1116  // std::cout << "Console CF: " << r->text << std::endl;
1117  }
1118  }
1119  else if (crtpLogGetInfoResponse::match(result)) {
1120  // handled in batch system
1121  }
1122  else if (crtpLogGetInfoV2Response::match(result)) {
1123  // handled in batch system
1124  }
1125  else if (crtpLogGetItemResponse::match(result)) {
1126  // handled in batch system
1127  }
1128  else if (crtpLogGetItemV2Response::match(result)) {
1129  // handled in batch system
1130  }
1131  else if (crtpLogControlResponse::match(result)) {
1132  // handled in batch system
1133  }
1134  else if (crtpLogDataResponse::match(result)) {
1136  auto iter = m_logBlockCb.find(r->blockId);
1137  if (iter != m_logBlockCb.end()) {
1138  iter->second(r, result.size - 5);
1139  }
1140  else {
1141  m_logger.warning("Received unrequested data for block: " + std::to_string((int)r->blockId));
1142  }
1143  }
1144  else if (crtpParamTocGetInfoResponse::match(result)) {
1145  // handled in batch system
1146  }
1147  else if (crtpParamTocGetItemResponse::match(result)) {
1148  // handled in batch system
1149  }
1150  else if (crtpParamTocGetInfoV2Response::match(result)) {
1151  // handled in batch system
1152  }
1153  else if (crtpParamTocGetItemV2Response::match(result)) {
1154  // handled in batch system
1155  }
1156  else if (crtpParamSetByNameResponse::match(result)) {
1157  // handled in batch system
1158  }
1159  else if (crtpMemoryGetNumberResponse::match(result)) {
1160  // handled in batch system
1161  }
1162  else if (crtpMemoryGetInfoResponse::match(result)) {
1163  // handled in batch system
1164  }
1165  else if (crtpParamValueResponse::match(result)) {
1166  // handled in batch system
1167  }
1168  else if (crtpMemoryGetNumberResponse::match(result)) {
1169  // handled in batch system
1170  }
1171  else if (crtpMemoryReadResponse::match(result)) {
1172  // handled in batch system
1173  }
1174  else if (crtpMemoryWriteResponse::match(result)) {
1175  // handled in batch system
1176  }
1177  else if (crtp(result.data[0]).port == 8) {
1178  // handled in batch system
1179  }
1180  else if (crtp(result.data[0]).port == 13) {
1181  // handled in batch system
1182  }
1183  else if (crtpPlatformRSSIAck::match(result)) {
1184  if (result.size >= 3) {
1186  if (m_emptyAckCallback) {
1187  m_emptyAckCallback(r);
1188  }
1189  }
1190  }
1191  else {
1192  crtp* header = (crtp*)result.data;
1193  m_logger.warning("Don't know ack: Port: " + std::to_string((int)header->port)
1194  + " Channel: " + std::to_string((int)header->channel)
1195  + " Len: " + std::to_string((int)result.size));
1196  // for (size_t i = 1; i < result.size; ++i) {
1197  // std::cout << " " << (int)result.data[i] << std::endl;
1198  // }
1200  m_genericPacketCallback(result);
1201  }
1202  }
1203 }
1204 
1206  const std::string& group,
1207  const std::string& name) const
1208 {
1209  for (auto&& entry : m_logTocEntries) {
1210  if (entry.group == group && entry.name == name) {
1211  return &entry;
1212  }
1213  }
1214  return nullptr;
1215 }
1216 
1218  const std::string& group,
1219  const std::string& name) const
1220 {
1221  for (auto&& entry : m_paramTocEntries) {
1222  if (entry.group == group && entry.name == name) {
1223  return &entry;
1224  }
1225  }
1226  return nullptr;
1227 }
1228 
1230  std::function<void(crtpLogDataResponse*, uint8_t)> cb)
1231 {
1232  for (uint8_t id = 0; id < 255; ++id) {
1233  if (m_logBlockCb.find(id) == m_logBlockCb.end()) {
1234  m_logBlockCb[id] = cb;
1235  return id;
1236  }
1237  }
1238  return 255;
1239 }
1240 
1242  uint8_t id)
1243 {
1244  m_logBlockCb.erase(m_logBlockCb.find(id));
1245  return true;
1246 }
1247 
1248 // Batch system
1249 
1251 {
1252  m_batchRequests.clear();
1253 }
1254 
1256  const uint8_t* data,
1257  size_t numBytes,
1258  size_t numBytesToMatch)
1259 {
1260  m_batchRequests.resize(m_batchRequests.size() + 1);
1261  m_batchRequests.back().request.resize(numBytes);
1262  memcpy(m_batchRequests.back().request.data(), data, numBytes);
1263  m_batchRequests.back().numBytesToMatch = numBytesToMatch;
1264  m_batchRequests.back().finished = false;
1265 }
1266 
1268  bool crtpMode,
1269  bool useSafeLink,
1270  float baseTime,
1271  float timePerRequest)
1272 {
1273  auto start = std::chrono::system_clock::now();
1277  bool sendPing = false;
1278  const size_t queueSize = 16;
1279 
1280  float timeout = baseTime + timePerRequest * m_batchRequests.size();
1281 
1282  // Workaround for https://github.com/USC-ACTLab/crazyswarm/issues/172
1283  // Disable safelink for now, until packets are really not dropped
1284  // anymore.
1285  if (false /*useSafeLink*/) {
1286 
1287  const size_t numRequests = m_batchRequests.size();
1288  size_t remainingRequests = numRequests;
1289  size_t requestIdx = 0;
1290 
1291  while (remainingRequests > 0) {
1292  remainingRequests = numRequests - m_numRequestsFinished;
1293  // std::cout << "rR: " << remainingRequests << " " << m_numRequestsEnqueued << std::endl;
1294  // enqueue up to queue size
1295  while(m_numRequestsEnqueued < queueSize && requestIdx < numRequests) {
1296  const auto& request = m_batchRequests[requestIdx++];
1297 
1298  do {
1299  sendPacketInternal(request.request.data(), request.request.size(), ack, useSafeLink);
1300  handleBatchAck(ack, crtpMode);
1301 
1302  auto end = std::chrono::system_clock::now();
1303  std::chrono::duration<double> elapsedSeconds = end-start;
1304  if (elapsedSeconds.count() > timeout) {
1305  throw std::runtime_error("timeout");
1306  }
1307  // std::cout << "send req " << requestIdx << std::endl;
1308  } while (!ack.ack);
1310  }
1311  // send ping's until at least one item in queue is done
1312  while(m_numRequestsEnqueued == queueSize
1313  || (m_numRequestsFinished < numRequests && requestIdx == numRequests)) {
1314  crtpEmpty ping;
1315  sendPacket(ping, ack, useSafeLink);
1316  handleBatchAck(ack, crtpMode);
1317 
1318  auto end = std::chrono::system_clock::now();
1319  std::chrono::duration<double> elapsedSeconds = end-start;
1320  if (elapsedSeconds.count() > timeout) {
1321  throw std::runtime_error("timeout");
1322  }
1323  // std::cout << "send ping " << m_numRequestsEnqueued << std::endl;
1324  }
1325  }
1326  } else {
1327  while (true) {
1328  if (!crtpMode || !sendPing) {
1329  for (const auto& request : m_batchRequests) {
1330  if (!request.finished) {
1331  // std::cout << "sendReq" << std::endl;
1332  sendPacketInternal(request.request.data(), request.request.size(), ack, useSafeLink);
1333  handleBatchAck(ack, crtpMode);
1334  auto end = std::chrono::system_clock::now();
1335  std::chrono::duration<double> elapsedSeconds = end-start;
1336  if (elapsedSeconds.count() > timeout) {
1337  throw std::runtime_error("timeout");
1338  }
1339  }
1340  }
1341  if (m_radio && !useSafeLink) {
1342  sendPing = true;
1343  }
1344  } else {
1345  size_t remainingRequests = m_batchRequests.size() - m_numRequestsFinished;
1346  for (size_t i = 0; i < remainingRequests; ++i) {
1347  crtpEmpty ping;
1348  sendPacket(ping, ack, useSafeLink);
1349  handleBatchAck(ack, crtpMode);
1350  // if (ack.ack && crtpPlatformRSSIAck::match(ack)) {
1351  // sendPing = false;
1352  // }
1353 
1354  auto end = std::chrono::system_clock::now();
1355  std::chrono::duration<double> elapsedSeconds = end-start;
1356  if (elapsedSeconds.count() > timeout) {
1357  throw std::runtime_error("timeout");
1358  }
1359  }
1360 
1361  sendPing = false;
1362  }
1363  if (m_numRequestsFinished == m_batchRequests.size()) {
1364  break;
1365  }
1366  }
1367  }
1368 }
1369 
1371  const ITransport::Ack& ack,
1372  bool crtpMode)
1373 {
1374  if (ack.ack) {
1375  for (auto& request : m_batchRequests) {
1376  if (crtpMode) {
1377  if ((crtp(ack.data[0]) == crtp(request.request[0]) || ack.data[0] == request.request[0])
1378  && memcmp(&ack.data[1], &request.request[1], request.numBytesToMatch) == 0
1379  && !request.finished) {
1380  request.ack = ack;
1381  request.finished = true;
1384  // std::cout << "gotack" <<std::endl;
1385  return;
1386  }
1387  } else {
1388  if (!request.finished
1389  && memcmp(&ack.data[0], &request.request[0], request.numBytesToMatch) == 0) {
1390  request.ack = ack;
1391  request.finished = true;
1394  // std::cout << m_numRequestsFinished / (float)m_batchRequests.size() * 100.0 << " %" << std::endl;
1395  return;
1396  }
1397  }
1398  }
1399  // ack is (also) handled in sendPacket
1400  }
1401 }
1402 
1404 {
1407  addRequest(request, 2);
1408  handleRequests();
1409 }
1410 
1411 void Crazyflie::takeoff(float height, float duration, uint8_t groupMask)
1412 {
1413  crtpCommanderHighLevelTakeoffRequest req(groupMask, height, duration);
1414  sendPacketOrTimeout(req);
1415 }
1416 
1417 void Crazyflie::land(float height, float duration, uint8_t groupMask)
1418 {
1419  crtpCommanderHighLevelLandRequest req(groupMask, height, duration);
1420  sendPacketOrTimeout(req);
1421 }
1422 
1424 {
1425  crtpCommanderHighLevelStopRequest req(groupMask);
1426  sendPacketOrTimeout(req);
1427 }
1428 
1429 void Crazyflie::goTo(float x, float y, float z, float yaw, float duration, bool relative, uint8_t groupMask)
1430 {
1431  crtpCommanderHighLevelGoToRequest req(groupMask, relative, x, y, z, yaw, duration);
1432  sendPacketOrTimeout(req);
1433 }
1434 
1436  uint8_t trajectoryId,
1437  uint32_t pieceOffset,
1438  const std::vector<poly4d>& pieces)
1439 {
1440  for (const auto& entry : m_memoryTocEntries) {
1441  if (entry.type == MemoryTypeTRAJ) {
1443  // upload pieces
1444  size_t remainingBytes = sizeof(poly4d) * pieces.size();
1445  size_t numRequests = ceil(remainingBytes / 24.0f);
1446  for (size_t i = 0; i < numRequests; ++i) {
1447  crtpMemoryWriteRequest req(entry.id, pieceOffset * sizeof(poly4d) + i*24);
1448  size_t size = std::min<size_t>(remainingBytes, 24);
1449  memcpy(req.data, reinterpret_cast<const uint8_t*>(pieces.data()) + i * 24, size);
1450  remainingBytes -= size;
1451  addRequestInternal(reinterpret_cast<const uint8_t*>(&req), 6 + size, 5);
1452  }
1453  // define trajectory
1457  req.description.trajectoryIdentifier.mem.offset = pieceOffset * sizeof(poly4d);
1458  req.description.trajectoryIdentifier.mem.n_pieces = (uint8_t)pieces.size();
1459  addRequest(req, 2);
1460  handleRequests();
1461  return;
1462  }
1463  }
1464  throw std::runtime_error("Could not find MemoryTypeTRAJ!");
1465 }
1466 
1468  uint8_t trajectoryId,
1469  float timescale,
1470  bool reversed,
1471  bool relative,
1472  uint8_t groupMask)
1473 {
1474  crtpCommanderHighLevelStartTrajectoryRequest req(groupMask, relative, reversed, trajectoryId, timescale);
1475  sendPacketOrTimeout(req);
1476 }
1477 
1479  std::vector<uint8_t>& data)
1480 {
1481  for (const auto& entry : m_memoryTocEntries) {
1482  if (entry.type == MemoryTypeUSD) {
1484  size_t remainingBytes = entry.size;
1485  size_t numRequests = ceil(remainingBytes / 24.0f);
1486  for (size_t i = 0; i < numRequests; ++i) {
1487  size_t size = std::min<size_t>(remainingBytes, 24);
1488  crtpMemoryReadRequest req(entry.id, i*24, size);
1489  remainingBytes -= size;
1490  addRequest(req, 5);
1491  }
1492  handleRequests();
1493  // put result in data vector
1494  data.resize(entry.size);
1495  remainingBytes = entry.size;
1496  for (size_t i = 0; i < numRequests; ++i) {
1497  size_t size = std::min<size_t>(remainingBytes, 24);
1498  const crtpMemoryReadResponse* response = getRequestResult<crtpMemoryReadResponse>(i);
1499  memcpy(&data[i*24], response->data, size);
1500  remainingBytes -= size;
1501  }
1502  return;
1503  }
1504  }
1505  throw std::runtime_error("Could not find MemoryTypeUSD!");
1506 }
1507 
1509 
1511  const std::string& link_uri)
1512  : m_radio(nullptr)
1513  , m_devId(0)
1514  , m_channel(0)
1515  , m_address(0)
1516  , m_datarate(Crazyradio::Datarate_250KPS)
1517 {
1518  int datarate;
1519  int channel;
1520  char datarateType;
1521  bool success = false;
1522 
1523  success = std::sscanf(link_uri.c_str(), "radio://%d/%d/%d%c/%" SCNx64,
1524  &m_devId, &channel, &datarate,
1525  &datarateType, &m_address) == 5;
1526  if (!success) {
1527  success = std::sscanf(link_uri.c_str(), "radio://%d/%d/%d%c",
1528  &m_devId, &channel, &datarate,
1529  &datarateType) == 4;
1530  m_address = 0xE7E7E7E7E7;
1531  }
1532 
1533  if (success)
1534  {
1535  m_channel = channel;
1536  if (datarate == 250 && datarateType == 'K') {
1538  }
1539  else if (datarate == 1 && datarateType == 'M') {
1541  }
1542  else if (datarate == 2 && datarateType == 'M') {
1544  }
1545 
1546  if (m_devId >= MAX_RADIOS) {
1547  throw std::runtime_error("This version does not support that many radios. Adjust MAX_RADIOS and recompile!");
1548  }
1549 
1550  {
1551  std::unique_lock<std::mutex> mlock(g_radioMutex[m_devId]);
1552  if (!g_crazyradios[m_devId]) {
1553  g_crazyradios[m_devId] = new Crazyradio(m_devId);
1554  g_crazyradios[m_devId]->enableLogging(LOG_COMMUNICATION);
1555  // g_crazyradios[m_devId]->setAckEnable(false);
1556  g_crazyradios[m_devId]->setAckEnable(true);
1557  g_crazyradios[m_devId]->setArc(0);
1558  }
1559  }
1560 
1561  m_radio = g_crazyradios[m_devId];
1562  }
1563  else {
1564  throw std::runtime_error("Uri is not valid!");
1565  }
1566 }
1567 
1569  const uint8_t* data,
1570  uint32_t length)
1571 {
1572  std::unique_lock<std::mutex> mlock(g_radioMutex[m_devId]);
1573  if (m_radio->getAddress() != m_address) {
1575  }
1576  if (m_radio->getChannel() != m_channel) {
1578  }
1579  if (m_radio->getDatarate() != m_datarate) {
1581  }
1582  if (m_radio->getAckEnable()) {
1583  m_radio->setAckEnable(false);
1584  }
1585  m_radio->sendPacketNoAck(data, length);
1586 }
1587 
1589  const uint8_t* data,
1590  uint32_t length)
1591 {
1592  std::unique_lock<std::mutex> mlock(g_radioMutex[m_devId]);
1593  if (m_radio->getAddress() != m_address) {
1595  }
1596  if (m_radio->getChannel() != m_channel) {
1598  }
1599  if (m_radio->getDatarate() != m_datarate) {
1601  }
1602  if (m_radio->getAckEnable()) {
1603  m_radio->setAckEnable(false);
1604  }
1605  m_radio->send2PacketsNoAck(data, length);
1606 }
1607 
1609 {
1610  crtpCommanderHighLevelTakeoffRequest req(groupMask, height, duration);
1611  sendPacket((uint8_t*)&req, sizeof(req));
1612 }
1613 
1615 {
1616  crtpCommanderHighLevelLandRequest req(groupMask, height, duration);
1617  sendPacket((uint8_t*)&req, sizeof(req));
1618 }
1619 
1621 {
1622  crtpCommanderHighLevelStopRequest req(groupMask);
1623  sendPacket((uint8_t*)&req, sizeof(req));
1624 }
1625 
1626 // This is always in relative coordinates
1627 void CrazyflieBroadcaster::goTo(float x, float y, float z, float yaw, float duration, uint8_t groupMask)
1628 {
1629  crtpCommanderHighLevelGoToRequest req(groupMask, true, x, y, z, yaw, duration);
1630  sendPacket((uint8_t*)&req, sizeof(req));
1631 }
1632 
1633 // This is always in relative coordinates
1634 // TODO: this does not support trajectories that are of a different length!
1636  uint8_t trajectoryId,
1637  float timescale,
1638  bool reversed,
1639  uint8_t groupMask)
1640 {
1641  crtpCommanderHighLevelStartTrajectoryRequest req(groupMask, true, reversed, trajectoryId, timescale);
1642  sendPacket((uint8_t*)&req, sizeof(req));
1643 }
1644 
1646  const std::vector<externalPosition>& data)
1647 {
1648  if (data.size() == 0) {
1649  return;
1650  }
1651 
1652  std::vector<crtpExternalPositionPacked> requests(ceil(data.size() / 4.0));
1653  for (size_t i = 0; i < data.size(); ++i) {
1654  size_t j = i / 4;
1655  requests[j].positions[i%4].id = data[i].id;
1656  requests[j].positions[i%4].x = data[i].x * 1000;
1657  requests[j].positions[i%4].y = data[i].y * 1000;
1658  requests[j].positions[i%4].z = data[i].z * 1000;
1659  }
1660  // 1 header byte per packet; 7 bytes for each position
1661  size_t numBytes = requests.size() + data.size() * 7;
1662 
1663  size_t remainingRequests = requests.size();
1664  size_t i = 0;
1665  while (remainingRequests > 0) {
1666  // the crazyradio requires the two packets to be the same size
1667  // -> only send2packets if this is possible
1668  if ( remainingRequests >= 2
1669  && numBytes >= 2 * sizeof(crtpExternalPositionPacked)) {
1670  size_t size = std::min(numBytes, 2 * sizeof(crtpExternalPositionPacked));
1671  send2Packets(reinterpret_cast<const uint8_t*>(&requests[i]), size);
1672  remainingRequests -= 2;
1673  numBytes -= size;
1674  i += 2;
1675  } else {
1676  size_t size = std::min(numBytes, sizeof(crtpExternalPositionPacked));
1677  sendPacket(reinterpret_cast<const uint8_t*>(&requests[i]), size);
1678  remainingRequests -= 1;
1679  numBytes -= size;
1680  i += 1;
1681  }
1682  }
1683  // assert(numBytes == 0);
1684 }
1685 
1687 {
1689  sendPacket((uint8_t*)&req, sizeof(req));
1690 }
1691 
1693 {
1695  sendPacket((uint8_t*)&req, sizeof(req));
1696 }
1697 
1698 // assumes input quaternion is normalized. will fail if not.
1699 static inline uint32_t quatcompress(float const q[4])
1700 {
1701  // we send the values of the quaternion's smallest 3 elements.
1702  unsigned i_largest = 0;
1703  for (unsigned i = 1; i < 4; ++i) {
1704  if (fabsf(q[i]) > fabsf(q[i_largest])) {
1705  i_largest = i;
1706  }
1707  }
1708 
1709  // since -q represents the same rotation as q,
1710  // transform the quaternion so the largest element is positive.
1711  // this avoids having to send its sign bit.
1712  unsigned negate = q[i_largest] < 0;
1713 
1714  // 1/sqrt(2) is the largest possible value
1715  // of the second-largest element in a unit quaternion.
1716 
1717  // do compression using sign bit and 9-bit precision per element.
1718  uint32_t comp = i_largest;
1719  for (unsigned i = 0; i < 4; ++i) {
1720  if (i != i_largest) {
1721  unsigned negbit = (q[i] < 0) ^ negate;
1722  unsigned mag = ((1 << 9) - 1) * (fabsf(q[i]) / (float)M_SQRT1_2) + 0.5f;
1723  comp = (comp << 10) | (negbit << 9) | mag;
1724  }
1725  }
1726 
1727  return comp;
1728 }
1729 
1731  const std::vector<externalPose>& data)
1732 {
1733  if (data.size() == 0) {
1734  return;
1735  }
1736 
1737  std::vector<crtpExternalPosePacked> requests(ceil(data.size() / 2.0));
1738  for (size_t i = 0; i < data.size(); ++i) {
1739  size_t j = i / 2;
1740  requests[j].poses[i%2].id = data[i].id;
1741  requests[j].poses[i%2].x = data[i].x * 1000;
1742  requests[j].poses[i%2].y = data[i].y * 1000;
1743  requests[j].poses[i%2].z = data[i].z * 1000;
1744  float q[4] = { data[i].qx, data[i].qy, data[i].qz, data[i].qw };
1745  requests[j].poses[i%2].quat = quatcompress(q);
1746  }
1747  // 2 header byte per packet; 11 bytes for each position
1748  size_t numBytes = requests.size() * 2 + data.size() * 11;
1749 
1750  size_t remainingRequests = requests.size();
1751  size_t i = 0;
1752  while (remainingRequests > 0) {
1753  // the crazyradio requires the two packets to be the same size
1754  // -> only send2packets if this is possible
1755  if ( remainingRequests >= 2
1756  && numBytes >= 2 * sizeof(crtpExternalPosePacked)) {
1757  size_t size = std::min(numBytes, 2 * sizeof(crtpExternalPosePacked));
1758  send2Packets(reinterpret_cast<const uint8_t*>(&requests[i]), size);
1759  remainingRequests -= 2;
1760  numBytes -= size;
1761  i += 2;
1762  } else {
1763  size_t size = std::min(numBytes, sizeof(crtpExternalPosePacked));
1764  sendPacket(reinterpret_cast<const uint8_t*>(&requests[i]), size);
1765  remainingRequests -= 1;
1766  numBytes -= size;
1767  i += 1;
1768  }
1769  }
1770  // assert(numBytes == 0);
1771 }
1772 
1773 // void CrazyflieBroadcaster::setParam(
1774 // uint8_t group,
1775 // uint8_t id,
1776 // Crazyflie::ParamType type,
1777 // const Crazyflie::ParamValue& value) {
1778 
1779 // switch (type) {
1780 // case Crazyflie::ParamTypeUint8:
1781 // {
1782 // crtpParamWriteBroadcastRequest<uint8_t> request(group, id, value.valueUint8);
1783 // sendPacket((const uint8_t*)&request, sizeof(request));
1784 // break;
1785 // }
1786 // case Crazyflie::ParamTypeInt8:
1787 // {
1788 // crtpParamWriteBroadcastRequest<int8_t> request(group, id, value.valueInt8);
1789 // sendPacket((const uint8_t*)&request, sizeof(request));
1790 // break;
1791 // }
1792 // case Crazyflie::ParamTypeUint16:
1793 // {
1794 // crtpParamWriteBroadcastRequest<uint16_t> request(group, id, value.valueUint16);
1795 // sendPacket((const uint8_t*)&request, sizeof(request));
1796 // break;
1797 // }
1798 // case Crazyflie::ParamTypeInt16:
1799 // {
1800 // crtpParamWriteBroadcastRequest<int16_t> request(group, id, value.valueInt16);
1801 // sendPacket((const uint8_t*)&request, sizeof(request));
1802 // break;
1803 // }
1804 // case Crazyflie::ParamTypeUint32:
1805 // {
1806 // crtpParamWriteBroadcastRequest<uint32_t> request(group, id, value.valueUint32);
1807 // sendPacket((const uint8_t*)&request, sizeof(request));
1808 // break;
1809 // }
1810 // case Crazyflie::ParamTypeInt32:
1811 // {
1812 // crtpParamWriteBroadcastRequest<int32_t> request(group, id, value.valueInt32);
1813 // sendPacket((const uint8_t*)&request, sizeof(request));
1814 // break;
1815 // }
1816 // case Crazyflie::ParamTypeFloat:
1817 // {
1818 // crtpParamWriteBroadcastRequest<float> request(group, id, value.valueFloat);
1819 // sendPacket((const uint8_t*)&request, sizeof(request));
1820 // break;
1821 // }
1822 // }
1823 // // TODO: technically we should update the internal copy of the value of each CF object
1824 // }
Crazyradio * m_radio
Definition: Crazyflie.h:975
static const int MAX_RADIOS
Definition: Crazyflie.cpp:19
void requestLogToc(bool forceNoCache=false)
Definition: Crazyflie.cpp:586
std::vector< crtpPacket_t > m_outgoing_packets
Definition: Crazyflie.h:405
uint8_t trajectoryId
Definition: crtp.h:1113
std::vector< batchRequest > m_batchRequests
Definition: Crazyflie.h:530
int version
Definition: crtp.h:1093
int m_devId
Definition: Crazyflie.h:499
float qy
Definition: crtp.h:456
bool getAckEnable() const
Definition: Crazyradio.h:77
void startTrajectory(uint8_t trajectoryId, float timescale=1.0, bool reversed=false, uint8_t groupMask=0)
Definition: Crazyflie.cpp:1635
Definition: crtp.h:54
float yawrate
Definition: crtp.h:441
uint8_t m_channel
Definition: Crazyflie.h:501
float vy
Definition: crtp.h:440
Crazyradio::Datarate m_datarate
Definition: Crazyflie.h:980
void sendExternalPoses(const std::vector< externalPose > &data)
Definition: Crazyflie.cpp:1730
std::vector< MemoryTocEntry > m_memoryTocEntries
Definition: Crazyflie.h:511
void sendFullStateSetpoint(float x, float y, float z, float vx, float vy, float vz, float ax, float ay, float az, float qx, float qy, float qz, float qw, float rollRate, float pitchRate, float yawRate)
Definition: Crazyflie.cpp:217
const ParamTocEntry * getParamTocEntry(const std::string &group, const std::string &name) const
Definition: Crazyflie.cpp:1217
int16_t vz
Definition: crtp.h:445
void setRequestedParams()
Definition: Crazyflie.cpp:1003
uint8_t reversed
Definition: crtp.h:1112
static const int MAX_USB
Definition: Crazyflie.cpp:20
const T value
Definition: crtp.h:26
float vx
Definition: crtp.h:439
uint32_t remainValidMillisecs
Definition: crtp.h:1086
void sendPing()
Definition: Crazyflie.cpp:264
uint8_t target
float z
Definition: crtp.h:445
void sendExternalPositions(const std::vector< externalPosition > &data)
Definition: Crazyflie.cpp:1645
float zDistance
Definition: crtp.h:442
void readFlash(BootloaderTarget target, size_t size, std::vector< uint8_t > &data)
Definition: Crazyflie.cpp:522
bool m_param_use_V2
Definition: Crazyflie.h:538
std::string getDeviceTypeName()
Definition: Crazyflie.cpp:152
uint8_t channel
Definition: crtp.h:33
void sendHoverSetpoint(float vx, float vy, float yawrate, float zDistance)
Definition: Crazyflie.cpp:207
bool unregisterLogBlock(uint8_t id)
Definition: Crazyflie.cpp:1241
uint8_t type
Definition: crtp.h:23
uint8_t data[24]
Definition: crtp.h:546
void setAckEnable(bool enable)
Definition: Crazyradio.cpp:125
void requestParamToc(bool forceNoCache=false)
Definition: Crazyflie.cpp:696
Definition: crtp.h:17
void startTrajectory(uint8_t trajectoryId, float timescale=1.0, bool reversed=false, bool relative=true, uint8_t groupMask=0)
Definition: Crazyflie.cpp:1467
uint32_t log_crc
Definition: crtp.h:442
static bool match(const Crazyradio::Ack &response)
Definition: crtp.h:148
float qw
Definition: crtp.h:458
void writeFlash(BootloaderTarget target, const std::vector< uint8_t > &data)
Definition: Crazyflie.cpp:374
CrazyflieUSB * g_crazyflieUSB[MAX_USB]
Definition: Crazyflie.cpp:26
uint64_t m_address
Definition: Crazyflie.h:502
void handleRequests(bool crtpMode=true, bool useSafeLink=ENABLE_SAFELINK, float baseTime=2.0, float timePerRequest=0.2)
Definition: Crazyflie.cpp:1267
virtual void sendPacket(const uint8_t *data, uint32_t length, Ack &result)=0
void requestMemoryToc()
Definition: Crazyflie.cpp:857
virtual void sendPacket(const uint8_t *data, uint32_t length, ITransport::Ack &result)
Definition: Crazyradio.cpp:136
uint8_t data[32]
Definition: ITransport.h:19
void notifySetpointsStop(uint32_t remainValidMillisecs)
Definition: Crazyflie.cpp:241
int getProtocolVersion()
Definition: Crazyflie.cpp:134
uint16_t valueUint16
Definition: Crazyflie.h:59
void startBatchRequest()
Definition: Crazyflie.cpp:1250
uint8_t length
Definition: crtp.h:22
uint8_t log_len
Definition: crtp.h:440
void sendVelocityWorldSetpoint(float x, float y, float z, float yawRate)
Definition: Crazyflie.cpp:233
std::mutex g_radioMutex[MAX_RADIOS]
Definition: Crazyflie.cpp:24
uint8_t trajectoryType
Definition: crtp.h:1247
float duration
Definition: Crazyflie.h:27
void emergencyStopWatchdog()
Definition: Crazyflie.cpp:191
static bool match(const Crazyradio::Ack &response)
Definition: crtp.h:588
void uploadTrajectory(uint8_t trajectoryId, uint32_t pieceOffset, const std::vector< poly4d > &pieces)
Definition: Crazyflie.cpp:1435
void setAddress(uint64_t address)
Definition: Crazyradio.cpp:57
const LogTocEntry * getLogTocEntry(const std::string &group, const std::string &name) const
Definition: Crazyflie.cpp:1205
uint16_t nBuffPage
static bool match(const Crazyradio::Ack &response)
Definition: crtp.h:799
constexpr crtp(uint8_t port, uint8_t channel)
Definition: crtp.h:15
void reboot()
Definition: Crazyflie.cpp:287
static bool match(const Crazyradio::Ack &response)
Definition: crtp.h:630
std::function< void(const crtpPlatformRSSIAck *)> m_emptyAckCallback
Definition: Crazyflie.h:513
void sendPacket(const R &request, ITransport::Ack &result, bool useSafeLink=ENABLE_SAFELINK)
Definition: Crazyflie.h:364
uint16_t address
void goTo(float x, float y, float z, float yaw, float duration, uint8_t groupMask=0)
Definition: Crazyflie.cpp:1627
uint8_t size
Definition: ITransport.h:21
uint8_t size
Definition: ITransport.h:1353
void handleAck(const ITransport::Ack &result)
Definition: Crazyflie.cpp:1107
void stop(uint8_t groupMask=0)
Definition: Crazyflie.cpp:1423
int m_curr_down
Definition: Crazyflie.h:535
Logger EmptyLogger
Definition: Crazyflie.cpp:29
void rebootFromBootloader()
Definition: Crazyflie.cpp:329
void transmitPackets()
Definition: Crazyflie.cpp:273
uint8_t data[29]
Definition: crtp.h:363
static bool match(const Crazyradio::Ack &response)
Definition: crtp.h:450
std::mutex g_crazyflieusbMutex[MAX_USB]
Definition: Crazyflie.cpp:27
uint8_t group
Definition: crtp.h:27
float yaw
Definition: crtp.h:442
float height
Definition: crtp.h:1107
char name[30]
Definition: crtp.h:1093
uint8_t result
Definition: crtp.h:440
BootloaderTarget
Definition: Crazyflie.h:84
static uint32_t quatcompress(float const q[4])
Definition: Crazyflie.cpp:1699
static bool match(const Crazyradio::Ack &response)
Definition: crtp.h:15
Logger & m_logger
Definition: Crazyflie.h:543
static const bool LOG_COMMUNICATION
Definition: Crazyflie.cpp:21
size_t m_numRequestsEnqueued
Definition: Crazyflie.h:532
static bool match(const Crazyradio::Ack &response)
Definition: crtp.h:486
void setParam(uint16_t id, const T &value)
Definition: Crazyflie.h:221
uint8_t channel
Definition: crtp.h:37
void sendPacketOrTimeoutInternal(const uint8_t *data, uint32_t length, bool useSafeLink=ENABLE_SAFELINK, float timeout=1.0)
Definition: Crazyflie.cpp:1025
uint8_t trajectoryLocation
Definition: crtp.h:1246
uint32_t crc
Definition: crtp.h:23
uint32_t valueUint32
Definition: Crazyflie.h:61
uint8_t relative
Definition: crtp.h:1115
std::map< uint8_t, std::function< void(crtpLogDataResponse *, uint8_t)> > m_logBlockCb
Definition: Crazyflie.h:506
static bool match(const Crazyradio::Ack &response)
Definition: crtp.h:62
ITransport * m_transport
Definition: Crazyflie.h:498
CrazyflieBroadcaster(const std::string &link_uri)
Definition: Crazyflie.cpp:1510
char text[31]
Definition: crtp.h:67
void send2Packets(const uint8_t *data, uint32_t length)
Definition: Crazyflie.cpp:1588
uint16_t pageSize
float yawRate
Definition: crtp.h:442
virtual void sendPacketNoAck(const uint8_t *data, uint32_t length)
Definition: Crazyradio.cpp:197
uint64_t getAddress() const
Definition: Crazyradio.h:51
static bool match(const Crazyradio::Ack &response)
Definition: crtp.h:841
static bool match(const Crazyradio::Ack &response)
Definition: crtp.h:246
int16_t ay
Definition: crtp.h:447
struct trajectoryDescription description
Definition: crtp.h:1270
Datarate getDatarate() const
Definition: Crazyradio.h:58
void sendExternalPositionUpdate(float x, float y, float z)
Definition: Crazyflie.cpp:247
int m_protocolVersion
Definition: Crazyflie.h:540
uint8_t getChannel() const
Definition: Crazyradio.h:44
static bool match(const Crazyradio::Ack &response)
Definition: crtp.h:285
float timescale
Definition: crtp.h:1114
void sendExternalPoseUpdate(float x, float y, float z, float qx, float qy, float qz, float qw)
Definition: Crazyflie.cpp:256
std::string getFirmwareVersion()
Definition: Crazyflie.cpp:143
Crazyradio * g_crazyradios[MAX_RADIOS]
Definition: Crazyflie.cpp:23
crtpParamTocGetItemRequest request
Definition: crtp.h:21
void handleBatchAck(const ITransport::Ack &result, bool crtpMode)
Definition: Crazyflie.cpp:1370
void addRequestInternal(const uint8_t *data, size_t numBytes, size_t numBytesToMatch)
Definition: Crazyflie.cpp:1255
void readUSDLogFile(std::vector< uint8_t > &data)
Definition: Crazyflie.cpp:1478
uint8_t port
Definition: crtp.h:39
void startSetParamRequest()
Definition: Crazyflie.cpp:889
void land(float height, float duration, uint8_t groupMask=0)
Definition: Crazyflie.cpp:1614
uint32_t offset
Definition: crtp.h:1248
void sendPacketInternal(const uint8_t *data, uint32_t length, ITransport::Ack &result, bool useSafeLink=ENABLE_SAFELINK)
Definition: Crazyflie.cpp:1041
bool m_log_use_V2
Definition: Crazyflie.h:537
uint16_t page
void setArc(uint8_t arc)
Definition: Crazyradio.cpp:95
int16_t az
Definition: crtp.h:448
float x
Definition: crtp.h:443
void setDatarate(Datarate datarate)
Definition: Crazyradio.cpp:84
uint32_t offset
Definition: crtp.h:1251
size_t m_numRequestsFinished
Definition: Crazyflie.h:531
void syson()
Definition: Crazyflie.cpp:353
virtual void info(const std::string &)
Definition: Crazyflie.h:23
void setGroupMask(uint8_t groupMask)
Definition: Crazyflie.cpp:1403
uint8_t id
Definition: crtp.h:31
uint8_t registerLogBlock(std::function< void(crtpLogDataResponse *, uint8_t)> cb)
Definition: Crazyflie.cpp:1229
void sysoff()
Definition: Crazyflie.cpp:337
virtual void warning(const std::string &)
Definition: Crazyflie.h:24
void sendPacket(const uint8_t *data, uint32_t length)
Definition: Crazyflie.cpp:1568
void stop(uint8_t groupMask=0)
Definition: Crazyflie.cpp:1620
uint8_t n_pieces
Definition: crtp.h:1252
void sendSetpoint(float roll, float pitch, float yawrate, uint16_t thrust)
Definition: Crazyflie.cpp:169
float qz
Definition: crtp.h:457
std::vector< LogTocEntry > m_logTocEntries
Definition: Crazyflie.h:505
void enableLogging(bool enable)
Definition: ITransport.cpp:3
void sendStop()
Definition: Crazyflie.cpp:179
uint8_t numberOfMemories
Definition: crtp.h:439
union trajectoryDescription::@8 trajectoryIdentifier
static bool match(const Crazyradio::Ack &response)
Definition: crtp.h:552
uint8_t groupMask
Definition: crtp.h:1102
std::vector< ParamTocEntry > m_paramTocEntries
Definition: Crazyflie.h:508
int16_t ax
Definition: crtp.h:446
uint8_t blockId
Definition: crtp.h:769
void land(float height, float duration, uint8_t groupMask=0)
Definition: Crazyflie.cpp:1417
const crtp header
Definition: crtp.h:29
static bool match(const Crazyradio::Ack &response)
Definition: crtp.h:749
static bool match(const Crazyradio::Ack &response)
Definition: crtp.h:1336
static bool match(const Crazyradio::Ack &response)
Definition: crtp.h:109
void alloff()
Definition: Crazyflie.cpp:345
#define ENABLE_SAFELINK
Definition: Crazyflie.h:15
static bool match(const Crazyradio::Ack &response)
Definition: crtp.h:763
static bool match(const Crazyradio::Ack &response)
Definition: crtp.h:520
static size_t size(LogType t)
Definition: Crazyflie.h:291
uint8_t ack
Definition: ITransport.h:1348
Crazyradio::Datarate m_datarate
Definition: Crazyflie.h:503
Crazyradio * m_radio
Definition: Crazyflie.h:497
virtual void send2PacketsNoAck(const uint8_t *data, uint32_t totalLength)
Definition: Crazyradio.cpp:233
float y
Definition: crtp.h:444
void addRequest(const R &request, size_t numBytesToMatch)
Definition: Crazyflie.h:415
uint64_t rebootToBootloader()
Definition: Crazyflie.cpp:298
float qx
Definition: crtp.h:455
void sendPositionSetpoint(float x, float y, float z, float yaw)
Definition: Crazyflie.cpp:197
uint16_t flashStart
Crazyflie(const std::string &link_uri, Logger &logger=EmptyLogger, std::function< void(const char *)> consoleCb=nullptr)
Definition: Crazyflie.cpp:32
std::map< uint16_t, ParamValue > m_paramValues
Definition: Crazyflie.h:509
std::function< void(const char *)> m_consoleCallback
Definition: Crazyflie.h:515
std::function< void(float)> m_linkQualityCallback
Definition: Crazyflie.h:514
uint8_t numParam
Definition: crtp.h:22
void sendPacketOrTimeout(const R &request, bool useSafeLink=ENABLE_SAFELINK)
Definition: Crazyflie.h:394
void takeoff(float height, float duration, uint8_t groupMask=0)
Definition: Crazyflie.cpp:1608
void addSetParam(uint16_t id, const T &value)
Definition: Crazyflie.h:257
void logReset()
Definition: Crazyflie.cpp:161
std::function< void(const ITransport::Ack &)> m_genericPacketCallback
Definition: Crazyflie.h:516
void emergencyStop()
Definition: Crazyflie.cpp:185
void goTo(float x, float y, float z, float yaw, float duration, bool relative=false, uint8_t groupMask=0)
Definition: Crazyflie.cpp:1429
int m_curr_up
Definition: Crazyflie.h:534
void setChannel(uint8_t channel)
Definition: Crazyradio.cpp:51
float vbat()
Definition: Crazyflie.cpp:361
void takeoff(float height, float duration, uint8_t groupMask=0)
Definition: Crazyflie.cpp:1411


crazyflie_cpp
Author(s): Wolfgang Hoenig
autogenerated on Mon Sep 28 2020 03:40:10