Commit 96b45e01 authored by Ian McInerney's avatar Ian McInerney

Removed the header from the returned packet data field

parent 69a57918
......@@ -370,14 +370,7 @@ CCRTPPacket *CCrazyRadio::sendPacket(int nRadioChannel, CCRTPPacket *crtpSend, C
delete cSendable;
if (crtpPacket) {
char *cData = crtpPacket->data();
int nLength = crtpPacket->dataLength();
if (nLength > 0) {
short sPort = (cData[0] & 0xf0) >> 4;
crtpPacket->setPort(sPort);
short sChannel = cData[0] & 0b00000011;
crtpPacket->setChannel(sChannel); // JRB: Why does the packet contain the channel also??
if ( crtpPacket->dataLength() > 0 ) {
crazyflie->callback_Packet(crtpPacket);
}
}
......@@ -406,8 +399,38 @@ CCRTPPacket *CCrazyRadio::readACK() {
crtpPacket = new CCRTPPacket(0);
/* Packet format for the CRTP packet received
7 6 5 4 3 2 1 0
+---+---+---+---+---+---+---+---+
| Random bits |
+---+---+---+---+---+---+---+---+
| Port | Res | Chan. |
+---+---+---+---+---+---+---+---+
| DATA 0 |
+---+---+---+---+---+---+---+---+
: : : : : : : : :
+---+---+---+---+---+---+---+---+
| Cksum |
+---+---+---+---+---+---+---+---+
*/
if (nBytesRead > 1) {
crtpPacket->setData(&cBuffer[1], nBytesRead); //JN: Need to change &cBuffer[1] to &cBuffer[0] for header info??
// Parse the header into its fields
char headerByte = cBuffer[1];
short sPort = (headerByte & 0xf0) >> 4;
crtpPacket->setPort(sPort);
short sChannel = headerByte & 0b00000011;
crtpPacket->setChannel(sChannel); // JRB: Why does the packet contain the channel also??
// There is no length field in the packet... grrr...
// use the length read in instead (minus status and header)
short nLength = nBytesRead - 2;
// The data is the remainder of the fields
if (nLength > 0) {
crtpPacket->setData(&cBuffer[2], nLength);
}
}
} else {
m_bAckReceived = false;
......
......@@ -68,8 +68,8 @@ bool CTOC::requestMetaData() {
CCRTPPacket* crtpReceived = m_crRadio->sendAndReceive(crtpPacket,
m_nRadioChannel, m_crazyflie);
if (crtpReceived->data()[1] == 0x01) {
m_nItemCount = crtpReceived->data()[2];
if (crtpReceived->data()[0] == 0x01) {
m_nItemCount = crtpReceived->data()[1];
bReturnvalue = true;
}
......@@ -121,13 +121,13 @@ bool CTOC::processItem(CCRTPPacket* crtpItem) {
char* cData = crtpItem->data();
//int nLength = crtpItem->dataLength(); -- unused variable
if (cData[1] == 0x0) { // Command identification ok?
int nID = cData[2];
int nType = cData[3];
if (cData[0] == 0x0) { // Command identification ok?
int nID = cData[1];
int nType = cData[2];
std::string strGroup;
int nI;
for (nI = 4; cData[nI] != '\0'; nI++) {
for (nI = 3; cData[nI] != '\0'; nI++) {
strGroup += cData[nI];
}
......@@ -243,8 +243,8 @@ bool CTOC::startLogging(std::string strName, std::string strBlockName) {
char* cData = crtpReceived->data();
bool bCreateOK = false;
if (cData[1] == 0x01 && cData[2] == lbCurrent.nID
&& cData[3] == 0x00) {
if (cData[0] == 0x01 && cData[1] == lbCurrent.nID
&& cData[2] == 0x00) {
bCreateOK = true;
} else {
printf("0x%x %x %x\n", cData[1], cData[2], cData[3]);
......@@ -395,13 +395,13 @@ bool CTOC::registerLoggingBlock(std::string strName, double dFrequency) {
char* cData = crtpReceived->data();
bool bCreateOK = false;
if (cData[1] == 0x00 && cData[2] == nID && cData[3] == 0x00) {
if (cData[0] == 0x00 && cData[1] == nID && cData[2] == 0x00) {
bCreateOK = true;
std::cout << "Registered logging block `" << strName << "'"
<< std::endl;
} else {
std::cout << "Unable to register block " << strName << " on the quadcopter" << std::endl;
printf("0x%x %x %x\n", cData[1], cData[2], cData[3] );
printf("0x%x %x %x\n", cData[0], cData[1], cData[2] );
}
if (crtpReceived) {
......@@ -486,14 +486,14 @@ void CTOC::processPackets(CCRTPPacket *crtpPacket) {
char* cData = crtpPacket->data();
float fValue;
memcpy(&fValue, &cData[5], 4);
memcpy(&fValue, &cData[4], 4);
char* cLogdata = &cData[5];
char* cLogdata = &cData[4];
int nOffset = 0;
int nIndex = 0;
//int nAvailableLogBytes = crtpPacket->dataLength() - 5; -- unused variable, commented out
int nBlockID = cData[1];
int nBlockID = cData[0];
bool bFound;
struct LoggingBlock lbCurrent = this->loggingBlockForID(nBlockID,
bFound);
......
......@@ -62,9 +62,16 @@ void CCrazyflie::callback_Packet(CCRTPPacket *crtpPacket) {
case CRTP_PORT_CONSOLE:
{
// Console
char cText[nLength];
std::memcpy(cText, &cData[1], nLength - 1);
cText[nLength - 1] = '\0';
if ( nLength < 1) {
// See if the length is 0, if it is leave
break;
}
char *cText;
cText = (char*) std::malloc(nLength + 1);
std::memcpy(cText, &cData[0], nLength);
cText[nLength + 1] = '\0';
this->file_console << cText;
this->file_console.flush();
......
......@@ -27,7 +27,7 @@ int CCrazyflie::queryControllerType() {
if (dataLength >= 1) {
// Parse the one data byte to get controller type
char *cData = crtpReceived->data();
controllerType = cData[1];
controllerType = cData[0];
retVal = controllerType;
} else {
// This isn't good...
......
Markdown is supported
0%
or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment