Go to the documentation of this file.00001 #include <iostream>
00002 #include <chrono>
00003
00004 #include <boost/program_options.hpp>
00005 #include <crazyflie_cpp/Crazyflie.h>
00006 #include <crazyflie_cpp/crtp.h>
00007
00008 struct crtpMyPacket
00009 {
00010 crtpMyPacket(
00011 uint64_t id,
00012 uint64_t time_since_epoch)
00013 : header(15, 0)
00014 , id(id)
00015 , time_since_epoch(time_since_epoch)
00016 {
00017 }
00018 const crtp header;
00019 const uint64_t id;
00020 uint64_t time_since_epoch;
00021 } __attribute__((packed));
00022
00023 uint64_t getTimestamp()
00024 {
00025 auto now = std::chrono::high_resolution_clock::now();
00026 auto now_us = std::chrono::time_point_cast<std::chrono::milliseconds>(now);
00027
00028 auto value = now_us.time_since_epoch();
00029 uint64_t time_since_epoch = value.count();
00030 return time_since_epoch;
00031 }
00032
00033
00034 uint32_t numPacketsReceived = 0;
00035 uint64_t sumRoundtripTime = 0;
00036
00037 uint32_t linkQualitySent = 0;
00038 uint32_t linkQualityAcked = 0;
00039
00040 void onGenericPacket(const ITransport::Ack& ack)
00041 {
00042 uint64_t time_since_epoch = getTimestamp();
00043
00044 const crtpMyPacket* packet = reinterpret_cast<const crtpMyPacket*>(ack.data);
00045 uint64_t roundtripTime = time_since_epoch - packet->time_since_epoch;
00046
00047
00048 sumRoundtripTime += roundtripTime;
00049 ++numPacketsReceived;
00050 }
00051
00052 void onEmptyAck(const crtpPlatformRSSIAck* cb)
00053 {
00054 std::cout << "RSSI: " << (int)cb->rssi << std::endl;
00055 }
00056
00057 void onLinkQuality(float quality)
00058 {
00059 linkQualitySent += 100;
00060 linkQualityAcked += quality * 100;
00061
00062 }
00063
00064 int main(int argc, char **argv)
00065 {
00066
00067 std::string uri;
00068 std::string defaultUri("radio://0/80/2M/E7E7E7E7E7");
00069 uint32_t numPackets;
00070
00071 namespace po = boost::program_options;
00072
00073 po::options_description desc("Allowed options");
00074 desc.add_options()
00075 ("help", "produce help message")
00076 ("uri", po::value<std::string>(&uri)->default_value(defaultUri), "unique ressource identifier")
00077 ("numPackets", po::value<uint32_t>(&numPackets)->default_value(1000), "unique ressource identifier")
00078 ;
00079
00080 try
00081 {
00082 po::variables_map vm;
00083 po::store(po::parse_command_line(argc, argv, desc), vm);
00084 po::notify(vm);
00085
00086 if (vm.count("help")) {
00087 std::cout << desc << "\n";
00088 return 0;
00089 }
00090 }
00091 catch(po::error& e)
00092 {
00093 std::cerr << e.what() << std::endl << std::endl;
00094 std::cerr << desc << std::endl;
00095 return 1;
00096 }
00097
00098 try
00099 {
00100 Crazyflie cf(uri);
00101
00102 cf.setEmptyAckCallback(onEmptyAck);
00103 cf.setLinkQualityCallback(onLinkQuality);
00104
00105
00106 for (size_t i = 0; i < 1000; ++i) {
00107 cf.sendPing();
00108 }
00109
00110 cf.setGenericPacketCallback(onGenericPacket);
00111
00112 uint64_t start = getTimestamp();
00113 for (uint64_t id = 0; id < numPackets; ++id) {
00114 uint64_t time_since_epoch = getTimestamp();
00115
00116 crtpMyPacket p(id, time_since_epoch);
00117
00118
00119 crtpPacket_t packet;
00120 packet.size = sizeof(crtpMyPacket)-1;
00121 packet.header = *reinterpret_cast<const uint8_t*>(&p.header);
00122 memcpy(packet.data, &p.id, sizeof(crtpMyPacket)-1);
00123
00124 cf.queueOutgoingPacket(packet);
00125 cf.transmitPackets();
00126 }
00127 uint64_t timeToSent = getTimestamp() - start;
00128
00129
00130 for (size_t i = 0; i < 1000; ++i) {
00131 cf.sendPing();
00132 }
00133
00134 std::cout << "numPacketsReceived: " << numPacketsReceived / (double)numPackets * 100.0f << " %" << std::endl;
00135 std::cout << "Avg. roundtrip time: " << sumRoundtripTime / (double) numPacketsReceived << " ms" << std::endl;
00136 std::cout << numPackets / (timeToSent / 1000.0f) << " packets/s sent" << std::endl;
00137 std::cout << "link quality: " << linkQualityAcked / (float)linkQualitySent << std::endl;
00138
00139
00140 return 0;
00141 }
00142 catch(std::exception& e)
00143 {
00144 std::cerr << e.what() << std::endl;
00145 return 1;
00146 }
00147 }