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

Implemented NACKs on UAVTalk on the ground implementation.

Browse Source
This commit is contained in:
elafargue 2011-04-30 16:52:07 +02:00
parent 0085256c6e
commit f234651d9c
4 changed files with 89 additions and 10 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
ground/openpilotgcs/src/plugins/uavtalk/telemetry.cpp
View File

@ -50,7 +50,7 @@ Telemetry::Telemetry(UAVTalk* utalk, UAVObjectManager* objMngr)
connect(objMngr, SIGNAL(newObject(UAVObject*)), this, SLOT(newObject(UAVObject*)));
connect(objMngr, SIGNAL(newInstance(UAVObject*)), this, SLOT(newInstance(UAVObject*)));
// Listen to transaction completions
connect(utalk, SIGNAL(transactionCompleted(UAVObject*)), this, SLOT(transactionCompleted(UAVObject*)));
connect(utalk, SIGNAL(transactionCompleted(UAVObject*,bool)), this, SLOT(transactionCompleted(UAVObject*,bool)));
// Get GCS stats object
gcsStatsObj = GCSTelemetryStats::GetInstance(objMngr);
// Setup transaction timer
@ -207,7 +207,7 @@ void Telemetry::updateObject(UAVObject* obj)
/**
* Called when a transaction is successfully completed (uavtalk event)
*/
void Telemetry::transactionCompleted(UAVObject* obj)
void Telemetry::transactionCompleted(UAVObject* obj, bool success)
{
// Check if there is a pending transaction and the objects match
if ( transPending && transInfo.obj->getObjID() == obj->getObjID() )
@ -217,7 +217,7 @@ void Telemetry::transactionCompleted(UAVObject* obj)
transTimer->stop();
transPending = false;
// Send signal
obj->emitTransactionCompleted(true);
obj->emitTransactionCompleted(success);
// Process new object updates from queue
processObjectQueue();
} else

2
ground/openpilotgcs/src/plugins/uavtalk/telemetry.h
View File

@ -68,7 +68,7 @@ private slots:
void newObject(UAVObject* obj);
void newInstance(UAVObject* obj);
void processPeriodicUpdates();
void transactionCompleted(UAVObject* obj);
void transactionCompleted(UAVObject* obj, bool success);
void transactionTimeout();
private:

84
ground/openpilotgcs/src/plugins/uavtalk/uavtalk.cpp
View File

@ -273,7 +273,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
rxObjId = (qint32)qFromLittleEndian<quint32>(rxTmpBuffer);
{
UAVObject *rxObj = objMngr->getObject(rxObjId);
if (rxObj == NULL)
if (rxObj == NULL && rxType != TYPE_NACK)
{
stats.rxErrors++;
rxState = STATE_SYNC;
@ -281,7 +281,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
}
// Determine data length
if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK)
if (rxType == TYPE_OBJ_REQ || rxType == TYPE_ACK || rxType == TYPE_NACK)
rxLength = 0;
else
rxLength = rxObj->getNumBytes();
@ -303,7 +303,14 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
}
// Check if this is a single instance object (i.e. if the instance ID field is coming next)
if (rxObj->isSingleInstance())
if (rxType == TYPE_NACK)
{
// If this is a NACK, just skip to checksum
rxState = STATE_CS;
rxInstId = 0;
rxCount = 0;
}
else if (rxObj->isSingleInstance())
{
// If there is a payload get it, otherwise receive checksum
if (rxLength > 0)
@ -395,7 +402,7 @@ bool UAVTalk::processInputByte(quint8 rxbyte)
/**
* Receive an object. This function process objects received through the telemetry stream.
* \param[in] type Type of received message (TYPE_OBJ, TYPE_OBJ_REQ, TYPE_OBJ_ACK, TYPE_ACK)
* \param[in] type Type of received message (TYPE_OBJ, TYPE_OBJ_REQ, TYPE_OBJ_ACK, TYPE_ACK, TYPE_NACK)
* \param[in] obj Handle of the received object
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
* \param[in] data Data buffer
@ -471,9 +478,29 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8*
}
else
{
// Object was not found, transmit a NACK with the
// objId which was not found.
transmitNack(objId);
error = true;
}
break;
case TYPE_NACK:
// All instances, not allowed for NACK messages
if (!allInstances)
{
// Get object
obj = objMngr->getObject(objId, instId);
// Check if object exists:
if (obj != NULL)
{
updateNack(obj);
}
else
{
error = true;
}
}
break;
case TYPE_ACK:
// All instances, not allowed for ACK messages
if (!allInstances)
@ -539,6 +566,19 @@ UAVObject* UAVTalk::updateObject(quint32 objId, quint16 instId, quint8* data)
}
}
/**
* Check if a transaction is pending and if yes complete it.
*/
void UAVTalk::updateNack(UAVObject* obj)
{
if (respObj != NULL && respObj->getObjID() == obj->getObjID() && (respObj->getInstID() == obj->getInstID() || respAllInstances))
{
respObj = NULL;
emit transactionCompleted(obj, false);
}
}
/**
* Check if a transaction is pending and if yes complete it.
*/
@ -547,10 +587,11 @@ void UAVTalk::updateAck(UAVObject* obj)
if (respObj != NULL && respObj->getObjID() == obj->getObjID() && (respObj->getInstID() == obj->getInstID() || respAllInstances))
{
respObj = NULL;
emit transactionCompleted(obj);
emit transactionCompleted(obj, true);
}
}
/**
* Send an object through the telemetry link.
* \param[in] obj Object to send
@ -607,6 +648,39 @@ bool UAVTalk::transmitObject(UAVObject* obj, quint8 type, bool allInstances)
}
}
/**
* Transmit a NACK through the telemetry link.
* \param[in] objId the ObjectID we rejected
*/
bool UAVTalk::transmitNack(quint32 objId)
{
txBuffer[0] = SYNC_VAL;
txBuffer[1] = TYPE_NACK;
qToLittleEndian<quint16>(0x0, &txBuffer[2]); // Length
qToLittleEndian<quint32>(objId, &txBuffer[4]);
// Calculate checksum
txBuffer[8] = updateCRC(0, txBuffer, 8);
// Send buffer, check that the transmit backlog does not grow above limit
if ( io->bytesToWrite() < TX_BUFFER_SIZE )
{
io->write((const char*)txBuffer, 8+CHECKSUM_LENGTH);
}
else
{
++stats.txErrors;
return false;
}
// Update stats
stats.txBytes += 8+CHECKSUM_LENGTH;
// Done
return true;
}
/**
* Send an object through the telemetry link.

7
ground/openpilotgcs/src/plugins/uavtalk/uavtalk.h
View File

@ -59,7 +59,7 @@ public:
void resetStats();
signals:
void transactionCompleted(UAVObject* obj);
void transactionCompleted(UAVObject* obj, bool success);
private slots:
void processInputStream(void);
@ -72,6 +72,7 @@ private:
static const int TYPE_OBJ_REQ = (TYPE_VER | 0x01);
static const int TYPE_OBJ_ACK = (TYPE_VER | 0x02);
static const int TYPE_ACK = (TYPE_VER | 0x03);
static const int TYPE_NACK = (TYPE_VER | 0x04);
static const int MIN_HEADER_LENGTH = 8; // sync(1), type (1), size(2), object ID(4)
static const int MAX_HEADER_LENGTH = 10; // sync(1), type (1), size(2), object ID (4), instance ID(2, not used in single objects)
@ -83,6 +84,8 @@ private:
static const int MAX_PACKET_LENGTH = (MAX_HEADER_LENGTH + MAX_PAYLOAD_LENGTH + CHECKSUM_LENGTH);
static const quint16 ALL_INSTANCES = 0xFFFF;
static const quint16 OBJID_NOTFOUND = 0x0000;
static const int TX_BUFFER_SIZE = 2*1024;
static const quint8 crc_table[256];
@ -117,6 +120,8 @@ private:
bool receiveObject(quint8 type, quint32 objId, quint16 instId, quint8* data, qint32 length);
UAVObject* updateObject(quint32 objId, quint16 instId, quint8* data);
void updateAck(UAVObject* obj);
void updateNack(UAVObject* obj);
bool transmitNack(quint32 objId);
bool transmitObject(UAVObject* obj, quint8 type, bool allInstances);
bool transmitSingleObject(UAVObject* obj, quint8 type, bool allInstances);
quint8 updateCRC(quint8 crc, const quint8 data);