Crazyflie.h
Go to the documentation of this file.
1 #pragma once
2 
3 #include <cstring>
4 #include <sstream>
5 #include <functional>
6 #include <math.h>
7 
8 #include "Crazyradio.h"
9 #include "crtp.h"
10 #include <list>
11 #include <set>
12 #include <map>
13 #include <chrono>
14 
15 #define ENABLE_SAFELINK 1
16 
17 class Logger
18 {
19 public:
20  Logger() {}
21  virtual ~Logger() {}
22 
23  virtual void info(const std::string& /*msg*/) {}
24  virtual void warning(const std::string& /*msg*/) {}
25  virtual void error(const std::string& /*msg*/) {}
26 };
27 
28 extern Logger EmptyLogger;
29 
30 class Crazyflie
31 {
32 public:
33  enum ParamType {
34  ParamTypeUint8 = 0x00 | (0x00<<2) | (0x01<<3),
35  ParamTypeInt8 = 0x00 | (0x00<<2) | (0x00<<3),
36  ParamTypeUint16 = 0x01 | (0x00<<2) | (0x01<<3),
37  ParamTypeInt16 = 0x01 | (0x00<<2) | (0x00<<3),
38  ParamTypeUint32 = 0x02 | (0x00<<2) | (0x01<<3),
39  ParamTypeInt32 = 0x02 | (0x00<<2) | (0x00<<3),
40  ParamTypeFloat = 0x02 | (0x01<<2) | (0x00<<3),
41  };
42 
43  struct ParamTocEntry {
44  uint16_t id;
46  bool readonly;
47  // ParamLength length;
48  // ParamType type;
49  // ParamSign sign;
50  // bool readonly;
51  // ParamGroup paramGroup;
52  std::string group;
53  std::string name;
54  };
55 
56  union ParamValue{
57  uint8_t valueUint8;
58  int8_t valueInt8;
59  uint16_t valueUint16;
60  int16_t valueInt16;
61  uint32_t valueUint32;
62  int32_t valueInt32;
63  float valueFloat;
64  };
65 
66  enum LogType {
67  LogTypeUint8 = 1,
68  LogTypeUint16 = 2,
69  LogTypeUint32 = 3,
70  LogTypeInt8 = 4,
71  LogTypeInt16 = 5,
72  LogTypeInt32 = 6,
73  LogTypeFloat = 7,
74  LogTypeFP16 = 8,
75  };
76 
77  struct LogTocEntry {
78  uint16_t id;
80  std::string group;
81  std::string name;
82  };
83 
85  TargetSTM32 = 0xFF,
86  TargetNRF51 = 0xFE,
87  };
88 
89  enum MemoryType {
90  MemoryTypeEEPROM = 0x00,
91  MemoryTypeOneWire = 0x01,
92  MemoryTypeLED12 = 0x10,
93  MemoryTypeLOCO = 0x11,
94  MemoryTypeTRAJ = 0x12,
95  MemoryTypeLOCO2 = 0x13,
96  MemoryTypeLH = 0x14,
97  MemoryTypeTester = 0x15,
98  MemoryTypeUSD = 0x16, // Crazyswarm experimental
99  };
100 
101  struct MemoryTocEntry {
102  uint16_t id;
104  uint32_t size;
105  uint64_t addr;
106  };
107 
108  struct poly4d {
109  float p[4][8];
110  float duration;
111  } __attribute__((packed));
112 
113 public:
114  Crazyflie(
115  const std::string& link_uri,
116  Logger& logger = EmptyLogger,
117  std::function<void(const char*)> consoleCb = nullptr);
118 
119  int getProtocolVersion();
120 
121  std::string getFirmwareVersion();
122 
123  std::string getDeviceTypeName();
124 
125  void logReset();
126 
127  void sendSetpoint(
128  float roll,
129  float pitch,
130  float yawrate,
131  uint16_t thrust);
132 
133  void sendFullStateSetpoint(
134  float x, float y, float z,
135  float vx, float vy, float vz,
136  float ax, float ay, float az,
137  float qx, float qy, float qz, float qw,
138  float rollRate, float pitchRate, float yawRate);
139 
140  void sendHoverSetpoint(
141  float vx,
142  float vy,
143  float yawrate,
144  float zDistance);
145 
146  void sendStop();
147 
148  void emergencyStop();
149 
150  void emergencyStopWatchdog();
151 
152  void sendPositionSetpoint(
153  float x,
154  float y,
155  float z,
156  float yaw);
157 
158  void sendExternalPositionUpdate(
159  float x,
160  float y,
161  float z);
162 
163  void sendExternalPoseUpdate(
164  float x, float y, float z,
165  float qx, float qy, float qz, float qw);
166 
167  void sendPing();
168 
169  void reboot();
170  // returns new address
171  uint64_t rebootToBootloader();
172 
173  void rebootFromBootloader();
174 
175  void sysoff();
176  void alloff();
177  void syson();
178  float vbat();
179 
180  void writeFlash(
182  const std::vector<uint8_t>& data);
183  void readFlash(
185  size_t size,
186  std::vector<uint8_t>& data);
187 
188  void requestLogToc(bool forceNoCache=false);
189 
190  void requestParamToc(bool forceNoCache=false);
191 
192  void requestMemoryToc();
193 
194  std::vector<ParamTocEntry>::const_iterator paramsBegin() const {
195  return m_paramTocEntries.begin();
196  }
197  std::vector<ParamTocEntry>::const_iterator paramsEnd() const {
198  return m_paramTocEntries.end();
199  }
200 
201  std::vector<LogTocEntry>::const_iterator logVariablesBegin() const {
202  return m_logTocEntries.begin();
203  }
204  std::vector<LogTocEntry>::const_iterator logVariablesEnd() const {
205  return m_logTocEntries.end();
206  }
207 
208  std::vector<MemoryTocEntry>::const_iterator memoriesBegin() const {
209  return m_memoryTocEntries.begin();
210  }
211  std::vector<MemoryTocEntry>::const_iterator memoriesEnd() const {
212  return m_memoryTocEntries.end();
213  }
214 
215  template<class T>
216  void setParam(uint16_t id, const T& value) {
217  ParamValue v;
218  memcpy(&v, &value, sizeof(value));
219  setParam(id, v);
220  }
221 
222  template<class T>
223  void setParamByName(const char* group, const char* name, const T& value) {
224  crtpParamSetByNameRequest<T> request(group, name, value);
225  // sendPacketOrTimeoutInternal(reinterpret_cast<const uint8_t*>(&request), request.size());
226  startBatchRequest();
227  addRequestInternal(
228  reinterpret_cast<const uint8_t*>(&request), request.size(), request.responseSize() - 1);
229  handleRequests();
230  auto response = getRequestResult<crtpParamSetByNameResponse>(0);
231 
232  uint8_t error = response->error(request.responseSize());
233  if (error != 0) {
234  std::stringstream sstr;
235  sstr << "Couldn't set parameter " << group << "." << name << "!";
236  if (error == ENOENT) {
237  sstr << "No such variable." << std::endl;
238  } else if (error == EINVAL) {
239  sstr << "Wrong type." << std::endl;
240  } else if (error == EACCES) {
241  sstr << "Variable is readonly." << std::endl;
242  } else {
243  sstr << " Error: " << (int)error << std::endl;
244  }
245  throw std::runtime_error(sstr.str());
246  }
247  }
248 
249  void startSetParamRequest();
250 
251  template<class T>
252  void addSetParam(uint16_t id, const T& value) {
253  ParamValue v;
254  memcpy(&v, &value, sizeof(value));
255  addSetParam(id, v);
256  }
257 
258  void setRequestedParams();
259 
260 
261  template<class T>
262  T getParam(uint16_t id) const {
263  ParamValue v = getParam(id);
264  return *(reinterpret_cast<T*>(&v));
265  }
266 
267  const ParamTocEntry* getParamTocEntry(
268  const std::string& group,
269  const std::string& name) const;
270 
272  std::function<void(const crtpPlatformRSSIAck*)> cb) {
273  m_emptyAckCallback = cb;
274  }
275 
277  std::function<void(float)> cb) {
278  m_linkQualityCallback = cb;
279  }
280 
282  std::function<void(const char*)> cb) {
283  m_consoleCallback = cb;
284  }
285 
286  static size_t size(LogType t) {
287  switch(t) {
288  case LogTypeUint8:
289  case LogTypeInt8:
290  return 1;
291  break;
292  case LogTypeUint16:
293  case LogTypeInt16:
294  case LogTypeFP16:
295  return 2;
296  break;
297  case LogTypeUint32:
298  case LogTypeInt32:
299  case LogTypeFloat:
300  return 4;
301  break;
302  default:
303  // assert(false);
304  return 0;
305  }
306  }
307 
308 
310  std::function<void(const ITransport::Ack&)> cb) {
311  m_genericPacketCallback = cb;
312  }
313 
318  void queueOutgoingPacket(const crtpPacket_t& packet) {
319  m_outgoing_packets.push_back(packet);
320  }
321 
322  void transmitPackets();
323 
324  // High-Level setpoints
325  void setGroupMask(uint8_t groupMask);
326 
327  void takeoff(float height, float duration, uint8_t groupMask = 0);
328 
329  void land(float height, float duration, uint8_t groupMask = 0);
330 
331  void stop(uint8_t groupMask = 0);
332 
333  void goTo(float x, float y, float z, float yaw, float duration, bool relative = false, uint8_t groupMask = 0);
334 
335  void uploadTrajectory(
336  uint8_t trajectoryId,
337  uint32_t pieceOffset,
338  const std::vector<poly4d>& pieces);
339 
340  void startTrajectory(
341  uint8_t trajectoryId,
342  float timescale = 1.0,
343  bool reversed = false,
344  bool relative = true,
345  uint8_t groupMask = 0);
346 
347  // Memory subsystem
348  void readUSDLogFile(
349  std::vector<uint8_t>& data);
350 
351 private:
352  void sendPacketInternal(
353  const uint8_t* data,
354  uint32_t length,
356  bool useSafeLink = ENABLE_SAFELINK);
357 
358  template<typename R>
360  const R& request,
361  ITransport::Ack& result,
362  bool useSafeLink = ENABLE_SAFELINK)
363  {
364  sendPacketInternal(
365  reinterpret_cast<const uint8_t*>(&request), sizeof(request), result, useSafeLink);
366  }
367 
368  bool sendPacketInternal(
369  const uint8_t* data,
370  uint32_t length,
371  bool useSafeLink = ENABLE_SAFELINK);
372 
373  template<typename R>
375  const R& request,
376  bool useSafeLink = ENABLE_SAFELINK)
377  {
378  sendPacketInternal(
379  reinterpret_cast<const uint8_t*>(&request), sizeof(request), useSafeLink);
380  }
381 
382  void sendPacketOrTimeoutInternal(
383  const uint8_t* data,
384  uint32_t length,
385  bool useSafeLink = ENABLE_SAFELINK,
386  float timeout = 1.0);
387 
388  template<typename R>
390  const R& request,
391  bool useSafeLink = ENABLE_SAFELINK)
392  {
393  sendPacketOrTimeoutInternal(
394  reinterpret_cast<const uint8_t*>(&request), sizeof(request), useSafeLink);
395  }
396 
397  void handleAck(
398  const ITransport::Ack& result);
399 
400  std::vector<crtpPacket_t> m_outgoing_packets;
401 
402  void startBatchRequest();
403 
404  void addRequestInternal(
405  const uint8_t* data,
406  size_t numBytes,
407  size_t numBytesToMatch);
408 
409  template<typename R>
411  const R& request,
412  size_t numBytesToMatch)
413  {
414  addRequestInternal(
415  reinterpret_cast<const uint8_t*>(&request), sizeof(request), numBytesToMatch);
416  }
417 
418  void handleRequests(
419  bool crtpMode = true,
420  bool useSafeLink = ENABLE_SAFELINK,
421  float baseTime = 2.0,
422  float timePerRequest = 0.2);
423 
424 
425  void handleBatchAck(
426  const ITransport::Ack& result,
427  bool crtpMode);
428 
429  template<typename R>
430  const R* getRequestResult(size_t index) const {
431  return reinterpret_cast<const R*>(m_batchRequests[index].ack.data);
432  }
433 
434 private:
435  struct logInfo {
436  uint8_t len;
437  uint32_t log_crc;
438  uint8_t log_max_packet;
439  uint8_t log_max_ops;
440  };
441 
443 
444  struct paramInfo {
445  uint8_t len;
446  uint32_t crc;
447  };
448 
449  // enum ParamLength {
450  // ParamLength1Byte = 0,
451  // ParamLength2Bytes = 1,
452  // ParamLength3Bytes = 2,
453  // ParamLength4Bytes = 3,
454  // };
455 
456  // enum ParamType {
457  // ParamTypeInt = 0,
458  // ParamTypeFloat = 1,
459  // };
460 
461  // enum ParamSign {
462  // ParamSignSigned = 0,
463  // ParamSignUnsigned = 1,
464  // };
465 
466  // enum ParamGroup {
467  // ParamGroupVariable = 0,
468  // ParamGroupGroup = 1,
469  // };
470 
471 
472 private:
473  const LogTocEntry* getLogTocEntry(
474  const std::string& group,
475  const std::string& name) const;
476 
477  uint8_t registerLogBlock(
478  std::function<void(crtpLogDataResponse*, uint8_t)> cb);
479 
480  bool unregisterLogBlock(
481  uint8_t id);
482 
483  void setParam(uint16_t id, const ParamValue& value);
484  void addSetParam(uint16_t id, const ParamValue& value);
485 
486 
487  const ParamValue& getParam(uint16_t id) const {
488  return m_paramValues.at(id);
489  }
490 
491 private:
494  int m_devId;
495 
496  uint8_t m_channel;
497  uint64_t m_address;
499 
500  std::vector<LogTocEntry> m_logTocEntries;
501  std::map<uint8_t, std::function<void(crtpLogDataResponse*, uint8_t)> > m_logBlockCb;
502 
503  std::vector<ParamTocEntry> m_paramTocEntries;
504  std::map<uint16_t, ParamValue> m_paramValues;
505 
506  std::vector<MemoryTocEntry> m_memoryTocEntries;
507 
508  std::function<void(const crtpPlatformRSSIAck*)> m_emptyAckCallback;
509  std::function<void(float)> m_linkQualityCallback;
510  std::function<void(const char*)> m_consoleCallback;
511  std::function<void(const ITransport::Ack&)> m_genericPacketCallback;
512 
513  template<typename T>
514  friend class LogBlock;
515  friend class LogBlockGeneric;
516 
517  // batch system
519  {
520  std::vector<uint8_t> request;
523  bool finished;
524  };
525  std::vector<batchRequest> m_batchRequests;
528 
531 
534 
536 
537  // logging
539 };
540 
541 template<class T>
542 class LogBlock
543 {
544 public:
546  Crazyflie* cf,
547  std::list<std::pair<std::string, std::string> > variables,
548  std::function<void(uint32_t, T*)>& callback)
549  : m_cf(cf)
550  , m_callback(callback)
551  , m_id(0)
552  {
553  m_id = m_cf->registerLogBlock([=](crtpLogDataResponse* r, uint8_t s) { this->handleData(r, s);});
554  if (m_cf->m_log_use_V2) {
555  std::vector<logBlockItemV2> logBlockItems;
556  size_t s = 0;
557  for (auto&& pair : variables) {
558  const Crazyflie::LogTocEntry* entry = m_cf->getLogTocEntry(pair.first, pair.second);
559  if (entry) {
560  s += Crazyflie::size(entry->type);
561  if (s > 26) {
562  std::stringstream sstr;
563  sstr << "Can't configure that many variables in a single log block!"
564  << " Ignoring " << pair.first << "." << pair.second << std::endl;
565  throw std::runtime_error(sstr.str());
566  } else {
567  logBlockItems.push_back({static_cast<uint8_t>(entry->type), entry->id});
568  }
569  }
570  else {
571  std::stringstream sstr;
572  sstr << "Could not find " << pair.first << "." << pair.second << " in log toc!";
573  throw std::runtime_error(sstr.str());
574  }
575  }
576 
577  // we can use up to 9 items per request
578  size_t requests = ceil(logBlockItems.size() / 9.0f);
579  size_t i = 0;
580  for (size_t r = 0; r < requests; ++r) {
581  size_t numElements = std::min<size_t>(logBlockItems.size() - i, 9);
582  if (r == 0) {
584  request.id = m_id;
585  memcpy(request.items, &logBlockItems[i], sizeof(logBlockItemV2) * numElements);
586  m_cf->sendPacketOrTimeoutInternal(reinterpret_cast<const uint8_t*>(&request), 3 + 3*numElements);
587  } else {
589  request.id = m_id;
590  memcpy(request.items, &logBlockItems[i], sizeof(logBlockItemV2) * numElements);
591  m_cf->sendPacketOrTimeoutInternal(reinterpret_cast<const uint8_t*>(&request), 3 + 3*numElements);
592  }
593  i += numElements;
594  }
595  // auto r = m_cf->getRequestResult<crtpLogControlResponse>(0);
596  // if (r->result != crtpLogControlResultOk
597  // && r->result != crtpLogControlResultBlockExists) {
598  // std::stringstream sstr;
599  // sstr << "Could not create log block! Result: " << (int)r->result << " " << (int)m_id << " " << (int)r->requestByte1 << " " << (int)r->command;
600  // throw std::runtime_error(sstr.str());
601  // }
602  } else {
604  request.id = m_id;
605  int i = 0;
606  for (auto&& pair : variables) {
607  const Crazyflie::LogTocEntry* entry = m_cf->getLogTocEntry(pair.first, pair.second);
608  if (entry) {
609  request.items[i].logType = entry->type;
610  request.items[i].id = entry->id;
611  ++i;
612  }
613  else {
614  std::stringstream sstr;
615  sstr << "Could not find " << pair.first << "." << pair.second << " in log toc!";
616  throw std::runtime_error(sstr.str());
617  }
618  }
619 
620  m_cf->startBatchRequest();
621  m_cf->addRequestInternal(reinterpret_cast<const uint8_t*>(&request), 3 + 2*i, 2);
622  m_cf->handleRequests();
623  auto r = m_cf->getRequestResult<crtpLogControlResponse>(0);
624  if (r->result != crtpLogControlResultOk
625  && r->result != crtpLogControlResultBlockExists) {
626  std::stringstream sstr;
627  sstr << "Could not create log block! Result: " << (int)r->result << " " << (int)m_id << " " << (int)r->requestByte1 << " " << (int)r->command;
628  throw std::runtime_error(sstr.str());
629  }
630  }
631  }
632 
634  {
635  stop();
636  m_cf->unregisterLogBlock(m_id);
637  }
638 
639 
640  void start(uint8_t period)
641  {
642  crtpLogStartRequest request(m_id, period);
643  m_cf->startBatchRequest();
644  m_cf->addRequest(request, 2);
645  m_cf->handleRequests();
646  }
647 
648  void stop()
649  {
651  m_cf->startBatchRequest();
652  m_cf->addRequest(request, 2);
653  m_cf->handleRequests();
654  }
655 
656 private:
657  void handleData(crtpLogDataResponse* response, uint8_t size) {
658  if (size == sizeof(T)) {
659  uint32_t time_in_ms = ((uint32_t)response->timestampHi << 8) | (response->timestampLo);
660  T* t = (T*)response->data;
661  m_callback(time_in_ms, t);
662  }
663  else {
664  std::stringstream sstr;
665  sstr << "Size doesn't match! Is: " << (size_t)size << " expected: " << sizeof(T);
666  throw std::runtime_error(sstr.str());
667  }
668  }
669 
670 private:
672  std::function<void(uint32_t, T*)> m_callback;
673  uint8_t m_id;
674 };
675 
677 
679 {
680 public:
682  Crazyflie* cf,
683  const std::vector<std::string>& variables,
684  void* userData,
685  std::function<void(uint32_t, std::vector<double>*, void* userData)>& callback)
686  : m_cf(cf)
687  , m_userData(userData)
688  , m_callback(callback)
689  , m_id(0)
690  {
691  m_id = m_cf->registerLogBlock([=](crtpLogDataResponse* r, uint8_t s) { this->handleData(r, s);});
692  if (m_cf->m_log_use_V2) {
694  request.id = m_id;
695  int i = 0;
696  size_t s = 0;
697  for (auto&& var : variables) {
698  auto pos = var.find(".");
699  std::string first = var.substr(0, pos);
700  std::string second = var.substr(pos+1);
701  const Crazyflie::LogTocEntry* entry = m_cf->getLogTocEntry(first, second);
702  if (entry) {
703  s += Crazyflie::size(entry->type);
704  if (s > 26) {
705  std::stringstream sstr;
706  sstr << "Can't configure that many variables in a single log block!"
707  << " Ignoring " << first << "." << second << std::endl;
708  throw std::runtime_error(sstr.str());
709  } else {
710  if (i < 9) {
711  request.items[i].logType = entry->type;
712  request.items[i].id = entry->id;
713  ++i;
714  m_types.push_back(entry->type);
715  } else {
716  std::stringstream sstr;
717  sstr << "Can only log up to 9 variables at a time!"
718  << " Ignoring " << first << "." << second << std::endl;
719  throw std::runtime_error(sstr.str());
720  }
721  }
722  }
723  else {
724  std::stringstream sstr;
725  sstr << "Could not find " << first << "." << second << " in log toc!";
726  throw std::runtime_error(sstr.str());
727  }
728  }
729  m_cf->startBatchRequest();
730  m_cf->addRequestInternal(reinterpret_cast<const uint8_t*>(&request), 3 + 3*i, 2);
731  m_cf->handleRequests();
732  auto r = m_cf->getRequestResult<crtpLogControlResponse>(0);
733  if (r->result != crtpLogControlResultOk
734  && r->result != crtpLogControlResultBlockExists) {
735  throw std::runtime_error("Could not create log block!");
736  }
737  } else {
739  request.id = m_id;
740  int i = 0;
741  size_t s = 0;
742  for (auto&& var : variables) {
743  auto pos = var.find(".");
744  std::string first = var.substr(0, pos);
745  std::string second = var.substr(pos+1);
746  const Crazyflie::LogTocEntry* entry = m_cf->getLogTocEntry(first, second);
747  if (entry) {
748  s += Crazyflie::size(entry->type);
749  if (s > 26) {
750  std::stringstream sstr;
751  sstr << "Can't configure that many variables in a single log block!"
752  << " Ignoring " << first << "." << second << std::endl;
753  throw std::runtime_error(sstr.str());
754  } else {
755  request.items[i].logType = entry->type;
756  request.items[i].id = entry->id;
757  ++i;
758  m_types.push_back(entry->type);
759  }
760  }
761  else {
762  std::stringstream sstr;
763  sstr << "Could not find " << first << "." << second << " in log toc!";
764  throw std::runtime_error(sstr.str());
765  }
766  }
767  m_cf->startBatchRequest();
768  m_cf->addRequestInternal(reinterpret_cast<const uint8_t*>(&request), 3 + 2*i, 2);
769  m_cf->handleRequests();
770  auto r = m_cf->getRequestResult<crtpLogControlResponse>(0);
771  if (r->result != crtpLogControlResultOk
772  && r->result != crtpLogControlResultBlockExists) {
773  throw std::runtime_error("Could not create log block!");
774  }
775  }
776  }
777 
779  {
780  stop();
781  m_cf->unregisterLogBlock(m_id);
782  }
783 
784 
785  void start(uint8_t period)
786  {
787  crtpLogStartRequest request(m_id, period);
788  m_cf->startBatchRequest();
789  m_cf->addRequest(request, 2);
790  m_cf->handleRequests();
791  }
792 
793  void stop()
794  {
796  m_cf->startBatchRequest();
797  m_cf->addRequest(request, 2);
798  m_cf->handleRequests();
799  }
800 
801 private:
802  void handleData(crtpLogDataResponse* response, uint8_t /*size*/) {
803 
804  std::vector<double> result;
805  size_t pos = 0;
806  for (size_t i = 0; i < m_types.size(); ++i)
807  {
808  switch (m_types[i])
809  {
811  {
812  uint8_t value;
813  memcpy(&value, &response->data[pos], sizeof(uint8_t));
814  result.push_back(value);
815  pos += sizeof(uint8_t);
816  break;
817  }
819  {
820  int8_t value;
821  memcpy(&value, &response->data[pos], sizeof(int8_t));
822  result.push_back(value);
823  pos += sizeof(int8_t);
824  break;
825  }
827  {
828  uint16_t value;
829  memcpy(&value, &response->data[pos], sizeof(uint16_t));
830  result.push_back(value);
831  pos += sizeof(uint16_t);
832  break;
833  }
835  {
836  int16_t value;
837  memcpy(&value, &response->data[pos], sizeof(int16_t));
838  result.push_back(value);
839  pos += sizeof(int16_t);
840  break;
841  }
843  {
844  uint32_t value;
845  memcpy(&value, &response->data[pos], sizeof(uint32_t));
846  result.push_back(value);
847  pos += sizeof(uint32_t);
848  break;
849  }
851  {
852  int32_t value;
853  memcpy(&value, &response->data[pos], sizeof(int32_t));
854  result.push_back(value);
855  pos += sizeof(int32_t);
856  break;
857  }
859  {
860  float value;
861  memcpy(&value, &response->data[pos], sizeof(float));
862  result.push_back(value);
863  pos += sizeof(float);
864  break;
865  }
867  {
868  double value;
869  memcpy(&value, &response->data[pos], sizeof(double));
870  result.push_back(value);
871  pos += sizeof(double);
872  break;
873  }
874  }
875  }
876 
877  uint32_t time_in_ms = ((uint32_t)response->timestampHi << 8) | (response->timestampLo);
878  m_callback(time_in_ms, &result, m_userData);
879  }
880 
881 private:
883  void* m_userData;
884  std::function<void(uint32_t, std::vector<double>*, void*)> m_callback;
885  uint8_t m_id;
886  std::vector<Crazyflie::LogType> m_types;
887 };
888 
890 
892 {
893 public:
895  const std::string& link_uri);
896 
897  // High-Level setpoints
898  void takeoff(float height, float duration, uint8_t groupMask = 0);
899 
900  void land(float height, float duration, uint8_t groupMask = 0);
901 
902  void stop(uint8_t groupMask = 0);
903 
904  // This is always in relative coordinates
905  void goTo(float x, float y, float z, float yaw, float duration, uint8_t groupMask = 0);
906 
907  // This is always in relative coordinates
908  void startTrajectory(
909  uint8_t trajectoryId,
910  float timescale = 1.0,
911  bool reversed = false,
912  uint8_t groupMask = 0);
913 
915  {
916  uint8_t id;
917  float x;
918  float y;
919  float z;
920  };
921 
922  void sendExternalPositions(
923  const std::vector<externalPosition>& data);
924 
926  {
927  uint8_t id;
928  float x;
929  float y;
930  float z;
931  float qx;
932  float qy;
933  float qz;
934  float qw;
935  };
936 
937  void sendExternalPoses(
938  const std::vector<externalPose>& data);
939 
940  void emergencyStop();
941 
942  void emergencyStopWatchdog();
943 
944  template<class T>
945  void setParam(
946  const char* group,
947  const char* name,
948  const T& value)
949  {
950  crtpParamSetByNameRequest<T> request(group, name, value);
951  sendPacket(reinterpret_cast<const uint8_t*>(&request), request.size());
952  }
953 
954 protected:
955  void sendPacket(
956  const uint8_t* data,
957  uint32_t length);
958 
959  void send2Packets(
960  const uint8_t* data,
961  uint32_t length);
962 
963  // void setParam(
964  // uint8_t group,
965  // uint8_t id,
966  // Crazyflie::ParamType type,
967  // const Crazyflie::ParamValue& value);
968 
969 private:
971  int m_devId;
972 
973  uint8_t m_channel;
974  uint64_t m_address;
976 };
Crazyradio * m_radio
Definition: Crazyflie.h:970
void setEmptyAckCallback(std::function< void(const crtpPlatformRSSIAck *)> cb)
Definition: Crazyflie.h:271
std::vector< crtpPacket_t > m_outgoing_packets
Definition: Crazyflie.h:400
uint8_t trajectoryId
Definition: crtp.h:453
LogBlockGeneric(Crazyflie *cf, const std::vector< std::string > &variables, void *userData, std::function< void(uint32_t, std::vector< double > *, void *userData)> &callback)
Definition: Crazyflie.h:681
const R * getRequestResult(size_t index) const
Definition: Crazyflie.h:430
std::vector< batchRequest > m_batchRequests
Definition: Crazyflie.h:525
int m_devId
Definition: Crazyflie.h:494
float qy
Definition: crtp.h:456
std::vector< MemoryTocEntry >::const_iterator memoriesBegin() const
Definition: Crazyflie.h:208
uint8_t logType
Definition: crtp.h:854
uint8_t id
Definition: crtp.h:644
float yawrate
Definition: crtp.h:441
uint8_t m_channel
Definition: Crazyflie.h:496
float vy
Definition: crtp.h:440
Crazyradio::Datarate m_datarate
Definition: Crazyflie.h:975
std::vector< MemoryTocEntry > m_memoryTocEntries
Definition: Crazyflie.h:506
float p[4][8]
Definition: Crazyflie.h:26
int16_t vz
Definition: crtp.h:445
uint8_t reversed
Definition: crtp.h:452
const T value
Definition: crtp.h:26
uint8_t responseSize() const
Definition: crtp.h:374
float vx
Definition: crtp.h:439
virtual ~Logger()
Definition: Crazyflie.h:21
uint8_t target
float z
Definition: crtp.h:445
float zDistance
Definition: crtp.h:442
bool m_param_use_V2
Definition: Crazyflie.h:533
uint8_t timestampLo
Definition: crtp.h:770
void stop()
Definition: Crazyflie.h:648
void setGenericPacketCallback(std::function< void(const ITransport::Ack &)> cb)
Definition: Crazyflie.h:309
void * m_userData
Definition: Crazyflie.h:883
LogBlock(Crazyflie *cf, std::list< std::pair< std::string, std::string > > variables, std::function< void(uint32_t, T *)> &callback)
Definition: Crazyflie.h:545
void setConsoleCallback(std::function< void(const char *)> cb)
Definition: Crazyflie.h:281
uint8_t data[26]
Definition: crtp.h:772
logBlockItemV2 items[9]
Definition: crtp.h:869
float qw
Definition: crtp.h:458
typedef __attribute__
uint64_t m_address
Definition: Crazyflie.h:497
uint16_t valueUint16
Definition: Crazyflie.h:59
uint8_t length
Definition: crtp.h:22
Logger()
Definition: Crazyflie.h:20
std::string group
Definition: Crazyflie.h:80
void setParamByName(const char *group, const char *name, const T &value)
Definition: Crazyflie.h:223
void queueOutgoingPacket(const crtpPacket_t &packet)
Definition: Crazyflie.h:318
std::string name
Definition: Crazyflie.h:81
std::vector< ParamTocEntry >::const_iterator paramsBegin() const
Definition: Crazyflie.h:194
float duration
Definition: Crazyflie.h:27
logBlockItem items[14]
Definition: crtp.h:658
void handleData(crtpLogDataResponse *response, uint8_t size)
Definition: Crazyflie.h:657
Crazyflie * m_cf
Definition: Crazyflie.h:671
std::function< void(const crtpPlatformRSSIAck *)> m_emptyAckCallback
Definition: Crazyflie.h:508
void sendPacket(const R &request, ITransport::Ack &result, bool useSafeLink=ENABLE_SAFELINK)
Definition: Crazyflie.h:359
std::vector< MemoryTocEntry >::const_iterator memoriesEnd() const
Definition: Crazyflie.h:211
uint8_t size
Definition: ITransport.h:1325
int m_curr_down
Definition: Crazyflie.h:530
uint8_t period
Definition: crtp.h:445
uint8_t data[29]
Definition: crtp.h:363
uint16_t id
Definition: crtp.h:855
uint8_t group
Definition: crtp.h:27
float yaw
Definition: crtp.h:442
float height
Definition: crtp.h:447
std::function< void(uint32_t, T *)> m_callback
Definition: Crazyflie.h:672
char name[30]
Definition: crtp.h:433
uint8_t result
Definition: crtp.h:440
BootloaderTarget
Definition: Crazyflie.h:84
Logger & m_logger
Definition: Crazyflie.h:538
size_t m_numRequestsEnqueued
Definition: Crazyflie.h:527
void setParam(uint16_t id, const T &value)
Definition: Crazyflie.h:216
std::vector< ParamTocEntry >::const_iterator paramsEnd() const
Definition: Crazyflie.h:197
uint8_t m_id
Definition: Crazyflie.h:673
uint32_t valueUint32
Definition: Crazyflie.h:61
uint8_t relative
Definition: crtp.h:455
std::map< uint8_t, std::function< void(crtpLogDataResponse *, uint8_t)> > m_logBlockCb
Definition: Crazyflie.h:501
ITransport * m_transport
Definition: Crazyflie.h:493
uint16_t timestampHi
Definition: crtp.h:771
ITransport::Ack ack
Definition: Crazyflie.h:522
uint8_t m_id
Definition: Crazyflie.h:885
void setParam(const char *group, const char *name, const T &value)
Definition: Crazyflie.h:945
const ParamValue & getParam(uint16_t id) const
Definition: Crazyflie.h:487
int16_t ay
Definition: crtp.h:447
int m_protocolVersion
Definition: Crazyflie.h:535
void start(uint8_t period)
Definition: Crazyflie.h:640
std::vector< LogTocEntry >::const_iterator logVariablesEnd() const
Definition: Crazyflie.h:204
Crazyflie * m_cf
Definition: Crazyflie.h:882
float timescale
Definition: crtp.h:454
~LogBlock()
Definition: Crazyflie.h:633
uint8_t size() const
Definition: crtp.h:370
void sendPacket(const R &request, bool useSafeLink=ENABLE_SAFELINK)
Definition: Crazyflie.h:374
crtpParamTocGetItemRequest request
Definition: crtp.h:21
virtual void error(const std::string &)
Definition: Crazyflie.h:25
Logger EmptyLogger
Definition: Crazyflie.cpp:29
bool m_log_use_V2
Definition: Crazyflie.h:532
std::vector< uint8_t > request
Definition: Crazyflie.h:520
std::vector< LogTocEntry >::const_iterator logVariablesBegin() const
Definition: Crazyflie.h:201
uint8_t log_max_packet
Definition: Crazyflie.h:438
int16_t az
Definition: crtp.h:448
float x
Definition: crtp.h:443
logBlockItemV2 items[9]
Definition: crtp.h:884
uint8_t log_max_ops
Definition: Crazyflie.h:439
size_t m_numRequestsFinished
Definition: Crazyflie.h:526
virtual void info(const std::string &)
Definition: Crazyflie.h:23
virtual void warning(const std::string &)
Definition: Crazyflie.h:24
float qz
Definition: crtp.h:457
std::vector< LogTocEntry > m_logTocEntries
Definition: Crazyflie.h:500
std::vector< Crazyflie::LogType > m_types
Definition: Crazyflie.h:886
uint8_t groupMask
Definition: crtp.h:442
std::vector< ParamTocEntry > m_paramTocEntries
Definition: Crazyflie.h:503
int16_t ax
Definition: crtp.h:446
#define ENABLE_SAFELINK
Definition: Crazyflie.h:15
float vbat
Definition: crtpNRF51.h:222
T getParam(uint16_t id) const
Definition: Crazyflie.h:262
static size_t size(LogType t)
Definition: Crazyflie.h:286
Crazyradio::Datarate m_datarate
Definition: Crazyflie.h:498
uint32_t log_crc
Definition: Crazyflie.h:437
void handleData(crtpLogDataResponse *response, uint8_t)
Definition: Crazyflie.h:802
Crazyradio * m_radio
Definition: Crazyflie.h:492
void start(uint8_t period)
Definition: Crazyflie.h:785
float y
Definition: crtp.h:444
void addRequest(const R &request, size_t numBytesToMatch)
Definition: Crazyflie.h:410
float qx
Definition: crtp.h:455
uint8_t logType
Definition: crtp.h:643
std::map< uint16_t, ParamValue > m_paramValues
Definition: Crazyflie.h:504
std::function< void(const char *)> m_consoleCallback
Definition: Crazyflie.h:510
std::function< void(float)> m_linkQualityCallback
Definition: Crazyflie.h:509
void sendPacketOrTimeout(const R &request, bool useSafeLink=ENABLE_SAFELINK)
Definition: Crazyflie.h:389
void setLinkQualityCallback(std::function< void(float)> cb)
Definition: Crazyflie.h:276
void addSetParam(uint16_t id, const T &value)
Definition: Crazyflie.h:252
std::function< void(const ITransport::Ack &)> m_genericPacketCallback
Definition: Crazyflie.h:511
int m_curr_up
Definition: Crazyflie.h:529


crazyflie_cpp
Author(s): Wolfgang Hoenig
autogenerated on Fri Nov 1 2019 03:39:56