00001 #pragma once
00002
00003 #include <cstring>
00004 #include <sstream>
00005 #include <functional>
00006
00007 #include "Crazyradio.h"
00008 #include "crtp.h"
00009 #include <list>
00010 #include <set>
00011 #include <map>
00012 #include <chrono>
00013
00014 #define ENABLE_SAFELINK 1
00015
00016 class Logger
00017 {
00018 public:
00019 Logger() {}
00020 virtual ~Logger() {}
00021
00022 virtual void info(const std::string& ) {}
00023 virtual void warning(const std::string& ) {}
00024 virtual void error(const std::string& ) {}
00025 };
00026
00027 extern Logger EmptyLogger;
00028
00029 class Crazyflie
00030 {
00031 public:
00032 enum ParamType {
00033 ParamTypeUint8 = 0x00 | (0x00<<2) | (0x01<<3),
00034 ParamTypeInt8 = 0x00 | (0x00<<2) | (0x00<<3),
00035 ParamTypeUint16 = 0x01 | (0x00<<2) | (0x01<<3),
00036 ParamTypeInt16 = 0x01 | (0x00<<2) | (0x00<<3),
00037 ParamTypeUint32 = 0x02 | (0x00<<2) | (0x01<<3),
00038 ParamTypeInt32 = 0x02 | (0x00<<2) | (0x00<<3),
00039 ParamTypeFloat = 0x02 | (0x01<<2) | (0x00<<3),
00040 };
00041
00042 struct ParamTocEntry {
00043 uint16_t id;
00044 ParamType type;
00045 bool readonly;
00046
00047
00048
00049
00050
00051 std::string group;
00052 std::string name;
00053 };
00054
00055 union ParamValue{
00056 uint8_t valueUint8;
00057 int8_t valueInt8;
00058 uint16_t valueUint16;
00059 int16_t valueInt16;
00060 uint32_t valueUint32;
00061 int32_t valueInt32;
00062 float valueFloat;
00063 };
00064
00065 enum LogType {
00066 LogTypeUint8 = 1,
00067 LogTypeUint16 = 2,
00068 LogTypeUint32 = 3,
00069 LogTypeInt8 = 4,
00070 LogTypeInt16 = 5,
00071 LogTypeInt32 = 6,
00072 LogTypeFloat = 7,
00073 LogTypeFP16 = 8,
00074 };
00075
00076 struct LogTocEntry {
00077 uint8_t id;
00078 LogType type;
00079 std::string group;
00080 std::string name;
00081 };
00082
00083 enum BootloaderTarget {
00084 TargetSTM32 = 0xFF,
00085 TargetNRF51 = 0xFE,
00086 };
00087
00088 enum MemoryType {
00089 MemoryTypeEEPROM = 0x00,
00090 MemoryTypeOneWire = 0x01,
00091 MemoryTypeLED12 = 0x10,
00092 MemoryTypeLOCO = 0x11,
00093 MemoryTypeTRAJ = 0x12,
00094 };
00095
00096 struct MemoryTocEntry {
00097 uint16_t id;
00098 MemoryType type;
00099 uint32_t size;
00100 uint64_t addr;
00101 };
00102
00103 struct poly4d {
00104 float p[4][8];
00105 float duration;
00106 } __attribute__((packed));
00107
00108 public:
00109 Crazyflie(
00110 const std::string& link_uri,
00111 Logger& logger = EmptyLogger,
00112 std::function<void(const char*)> consoleCb = nullptr);
00113
00114 int getProtocolVersion();
00115
00116 std::string getFirmwareVersion();
00117
00118 std::string getDeviceTypeName();
00119
00120 void logReset();
00121
00122 void sendSetpoint(
00123 float roll,
00124 float pitch,
00125 float yawrate,
00126 uint16_t thrust);
00127
00128 void sendFullStateSetpoint(
00129 float x, float y, float z,
00130 float vx, float vy, float vz,
00131 float ax, float ay, float az,
00132 float qx, float qy, float qz, float qw,
00133 float rollRate, float pitchRate, float yawRate);
00134
00135 void sendHoverSetpoint(
00136 float vx,
00137 float vy,
00138 float yawrate,
00139 float zDistance);
00140
00141 void sendStop();
00142
00143 void sendPositionSetpoint(
00144 float x,
00145 float y,
00146 float z,
00147 float yaw);
00148
00149 void sendExternalPositionUpdate(
00150 float x,
00151 float y,
00152 float z);
00153
00154 void sendPing();
00155
00156 void reboot();
00157
00158 uint64_t rebootToBootloader();
00159
00160 void rebootFromBootloader();
00161
00162 void sysoff();
00163 void alloff();
00164 void syson();
00165 float vbat();
00166
00167 void writeFlash(
00168 BootloaderTarget target,
00169 const std::vector<uint8_t>& data);
00170 void readFlash(
00171 BootloaderTarget target,
00172 size_t size,
00173 std::vector<uint8_t>& data);
00174
00175 void requestLogToc(bool forceNoCache=false);
00176
00177 void requestParamToc(bool forceNoCache=false);
00178
00179 void requestMemoryToc();
00180
00181 std::vector<ParamTocEntry>::const_iterator paramsBegin() const {
00182 return m_paramTocEntries.begin();
00183 }
00184 std::vector<ParamTocEntry>::const_iterator paramsEnd() const {
00185 return m_paramTocEntries.end();
00186 }
00187
00188 std::vector<LogTocEntry>::const_iterator logVariablesBegin() const {
00189 return m_logTocEntries.begin();
00190 }
00191 std::vector<LogTocEntry>::const_iterator logVariablesEnd() const {
00192 return m_logTocEntries.end();
00193 }
00194
00195 std::vector<MemoryTocEntry>::const_iterator memoriesBegin() const {
00196 return m_memoryTocEntries.begin();
00197 }
00198 std::vector<MemoryTocEntry>::const_iterator memoriesEnd() const {
00199 return m_memoryTocEntries.end();
00200 }
00201
00202 template<class T>
00203 void setParam(uint8_t id, const T& value) {
00204 ParamValue v;
00205 memcpy(&v, &value, sizeof(value));
00206 setParam(id, v);
00207 }
00208
00209 void startSetParamRequest();
00210
00211 template<class T>
00212 void addSetParam(uint8_t id, const T& value) {
00213 ParamValue v;
00214 memcpy(&v, &value, sizeof(value));
00215 addSetParam(id, v);
00216 }
00217
00218 void setRequestedParams();
00219
00220
00221 template<class T>
00222 T getParam(uint8_t id) const {
00223 ParamValue v = getParam(id);
00224 return *(reinterpret_cast<T*>(&v));
00225 }
00226
00227 const ParamTocEntry* getParamTocEntry(
00228 const std::string& group,
00229 const std::string& name) const;
00230
00231 void setEmptyAckCallback(
00232 std::function<void(const crtpPlatformRSSIAck*)> cb) {
00233 m_emptyAckCallback = cb;
00234 }
00235
00236 void setLinkQualityCallback(
00237 std::function<void(float)> cb) {
00238 m_linkQualityCallback = cb;
00239 }
00240
00241 void setConsoleCallback(
00242 std::function<void(const char*)> cb) {
00243 m_consoleCallback = cb;
00244 }
00245
00246 static size_t size(LogType t) {
00247 switch(t) {
00248 case LogTypeUint8:
00249 case LogTypeInt8:
00250 return 1;
00251 break;
00252 case LogTypeUint16:
00253 case LogTypeInt16:
00254 case LogTypeFP16:
00255 return 2;
00256 break;
00257 case LogTypeUint32:
00258 case LogTypeInt32:
00259 case LogTypeFloat:
00260 return 4;
00261 break;
00262 default:
00263
00264 return 0;
00265 }
00266 }
00267
00268
00269 void setGenericPacketCallback(
00270 std::function<void(const ITransport::Ack&)> cb) {
00271 m_genericPacketCallback = cb;
00272 }
00273
00278 void queueOutgoingPacket(const crtpPacket_t& packet) {
00279 m_outgoing_packets.push_back(packet);
00280 }
00281
00282 void transmitPackets();
00283
00284
00285 void setGroupMask(uint8_t groupMask);
00286
00287 void takeoff(float height, float duration, uint8_t groupMask = 0);
00288
00289 void land(float height, float duration, uint8_t groupMask = 0);
00290
00291 void stop(uint8_t groupMask = 0);
00292
00293 void goTo(float x, float y, float z, float yaw, float duration, bool relative = false, uint8_t groupMask = 0);
00294
00295 void uploadTrajectory(
00296 uint8_t trajectoryId,
00297 uint32_t pieceOffset,
00298 const std::vector<poly4d>& pieces);
00299
00300 void startTrajectory(
00301 uint8_t trajectoryId,
00302 float timescale = 1.0,
00303 bool reversed = false,
00304 bool relative = true,
00305 uint8_t groupMask = 0);
00306
00307 private:
00308 void sendPacket(
00309 const uint8_t* data,
00310 uint32_t length,
00311 ITransport::Ack& result,
00312 bool useSafeLink = ENABLE_SAFELINK);
00313
00314 template<typename R>
00315 void sendPacket(
00316 const R& request,
00317 ITransport::Ack& result,
00318 bool useSafeLink = ENABLE_SAFELINK)
00319 {
00320 sendPacket(
00321 reinterpret_cast<const uint8_t*>(&request), sizeof(request), result, useSafeLink);
00322 }
00323
00324 bool sendPacket(
00325 const uint8_t* data,
00326 uint32_t length,
00327 bool useSafeLink = ENABLE_SAFELINK);
00328
00329 template<typename R>
00330 void sendPacket(
00331 const R& request,
00332 bool useSafeLink = ENABLE_SAFELINK)
00333 {
00334 sendPacket(
00335 reinterpret_cast<const uint8_t*>(&request), sizeof(request), useSafeLink);
00336 }
00337
00338 void sendPacketOrTimeout(
00339 const uint8_t* data,
00340 uint32_t length,
00341 bool useSafeLink = ENABLE_SAFELINK,
00342 float timeout = 1.0);
00343
00344 template<typename R>
00345 void sendPacketOrTimeout(
00346 const R& request,
00347 bool useSafeLink = ENABLE_SAFELINK)
00348 {
00349 sendPacketOrTimeout(
00350 reinterpret_cast<const uint8_t*>(&request), sizeof(request), useSafeLink);
00351 }
00352
00353 void handleAck(
00354 const ITransport::Ack& result);
00355
00356 std::vector<crtpPacket_t> m_outgoing_packets;
00357
00358 void startBatchRequest();
00359
00360 void addRequest(
00361 const uint8_t* data,
00362 size_t numBytes,
00363 size_t numBytesToMatch);
00364
00365 template<typename R>
00366 void addRequest(
00367 const R& request,
00368 size_t numBytesToMatch)
00369 {
00370 addRequest(
00371 reinterpret_cast<const uint8_t*>(&request), sizeof(request), numBytesToMatch);
00372 }
00373
00374 void handleRequests(
00375 bool crtpMode = true,
00376 bool useSafeLink = ENABLE_SAFELINK,
00377 float baseTime = 2.0,
00378 float timePerRequest = 0.2);
00379
00380
00381 void handleBatchAck(
00382 const ITransport::Ack& result,
00383 bool crtpMode);
00384
00385 template<typename R>
00386 const R* getRequestResult(size_t index) const {
00387 return reinterpret_cast<const R*>(m_batchRequests[index].ack.data);
00388 }
00389
00390 private:
00391 struct logInfo {
00392 uint8_t len;
00393 uint32_t log_crc;
00394 uint8_t log_max_packet;
00395 uint8_t log_max_ops;
00396 };
00397
00399
00400 struct paramInfo {
00401 uint8_t len;
00402 uint32_t crc;
00403 };
00404
00405
00406
00407
00408
00409
00410
00411
00412
00413
00414
00415
00416
00417
00418
00419
00420
00421
00422
00423
00424
00425
00426
00427
00428 private:
00429 const LogTocEntry* getLogTocEntry(
00430 const std::string& group,
00431 const std::string& name) const;
00432
00433 uint8_t registerLogBlock(
00434 std::function<void(crtpLogDataResponse*, uint8_t)> cb);
00435
00436 bool unregisterLogBlock(
00437 uint8_t id);
00438
00439 void setParam(uint8_t id, const ParamValue& value);
00440 void addSetParam(uint8_t id, const ParamValue& value);
00441
00442
00443 const ParamValue& getParam(uint8_t id) const {
00444 return m_paramValues.at(id);
00445 }
00446
00447 private:
00448 Crazyradio* m_radio;
00449 ITransport* m_transport;
00450 int m_devId;
00451
00452 uint8_t m_channel;
00453 uint64_t m_address;
00454 Crazyradio::Datarate m_datarate;
00455
00456 std::vector<LogTocEntry> m_logTocEntries;
00457 std::map<uint8_t, std::function<void(crtpLogDataResponse*, uint8_t)> > m_logBlockCb;
00458
00459 std::vector<ParamTocEntry> m_paramTocEntries;
00460 std::map<uint8_t, ParamValue> m_paramValues;
00461
00462 std::vector<MemoryTocEntry> m_memoryTocEntries;
00463
00464 std::function<void(const crtpPlatformRSSIAck*)> m_emptyAckCallback;
00465 std::function<void(float)> m_linkQualityCallback;
00466 std::function<void(const char*)> m_consoleCallback;
00467 std::function<void(const ITransport::Ack&)> m_genericPacketCallback;
00468
00469 template<typename T>
00470 friend class LogBlock;
00471 friend class LogBlockGeneric;
00472
00473
00474 struct batchRequest
00475 {
00476 std::vector<uint8_t> request;
00477 size_t numBytesToMatch;
00478 ITransport::Ack ack;
00479 bool finished;
00480 };
00481 std::vector<batchRequest> m_batchRequests;
00482 size_t m_numRequestsFinished;
00483
00484 int m_curr_up;
00485 int m_curr_down;
00486
00487 bool m_log_use_V2;
00488 bool m_param_use_V2;
00489
00490 int m_protocolVersion;
00491
00492
00493 Logger& m_logger;
00494 };
00495
00496 template<class T>
00497 class LogBlock
00498 {
00499 public:
00500 LogBlock(
00501 Crazyflie* cf,
00502 std::list<std::pair<std::string, std::string> > variables,
00503 std::function<void(uint32_t, T*)>& callback)
00504 : m_cf(cf)
00505 , m_callback(callback)
00506 , m_id(0)
00507 {
00508 m_id = m_cf->registerLogBlock([=](crtpLogDataResponse* r, uint8_t s) { this->handleData(r, s);});
00509 if (m_cf->m_log_use_V2) {
00510 crtpLogCreateBlockV2Request request;
00511 request.id = m_id;
00512 int i = 0;
00513 for (auto&& pair : variables) {
00514 const Crazyflie::LogTocEntry* entry = m_cf->getLogTocEntry(pair.first, pair.second);
00515 if (entry) {
00516 request.items[i].logType = entry->type;
00517 request.items[i].id = entry->id;
00518 ++i;
00519 }
00520 else {
00521 std::stringstream sstr;
00522 sstr << "Could not find " << pair.first << "." << pair.second << " in log toc!";
00523 throw std::runtime_error(sstr.str());
00524 }
00525 }
00526
00527 m_cf->startBatchRequest();
00528 m_cf->addRequest(reinterpret_cast<const uint8_t*>(&request), 3 + 3*i, 2);
00529 m_cf->handleRequests();
00530 auto r = m_cf->getRequestResult<crtpLogControlResponse>(0);
00531 if (r->result != crtpLogControlResultOk
00532 && r->result != crtpLogControlResultBlockExists) {
00533 std::stringstream sstr;
00534 sstr << "Could not create log block! Result: " << (int)r->result << " " << (int)m_id << " " << (int)r->requestByte1 << " " << (int)r->command;
00535 throw std::runtime_error(sstr.str());
00536 }
00537 } else {
00538 crtpLogCreateBlockRequest request;
00539 request.id = m_id;
00540 int i = 0;
00541 for (auto&& pair : variables) {
00542 const Crazyflie::LogTocEntry* entry = m_cf->getLogTocEntry(pair.first, pair.second);
00543 if (entry) {
00544 request.items[i].logType = entry->type;
00545 request.items[i].id = entry->id;
00546 ++i;
00547 }
00548 else {
00549 std::stringstream sstr;
00550 sstr << "Could not find " << pair.first << "." << pair.second << " in log toc!";
00551 throw std::runtime_error(sstr.str());
00552 }
00553 }
00554
00555 m_cf->startBatchRequest();
00556 m_cf->addRequest(reinterpret_cast<const uint8_t*>(&request), 3 + 2*i, 2);
00557 m_cf->handleRequests();
00558 auto r = m_cf->getRequestResult<crtpLogControlResponse>(0);
00559 if (r->result != crtpLogControlResultOk
00560 && r->result != crtpLogControlResultBlockExists) {
00561 std::stringstream sstr;
00562 sstr << "Could not create log block! Result: " << (int)r->result << " " << (int)m_id << " " << (int)r->requestByte1 << " " << (int)r->command;
00563 throw std::runtime_error(sstr.str());
00564 }
00565 }
00566 }
00567
00568 ~LogBlock()
00569 {
00570 stop();
00571 m_cf->unregisterLogBlock(m_id);
00572 }
00573
00574
00575 void start(uint8_t period)
00576 {
00577 crtpLogStartRequest request(m_id, period);
00578 m_cf->startBatchRequest();
00579 m_cf->addRequest(request, 2);
00580 m_cf->handleRequests();
00581 }
00582
00583 void stop()
00584 {
00585 crtpLogStopRequest request(m_id);
00586 m_cf->startBatchRequest();
00587 m_cf->addRequest(request, 2);
00588 m_cf->handleRequests();
00589 }
00590
00591 private:
00592 void handleData(crtpLogDataResponse* response, uint8_t size) {
00593 if (size == sizeof(T)) {
00594 uint32_t time_in_ms = ((uint32_t)response->timestampHi << 8) | (response->timestampLo);
00595 T* t = (T*)response->data;
00596 m_callback(time_in_ms, t);
00597 }
00598 else {
00599 std::stringstream sstr;
00600 sstr << "Size doesn't match! Is: " << (size_t)size << " expected: " << sizeof(T);
00601 throw std::runtime_error(sstr.str());
00602 }
00603 }
00604
00605 private:
00606 Crazyflie* m_cf;
00607 std::function<void(uint32_t, T*)> m_callback;
00608 uint8_t m_id;
00609 };
00610
00612
00613 class LogBlockGeneric
00614 {
00615 public:
00616 LogBlockGeneric(
00617 Crazyflie* cf,
00618 const std::vector<std::string>& variables,
00619 void* userData,
00620 std::function<void(uint32_t, std::vector<double>*, void* userData)>& callback)
00621 : m_cf(cf)
00622 , m_userData(userData)
00623 , m_callback(callback)
00624 , m_id(0)
00625 {
00626 m_id = m_cf->registerLogBlock([=](crtpLogDataResponse* r, uint8_t s) { this->handleData(r, s);});
00627 if (m_cf->m_log_use_V2) {
00628 crtpLogCreateBlockV2Request request;
00629 request.id = m_id;
00630 int i = 0;
00631 size_t s = 0;
00632 for (auto&& var : variables) {
00633 auto pos = var.find(".");
00634 std::string first = var.substr(0, pos);
00635 std::string second = var.substr(pos+1);
00636 const Crazyflie::LogTocEntry* entry = m_cf->getLogTocEntry(first, second);
00637 if (entry) {
00638 s += Crazyflie::size(entry->type);
00639 if (s > 26) {
00640 std::stringstream sstr;
00641 sstr << "Can't configure that many variables in a single log block!"
00642 << " Ignoring " << first << "." << second << std::endl;
00643 throw std::runtime_error(sstr.str());
00644 } else {
00645 request.items[i].logType = entry->type;
00646 request.items[i].id = entry->id;
00647 ++i;
00648 m_types.push_back(entry->type);
00649 }
00650 }
00651 else {
00652 std::stringstream sstr;
00653 sstr << "Could not find " << first << "." << second << " in log toc!";
00654 throw std::runtime_error(sstr.str());
00655 }
00656 }
00657 m_cf->startBatchRequest();
00658 m_cf->addRequest(reinterpret_cast<const uint8_t*>(&request), 3 + 3*i, 2);
00659 m_cf->handleRequests();
00660 auto r = m_cf->getRequestResult<crtpLogControlResponse>(0);
00661 if (r->result != crtpLogControlResultOk
00662 && r->result != crtpLogControlResultBlockExists) {
00663 throw std::runtime_error("Could not create log block!");
00664 }
00665 } else {
00666 crtpLogCreateBlockRequest request;
00667 request.id = m_id;
00668 int i = 0;
00669 size_t s = 0;
00670 for (auto&& var : variables) {
00671 auto pos = var.find(".");
00672 std::string first = var.substr(0, pos);
00673 std::string second = var.substr(pos+1);
00674 const Crazyflie::LogTocEntry* entry = m_cf->getLogTocEntry(first, second);
00675 if (entry) {
00676 s += Crazyflie::size(entry->type);
00677 if (s > 26) {
00678 std::stringstream sstr;
00679 sstr << "Can't configure that many variables in a single log block!"
00680 << " Ignoring " << first << "." << second << std::endl;
00681 throw std::runtime_error(sstr.str());
00682 } else {
00683 request.items[i].logType = entry->type;
00684 request.items[i].id = entry->id;
00685 ++i;
00686 m_types.push_back(entry->type);
00687 }
00688 }
00689 else {
00690 std::stringstream sstr;
00691 sstr << "Could not find " << first << "." << second << " in log toc!";
00692 throw std::runtime_error(sstr.str());
00693 }
00694 }
00695 m_cf->startBatchRequest();
00696 m_cf->addRequest(reinterpret_cast<const uint8_t*>(&request), 3 + 2*i, 2);
00697 m_cf->handleRequests();
00698 auto r = m_cf->getRequestResult<crtpLogControlResponse>(0);
00699 if (r->result != crtpLogControlResultOk
00700 && r->result != crtpLogControlResultBlockExists) {
00701 throw std::runtime_error("Could not create log block!");
00702 }
00703 }
00704 }
00705
00706 ~LogBlockGeneric()
00707 {
00708 stop();
00709 m_cf->unregisterLogBlock(m_id);
00710 }
00711
00712
00713 void start(uint8_t period)
00714 {
00715 crtpLogStartRequest request(m_id, period);
00716 m_cf->startBatchRequest();
00717 m_cf->addRequest(request, 2);
00718 m_cf->handleRequests();
00719 }
00720
00721 void stop()
00722 {
00723 crtpLogStopRequest request(m_id);
00724 m_cf->startBatchRequest();
00725 m_cf->addRequest(request, 2);
00726 m_cf->handleRequests();
00727 }
00728
00729 private:
00730 void handleData(crtpLogDataResponse* response, uint8_t ) {
00731
00732 std::vector<double> result;
00733 size_t pos = 0;
00734 for (size_t i = 0; i < m_types.size(); ++i)
00735 {
00736 switch (m_types[i])
00737 {
00738 case Crazyflie::LogTypeUint8:
00739 {
00740 uint8_t value;
00741 memcpy(&value, &response->data[pos], sizeof(uint8_t));
00742 result.push_back(value);
00743 pos += sizeof(uint8_t);
00744 break;
00745 }
00746 case Crazyflie::LogTypeInt8:
00747 {
00748 int8_t value;
00749 memcpy(&value, &response->data[pos], sizeof(int8_t));
00750 result.push_back(value);
00751 pos += sizeof(int8_t);
00752 break;
00753 }
00754 case Crazyflie::LogTypeUint16:
00755 {
00756 uint16_t value;
00757 memcpy(&value, &response->data[pos], sizeof(uint16_t));
00758 result.push_back(value);
00759 pos += sizeof(uint16_t);
00760 break;
00761 }
00762 case Crazyflie::LogTypeInt16:
00763 {
00764 int16_t value;
00765 memcpy(&value, &response->data[pos], sizeof(int16_t));
00766 result.push_back(value);
00767 pos += sizeof(int16_t);
00768 break;
00769 }
00770 case Crazyflie::LogTypeUint32:
00771 {
00772 uint32_t value;
00773 memcpy(&value, &response->data[pos], sizeof(uint32_t));
00774 result.push_back(value);
00775 pos += sizeof(uint32_t);
00776 break;
00777 }
00778 case Crazyflie::LogTypeInt32:
00779 {
00780 int32_t value;
00781 memcpy(&value, &response->data[pos], sizeof(int32_t));
00782 result.push_back(value);
00783 pos += sizeof(int32_t);
00784 break;
00785 }
00786 case Crazyflie::LogTypeFloat:
00787 {
00788 float value;
00789 memcpy(&value, &response->data[pos], sizeof(float));
00790 result.push_back(value);
00791 pos += sizeof(float);
00792 break;
00793 }
00794 case Crazyflie::LogTypeFP16:
00795 {
00796 double value;
00797 memcpy(&value, &response->data[pos], sizeof(double));
00798 result.push_back(value);
00799 pos += sizeof(double);
00800 break;
00801 }
00802 }
00803 }
00804
00805 uint32_t time_in_ms = ((uint32_t)response->timestampHi << 8) | (response->timestampLo);
00806 m_callback(time_in_ms, &result, m_userData);
00807 }
00808
00809 private:
00810 Crazyflie* m_cf;
00811 void* m_userData;
00812 std::function<void(uint32_t, std::vector<double>*, void*)> m_callback;
00813 uint8_t m_id;
00814 std::vector<Crazyflie::LogType> m_types;
00815 };
00816
00818
00819 class CrazyflieBroadcaster
00820 {
00821 public:
00822 CrazyflieBroadcaster(
00823 const std::string& link_uri);
00824
00825
00826 void takeoff(float height, float duration, uint8_t groupMask = 0);
00827
00828 void land(float height, float duration, uint8_t groupMask = 0);
00829
00830 void stop(uint8_t groupMask = 0);
00831
00832
00833 void goTo(float x, float y, float z, float yaw, float duration, uint8_t groupMask = 0);
00834
00835
00836 void startTrajectory(
00837 uint8_t trajectoryId,
00838 float timescale = 1.0,
00839 bool reversed = false,
00840 uint8_t groupMask = 0);
00841
00842 struct externalPosition
00843 {
00844 uint8_t id;
00845 float x;
00846 float y;
00847 float z;
00848 };
00849
00850 void sendExternalPositions(
00851 const std::vector<externalPosition>& data);
00852
00853 struct externalPose
00854 {
00855 uint8_t id;
00856 float x;
00857 float y;
00858 float z;
00859 float qx;
00860 float qy;
00861 float qz;
00862 float qw;
00863 };
00864
00865
00866 void sendExternalPoses(
00867 const std::vector<externalPose>& data);
00868
00869
00870
00871
00872
00873
00874
00875
00876
00877
00878
00879
00880
00881
00882 protected:
00883 void sendPacket(
00884 const uint8_t* data,
00885 uint32_t length);
00886
00887 void send2Packets(
00888 const uint8_t* data,
00889 uint32_t length);
00890
00891
00892
00893
00894
00895
00896
00897 private:
00898 Crazyradio* m_radio;
00899 int m_devId;
00900
00901 uint8_t m_channel;
00902 uint64_t m_address;
00903 Crazyradio::Datarate m_datarate;
00904 };