mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-02 10:24:11 +01:00
UAVTalk: Insert a sync byte (0x3C) and the packet size to facilitating handling
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
peabody124
parent
33e6cbebdc
commit
4570d6df23
@ -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;
|
||||
@ -232,13 +233,14 @@ static int32_t objectTransaction(UAVObjHandle obj, uint16_t instId, uint8_t type
|
||||
* Process an byte from the telemetry stream.
|
||||
* \param[in] rxbyte Received byte
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
* \return -1 Failure
|
||||
*/
|
||||
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,14 +252,35 @@ 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:
|
||||
tmpBuffer[rxCount++] = rxbyte;
|
||||
if (rxCount == 4)
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
|
||||
Loading…
Reference in New Issue
Block a user