rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2025-03-01 18:29:16 +01:00
Code Issues Releases Activity

UAVTalk: Insert a sync byte (0x3C) and the packet size to facilitating handling

Browse Source 
by PipX modems.  This is a change to UAVTalk so GCS and the hardware must all
be updated.

git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@2016 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
peabody124 2010-10-27 17:16:29 +00:00 committed by peabody124
parent 33e6cbebdc
commit 4570d6df23
3 changed files with 83 additions and 23 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

53
flight/OpenPilot/UAVTalk/uavtalk.c
View File

@ -32,6 +32,7 @@
#include "openpilot.h" #include "openpilot.h"
// Private constants // Private constants
#define SYNC_VAL 0x3C
#define TYPE_MASK 0xFC #define TYPE_MASK 0xFC
#define TYPE_VER 0x10 #define TYPE_VER 0x10
#define TYPE_OBJ (TYPE_VER | 0x00) #define TYPE_OBJ (TYPE_VER | 0x00)
@ -65,7 +66,7 @@ static const uint8_t crc_table[256] = {
}; };
// Private types // Private types
typedef enum {STATE_SYNC, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS} RxState; typedef enum {STATE_SYNC, STATE_TYPE, STATE_SIZE, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS} RxState;
// Private variables // Private variables
static UAVTalkOutputStream outStream; static UAVTalkOutputStream outStream;
@ -239,6 +240,7 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte)
static uint8_t tmpBuffer[4]; static uint8_t tmpBuffer[4];
static UAVObjHandle obj; static UAVObjHandle obj;
static uint8_t type; static uint8_t type;
static uint16_t packet_size;
static uint32_t objId; static uint32_t objId;
static uint16_t instId; static uint16_t instId;
static uint32_t length; static uint32_t length;
@ -250,12 +252,33 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte)
++stats.rxBytes; ++stats.rxBytes;
switch (state) { switch (state) {
case STATE_SYNC: case STATE_SYNC:
if (rxbyte == SYNC_VAL) {
cs = updateCRC(0, &rxbyte, 1);
state = STATE_TYPE;
}
break;
case STATE_TYPE:
if ((rxbyte & TYPE_MASK) == TYPE_VER ) if ((rxbyte & TYPE_MASK) == TYPE_VER )
{ {
cs = updateCRC(0, &rxbyte, 1); cs = updateCRC(cs, &rxbyte, 1);
type = rxbyte; type = rxbyte;
state = STATE_OBJID; packet_size = 0;
state = STATE_SIZE;
rxCount = 0; rxCount = 0;
} else {
state = STATE_SYNC;
}
break;
case STATE_SIZE:
if(rxCount++ == 0) {
cs = updateCRC(cs, &rxbyte, 1);
packet_size += rxbyte;
}
else {
cs = updateCRC(cs, &rxbyte, 1);
packet_size += rxbyte << 8;
rxCount = 0;
state = STATE_OBJID;
} }
break; break;
case STATE_OBJID: case STATE_OBJID:
@ -526,22 +549,24 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
// Setup type and object id fields // Setup type and object id fields
objId = UAVObjGetID(obj); objId = UAVObjGetID(obj);
txBuffer[0] = type; txBuffer[0] = SYNC_VAL; // sync byte
txBuffer[1] = (uint8_t)(objId & 0xFF); txBuffer[1] = type;
txBuffer[2] = (uint8_t)((objId >> 8) & 0xFF); // data length inserted here below
txBuffer[3] = (uint8_t)((objId >> 16) & 0xFF); txBuffer[4] = (uint8_t)(objId & 0xFF);
txBuffer[4] = (uint8_t)((objId >> 24) & 0xFF); txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF);
txBuffer[6] = (uint8_t)((objId >> 16) & 0xFF);
txBuffer[7] = (uint8_t)((objId >> 24) & 0xFF);
// Setup instance ID if one is required // Setup instance ID if one is required
if (UAVObjIsSingleInstance(obj)) if (UAVObjIsSingleInstance(obj))
{ {
dataOffset = 5; dataOffset = 8;
} }
else else
{ {
txBuffer[5] = (uint8_t)(instId & 0xFF); txBuffer[8] = (uint8_t)(instId & 0xFF);
txBuffer[6] = (uint8_t)((instId >> 8) & 0xFF); txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF);
dataOffset = 7; dataOffset = 10;
} }
// Determine data length // Determine data length
@ -569,6 +594,10 @@ static int32_t sendSingleObject(UAVObjHandle obj, uint16_t instId, uint8_t type)
} }
} }
// Store the packet length
txBuffer[2] = (uint8_t)((dataOffset+length) & 0xFF);
txBuffer[3] = (uint8_t)(((dataOffset+length) >> 8) & 0xFF);
// Calculate checksum // Calculate checksum
txBuffer[dataOffset+length] = updateCRC(0, txBuffer, dataOffset+length); txBuffer[dataOffset+length] = updateCRC(0, txBuffer, dataOffset+length);

45
ground/src/plugins/uavtalk/uavtalk.cpp
View File

@ -26,6 +26,9 @@
*/ */
#include "uavtalk.h" #include "uavtalk.h"
#include <QtEndian> #include <QtEndian>
#include <QDebug>
#define SYNC_VAL 0x3C
const quint8 UAVTalk::crc_table[256] = { const quint8 UAVTalk::crc_table[256] = {
0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d, 0x00, 0x07, 0x0e, 0x09, 0x1c, 0x1b, 0x12, 0x15, 0x38, 0x3f, 0x36, 0x31, 0x24, 0x23, 0x2a, 0x2d,
@ -188,12 +191,35 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
// Receive state machine // Receive state machine
switch (rxState) { switch (rxState) {
case STATE_SYNC: case STATE_SYNC:
if (rxbyte == SYNC_VAL) {
rxCS = updateCRC(0, &rxbyte, 1);
rxState = STATE_TYPE;
}
break;
case STATE_TYPE:
if ((rxbyte & TYPE_MASK) == TYPE_VER ) if ((rxbyte & TYPE_MASK) == TYPE_VER )
{ {
rxCS = updateCRC(0, &rxbyte, 1); rxCS = updateCRC(rxCS, &rxbyte, 1);
rxType = rxbyte; rxType = rxbyte;
rxState = STATE_OBJID; packetSize = 0;
rxState = STATE_SIZE;
rxCount = 0; rxCount = 0;
} else {
rxState = STATE_SYNC;
}
break;
case STATE_SIZE:
if(rxCount++ == 0) {
rxCS = updateCRC(rxCS, &rxbyte, 1);
packetSize += rxbyte;
rxCount++;
}
else {
rxCS = updateCRC(rxCS, &rxbyte, 1);
rxCount++;
packetSize += rxbyte << 8;
rxCount = 0;
rxState = STATE_OBJID;
} }
break; break;
case STATE_OBJID: case STATE_OBJID:
@ -536,27 +562,28 @@ bool UAVTalk::transmitSingleObject(UAVObject* obj, quint8 type, bool allInstance
// Setup type and object id fields // Setup type and object id fields
objId = obj->getObjID(); objId = obj->getObjID();
txBuffer[0] = type; txBuffer[0] = SYNC_VAL;
qToLittleEndian<quint32>(objId, &txBuffer[1]); txBuffer[1] = type;
qToLittleEndian<quint32>(objId, &txBuffer[4]);
// Setup instance ID if one is required // Setup instance ID if one is required
if ( obj->isSingleInstance() ) if ( obj->isSingleInstance() )
{ {
dataOffset = 5; dataOffset = 8;
} }
else else
{ {
// Check if all instances are requested // Check if all instances are requested
if (allInstances) if (allInstances)
{ {
qToLittleEndian<quint16>(allInstId, &txBuffer[5]); qToLittleEndian<quint16>(allInstId, &txBuffer[8]);
} }
else else
{ {
instId = obj->getInstID(); instId = obj->getInstID();
qToLittleEndian<quint16>(instId, &txBuffer[5]); qToLittleEndian<quint16>(instId, &txBuffer[8]);
} }
dataOffset = 7; dataOffset = 10;
} }
// Determine data length // Determine data length
@ -584,6 +611,8 @@ bool UAVTalk::transmitSingleObject(UAVObject* obj, quint8 type, bool allInstance
} }
} }
qToLittleEndian<quint16>(dataOffset + length, &txBuffer[2]);
// Calculate checksum // Calculate checksum
txBuffer[dataOffset+length] = updateCRC(0, txBuffer, dataOffset+length); txBuffer[dataOffset+length] = updateCRC(0, txBuffer, dataOffset+length);

4
ground/src/plugins/uavtalk/uavtalk.h
View File

@ -81,7 +81,7 @@ private:
static const quint8 crc_table[256]; static const quint8 crc_table[256];
// Types // Types
typedef enum {STATE_SYNC, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS} RxStateType; typedef enum {STATE_SYNC, STATE_TYPE, STATE_SIZE, STATE_OBJID, STATE_INSTID, STATE_DATA, STATE_CS} RxStateType;
// Variables // Variables
QIODevice* io; QIODevice* io;
@ -97,8 +97,10 @@ private:
quint32 rxObjId; quint32 rxObjId;
quint16 rxInstId; quint16 rxInstId;
quint16 rxLength; quint16 rxLength;
quint8 rxCSPacket, rxCS; quint8 rxCSPacket, rxCS;
qint32 rxCount; qint32 rxCount;
qint32 packetSize;
RxStateType rxState; RxStateType rxState;
ComStats stats; ComStats stats;