Skip to content

Commit

Permalink
add support for parameters
Browse files Browse the repository at this point in the history
  • Loading branch information
@whoenig
whoenig committed Jul 2, 2015
1 parent 2d7a361 commit 95352f3
Show file tree
Hide file tree
Showing 5 changed files with 432 additions and 6 deletions.
125 changes: 125 additions & 0 deletions crazyflie/src/Crazyflie.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -7,6 +7,7 @@
#include "Crazyradio.h"

#include <iostream>
#include <cstring>

#define MAX_RADIOS 4

Expand All @@ -27,6 +28,9 @@ Crazyflie::Crazyflie(
, m_blockCreated()
, m_blockStarted()
, m_blockStopped()
, m_paramInfo()
, m_paramTocEntries()
, m_paramValues()
{
int datarate;
int channel;
Expand Down Expand Up @@ -130,6 +134,94 @@ void Crazyflie::requestLogToc()
}
}

void Crazyflie::requestParamToc()
{
m_paramInfo.len = 0;
m_paramTocEntries.clear();
m_paramValues.clear();
do
{
crtpParamTocGetInfoRequest request;
sendPacket((const uint8_t*)&request, sizeof(request));
} while(m_paramInfo.len == 0);
std::cout << "Params: " << (int)m_paramInfo.len << std::endl;

for (size_t i = 0; i < m_paramInfo.len; ++i)
{
// std::cout << i << std::endl;
do
{
crtpParamTocGetItemRequest request(i);
sendPacket((const uint8_t*)&request, sizeof(request));
} while(m_paramTocEntries.size() < i + 1);

do
{
crtpParamReadRequest request(i);
sendPacket((const uint8_t*)&request, sizeof(request));
} while(m_paramValues.find(i) == m_paramValues.end());
}
}

void Crazyflie::setParam(uint8_t id, const ParamValue& value) {

m_paramValues.erase(id);
for (auto&& entry : m_paramTocEntries) {
if (entry.id == id) {
do
{
switch (entry.type) {
case ParamTypeUint8:
{
crtpParamWriteRequest<uint8_t> request(id, value.valueUint8);
sendPacket((const uint8_t*)&request, sizeof(request));
break;
}
case ParamTypeInt8:
{
crtpParamWriteRequest<int8_t> request(id, value.valueInt8);
sendPacket((const uint8_t*)&request, sizeof(request));
break;
}
case ParamTypeUint16:
{
crtpParamWriteRequest<uint16_t> request(id, value.valueUint16);
sendPacket((const uint8_t*)&request, sizeof(request));
break;
}
case ParamTypeInt16:
{
crtpParamWriteRequest<int16_t> request(id, value.valueInt16);
sendPacket((const uint8_t*)&request, sizeof(request));
break;
}
case ParamTypeUint32:
{
crtpParamWriteRequest<uint32_t> request(id, value.valueUint32);
sendPacket((const uint8_t*)&request, sizeof(request));
break;
}
case ParamTypeInt32:
{
crtpParamWriteRequest<int32_t> request(id, value.valueInt32);
sendPacket((const uint8_t*)&request, sizeof(request));
break;
}
case ParamTypeFloat:
{
crtpParamWriteRequest<float> request(id, value.valueFloat);
sendPacket((const uint8_t*)&request, sizeof(request));
break;
}
}
} while(m_paramValues.find(id) == m_paramValues.end());
break;
}
}


}

void Crazyflie::sendPacket(
const uint8_t* data,
uint32_t length)
Expand Down Expand Up @@ -204,6 +296,27 @@ void Crazyflie::handleAck(
std::cout << "Received unrequested data for block: " << (int)r->blockId << std::endl;
}
}
else if (crtpParamTocGetInfoResponse::match(result)) {
crtpParamTocGetInfoResponse* r = (crtpParamTocGetInfoResponse*)result.data;
m_paramInfo.len = r->numParam;
}
else if (crtpParamTocGetItemResponse::match(result)) {
crtpParamTocGetItemResponse* r = (crtpParamTocGetItemResponse*)result.data;
m_paramTocEntries.resize(r->request.id + 1);
ParamTocEntry& entry = m_paramTocEntries[r->request.id];
entry.id = r->request.id;
entry.type = (ParamType)(r->length | r-> type << 2 | r->sign << 3);
entry.readonly = r->readonly;
entry.group = std::string(&r->text[0]);
entry.name = std::string(&r->text[entry.group.size() + 1]);
}
else if (crtpParamValueResponse::match(result)) {
crtpParamValueResponse* r = (crtpParamValueResponse*)result.data;
ParamValue v;
std::memcpy(&v, &r->valueFloat, 4);
// *v = r->valueFloat;
m_paramValues[r->id] = v;//(ParamValue)(r->valueFloat);
}
else {
crtp* header = (crtp*)result.data;
std::cout << "Don't know ack: Port: " << (int)header->port << " Channel: " << (int)header->channel << " Len: " << (int)result.size << std::endl;
Expand All @@ -226,6 +339,18 @@ const Crazyflie::LogTocEntry* Crazyflie::getLogTocEntry(
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)
{
Expand Down
100 changes: 100 additions & 0 deletions crazyflie/src/Crazyflie.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -9,6 +9,41 @@

class Crazyflie
{
public:
enum ParamType {
ParamTypeUint8 = 0x00 | (0x00<<2) | (0x01<<3),
ParamTypeInt8 = 0x00 | (0x00<<2) | (0x00<<3),
ParamTypeUint16 = 0x01 | (0x00<<2) | (0x01<<3),
ParamTypeInt16 = 0x01 | (0x00<<2) | (0x00<<3),
ParamTypeUint32 = 0x02 | (0x00<<2) | (0x01<<3),
ParamTypeInt32 = 0x02 | (0x00<<2) | (0x00<<3),
ParamTypeFloat = 0x02 | (0x01<<2) | (0x00<<3),
};

struct ParamTocEntry {
uint8_t id;
ParamType type;
bool readonly;
// ParamLength length;
// ParamType type;
// ParamSign sign;
// bool readonly;
// ParamGroup paramGroup;
std::string group;
std::string name;
};

union ParamValue{
uint8_t valueUint8;
int8_t valueInt8;
uint16_t valueUint16;
int16_t valueInt16;
uint32_t valueUint32;
int32_t valueInt32;
float valueFloat;
};


public:
Crazyflie(
const std::string& link_uri);
Expand All @@ -25,6 +60,32 @@ class Crazyflie

void requestLogToc();

void requestParamToc();

std::vector<ParamTocEntry>::const_iterator paramsBegin() const {
return m_paramTocEntries.begin();
}
std::vector<ParamTocEntry>::const_iterator paramsEnd() const {
return m_paramTocEntries.end();
}

template<class T>
void setParam(uint8_t id, const T& value) {
ParamValue v;
memcpy(&v, &value, sizeof(value));
setParam(id, v);
}

template<class T>
T getParam(uint8_t id) const {
ParamValue v = getParam(id);
return *(reinterpret_cast<T*>(&v));
}

const ParamTocEntry* getParamTocEntry(
const std::string& group,
const std::string& name) const;

protected:
void sendPacket(
const uint8_t* data,
Expand Down Expand Up @@ -59,6 +120,36 @@ class Crazyflie
std::string name;
};

/////////

struct paramInfo {
uint8_t len;
uint32_t crc;
};

// enum ParamLength {
// ParamLength1Byte = 0,
// ParamLength2Bytes = 1,
// ParamLength3Bytes = 2,
// ParamLength4Bytes = 3,
// };

// enum ParamType {
// ParamTypeInt = 0,
// ParamTypeFloat = 1,
// };

// enum ParamSign {
// ParamSignSigned = 0,
// ParamSignUnsigned = 1,
// };

// enum ParamGroup {
// ParamGroupVariable = 0,
// ParamGroupGroup = 1,
// };


private:
const LogTocEntry* getLogTocEntry(
const std::string& group,
Expand All @@ -70,6 +161,11 @@ class Crazyflie
bool unregisterLogBlock(
uint8_t id);

void setParam(uint8_t id, const ParamValue& value);
const ParamValue& getParam(uint8_t id) const {
return m_paramValues.at(id);
}

private:
Crazyradio* m_radio;
int m_devId;
Expand All @@ -88,6 +184,10 @@ class Crazyflie
std::set<uint8_t> m_blockStarted;
std::set<uint8_t> m_blockStopped;

paramInfo m_paramInfo;
std::vector<ParamTocEntry> m_paramTocEntries;
std::map<uint8_t, ParamValue> m_paramValues;

template<typename T>
friend class LogBlock;
};
Expand Down
Loading

0 comments on commit 95352f3

Please sign in to comment.