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

OP-1122 OP-1125 uavtalk - object request messages are now acked only by object messages and acked object messages are acked only by ack messages

Browse Source
This commit is contained in:
Philippe Renon 2013-11-29 02:51:46 +01:00
parent 15c4392ed5
commit 390c88b92a
6 changed files with 71 additions and 54 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

3
flight/uavtalk/inc/uavtalk_priv.h
View File

@ -77,7 +77,8 @@ typedef struct {
xSemaphoreHandle lock;
xSemaphoreHandle transLock;
xSemaphoreHandle respSema;
UAVObjHandle respObj;
uint8_t respType;
uint32_t respObjId;
uint16_t respInstId;
UAVTalkStats stats;
UAVTalkInputProcessor iproc;

17
flight/uavtalk/uavtalk.c
View File

@ -40,7 +40,7 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type
static int32_t sendObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj);
static int32_t sendSingleObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, UAVObjHandle obj);
static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t *data, int32_t length);
static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId);
static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId);
/**
* Initialize the UAVTalk library
@ -289,7 +289,9 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type
xSemaphoreTakeRecursive(connection->transLock, portMAX_DELAY);
// Send object
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
connection->respObj = obj;
// expected response type
connection->respType = (type == UAVTALK_TYPE_OBJ_REQ) ? UAVTALK_TYPE_OBJ : UAVTALK_TYPE_ACK;
connection->respObjId = UAVObjGetID(obj);
connection->respInstId = instId;
sendObject(connection, type, UAVObjGetID(obj), instId, obj);
xSemaphoreGiveRecursive(connection->lock);
@ -300,7 +302,6 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, uint8_t type
// Cancel transaction
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
xSemaphoreTake(connection->respSema, 0); // non blocking call to make sure the value is reset to zero (binary sema)
connection->respObj = 0;
xSemaphoreGiveRecursive(connection->lock);
xSemaphoreGiveRecursive(connection->transLock);
return -1;
@ -725,7 +726,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
// Check if this object acks a pending OBJ_REQ message
// any OBJ message can ack a pending OBJ_REQ message
// even one that was not sent in response to the OBJ_REQ message
updateAck(connection, obj, instId);
updateAck(connection, type, objId, instId);
}
else {
ret = -1;
@ -770,12 +771,13 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
break;
case UAVTALK_TYPE_NACK:
// Do nothing on flight side, let it time out.
// Why?
break;
case UAVTALK_TYPE_ACK:
// All instances, not allowed for ACK messages
if (obj && (instId != UAVOBJ_ALL_INSTANCES)) {
// Check if an ACK is pending
updateAck(connection, obj, instId);
updateAck(connection, type, objId, instId);
} else {
ret = -1;
}
@ -797,11 +799,10 @@ static int32_t receiveObject(UAVTalkConnectionData *connection,
* \param[in] obj Object
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
*/
static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId)
static void updateAck(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId)
{
if (connection->respObj == obj && (connection->respInstId == instId || connection->respInstId == UAVOBJ_ALL_INSTANCES)) {
if (connection->respType == type && connection->respObjId == objId && (connection->respInstId == instId || connection->respInstId == UAVOBJ_ALL_INSTANCES)) {
xSemaphoreGive(connection->respSema);
connection->respObj = 0;
}
}

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

@ -374,7 +374,7 @@ void Telemetry::processObjectQueue()
}
transInfo->telem = this;
// Insert the transaction into the transaction map.
openTransaction(transInfo);
startTransaction(transInfo);
processObjectTransaction(transInfo);
}
@ -542,7 +542,7 @@ ObjectTransactionInfo *Telemetry::findTransaction(UAVObject *obj)
return NULL;
}
void Telemetry::openTransaction(ObjectTransactionInfo *trans)
void Telemetry::startTransaction(ObjectTransactionInfo *trans)
{
QMap<quint32, ObjectTransactionInfo *> *objTransactions = transMap.value(trans->obj->getObjID());
if (objTransactions == NULL) {

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

@ -137,7 +137,7 @@ private:
void processObjectQueue();
ObjectTransactionInfo *findTransaction(UAVObject *obj);
void openTransaction(ObjectTransactionInfo *trans);
void startTransaction(ObjectTransactionInfo *trans);
void closeTransaction(ObjectTransactionInfo *trans);
void closeAllTransactions();

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

@ -155,7 +155,15 @@ bool UAVTalk::sendObjectRequest(UAVObject *obj, bool allInstances)
{
QMutexLocker locker(mutex);
return objectTransaction(TYPE_OBJ_REQ, obj, allInstances);
quint16 instId = 0;
if (allInstances) {
instId = ALL_INSTANCES;
}
else if (obj) {
instId = obj->getInstID();
}
return objectTransaction(TYPE_OBJ_REQ, obj->getObjID(), instId, obj);
}
/**
@ -169,10 +177,18 @@ bool UAVTalk::sendObject(UAVObject *obj, bool acked, bool allInstances)
{
QMutexLocker locker(mutex);
quint16 instId = 0;
if (allInstances) {
instId = ALL_INSTANCES;
}
else if (obj) {
instId = obj->getInstID();
}
if (acked) {
return objectTransaction(TYPE_OBJ_ACK, obj, allInstances);
return objectTransaction(TYPE_OBJ_ACK, obj->getObjID(), instId, obj);
} else {
return objectTransaction(TYPE_OBJ, obj, allInstances);
return objectTransaction(TYPE_OBJ, obj->getObjID(), instId, obj);
}
}
@ -186,7 +202,7 @@ void UAVTalk::cancelTransaction(UAVObject *obj)
if (io.isNull()) {
return;
}
Transaction *trans = findTransaction(obj);
Transaction *trans = findTransaction(obj->getObjID(), obj->getInstID());
if (trans != NULL) {
closeTransaction(trans);
delete trans;
@ -203,24 +219,24 @@ void UAVTalk::cancelTransaction(UAVObject *obj)
* \param[in] allInstances If set true then all instances will be updated
* \return Success (true), Failure (false)
*/
bool UAVTalk::objectTransaction(quint8 type, UAVObject *obj, bool allInstances)
bool UAVTalk::objectTransaction(quint8 type, quint32 objId, quint16 instId, UAVObject *obj)
{
Q_ASSERT(obj);
// Send object depending on if a response is needed
// transactions of TYPE_OBJ_REQ are acked by the response
quint16 instId = allInstances ? ALL_INSTANCES : obj->getInstID();
if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) {
if (transmitObject(type, obj->getObjID(), instId, obj)) {
if (transmitObject(type, objId, instId, obj)) {
Transaction *trans = new Transaction();
trans->obj = obj;
trans->allInstances = allInstances;
openTransaction(trans);
trans->respType = (type == TYPE_OBJ_REQ) ? TYPE_OBJ : TYPE_ACK;
trans->respObjId = objId;
trans->respInstId = instId;
startTransaction(trans);
return true;
} else {
return false;
}
} else if (type == TYPE_OBJ) {
return transmitObject(type, obj->getObjID(), instId, obj);
return transmitObject(type, objId, instId, obj);
} else {
return false;
}
@ -494,10 +510,10 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
qDebug() << "UAVTalk - received object" << objId << instId << (obj != NULL ? obj->toStringBrief() : "<null object>");
#endif
if (obj != NULL) {
// Check if an ACK is pending
// TODO is it necessary to do that check?
// TODO if yes, why is the same check not done for OBJ_ACK below?
updateAck(obj);
// Check if this object acks a pending OBJ_REQ message
// any OBJ message can ack a pending OBJ_REQ message
// even one that was not sent in response to the OBJ_REQ message
updateAck(type, objId, instId, obj);
} else {
error = true;
}
@ -559,7 +575,7 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
#endif
if (obj != NULL) {
// Check if a NACK is pending
updateNack(obj);
updateNack(type, objId, instId, obj);
} else {
error = true;
}
@ -575,7 +591,7 @@ bool UAVTalk::receiveObject(quint8 type, quint32 objId, quint16 instId, quint8 *
#endif
if (obj != NULL) {
// Check if an ACK is pending
updateAck(obj);
updateAck(type, objId, instId, obj);
} else {
error = true;
}
@ -630,15 +646,14 @@ 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)
void UAVTalk::updateNack(quint8 type, quint32 objId, quint16 instId, UAVObject *obj)
{
Q_ASSERT(obj);
if (!obj) {
return;
}
Transaction *trans = findTransaction(obj);
// TODO handle allInstances
if (trans != NULL /* || itr.value()->allInstances)*/) {
Transaction *trans = findTransaction(objId, instId);
if (trans) {
closeTransaction(trans);
delete trans;
emit transactionCompleted(obj, false);
@ -648,15 +663,14 @@ void UAVTalk::updateNack(UAVObject *obj)
/**
* Check if a transaction is pending and if yes complete it.
*/
void UAVTalk::updateAck(UAVObject *obj)
void UAVTalk::updateAck(quint8 type, quint32 objId, quint16 instId, UAVObject *obj)
{
Q_ASSERT(obj);
if (!obj) {
return;
}
Transaction *trans = findTransaction(obj);
// TODO handle allInstances
if (trans != NULL /* || itr.value()->allInstances)*/) {
Transaction *trans = findTransaction(objId, instId);
if (trans && trans->respType == type) {
closeTransaction(trans);
delete trans;
emit transactionCompleted(obj, true);
@ -814,31 +828,31 @@ quint8 UAVTalk::updateCRC(quint8 crc, const quint8 *data, qint32 length)
return crc;
}
UAVTalk::Transaction *UAVTalk::findTransaction(UAVObject *obj)
UAVTalk::Transaction *UAVTalk::findTransaction(quint32 objId, quint16 instId)
{
// Lookup the transaction in the transaction map
QMap<quint32, Transaction *> *objTransactions = transMap.value(obj->getObjID());
QMap<quint32, Transaction *> *objTransactions = transMap.value(objId);
if (objTransactions != NULL) {
return objTransactions->value(obj->getInstID());
return objTransactions->value(instId);
}
return NULL;
}
void UAVTalk::openTransaction(Transaction *trans)
void UAVTalk::startTransaction(Transaction *trans)
{
QMap<quint32, Transaction *> *objTransactions = transMap.value(trans->obj->getObjID());
QMap<quint32, Transaction *> *objTransactions = transMap.value(trans->respObjId);
if (objTransactions == NULL) {
objTransactions = new QMap<quint32, Transaction *>();
transMap.insert(trans->obj->getObjID(), objTransactions);
transMap.insert(trans->respObjId, objTransactions);
}
objTransactions->insert(trans->obj->getInstID(), trans);
objTransactions->insert(trans->respInstId, trans);
}
void UAVTalk::closeTransaction(Transaction *trans)
{
QMap<quint32, Transaction *> *objTransactions = transMap.value(trans->obj->getObjID());
QMap<quint32, Transaction *> *objTransactions = transMap.value(trans->respObjId);
if (objTransactions != NULL) {
objTransactions->remove(trans->obj->getInstID());
objTransactions->remove(trans->respInstId);
// Keep the map even if it is empty
// There are at most 100 different object IDs...
}
@ -850,7 +864,7 @@ void UAVTalk::closeAllTransactions()
QMap<quint32, Transaction *> *objTransactions = transMap.value(objId);
foreach(quint32 instId, objTransactions->keys()) {
Transaction *trans = objTransactions->value(instId);
qWarning() << "UAVTalk - closing active transaction for object" << trans->obj->toStringBrief();
qWarning() << "UAVTalk - closing active transaction for object" << trans->respObjId;
objTransactions->remove(instId);
delete trans;
}

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

@ -70,8 +70,9 @@ private slots:
private:
typedef struct {
UAVObject *obj;
bool allInstances;
quint8 respType;
quint32 respObjId;
quint16 respInstId;
} Transaction;
// Constants
@ -129,19 +130,19 @@ private:
QByteArray rxDataArray;
// Methods
bool objectTransaction(quint8 type, UAVObject *obj, bool allInstances);
bool objectTransaction(quint8 type, quint32 objId, quint16 instId, UAVObject *obj);
bool processInputByte(quint8 rxbyte);
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);
void updateAck(quint8 type, quint32 objId, quint16 instId, UAVObject *obj);
void updateNack(quint8 type, quint32 objId, quint16 instId, UAVObject *obj);
bool transmitObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj);
bool transmitSingleObject(quint8 type, quint32 objId, quint16 instId, UAVObject *obj);
quint8 updateCRC(quint8 crc, const quint8 data);
quint8 updateCRC(quint8 crc, const quint8 *data, qint32 length);
Transaction *findTransaction(UAVObject *obj);
void openTransaction(Transaction *trans);
Transaction *findTransaction(quint32 objId, quint16 instId);
void startTransaction(Transaction *trans);
void closeTransaction(Transaction *trans);
void closeAllTransactions();
};