rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-02 10:24:11 +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"
// Private constants
#define SYNC_VAL 0x3C
#define TYPE_MASK 0xFC
#define TYPE_VER 0x10
#define TYPE_OBJ (TYPE_VER | 0x00)
@ -65,7 +66,7 @@ static const uint8_t crc_table[256] = {
};
// 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
static UAVTalkOutputStream outStream;
@ -239,6 +240,7 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte)
static uint8_t tmpBuffer[4];
static UAVObjHandle obj;
static uint8_t type;
static uint16_t packet_size;
static uint32_t objId;
static uint16_t instId;
static uint32_t length;
@ -250,12 +252,33 @@ int32_t UAVTalkProcessInputStream(uint8_t rxbyte)
++stats.rxBytes;
switch (state) {
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 )
{
cs = updateCRC(0, &rxbyte, 1);
cs = updateCRC(cs, &rxbyte, 1);
type = rxbyte;
state = STATE_OBJID;
packet_size = 0;
state = STATE_SIZE;
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;
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
objId = UAVObjGetID(obj);
txBuffer[0] = type;
txBuffer[1] = (uint8_t)(objId & 0xFF);
txBuffer[2] = (uint8_t)((objId >> 8) & 0xFF);
txBuffer[3] = (uint8_t)((objId >> 16) & 0xFF);
txBuffer[4] = (uint8_t)((objId >> 24) & 0xFF);
txBuffer[0] = SYNC_VAL; // sync byte
txBuffer[1] = type;
// data length inserted here below
txBuffer[4] = (uint8_t)(objId & 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
if (UAVObjIsSingleInstance(obj))
{
dataOffset = 5;
dataOffset = 8;
}
else
{
txBuffer[5] = (uint8_t)(instId & 0xFF);
txBuffer[6] = (uint8_t)((instId >> 8) & 0xFF);
dataOffset = 7;
txBuffer[8] = (uint8_t)(instId & 0xFF);
txBuffer[9] = (uint8_t)((instId >> 8) & 0xFF);
dataOffset = 10;
}
// 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
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 <QtEndian>
#include <QDebug>
#define SYNC_VAL 0x3C
const quint8 UAVTalk::crc_table[256] = {
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
switch (rxState) {
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 )
{
rxCS = updateCRC(0, &rxbyte, 1);
rxCS = updateCRC(rxCS, &rxbyte, 1);
rxType = rxbyte;
rxState = STATE_OBJID;
packetSize = 0;
rxState = STATE_SIZE;
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;
case STATE_OBJID:
@ -536,27 +562,28 @@ bool UAVTalk::transmitSingleObject(UAVObject* obj, quint8 type, bool allInstance
// Setup type and object id fields
objId = obj->getObjID();
txBuffer[0] = type;
qToLittleEndian<quint32>(objId, &txBuffer[1]);
txBuffer[0] = SYNC_VAL;
txBuffer[1] = type;
qToLittleEndian<quint32>(objId, &txBuffer[4]);
// Setup instance ID if one is required
if ( obj->isSingleInstance() )
{
dataOffset = 5;
dataOffset = 8;
}
else
{
// Check if all instances are requested
if (allInstances)
{
qToLittleEndian<quint16>(allInstId, &txBuffer[5]);
qToLittleEndian<quint16>(allInstId, &txBuffer[8]);
}
else
{
instId = obj->getInstID();
qToLittleEndian<quint16>(instId, &txBuffer[5]);
qToLittleEndian<quint16>(instId, &txBuffer[8]);
}
dataOffset = 7;
dataOffset = 10;
}
// Determine data length
@ -584,6 +611,8 @@ bool UAVTalk::transmitSingleObject(UAVObject* obj, quint8 type, bool allInstance
}
}
qToLittleEndian<quint16>(dataOffset + length, &txBuffer[2]);
// Calculate checksum
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];
// 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
QIODevice* io;
@ -97,8 +97,10 @@ private:
quint32 rxObjId;
quint16 rxInstId;
quint16 rxLength;
quint8 rxCSPacket, rxCS;
qint32 rxCount;
qint32 packetSize;
RxStateType rxState;
ComStats stats;