Crazyflie.cpp

//#include <regex>
#include <mutex>
#include <cassert>

#include "Crazyflie.h"
#include "crtp.h"
#include "crtpBootloader.h"
#include "crtpNRF51.h"

#include <fstream>
#include <cstring>
#include <stdexcept>
#include <thread>
#include <cmath>
#include <inttypes.h>

Logger EmptyLogger;


Crazyflie::Crazyflie(
  const std::string& link_uri,
  Logger& logger,
  std::function<void(const char*)> consoleCb)
  // : m_logTocEntries()
  // , m_logBlockCb()
  // , m_paramTocEntries()
  // , m_paramValues()
  // , m_emptyAckCallback(nullptr)
  // , m_linkQualityCallback(nullptr)
  : m_consoleCallback(consoleCb)
  // , m_log_use_V2(false)
  , m_protocolVersion(-1)
  , m_logger(logger)
  , m_connection(link_uri)
{
}

std::vector<std::string> Crazyflie::scan(
    uint64_t address)
{
  return bitcraze::crazyflieLinkCpp::Connection::scan(address);
}


int Crazyflie::getProtocolVersion()
{
  crtpGetProtocolVersionRequest req;
  m_connection.send(req);
  using res = crtpGetProtocolVersionResponse;
  auto p = waitForResponse(&res::valid);
  return res::version(p);
}

std::string Crazyflie::getFirmwareVersion()
{
  crtpGetFirmwareVersionRequest req;
  m_connection.send(req);
  using res = crtpGetFirmwareVersionResponse;
  auto p = waitForResponse(&res::valid);
  return res::version(p);
}

std::string Crazyflie::getDeviceTypeName()
{
  crtpGetDeviceTypeNameRequest req;
  m_connection.send(req);
  using res = crtpGetDeviceTypeNameResponse;
  auto p = waitForResponse(&res::valid);
  return res::name(p);
}

#if 0
void Crazyflie::logReset()
{
  crtpLogResetRequest request;
  startBatchRequest();
  addRequest(request, 1);
  handleRequests();
}

void Crazyflie::sendSetpoint(
  float roll,
  float pitch,
  float yawrate,
  uint16_t thrust)
{
  crtpSetpointRequest request(roll, pitch, yawrate, thrust);
  sendPacket(request);
}

void Crazyflie::sendStop()
{
  crtpStopRequest request;
  sendPacket(request);
}

void Crazyflie::emergencyStop()
{
  crtpEmergencyStopRequest request;
  sendPacketOrTimeout(request);
}

void Crazyflie::emergencyStopWatchdog()
{
  crtpEmergencyStopWatchdogRequest request;
  sendPacketOrTimeout(request);
}

void Crazyflie::sendPositionSetpoint(
  float x,
  float y,
  float z,
  float yaw)
{
  crtpPositionSetpointRequest request(x, y, z, yaw);
  sendPacket(request);
}

void Crazyflie::sendHoverSetpoint(
  float vx,
  float vy,
  float yawrate,
  float zDistance)
{
  crtpHoverSetpointRequest request(vx, vy, yawrate, zDistance);
  sendPacket(request);
}

void Crazyflie::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)
{
  crtpFullStateSetpointRequest request(
    x, y, z,
    vx, vy, vz,
    ax, ay, az,
    qx, qy, qz, qw,
    rollRate, pitchRate, yawRate);
  sendPacket(request);
}

void Crazyflie::sendVelocityWorldSetpoint(
        float x, float y, float z, float yawRate)
{
  crtpVelocityWorldSetpointRequest request(
    x, y, z, yawRate);
  sendPacket(request);
}

void Crazyflie::notifySetpointsStop(uint32_t remainValidMillisecs)
{
  crtpNotifySetpointsStopRequest request(remainValidMillisecs);
  sendPacketOrTimeout(request);
}

void Crazyflie::sendExternalPositionUpdate(
  float x,
  float y,
  float z)
{
  crtpExternalPositionUpdate position(x, y, z);
  sendPacket(position);
}

void Crazyflie::sendExternalPoseUpdate(
  float x, float y, float z,
  float qx, float qy, float qz, float qw)
{
  crtpExternalPoseUpdate pose(x, y, z, qx, qy, qz, qw);
  sendPacket(pose);
}
#endif
void Crazyflie::sendPing()
{
  waitForResponse([](const auto&) {return true;});
}
#if 0
/**
 * Transmits any outgoing packets to the crazyflie.
 */
void Crazyflie::transmitPackets()
{
  if (!m_outgoing_packets.empty())
  {
    std::vector<crtpPacket_t>::iterator it;
    for (it = m_outgoing_packets.begin(); it != m_outgoing_packets.end(); it++)
    {
      sendPacketInternal(it->raw, it->size+1);
    }
    m_outgoing_packets.clear();
  }
}

// https://forum.bitcraze.io/viewtopic.php?f=9&t=1488
void Crazyflie::reboot()
{
  if (m_radio) {
    crtpNrf51ResetInitRequest req1;
    sendPacketOrTimeout(req1);

    crtpNrf51ResetRequest req2(/*bootToFirmware*/ 1);
    sendPacketOrTimeout(req2);
  }
}

uint64_t Crazyflie::rebootToBootloader()
{
  if (m_radio) {
    crtpNrf51ResetInitRequest req;
    startBatchRequest();
    addRequest(req, 2);
    handleRequests();
    const crtpNrf51ResetInitResponse* response = getRequestResult<crtpNrf51ResetInitResponse>(0);

    uint64_t result =
        ((uint64_t)response->addr[0] << 0)
      | ((uint64_t)response->addr[1] << 8)
      | ((uint64_t)response->addr[2] << 16)
      | ((uint64_t)response->addr[3] << 24)
      | ((uint64_t)0xb1 << 32);

    crtpNrf51ResetRequest req2(/*bootToFirmware*/ 0);
    sendPacketOrTimeout(req2);

    // switch to new address
    m_address = result;
    m_channel = 0;
    m_datarate = Crazyradio::Datarate_2MPS;


    return result;
  } else {
    return -1;
  }
}

void Crazyflie::rebootFromBootloader()
{
  if (m_radio) {
    bootloaderResetRequest req(/*bootToFirmware*/ 1);
    sendPacketOrTimeout(req, /*useSafeLink*/ false);
  }
}

void Crazyflie::sysoff()
{
  if (m_radio) {
    crtpNrf51SysOffRequest req;
    sendPacketOrTimeout(req);
  }
}

void Crazyflie::alloff()
{
  if (m_radio) {
    crtpNrf51AllOffRequest req;
    sendPacketOrTimeout(req);
  }
}

void Crazyflie::syson()
{
  if (m_radio) {
    crtpNrf51SysOnRequest req;
    sendPacketOrTimeout(req);
  }
}

float Crazyflie::vbat()
{
  if (m_radio) {
    crtpNrf51GetVBatRequest req;
    startBatchRequest();
    addRequest(req, 2);
    handleRequests();
    return getRequestResult<crtpNrf51GetVBatResponse>(0)->vbat;
  } else {
    return nan("");
  }
}

void Crazyflie::writeFlash(
  BootloaderTarget target,
  const std::vector<uint8_t>& data)
{
  // Get info about the target
  bootloaderGetInfoRequest req(target);
  startBatchRequest();
  addRequest(req, 3);
  handleRequests(/*crtpMode=*/false, /*useSafeLink*/ false);
  const bootloaderGetInfoResponse* response = getRequestResult<bootloaderGetInfoResponse>(0);
  uint16_t pageSize = response->pageSize;
  uint16_t flashStart = response->flashStart;
  uint16_t nBuffPage = response->nBuffPage;

  uint16_t numPages = ceil(data.size() / (float)pageSize);
  if (numPages + flashStart >= response->nFlashPage) {
    std::stringstream sstr;
    sstr << "Requested size too large!";
    throw std::runtime_error(sstr.str());
  }

  std::stringstream sstr;
  sstr << "pageSize: " << pageSize
            << " nBuffPage: " << nBuffPage
            << " nFlashPage: " << response->nFlashPage
            << " flashStart: " << flashStart
            << " version: " << (int)response->version
            << " numPages: " << numPages;
  m_logger.info(sstr.str());

  // write flash
  size_t offset = 0;
  uint16_t usedBuffers = 0;
  // startBatchRequest();
  for (uint16_t page = flashStart; page < numPages + flashStart; ++page) {
    std::stringstream sstr;
    sstr << "page: " << page - flashStart + 1 << " / " << numPages;
    m_logger.info(sstr.str());
    for (uint16_t address = 0; address < pageSize; address += 25) {

      // std::cout << "request: " << page << " " << address << std::endl;
      bootloaderLoadBufferRequest req(target, usedBuffers, address);
      size_t requestedSize = std::min<size_t>(data.size() - offset, std::min<size_t>(25, pageSize - address));
      memcpy(req.data, &data[offset], requestedSize);
      // addRequest(req, 0);
      // for (size_t i = 0; i < 10; ++i)
      // std::cout << "request: " << req.page << " " << req.address << " " << requestedSize << std::endl;
      // for (size_t i = 0; i < 10; ++i) {

      // auto start = std::chrono::system_clock::now();
      // while (true) {
        sendPacketOrTimeoutInternal((uint8_t*)&req, 7 + requestedSize, false);
      //   startBatchRequest();
      //   bootloaderReadBufferRequest req2(target, usedBuffers, address);
      //   addRequest(req2, 7);
      //   handleRequests(/*crtpMode=*/false);
      //   const bootloaderReadBufferResponse* response = getRequestResult<bootloaderReadBufferResponse>(0);
      //   if (memcmp(req.data, response->data, requestedSize) == 0) {
      //     break;
      //   }
      //   auto end = std::chrono::system_clock::now();
      //   std::chrono::duration<double> elapsedSeconds = end-start;
      //   if (elapsedSeconds.count() > 1.0) {
      //     throw std::runtime_error("timeout");
      //   }
      // }
      offset += requestedSize;
      if (offset >= data.size()) {
        break;
      }
    }
    ++usedBuffers;
    if (usedBuffers == nBuffPage
        || page == numPages + flashStart - 1) {


      // startBatchRequest();
      // for (uint16_t buf = 0; buf < usedBuffers; ++buf) {
      //   for (uint16_t address = 0; address < pageSize; address += 25) {
      //     // std::cout << "request: " << page << " " << address << std::endl;
      //     bootloaderReadBufferRequest req(target, buf, address);
      //     addRequest(req, 7);
      //   }
      // }
      // handleRequests(/*crtpMode=*/false);


      // upload all the buffers now
      // std::cout << "try to upload buffers!" << std::endl;
      // handleRequests(/*crtpMode=*/false);
      // std::cout << "buffers uploaded!" << std::endl;

      // std::this_thread::sleep_for(std::chrono::milliseconds(100));

      // write flash
      bootloaderWriteFlashRequest req(target, 0, page - usedBuffers + 1, usedBuffers);
      sendPacketOrTimeoutInternal((uint8_t*)&req, sizeof(req), false);

      auto start = std::chrono::system_clock::now();

      size_t tries = 0;
      while (true) {
        ITransport::Ack ack;
        bootloaderFlashStatusRequest statReq(target);
        sendPacket(statReq, ack, false);
        if (   ack.ack
            && ack.size == 5
            && memcmp(&req, ack.data, 3) == 0) {
          if (ack.data[3] != 1 || ack.data[4] != 0) {
            throw std::runtime_error("Error during flashing!");
          }
          break;
        }

        // std::cout << page - usedBuffers + 1 << "," << usedBuffers << std::endl;
        // startBatchRequest();
        // bootloaderWriteFlashRequest req(target, 0, page - usedBuffers + 1, usedBuffers);
        // addRequest(req, 3);
        // handleRequests(/*crtpMode=*/false);
        // const bootloaderWriteFlashResponse* response = getRequestResult<bootloaderWriteFlashResponse>(0);
        // if (response->done == 1 && response->error == 0) {
        //   break;
        // }
        auto end = std::chrono::system_clock::now();
        std::chrono::duration<double> elapsedSeconds = end-start;
        if (elapsedSeconds.count() > 0.5) {
          start = end;
          sendPacketOrTimeout(req, false);
          ++tries;
          if (tries > 5) {
            throw std::runtime_error("timeout");
          }
        }
      }

      // std::cout << "Flashed: " << (page - flashStart) / (float)numPages * 100.0 << " %" << std::endl;

      // get ready to fill more buffers
      // if (page != numPages + flashStart - 1) {
      //   startBatchRequest();
      // }
      usedBuffers = 0;
    }
  }


}

void Crazyflie::readFlash(
  BootloaderTarget target,
  size_t size,
  std::vector<uint8_t>& data)
{
  // Get info about the target
  bootloaderGetInfoRequest req(target);
  startBatchRequest();
  addRequest(req, 3);
  handleRequests(/*crtpMode=*/false, /*useSafelink*/false);
  const bootloaderGetInfoResponse* response = getRequestResult<bootloaderGetInfoResponse>(0);
  uint16_t pageSize = response->pageSize;
  uint16_t flashStart = response->flashStart;

  uint16_t numPages = ceil(size / (float)pageSize);
  if (numPages + flashStart >= response->nFlashPage) {
    std::stringstream sstr;
    sstr << "Requested size too large!";
    throw std::runtime_error(sstr.str());
  }

  std::stringstream sstr;
  sstr << "pageSize: " << pageSize
       << " nFlashPage: " << response->nFlashPage
       << " flashStart: " << flashStart
       << " version: " << (int)response->version
       << " numPages: " << numPages;
  m_logger.info(sstr.str());

  // read flash
  size_t offset = 0;
  startBatchRequest();
  for (uint16_t page = flashStart; page < numPages + flashStart; ++page) {
    for (uint16_t address = 0; address < pageSize; address += 25) {
      // std::cout << "request: " << page << " " << address << std::endl;
      bootloaderReadFlashRequest req(target, page, address);
      addRequest(req, 7);
      size_t requestedSize = std::min(25, pageSize - address);
      offset += requestedSize;
      if (offset > size) {
        break;
      }
    }
  }
  handleRequests(/*crtpMode=*/false, /*useSafelink*/false);

  // update output
  data.resize(size);
  size_t i = 0;
  offset = 0;
  for (uint16_t page = flashStart; page < numPages + flashStart; ++page) {
    for (uint16_t address = 0; address < pageSize; address += 25) {
      const bootloaderReadFlashResponse* response = getRequestResult<bootloaderReadFlashResponse>(i++);
      size_t requestedSize = std::min(25, pageSize - address);
      // std::cout << "offset: " << offset << " reqS: " << requestedSize;
      memcpy(&data[offset], response->data, std::min(size - offset, requestedSize));
      offset += requestedSize;
      if (offset > size) {
        break;
      }
    }
  }
}
#endif

void Crazyflie::requestLogToc(bool forceNoCache)
{
  // Lazily initialize protocol version
  if (m_protocolVersion < 0)
  {
    m_protocolVersion = getProtocolVersion();
  }
  if (m_protocolVersion < 4)
  {
    m_logger.error("Old logging interface not supported! Please update your firmware.");
    return;
  }

  // Find the number of entries in TOC
  crtpLogGetInfoV2Request req;
  m_connection.send(req);
  using res = crtpLogGetInfoV2Response;
  auto p = waitForResponse(&res::valid);
  uint16_t numLogVariables = res::numLogVariables(p);
  uint32_t crc = res::crc(p);

  m_logger.info("Log TOC: " + std::to_string(numLogVariables) + " entries with CRC " + std::to_string(crc));

  // check if it is in the cache
  std::string fileName = "log" + std::to_string(crc) + ".csv";
  std::ifstream infile(fileName);

  m_logTocEntries.clear();
  if (!forceNoCache && infile.good())
  {
    m_logger.info("Log TOC: found cache.");
    std::string line, cell;
    std::getline(infile, line); // ignore header
    while (std::getline(infile, line)) {
      std::stringstream lineStream(line);
      m_logTocEntries.resize(m_logTocEntries.size() + 1);
      std::getline(lineStream, cell, ',');
      m_logTocEntries.back().id = std::stoi(cell);
      std::getline(lineStream, cell, ',');
      m_logTocEntries.back().type = (LogType)std::stoi(cell);
      std::getline(lineStream, cell, ',');
      m_logTocEntries.back().group = cell;
      std::getline(lineStream, cell, ',');
      m_logTocEntries.back().name = cell;
    }
    if (m_logTocEntries.size() != numLogVariables)
    {
      m_logger.warning("Log TOC: invalid cache.");
      m_logTocEntries.clear();
    }
  }
  if (m_logTocEntries.empty())
  {
    m_logger.info("Log TOC: not in cache");

    // Update internal structure with obtained data
    m_logTocEntries.resize(numLogVariables);

    // Request detailed information
    for (uint16_t i = 0; i < numLogVariables; ++i)
    {
      crtpLogGetItemV2Request req1(i);
      m_connection.send(req1);
    }

    for (uint16_t i = 0; i < numLogVariables; ++i)
    {
      using res = crtpLogGetItemV2Response;
      auto p = waitForResponse(&res::valid);

      LogTocEntry &entry = m_logTocEntries[i];
      entry.id = i;
      entry.type = (Crazyflie::LogType)res::type(p);
      auto groupAndName = res::groupAndName(p);
      entry.group = groupAndName.first;
      entry.name = groupAndName.second;

      assert(res::id(p) == i);
    }
    // Write a cache file
    {
      // Atomic file write: write in temporary file first to avoid race conditions
      std::string fileNameTemp = fileName + ".tmp";
      std::ofstream output(fileNameTemp);
      output << "id,type,group,name" << std::endl;
      for (const auto &entry : m_logTocEntries)
      {
        output << std::to_string(entry.id) << ","
               << std::to_string(entry.type) << ","
               << entry.group << ","
               << entry.name << std::endl;
      }
      // change the filename
      rename(fileNameTemp.c_str(), fileName.c_str());
    }
  }

}

void Crazyflie::requestParamToc(bool forceNoCache)
{
  // Lazily initialize protocol version
  if (m_protocolVersion < 0) {
    m_protocolVersion = getProtocolVersion();
  }
  if (m_protocolVersion < 4) {
    m_logger.error("Old parameter interface not supported! Please update your firmware.");
    return;
  }

  // Find the number of parameters in TOC
  crtpParamTocGetInfoV2Request req;
  m_connection.send(req);
  using res = crtpParamTocGetInfoV2Response;
  auto p = waitForResponse(&res::valid);
  uint16_t numParams = res::numParams(p);
  uint32_t crc = res::crc(p);

  m_logger.info("Param TOC: " + std::to_string(numParams) + " entries with CRC " + std::to_string(crc));

  // check if it is in the cache
  std::string fileName = "params" + std::to_string(crc) + ".csv";
  std::ifstream infile(fileName);

  m_paramTocEntries.clear();
  if (!forceNoCache && infile.good()) {
      m_logger.info("Param TOC: found cache.");
      std::string line, cell;
      std::getline(infile, line); // ignore header
      while (std::getline(infile, line)) {
        std::stringstream lineStream(line);
        m_paramTocEntries.resize(m_paramTocEntries.size() + 1);
        std::getline(lineStream, cell, ',');
        m_paramTocEntries.back().id = std::stoi(cell);
        std::getline(lineStream, cell, ',');
        m_paramTocEntries.back().type = (ParamType)std::stoi(cell);
        std::getline(lineStream, cell, ',');
        m_paramTocEntries.back().readonly = std::stoi(cell);
        std::getline(lineStream, cell, ',');
        m_paramTocEntries.back().group = cell;
        std::getline(lineStream, cell, ',');
        m_paramTocEntries.back().name = cell;
      }
      if (m_paramTocEntries.size() != numParams) {
        m_logger.warning("Param TOC: invalid cache.");
        m_paramTocEntries.clear();
      }
  }
  if (m_paramTocEntries.empty()) {
    m_logger.info("Param TOC: not in cache");

    // Request detailed information
    for (uint16_t i = 0; i < numParams; ++i) {
      crtpParamTocGetItemV2Request req1(i);
      m_connection.send(req1);
    }

    // Update internal structure with obtained data
    m_paramTocEntries.resize(numParams);

    for (uint16_t i = 0; i < numParams; ++i) {
      using res1 = crtpParamTocGetItemV2Response;
      auto p1 = waitForResponse(&res1::valid);

      ParamTocEntry &entry = m_paramTocEntries[i];
      entry.id = i;
      entry.type = (Crazyflie::ParamType) res1::type(p1);
      entry.readonly = res1::readonly(p1);
      auto groupAndName = res1::groupAndName(p1);
      entry.group = groupAndName.first;
      entry.name = groupAndName.second;

      assert(res1::id(p1) == i);
    }
    // Write a cache file
    {
      // Atomic file write: write in temporary file first to avoid race conditions
      std::string fileNameTemp = fileName + ".tmp";
      std::ofstream output(fileNameTemp);
      output << "id,type,readonly,group,name" << std::endl;
      for (const auto& entry : m_paramTocEntries) {
        output << std::to_string(entry.id) << ","
               << std::to_string(entry.type) << ","
               << std::to_string(entry.readonly) << ","
               << entry.group << ","
               << entry.name << std::endl;
      }
      // change the filename
      rename(fileNameTemp.c_str(), fileName.c_str());
    }
  }

  // Request values
  assert(m_paramTocEntries.size() == numParams);
  
  for (uint16_t i = 0; i < numParams; ++i)
  {
    crtpParamReadV2Request req2(i);
    m_connection.send(req2);
  }

  for (uint16_t i = 0; i < numParams; ++i)
  {
    using res2 = crtpParamValueV2Response;
    auto p2 = waitForResponse(&res2::valid);
    assert(res2::status(p2) == 0);

    const auto &entry = m_paramTocEntries[i];
    switch (entry.type)
    {
    case ParamTypeUint8:
      m_paramValues[i].valueUint8 = res2::value<uint8_t>(p2);
      break;
    case ParamTypeInt8:
      m_paramValues[i].valueInt8 = res2::value<int8_t>(p2);
      break;
    case ParamTypeUint16:
      m_paramValues[i].valueUint16 = res2::value<uint16_t>(p2);
      break;
    case ParamTypeInt16:
      m_paramValues[i].valueInt16 = res2::value<int16_t>(p2);
      break;
    case ParamTypeUint32:
      m_paramValues[i].valueUint32 = res2::value<uint32_t>(p2);
      break;
    case ParamTypeInt32:
      m_paramValues[i].valueInt32 = res2::value<int32_t>(p2);
      break;
    case ParamTypeFloat:
      m_paramValues[i].valueFloat = res2::value<float>(p2);
      break;
    default:
      assert(false);
    }
    assert(res2::id(p2) == i);
  }
}
void Crazyflie::requestMemoryToc()
{
  // Find the number of memories
  crtpMemoryGetNumberRequest req;
  m_connection.send(req);
  using res = crtpMemoryGetNumberResponse;
  auto p = waitForResponse(&res::valid);
  uint8_t numberOfMemories = res::numberOfMemories(p);

  m_logger.info("Memories: " + std::to_string(numberOfMemories));

  // Request detailed information
  for (uint8_t i = 0; i < numberOfMemories; ++i) {
    crtpMemoryGetInfoRequest req(i);
    m_connection.send(req);
  }

  // Update internal structure with obtained data
  m_memoryTocEntries.resize(numberOfMemories);
  for (uint8_t i = 0; i < numberOfMemories; ++i) {
    using res = crtpMemoryGetInfoResponse;
    auto p = waitForResponse(&res::valid);

    MemoryTocEntry& entry = m_memoryTocEntries[i];
    entry.id = i;
    entry.type = (MemoryType)res::type(p);
    entry.size = res::size(p);
    entry.addr = res::addr(p);

    assert(i == res::id(p));
  }
}

#if 0
void Crazyflie::startSetParamRequest()
{
  startBatchRequest();
}

void Crazyflie::addSetParam(uint16_t id, const ParamValue& value)
{
  bool found = false;
  for (auto&& entry : m_paramTocEntries) {
    if (entry.id == id) {
      found = true;
      if (!m_param_use_V2) {
        switch (entry.type) {
          case ParamTypeUint8:
            {
              crtpParamWriteRequest<uint8_t> request(id, value.valueUint8);
              addRequest(request, 1);
              break;
            }
          case ParamTypeInt8:
            {
              crtpParamWriteRequest<int8_t> request(id, value.valueInt8);
              addRequest(request, 1);
              break;
            }
          case ParamTypeUint16:
            {
              crtpParamWriteRequest<uint16_t> request(id, value.valueUint16);
              addRequest(request, 1);
              break;
            }
          case ParamTypeInt16:
            {
              crtpParamWriteRequest<int16_t> request(id, value.valueInt16);
              addRequest(request, 1);
              break;
            }
          case ParamTypeUint32:
            {
              crtpParamWriteRequest<uint32_t> request(id, value.valueUint32);
              addRequest(request, 1);
              break;
            }
          case ParamTypeInt32:
            {
              crtpParamWriteRequest<int32_t> request(id, value.valueInt32);
              addRequest(request, 1);
              break;
            }
          case ParamTypeFloat:
            {
              crtpParamWriteRequest<float> request(id, value.valueFloat);
              addRequest(request, 1);
              break;
            }
        }
      } else {
        switch (entry.type) {
          case ParamTypeUint8:
            {
              crtpParamWriteV2Request<uint8_t> request(id, value.valueUint8);
              addRequest(request, 2);
              break;
            }
          case ParamTypeInt8:
            {
              crtpParamWriteV2Request<int8_t> request(id, value.valueInt8);
              addRequest(request, 2);
              break;
            }
          case ParamTypeUint16:
            {
              crtpParamWriteV2Request<uint16_t> request(id, value.valueUint16);
              addRequest(request, 2);
              break;
            }
          case ParamTypeInt16:
            {
              crtpParamWriteV2Request<int16_t> request(id, value.valueInt16);
              addRequest(request, 2);
              break;
            }
          case ParamTypeUint32:
            {
              crtpParamWriteV2Request<uint32_t> request(id, value.valueUint32);
              addRequest(request, 2);
              break;
            }
          case ParamTypeInt32:
            {
              crtpParamWriteV2Request<int32_t> request(id, value.valueInt32);
              addRequest(request, 2);
              break;
            }
          case ParamTypeFloat:
            {
              crtpParamWriteV2Request<float> request(id, value.valueFloat);
              addRequest(request, 2);
              break;
            }
        }
      }
    }
  }

  if (!found) {
    std::stringstream sstr;
    sstr << "Could not find parameter with id " << id;
    throw std::runtime_error(sstr.str());
  }

  m_paramValues[id] = value;
}

void Crazyflie::setRequestedParams()
{
  handleRequests();
}

void Crazyflie::setParam(uint16_t id, const ParamValue& value)
{
  startBatchRequest();
  addSetParam(id, value);
  setRequestedParams();
}
#endif
bitcraze::crazyflieLinkCpp::Packet Crazyflie::waitForResponse(
    std::function<bool(const bitcraze::crazyflieLinkCpp::Packet &)> condition)
{
  while (true) {
    auto p = m_connection.recv(0);
    if (crtpConsoleResponse::valid(p)) {
      if (m_consoleCallback) {
        m_consoleCallback(crtpConsoleResponse::text(p).c_str());
      }
    }
    if (condition(p)) {
      return p;
    }
  }
}
#if 0
bool Crazyflie::sendPacketInternal(
  const uint8_t* data,
  uint32_t length,
  bool useSafeLink)
{
  ITransport::Ack ack;
  sendPacketInternal(data, length, ack, useSafeLink);
  return ack.ack;
}

 void Crazyflie::sendPacketOrTimeoutInternal(
   const uint8_t* data,
   uint32_t length,
   bool useSafeLink,
   float timeout)
{
  auto start = std::chrono::system_clock::now();
  while (!sendPacketInternal(data, length, useSafeLink)) {
    auto end = std::chrono::system_clock::now();
    std::chrono::duration<double> elapsedSeconds = end-start;
    if (elapsedSeconds.count() > timeout) {
      throw std::runtime_error("timeout");
    }
  }
}

void Crazyflie::sendPacketInternal(
  const uint8_t* data,
  uint32_t length,
  ITransport::Ack& ack,
  bool useSafeLink)
{
  static uint32_t numPackets = 0;
  static uint32_t numAcks = 0;

  numPackets++;

  if (m_radio) {
    std::unique_lock<std::mutex> mlock(g_radioMutex[m_devId]);
    if (m_radio->getAddress() != m_address) {
      m_radio->setAddress(m_address);
    }
    if (m_radio->getChannel() != m_channel) {
      m_radio->setChannel(m_channel);
    }
    if (m_radio->getDatarate() != m_datarate) {
      m_radio->setDatarate(m_datarate);
    }
    if (!m_radio->getAckEnable()) {
      m_radio->setAckEnable(true);
    }
    // consider safelink here:
    //    Adds 1bit counter to CRTP header to guarantee that no ack (downlink)
    //    payload are lost and no uplink packet are duplicated.
    //    The caller should resend packet if not acked (ie. same as with a
    //    direct call to crazyradio.send_packet)
    if (useSafeLink) {
      std::vector<uint8_t> dataCopy(data, data + length);
      dataCopy[0] &= 0xF3;
      dataCopy[0] |= m_curr_up << 3 | m_curr_down << 2;
      m_radio->sendPacket(dataCopy.data(), length, ack);
      if (ack.ack && ack.size > 0 && (ack.data[0] & 0x04) == (m_curr_down << 2)) {
        m_curr_down = 1 - m_curr_down;
      }
      if (ack.ack) {
        m_curr_up = 1 - m_curr_up;
      }
    } else {
      m_radio->sendPacket(data, length, ack);
    }

  } else {
    std::unique_lock<std::mutex> mlock(g_crazyflieusbMutex[m_devId]);
    m_transport->sendPacket(data, length, ack);
  }
  ack.data[ack.size] = 0;
  if (ack.ack) {
    handleAck(ack);
    numAcks++;
  }
  if (numPackets == 100) {
    if (m_linkQualityCallback) {
      // We just take the ratio of sent vs. acked packets here
      // for a sliding window of 100 packets
      float linkQuality = numAcks / (float)numPackets;
      m_linkQualityCallback(linkQuality);
    }
    numPackets = 0;
    numAcks = 0;
  }
}

void Crazyflie::handleAck(
  const ITransport::Ack& result)
{
  if (crtpConsoleResponse::match(result)) {
    if (result.size > 0) {
      crtpConsoleResponse* r = (crtpConsoleResponse*)result.data;
      if (m_consoleCallback) {
        m_consoleCallback(r->text);
      }
      // std::cout << "Console CF: " << r->text << std::endl;
    }
  }
  else if (crtpLogGetInfoResponse::match(result)) {
    // handled in batch system
  }
  else if (crtpLogGetInfoV2Response::match(result)) {
    // handled in batch system
  }
  else if (crtpLogGetItemResponse::match(result)) {
    // handled in batch system
  }
  else if (crtpLogGetItemV2Response::match(result)) {
    // handled in batch system
  }
  else if (crtpLogControlResponse::match(result)) {
    // handled in batch system
  }
  else if (crtpLogDataResponse::match(result)) {
    crtpLogDataResponse* r = (crtpLogDataResponse*)result.data;
    auto iter = m_logBlockCb.find(r->blockId);
    if (iter != m_logBlockCb.end()) {
      iter->second(r, result.size - 5);
    }
    else {
      m_logger.warning("Received unrequested data for block: " + std::to_string((int)r->blockId));
    }
  }
  else if (crtpParamTocGetInfoResponse::match(result)) {
    // handled in batch system
  }
  else if (crtpParamTocGetItemResponse::match(result)) {
    // handled in batch system
  }
  else if (crtpParamTocGetInfoV2Response::match(result)) {
    // handled in batch system
  }
  else if (crtpParamTocGetItemV2Response::match(result)) {
    // handled in batch system
  }
  else if (crtpParamSetByNameResponse::match(result)) {
    // handled in batch system
  }
  else if (crtpMemoryGetNumberResponse::match(result)) {
    // handled in batch system
  }
  else if (crtpMemoryGetInfoResponse::match(result)) {
    // handled in batch system
  }
  else if (crtpParamValueResponse::match(result)) {
    // handled in batch system
  }
  else if (crtpMemoryGetNumberResponse::match(result)) {
    // handled in batch system
  }
  else if (crtpMemoryReadResponse::match(result)) {
    // handled in batch system
  }
  else if (crtpMemoryWriteResponse::match(result)) {
    // handled in batch system
  }
  else if (crtp(result.data[0]).port == 8) {
    // handled in batch system
  }
  else if (crtp(result.data[0]).port == 13) {
    // handled in batch system
  }
  else if (crtpPlatformRSSIAck::match(result)) {
    if (result.size >= 3) {
      crtpPlatformRSSIAck* r = (crtpPlatformRSSIAck*)result.data;
      if (m_emptyAckCallback) {
        m_emptyAckCallback(r);
      }
    }
  }
  else {
    crtp* header = (crtp*)result.data;
    m_logger.warning("Don't know ack: Port: " + std::to_string((int)header->port)
      + " Channel: " + std::to_string((int)header->channel)
      + " Len: " + std::to_string((int)result.size));
    // for (size_t i = 1; i < result.size; ++i) {
    //   std::cout << "    " << (int)result.data[i] << std::endl;
    // }
    if (m_genericPacketCallback) {
      m_genericPacketCallback(result);
    }
  }
}

const Crazyflie::LogTocEntry* Crazyflie::getLogTocEntry(
  const std::string& group,
  const std::string& name) const
{
  for (auto&& entry : m_logTocEntries) {
    if (entry.group == group && entry.name == name) {
      return &entry;
    }
  }
  return nullptr;
}

const Crazyflie::ParamTocEntry* Crazyflie::getParamTocEntry(
  const std::string& group,
  const std::string& name) const
{
  for (auto&& entry : m_paramTocEntries) {
    if (entry.group == group && entry.name == name) {
      return &entry;
    }
  }
  return nullptr;
}

uint8_t Crazyflie::registerLogBlock(
  std::function<void(crtpLogDataResponse*, uint8_t)> cb)
{
  for (uint8_t id = 0; id < 255; ++id) {
    if (m_logBlockCb.find(id) == m_logBlockCb.end()) {
      m_logBlockCb[id] = cb;
      return id;
    }
  }
  return 255;
}

bool Crazyflie::unregisterLogBlock(
  uint8_t id)
{
  m_logBlockCb.erase(m_logBlockCb.find(id));
  return true;
}

// Batch system

void Crazyflie::startBatchRequest()
{
  m_batchRequests.clear();
}

void Crazyflie::addRequestInternal(
  const uint8_t* data,
  size_t numBytes,
  size_t numBytesToMatch)
{
  m_batchRequests.resize(m_batchRequests.size() + 1);
  m_batchRequests.back().request.resize(numBytes);
  memcpy(m_batchRequests.back().request.data(), data, numBytes);
  m_batchRequests.back().numBytesToMatch = numBytesToMatch;
  m_batchRequests.back().finished = false;
}

void Crazyflie::handleRequests(
  bool crtpMode,
  bool useSafeLink,
  float baseTime,
  float timePerRequest)
{
  auto start = std::chrono::system_clock::now();
  ITransport::Ack ack;
  m_numRequestsFinished = 0;
  m_numRequestsEnqueued = 0;
  bool sendPing = false;
  const size_t queueSize = 16;

  float timeout = baseTime + timePerRequest * m_batchRequests.size();

  // Workaround for https://github.com/USC-ACTLab/crazyswarm/issues/172
  // Disable safelink for now, until packets are really not dropped
  // anymore.
  if (false /*useSafeLink*/) {

    const size_t numRequests = m_batchRequests.size();
    size_t remainingRequests = numRequests;
    size_t requestIdx = 0;

    while (remainingRequests > 0) {
      remainingRequests = numRequests - m_numRequestsFinished;
      // std::cout << "rR: " << remainingRequests << " " << m_numRequestsEnqueued << std::endl;
      // enqueue up to queue size
      while(m_numRequestsEnqueued < queueSize && requestIdx < numRequests) {
        const auto& request = m_batchRequests[requestIdx++];

        do {
          sendPacketInternal(request.request.data(), request.request.size(), ack, useSafeLink);
          handleBatchAck(ack, crtpMode);

          auto end = std::chrono::system_clock::now();
          std::chrono::duration<double> elapsedSeconds = end-start;
          if (elapsedSeconds.count() > timeout) {
            throw std::runtime_error("timeout");
          }
          // std::cout << "send req " << requestIdx << std::endl;
        } while (!ack.ack);
        m_numRequestsEnqueued++;
      }
      // send ping's until at least one item in queue is done
      while(m_numRequestsEnqueued == queueSize
            || (m_numRequestsFinished < numRequests && requestIdx == numRequests)) {
        crtpEmpty ping;
        sendPacket(ping, ack, useSafeLink);
        handleBatchAck(ack, crtpMode);

        auto end = std::chrono::system_clock::now();
        std::chrono::duration<double> elapsedSeconds = end-start;
        if (elapsedSeconds.count() > timeout) {
          throw std::runtime_error("timeout");
        }
        // std::cout << "send ping " << m_numRequestsEnqueued << std::endl;
      }
    }
  } else {
    while (true) {
      if (!crtpMode || !sendPing) {
        for (const auto& request : m_batchRequests) {
          if (!request.finished) {
            // std::cout << "sendReq" << std::endl;
            sendPacketInternal(request.request.data(), request.request.size(), ack, useSafeLink);
            handleBatchAck(ack, crtpMode);
            auto end = std::chrono::system_clock::now();
            std::chrono::duration<double> elapsedSeconds = end-start;
            if (elapsedSeconds.count() > timeout) {
              throw std::runtime_error("timeout");
            }
          }
        }
        if (m_radio && !useSafeLink) {
          sendPing = true;
        }
      } else {
        size_t remainingRequests = m_batchRequests.size() - m_numRequestsFinished;
        for (size_t i = 0; i < remainingRequests; ++i) {
          crtpEmpty ping;
          sendPacket(ping, ack, useSafeLink);
          handleBatchAck(ack, crtpMode);
          // if (ack.ack && crtpPlatformRSSIAck::match(ack)) {
          //   sendPing = false;
          // }

          auto end = std::chrono::system_clock::now();
          std::chrono::duration<double> elapsedSeconds = end-start;
          if (elapsedSeconds.count() > timeout) {
            throw std::runtime_error("timeout");
          }
        }

        sendPing = false;
      }
      if (m_numRequestsFinished == m_batchRequests.size()) {
        break;
      }
    }
  }
}

void Crazyflie::handleBatchAck(
  const ITransport::Ack& ack,
  bool crtpMode)
{
  if (ack.ack) {
    for (auto& request : m_batchRequests) {
      if (crtpMode) {
        if ((crtp(ack.data[0]) == crtp(request.request[0]) || ack.data[0] == request.request[0])
            && memcmp(&ack.data[1], &request.request[1], request.numBytesToMatch) == 0
            && !request.finished) {
          request.ack = ack;
          request.finished = true;
          ++m_numRequestsFinished;
          --m_numRequestsEnqueued;
          // std::cout << "gotack" <<std::endl;
          return;
        }
      } else {
        if (!request.finished
            && memcmp(&ack.data[0], &request.request[0], request.numBytesToMatch) == 0) {
          request.ack = ack;
          request.finished = true;
          ++m_numRequestsFinished;
          --m_numRequestsEnqueued;
          // std::cout << m_numRequestsFinished / (float)m_batchRequests.size() * 100.0 << " %" << std::endl;
          return;
        }
      }
    }
    // ack is (also) handled in sendPacket
  }
}

void Crazyflie::setGroupMask(uint8_t groupMask)
{
  crtpCommanderHighLevelSetGroupMaskRequest request(groupMask);
  startBatchRequest();
  addRequest(request, 2);
  handleRequests();
}

void Crazyflie::takeoff(float height, float duration, uint8_t groupMask)
{
  crtpCommanderHighLevelTakeoffRequest req(groupMask, height, duration);
  sendPacketOrTimeout(req);
}

void Crazyflie::land(float height, float duration, uint8_t groupMask)
{
  crtpCommanderHighLevelLandRequest req(groupMask, height, duration);
  sendPacketOrTimeout(req);
}

void Crazyflie::stop(uint8_t groupMask)
{
  crtpCommanderHighLevelStopRequest req(groupMask);
  sendPacketOrTimeout(req);
}

void Crazyflie::goTo(float x, float y, float z, float yaw, float duration, bool relative, uint8_t groupMask)
{
  crtpCommanderHighLevelGoToRequest req(groupMask, relative, x, y, z, yaw, duration);
  sendPacketOrTimeout(req);
}

void Crazyflie::uploadTrajectory(
  uint8_t trajectoryId,
  uint32_t pieceOffset,
  const std::vector<poly4d>& pieces)
{
  for (const auto& entry : m_memoryTocEntries) {
    if (entry.type == MemoryTypeTRAJ) {
      startBatchRequest();
      // upload pieces
      size_t remainingBytes = sizeof(poly4d) * pieces.size();
      size_t numRequests = ceil(remainingBytes / 24.0f);
      for (size_t i = 0; i < numRequests; ++i) {
        crtpMemoryWriteRequest req(entry.id, pieceOffset * sizeof(poly4d) + i*24);
        size_t size = std::min<size_t>(remainingBytes, 24);
        memcpy(req.data, reinterpret_cast<const uint8_t*>(pieces.data()) + i * 24, size);
        remainingBytes -= size;
        addRequestInternal(reinterpret_cast<const uint8_t*>(&req), 6 + size, 5);
      }
      // define trajectory
      crtpCommanderHighLevelDefineTrajectoryRequest req(trajectoryId);
      req.description.trajectoryLocation = TRAJECTORY_LOCATION_MEM;
      req.description.trajectoryType = TRAJECTORY_TYPE_POLY4D;
      req.description.trajectoryIdentifier.mem.offset = pieceOffset * sizeof(poly4d);
      req.description.trajectoryIdentifier.mem.n_pieces = (uint8_t)pieces.size();
      addRequest(req, 2);
      handleRequests();
      return;
    }
  }
  throw std::runtime_error("Could not find MemoryTypeTRAJ!");
}

void Crazyflie::startTrajectory(
  uint8_t trajectoryId,
  float timescale,
  bool reversed,
  bool relative,
  uint8_t groupMask)
{
  crtpCommanderHighLevelStartTrajectoryRequest req(groupMask, relative, reversed, trajectoryId, timescale);
  sendPacketOrTimeout(req);
}

void Crazyflie::readUSDLogFile(
  std::vector<uint8_t>& data)
{
  for (const auto& entry : m_memoryTocEntries) {
    if (entry.type == MemoryTypeUSD) {
      startBatchRequest();
      size_t remainingBytes = entry.size;
      size_t numRequests = ceil(remainingBytes / 24.0f);
      for (size_t i = 0; i < numRequests; ++i) {
        size_t size = std::min<size_t>(remainingBytes, 24);
        crtpMemoryReadRequest req(entry.id, i*24, size);
        remainingBytes -= size;
        addRequest(req, 5);
      }
      handleRequests();
      // put result in data vector
      data.resize(entry.size);
      remainingBytes = entry.size;
      for (size_t i = 0; i < numRequests; ++i) {
        size_t size = std::min<size_t>(remainingBytes, 24);
        const crtpMemoryReadResponse* response = getRequestResult<crtpMemoryReadResponse>(i);
        memcpy(&data[i*24], response->data, size);
        remainingBytes -= size;
      }
      return;
    }
  }
  throw std::runtime_error("Could not find MemoryTypeUSD!");
}

////////////////////////////////////////////////////////////////

CrazyflieBroadcaster::CrazyflieBroadcaster(
  const std::string& link_uri)
  : m_radio(nullptr)
  , m_devId(0)
  , m_channel(0)
  , m_address(0)
  , m_datarate(Crazyradio::Datarate_250KPS)
{
  int datarate;
  int channel;
  char datarateType;
  bool success = false;

  success = std::sscanf(link_uri.c_str(), "radio://%d/%d/%d%c/%" SCNx64,
     &m_devId, &channel, &datarate,
     &datarateType, &m_address) == 5;
  if (!success) {
    success = std::sscanf(link_uri.c_str(), "radio://%d/%d/%d%c",
       &m_devId, &channel, &datarate,
       &datarateType) == 4;
    m_address = 0xE7E7E7E7E7;
  }

  if (success)
  {
    m_channel = channel;
    if (datarate == 250 && datarateType == 'K') {
      m_datarate = Crazyradio::Datarate_250KPS;
    }
    else if (datarate == 1 && datarateType == 'M') {
      m_datarate = Crazyradio::Datarate_1MPS;
    }
    else if (datarate == 2 && datarateType == 'M') {
      m_datarate = Crazyradio::Datarate_2MPS;
    }

    if (m_devId >= MAX_RADIOS) {
      throw std::runtime_error("This version does not support that many radios. Adjust MAX_RADIOS and recompile!");
    }

    {
      std::unique_lock<std::mutex> mlock(g_radioMutex[m_devId]);
      if (!g_crazyradios[m_devId]) {
        g_crazyradios[m_devId] = new Crazyradio(m_devId);
        g_crazyradios[m_devId]->enableLogging(LOG_COMMUNICATION);
        // g_crazyradios[m_devId]->setAckEnable(false);
        g_crazyradios[m_devId]->setAckEnable(true);
        g_crazyradios[m_devId]->setArc(0);
      }
    }

    m_radio = g_crazyradios[m_devId];
  }
  else {
    throw std::runtime_error("Uri is not valid!");
  }
}

void CrazyflieBroadcaster::sendPacket(
  const uint8_t* data,
  uint32_t length)
{
  std::unique_lock<std::mutex> mlock(g_radioMutex[m_devId]);
  if (m_radio->getAddress() != m_address) {
    m_radio->setAddress(m_address);
  }
  if (m_radio->getChannel() != m_channel) {
    m_radio->setChannel(m_channel);
  }
  if (m_radio->getDatarate() != m_datarate) {
    m_radio->setDatarate(m_datarate);
  }
  if (m_radio->getAckEnable()) {
    m_radio->setAckEnable(false);
  }
  m_radio->sendPacketNoAck(data, length);
}

void CrazyflieBroadcaster::send2Packets(
  const uint8_t* data,
  uint32_t length)
{
  std::unique_lock<std::mutex> mlock(g_radioMutex[m_devId]);
  if (m_radio->getAddress() != m_address) {
    m_radio->setAddress(m_address);
  }
  if (m_radio->getChannel() != m_channel) {
    m_radio->setChannel(m_channel);
  }
  if (m_radio->getDatarate() != m_datarate) {
    m_radio->setDatarate(m_datarate);
  }
  if (m_radio->getAckEnable()) {
    m_radio->setAckEnable(false);
  }
  m_radio->send2PacketsNoAck(data, length);
}

void CrazyflieBroadcaster::takeoff(float height, float duration, uint8_t groupMask)
{
  crtpCommanderHighLevelTakeoffRequest req(groupMask, height, duration);
  sendPacket((uint8_t*)&req, sizeof(req));
}

void CrazyflieBroadcaster::land(float height, float duration, uint8_t groupMask)
{
  crtpCommanderHighLevelLandRequest req(groupMask, height, duration);
  sendPacket((uint8_t*)&req, sizeof(req));
}

void CrazyflieBroadcaster::stop(uint8_t groupMask)
{
  crtpCommanderHighLevelStopRequest req(groupMask);
  sendPacket((uint8_t*)&req, sizeof(req));
}

// This is always in relative coordinates
void CrazyflieBroadcaster::goTo(float x, float y, float z, float yaw, float duration, uint8_t groupMask)
{
  crtpCommanderHighLevelGoToRequest req(groupMask, true, x, y, z, yaw, duration);
  sendPacket((uint8_t*)&req, sizeof(req));
}

// This is always in relative coordinates
// TODO: this does not support trajectories that are of a different length!
void CrazyflieBroadcaster::startTrajectory(
  uint8_t trajectoryId,
  float timescale,
  bool reversed,
  uint8_t groupMask)
{
  crtpCommanderHighLevelStartTrajectoryRequest req(groupMask, true, reversed, trajectoryId, timescale);
  sendPacket((uint8_t*)&req, sizeof(req));
}

void CrazyflieBroadcaster::sendExternalPositions(
  const std::vector<externalPosition>& data)
{
  if (data.size() == 0) {
    return;
  }

  std::vector<crtpExternalPositionPacked> requests(ceil(data.size() / 4.0));
  for (size_t i = 0; i < data.size(); ++i) {
    size_t j = i / 4;
    requests[j].positions[i%4].id = data[i].id;
    requests[j].positions[i%4].x = data[i].x * 1000;
    requests[j].positions[i%4].y = data[i].y * 1000;
    requests[j].positions[i%4].z = data[i].z * 1000;
  }
  // 1 header byte per packet; 7 bytes for each position
  size_t numBytes = requests.size() + data.size() * 7;

  size_t remainingRequests = requests.size();
  size_t i = 0;
  while (remainingRequests > 0) {
    // the crazyradio requires the two packets to be the same size
    // -> only send2packets if this is possible
    if (   remainingRequests >= 2
        && numBytes >= 2 * sizeof(crtpExternalPositionPacked)) {
      size_t size = std::min(numBytes, 2 * sizeof(crtpExternalPositionPacked));
      send2Packets(reinterpret_cast<const uint8_t*>(&requests[i]), size);
      remainingRequests -= 2;
      numBytes -= size;
      i += 2;
    } else {
      size_t size = std::min(numBytes, sizeof(crtpExternalPositionPacked));
      sendPacket(reinterpret_cast<const uint8_t*>(&requests[i]), size);
      remainingRequests -= 1;
      numBytes -= size;
      i += 1;
    }
  }
  // assert(numBytes == 0);
}

void CrazyflieBroadcaster::emergencyStop()
{
  crtpEmergencyStopRequest req;
  sendPacket((uint8_t*)&req, sizeof(req));
}

void CrazyflieBroadcaster::emergencyStopWatchdog()
{
  crtpEmergencyStopWatchdogRequest req;
  sendPacket((uint8_t*)&req, sizeof(req));
}

// assumes input quaternion is normalized. will fail if not.
static inline uint32_t quatcompress(float const q[4])
{
  // we send the values of the quaternion's smallest 3 elements.
  unsigned i_largest = 0;
  for (unsigned i = 1; i < 4; ++i) {
    if (fabsf(q[i]) > fabsf(q[i_largest])) {
      i_largest = i;
    }
  }

  // since -q represents the same rotation as q,
  // transform the quaternion so the largest element is positive.
  // this avoids having to send its sign bit.
  unsigned negate = q[i_largest] < 0;

  // 1/sqrt(2) is the largest possible value
  // of the second-largest element in a unit quaternion.

  // do compression using sign bit and 9-bit precision per element.
  uint32_t comp = i_largest;
  for (unsigned i = 0; i < 4; ++i) {
    if (i != i_largest) {
      unsigned negbit = (q[i] < 0) ^ negate;
      unsigned mag = ((1 << 9) - 1) * (fabsf(q[i]) / (float)M_SQRT1_2) + 0.5f;
      comp = (comp << 10) | (negbit << 9) | mag;
    }
  }

  return comp;
}

void CrazyflieBroadcaster::sendExternalPoses(
  const std::vector<externalPose>& data)
{
  if (data.size() == 0) {
    return;
  }

  std::vector<crtpExternalPosePacked> requests(ceil(data.size() / 2.0));
  for (size_t i = 0; i < data.size(); ++i) {
    size_t j = i / 2;
    requests[j].poses[i%2].id = data[i].id;
    requests[j].poses[i%2].x = data[i].x * 1000;
    requests[j].poses[i%2].y = data[i].y * 1000;
    requests[j].poses[i%2].z = data[i].z * 1000;
    float q[4] = { data[i].qx, data[i].qy, data[i].qz, data[i].qw };
    requests[j].poses[i%2].quat = quatcompress(q);
  }
  // 2 header byte per packet; 11 bytes for each position
  size_t numBytes = requests.size() * 2 + data.size() * 11;

  size_t remainingRequests = requests.size();
  size_t i = 0;
  while (remainingRequests > 0) {
    // the crazyradio requires the two packets to be the same size
    // -> only send2packets if this is possible
    if (   remainingRequests >= 2
        && numBytes >= 2 * sizeof(crtpExternalPosePacked)) {
      size_t size = std::min(numBytes, 2 * sizeof(crtpExternalPosePacked));
      send2Packets(reinterpret_cast<const uint8_t*>(&requests[i]), size);
      remainingRequests -= 2;
      numBytes -= size;
      i += 2;
    } else {
      size_t size = std::min(numBytes, sizeof(crtpExternalPosePacked));
      sendPacket(reinterpret_cast<const uint8_t*>(&requests[i]), size);
      remainingRequests -= 1;
      numBytes -= size;
      i += 1;
    }
  }
  // assert(numBytes == 0);
}

// void CrazyflieBroadcaster::setParam(
//   uint8_t group,
//   uint8_t id,
//   Crazyflie::ParamType type,
//   const Crazyflie::ParamValue& value) {

//   switch (type) {
//     case Crazyflie::ParamTypeUint8:
//       {
//         crtpParamWriteBroadcastRequest<uint8_t> request(group, id, value.valueUint8);
//         sendPacket((const uint8_t*)&request, sizeof(request));
//         break;
//       }
//     case Crazyflie::ParamTypeInt8:
//       {
//         crtpParamWriteBroadcastRequest<int8_t> request(group, id, value.valueInt8);
//         sendPacket((const uint8_t*)&request, sizeof(request));
//         break;
//       }
//     case Crazyflie::ParamTypeUint16:
//       {
//         crtpParamWriteBroadcastRequest<uint16_t> request(group, id, value.valueUint16);
//         sendPacket((const uint8_t*)&request, sizeof(request));
//         break;
//       }
//     case Crazyflie::ParamTypeInt16:
//       {
//         crtpParamWriteBroadcastRequest<int16_t> request(group, id, value.valueInt16);
//         sendPacket((const uint8_t*)&request, sizeof(request));
//         break;
//       }
//     case Crazyflie::ParamTypeUint32:
//       {
//         crtpParamWriteBroadcastRequest<uint32_t> request(group, id, value.valueUint32);
//         sendPacket((const uint8_t*)&request, sizeof(request));
//         break;
//       }
//     case Crazyflie::ParamTypeInt32:
//       {
//         crtpParamWriteBroadcastRequest<int32_t> request(group, id, value.valueInt32);
//         sendPacket((const uint8_t*)&request, sizeof(request));
//         break;
//       }
//     case Crazyflie::ParamTypeFloat:
//       {
//         crtpParamWriteBroadcastRequest<float> request(group, id, value.valueFloat);
//         sendPacket((const uint8_t*)&request, sizeof(request));
//         break;
//       }
//   }
//   // TODO: technically we should update the internal copy of the value of each CF object
// }
#endif