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


crazyflie_tools
Author(s): Wolfgang Hoenig
autogenerated on Mon Sep 28 2020 03:40:14