mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-01-18 03:52:11 +01:00
VTalk (flight): Buffers allocated dynamically with size calculated by UAVObjGenerator. Support for tx packet sizes < object size.i
This commit is contained in:
Corvus Corax
parent
926246b402
commit
f52f6d30c6
@ -103,7 +103,7 @@ int32_t TelemetryInitialize(void)
|
||||
updateSettings();
|
||||
|
||||
// Initialise UAVTalk
|
||||
UAVTalkInitialize(&uavTalkCon, &transmitData);
|
||||
uavTalkCon = UAVTalkInitialize(&transmitData,256);
|
||||
|
||||
// Process all registered objects and connect queue for updates
|
||||
UAVObjIterate(®isterObject);
|
||||
@ -222,7 +222,7 @@ static void processObjEvent(UAVObjEvent * ev)
|
||||
if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL) {
|
||||
// Send update to GCS (with retries)
|
||||
while (retries < MAX_RETRIES && success == -1) {
|
||||
success = UAVTalkSendObject(&uavTalkCon, ev->obj, ev->instId, metadata.telemetryAcked, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
|
||||
success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, metadata.telemetryAcked, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
|
||||
++retries;
|
||||
}
|
||||
// Update stats
|
||||
@ -233,7 +233,7 @@ static void processObjEvent(UAVObjEvent * ev)
|
||||
} else if (ev->event == EV_UPDATE_REQ) {
|
||||
// Request object update from GCS (with retries)
|
||||
while (retries < MAX_RETRIES && success == -1) {
|
||||
success = UAVTalkSendObjectRequest(&uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout
|
||||
success = UAVTalkSendObjectRequest(uavTalkCon, ev->obj, ev->instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout
|
||||
++retries;
|
||||
}
|
||||
// Update stats
|
||||
@ -310,7 +310,7 @@ static void telemetryRxTask(void *parameters)
|
||||
// TODO: Currently we periodically check the buffer for data, update once the PIOS_COM is made blocking
|
||||
len = PIOS_COM_ReceiveBufferUsed(inputPort);
|
||||
for (int32_t n = 0; n < len; ++n) {
|
||||
UAVTalkProcessInputStream(&uavTalkCon, PIOS_COM_ReceiveBuffer(inputPort));
|
||||
UAVTalkProcessInputStream(uavTalkCon, PIOS_COM_ReceiveBuffer(inputPort));
|
||||
}
|
||||
vTaskDelay(5); // <- remove when blocking calls are implemented
|
||||
|
||||
@ -404,8 +404,8 @@ static void updateTelemetryStats()
|
||||
uint32_t timeNow;
|
||||
|
||||
// Get stats
|
||||
UAVTalkGetStats(&uavTalkCon, &utalkStats);
|
||||
UAVTalkResetStats(&uavTalkCon);
|
||||
UAVTalkGetStats(uavTalkCon, &utalkStats);
|
||||
UAVTalkResetStats(uavTalkCon);
|
||||
|
||||
// Get object data
|
||||
FlightTelemetryStatsGet(&flightStats);
|
||||
|
||||
@ -29,4 +29,6 @@
|
||||
|
||||
void UAVObjectsInitializeAll();
|
||||
|
||||
#define UAVOBJECTS_LARGEST $(SIZECALCULATION)
|
||||
|
||||
#endif // UAVOBJECTSINIT_H
|
||||
@ -29,24 +29,9 @@
|
||||
#ifndef UAVTALK_H
|
||||
#define UAVTALK_H
|
||||
|
||||
// Public constants
|
||||
#define UAVTALK_WAITFOREVER -1
|
||||
#define UAVTALK_NOWAIT 0
|
||||
#define UAVTALK_MIN_HEADER_LENGTH 8 // sync(1), type (1), size (2), object ID (4)
|
||||
#define UAVTALK_MAX_HEADER_LENGTH 10 // sync(1), type (1), size (2), object ID (4), instance ID (2, not used in single objects)
|
||||
|
||||
#define UAVTALK_CHECKSUM_LENGTH 1
|
||||
|
||||
#define UAVTALK_MAX_PAYLOAD_LENGTH 256
|
||||
|
||||
#define UAVTALK_MAX_PACKET_LENGTH (UAVTALK_MAX_HEADER_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH + UAVTALK_CHECKSUM_LENGTH)
|
||||
|
||||
|
||||
// Public types
|
||||
typedef int32_t (*UAVTalkOutputStream)(uint8_t* data, int32_t length);
|
||||
|
||||
typedef enum {UAVTALK_STATE_SYNC, UAVTALK_STATE_TYPE, UAVTALK_STATE_SIZE, UAVTALK_STATE_OBJID, UAVTALK_STATE_INSTID, UAVTALK_STATE_DATA, UAVTALK_STATE_CS} UAVTalkRxState;
|
||||
|
||||
typedef struct {
|
||||
uint32_t txBytes;
|
||||
uint32_t rxBytes;
|
||||
@ -58,41 +43,17 @@ typedef struct {
|
||||
uint32_t rxErrors;
|
||||
} UAVTalkStats;
|
||||
|
||||
typedef struct {
|
||||
UAVObjHandle obj;
|
||||
uint8_t type;
|
||||
uint16_t packet_size;
|
||||
uint32_t objId;
|
||||
uint16_t instId;
|
||||
uint32_t length;
|
||||
uint8_t cs;
|
||||
int32_t rxCount;
|
||||
UAVTalkRxState state;
|
||||
uint16_t rxPacketLength;
|
||||
} UAVTalkInputProcessor;
|
||||
|
||||
typedef struct {
|
||||
UAVTalkOutputStream outStream;
|
||||
xSemaphoreHandle lock;
|
||||
xSemaphoreHandle transLock;
|
||||
xSemaphoreHandle respSema;
|
||||
UAVObjHandle respObj;
|
||||
uint16_t respInstId;
|
||||
uint8_t rxBuffer[UAVTALK_MAX_PACKET_LENGTH];
|
||||
uint8_t txBuffer[UAVTALK_MAX_PACKET_LENGTH];
|
||||
UAVTalkStats stats;
|
||||
UAVTalkInputProcessor iproc;
|
||||
} UAVTalkConnection;
|
||||
typedef void* UAVTalkConnection;
|
||||
|
||||
// Public functions
|
||||
int32_t UAVTalkInitialize(UAVTalkConnection *connection, UAVTalkOutputStream outputStream);
|
||||
int32_t UAVTalkSetOutputStream(UAVTalkConnection *connection, UAVTalkOutputStream outputStream);
|
||||
UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection *connection);
|
||||
int32_t UAVTalkSendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs);
|
||||
int32_t UAVTalkSendObjectRequest(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs);
|
||||
int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte);
|
||||
void UAVTalkGetStats(UAVTalkConnection *connection, UAVTalkStats* stats);
|
||||
void UAVTalkResetStats(UAVTalkConnection *connection);
|
||||
UAVTalkConnection UAVTalkInitialize(UAVTalkOutputStream outputStream, uint32_t maxPacketSize);
|
||||
int32_t UAVTalkSetOutputStream(UAVTalkConnection connection, UAVTalkOutputStream outputStream);
|
||||
UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connection);
|
||||
int32_t UAVTalkSendObject(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs);
|
||||
int32_t UAVTalkSendObjectRequest(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs);
|
||||
int32_t UAVTalkProcessInputStream(UAVTalkConnection connection, uint8_t rxbyte);
|
||||
void UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats);
|
||||
void UAVTalkResetStats(UAVTalkConnection connection);
|
||||
|
||||
#endif // UAVTALK_H
|
||||
/**
|
||||
|
||||
111
flight/UAVTalk/inc/uavtalk_priv.h
Normal file
111
flight/UAVTalk/inc/uavtalk_priv.h
Normal file
@ -0,0 +1,111 @@
|
||||
/**
|
||||
******************************************************************************
|
||||
* @addtogroup OpenPilotSystem OpenPilot System
|
||||
* @{
|
||||
* @addtogroup OpenPilotLibraries OpenPilot System Libraries
|
||||
* @{
|
||||
* @file uavtalk.h
|
||||
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
|
||||
* @brief Private include file of the UAVTalk library
|
||||
* @see The GNU Public License (GPL) Version 3
|
||||
*
|
||||
*****************************************************************************/
|
||||
/*
|
||||
* This program is free software; you can redistribute it and/or modify
|
||||
* it under the terms of the GNU General Public License as published by
|
||||
* the Free Software Foundation; either version 3 of the License, or
|
||||
* (at your option) any later version.
|
||||
*
|
||||
* This program is distributed in the hope that it will be useful, but
|
||||
* WITHOUT ANY WARRANTY; without even the implied warranty of MERCHANTABILITY
|
||||
* or FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License
|
||||
* for more details.
|
||||
*
|
||||
* You should have received a copy of the GNU General Public License along
|
||||
* with this program; if not, write to the Free Software Foundation, Inc.,
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#ifndef UAVTALK_PRIV_H
|
||||
#define UAVTALK_PRIV_H
|
||||
|
||||
#include "uavobjectsinit.h"
|
||||
|
||||
// Private types and constants
|
||||
typedef struct {
|
||||
uint8_t sync;
|
||||
uint8_t type;
|
||||
uint16_t size;
|
||||
uint32_t objId;
|
||||
} uavtalk_min_header;
|
||||
#define UAVTALK_MIN_HEADER_LENGTH sizeof(uavtalk_min_header)
|
||||
|
||||
typedef struct {
|
||||
uint8_t sync;
|
||||
uint8_t type;
|
||||
uint16_t size;
|
||||
uint32_t objId;
|
||||
uint16_t instId;
|
||||
} uavtalk_max_header;
|
||||
#define UAVTALK_MAX_HEADER_LENGTH sizeof(uavtalk_max_header)
|
||||
|
||||
typedef uint8_t uavtalk_checksum;
|
||||
#define UAVTALK_CHECKSUM_LENGTH sizeof(uavtalk_checksum)
|
||||
#define UAVTALK_MAX_PAYLOAD_LENGTH UAVOBJECTS_LARGEST
|
||||
#define UAVTALK_MIN_PACKET_LENGTH UAVTALK_MAX_HEADER_LENGTH + UAVTALK_CHECKSUM_LENGTH
|
||||
#define UAVTALK_MAX_PACKET_LENGTH UAVTALK_MIN_PACKET_LENGTH + UAVTALK_MAX_PAYLOAD_LENGTH
|
||||
|
||||
typedef enum {UAVTALK_STATE_SYNC, UAVTALK_STATE_TYPE, UAVTALK_STATE_SIZE, UAVTALK_STATE_OBJID, UAVTALK_STATE_INSTID, UAVTALK_STATE_DATA, UAVTALK_STATE_CS} UAVTalkRxState;
|
||||
|
||||
typedef struct {
|
||||
UAVObjHandle obj;
|
||||
uint8_t type;
|
||||
uint16_t packet_size;
|
||||
uint32_t objId;
|
||||
uint16_t instId;
|
||||
uint32_t length;
|
||||
uint8_t cs;
|
||||
int32_t rxCount;
|
||||
UAVTalkRxState state;
|
||||
uint16_t rxPacketLength;
|
||||
} UAVTalkInputProcessor;
|
||||
|
||||
typedef struct {
|
||||
uint8_t canari;
|
||||
UAVTalkOutputStream outStream;
|
||||
xSemaphoreHandle lock;
|
||||
xSemaphoreHandle transLock;
|
||||
xSemaphoreHandle respSema;
|
||||
UAVObjHandle respObj;
|
||||
uint16_t respInstId;
|
||||
UAVTalkStats stats;
|
||||
UAVTalkInputProcessor iproc;
|
||||
uint8_t *rxBuffer;
|
||||
uint32_t txSize;
|
||||
uint8_t *txBuffer;
|
||||
} UAVTalkConnectionData;
|
||||
|
||||
#define UAVTALK_CANARI 0xCA
|
||||
#define UAVTALK_WAITFOREVER -1
|
||||
#define UAVTALK_NOWAIT 0
|
||||
#define UAVTALK_SYNC_VAL 0x3C
|
||||
#define UAVTALK_TYPE_MASK 0xF8
|
||||
#define UAVTALK_TYPE_VER 0x20
|
||||
#define UAVTALK_TYPE_OBJ (UAVTALK_TYPE_VER | 0x00)
|
||||
#define UAVTALK_TYPE_OBJ_REQ (UAVTALK_TYPE_VER | 0x01)
|
||||
#define UAVTALK_TYPE_OBJ_ACK (UAVTALK_TYPE_VER | 0x02)
|
||||
#define UAVTALK_TYPE_ACK (UAVTALK_TYPE_VER | 0x03)
|
||||
#define UAVTALK_TYPE_NACK (UAVTALK_TYPE_VER | 0x04)
|
||||
|
||||
//macros
|
||||
#define CHECKCONHANDLE(handle,variable,failcommand) \
|
||||
variable = (UAVTalkConnectionData*) handle; \
|
||||
if (variable == NULL || variable->canari != UAVTALK_CANARI) { \
|
||||
failcommand; \
|
||||
}
|
||||
|
||||
#endif // UAVTALK__PRIV_H
|
||||
/**
|
||||
* @}
|
||||
* @}
|
||||
*/
|
||||
@ -30,26 +30,16 @@
|
||||
*/
|
||||
|
||||
#include "openpilot.h"
|
||||
#include "uavtalk_priv.h"
|
||||
|
||||
// Private constants
|
||||
#define SYNC_VAL 0x3C
|
||||
#define TYPE_MASK 0xF8
|
||||
#define TYPE_VER 0x20
|
||||
#define TYPE_OBJ (TYPE_VER | 0x00)
|
||||
#define TYPE_OBJ_REQ (TYPE_VER | 0x01)
|
||||
#define TYPE_OBJ_ACK (TYPE_VER | 0x02)
|
||||
#define TYPE_ACK (TYPE_VER | 0x03)
|
||||
#define TYPE_NACK (TYPE_VER | 0x04)
|
||||
|
||||
// Private types
|
||||
|
||||
// Private functions
|
||||
static int32_t objectTransaction(UAVTalkConnection *connection, UAVObjHandle objectId, uint16_t instId, uint8_t type, int32_t timeout);
|
||||
static int32_t sendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t type);
|
||||
static int32_t sendSingleObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t type);
|
||||
static int32_t sendNack(UAVTalkConnection *connection, uint32_t objId);
|
||||
static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length);
|
||||
static void updateAck(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId);
|
||||
static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle objectId, uint16_t instId, uint8_t type, int32_t timeout);
|
||||
static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type);
|
||||
static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type);
|
||||
static int32_t sendNack(UAVTalkConnectionData *connection, uint32_t objId);
|
||||
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);
|
||||
|
||||
/**
|
||||
* Initialize the UAVTalk library
|
||||
@ -58,17 +48,28 @@ static void updateAck(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t UAVTalkInitialize(UAVTalkConnection *connection, UAVTalkOutputStream outputStream)
|
||||
UAVTalkConnection UAVTalkInitialize(UAVTalkOutputStream outputStream, uint32_t maxPacketSize)
|
||||
{
|
||||
if (maxPacketSize<1) return 0;
|
||||
// allocate object
|
||||
UAVTalkConnectionData * connection = pvPortMalloc(sizeof(UAVTalkConnectionData));
|
||||
if (!connection) return 0;
|
||||
connection->canari = UAVTALK_CANARI;
|
||||
connection->iproc.rxPacketLength = 0;
|
||||
connection->iproc.state = UAVTALK_STATE_SYNC;
|
||||
connection->outStream = outputStream;
|
||||
connection->lock = xSemaphoreCreateRecursiveMutex();
|
||||
connection->transLock = xSemaphoreCreateRecursiveMutex();
|
||||
connection->txSize = maxPacketSize;
|
||||
// allocate buffers
|
||||
connection->rxBuffer = pvPortMalloc(UAVTALK_MAX_PACKET_LENGTH);
|
||||
if (!connection->rxBuffer) return 0;
|
||||
connection->txBuffer = pvPortMalloc(UAVTALK_MAX_PACKET_LENGTH);
|
||||
if (!connection->txBuffer) return 0;
|
||||
vSemaphoreCreateBinary(connection->respSema);
|
||||
xSemaphoreTake(connection->respSema, 0); // reset to zero
|
||||
UAVTalkResetStats(connection);
|
||||
return 0;
|
||||
UAVTalkResetStats( (UAVTalkConnection) connection );
|
||||
return (UAVTalkConnection) connection;
|
||||
}
|
||||
|
||||
/**
|
||||
@ -78,8 +79,12 @@ int32_t UAVTalkInitialize(UAVTalkConnection *connection, UAVTalkOutputStream out
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t UAVTalkSetOutputStream(UAVTalkConnection *connection, UAVTalkOutputStream outputStream)
|
||||
int32_t UAVTalkSetOutputStream(UAVTalkConnection connectionHandle, UAVTalkOutputStream outputStream)
|
||||
{
|
||||
|
||||
UAVTalkConnectionData *connection;
|
||||
CHECKCONHANDLE(connectionHandle,connection,return -1);
|
||||
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
|
||||
|
||||
@ -98,8 +103,10 @@ int32_t UAVTalkSetOutputStream(UAVTalkConnection *connection, UAVTalkOutputStrea
|
||||
* \param[in] connection UAVTalkConnection to be used
|
||||
* @return UAVTarlkOutputStream the output stream used
|
||||
*/
|
||||
UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection *connection)
|
||||
UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connectionHandle)
|
||||
{
|
||||
UAVTalkConnectionData *connection;
|
||||
CHECKCONHANDLE(connectionHandle,connection,return NULL);
|
||||
return connection->outStream;
|
||||
}
|
||||
|
||||
@ -108,8 +115,11 @@ UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection *connection)
|
||||
* \param[in] connection UAVTalkConnection to be used
|
||||
* @param[out] statsOut Statistics counters
|
||||
*/
|
||||
void UAVTalkGetStats(UAVTalkConnection *connection, UAVTalkStats* statsOut)
|
||||
void UAVTalkGetStats(UAVTalkConnection connectionHandle, UAVTalkStats* statsOut)
|
||||
{
|
||||
UAVTalkConnectionData *connection;
|
||||
CHECKCONHANDLE(connectionHandle,connection,return );
|
||||
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
|
||||
|
||||
@ -124,8 +134,11 @@ void UAVTalkGetStats(UAVTalkConnection *connection, UAVTalkStats* statsOut)
|
||||
* Reset the statistics counters.
|
||||
* \param[in] connection UAVTalkConnection to be used
|
||||
*/
|
||||
void UAVTalkResetStats(UAVTalkConnection *connection)
|
||||
void UAVTalkResetStats(UAVTalkConnection connectionHandle)
|
||||
{
|
||||
UAVTalkConnectionData *connection;
|
||||
CHECKCONHANDLE(connectionHandle,connection,return);
|
||||
|
||||
// Lock
|
||||
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
|
||||
|
||||
@ -146,9 +159,11 @@ void UAVTalkResetStats(UAVTalkConnection *connection)
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t UAVTalkSendObjectRequest(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, int32_t timeout)
|
||||
int32_t UAVTalkSendObjectRequest(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, int32_t timeout)
|
||||
{
|
||||
return objectTransaction(connection, obj, instId, TYPE_OBJ_REQ, timeout);
|
||||
UAVTalkConnectionData *connection;
|
||||
CHECKCONHANDLE(connectionHandle,connection,return -1);
|
||||
return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ, timeout);
|
||||
}
|
||||
|
||||
/**
|
||||
@ -161,16 +176,18 @@ int32_t UAVTalkSendObjectRequest(UAVTalkConnection *connection, UAVObjHandle obj
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t UAVTalkSendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs)
|
||||
int32_t UAVTalkSendObject(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs)
|
||||
{
|
||||
UAVTalkConnectionData *connection;
|
||||
CHECKCONHANDLE(connectionHandle,connection,return -1);
|
||||
// Send object
|
||||
if (acked == 1)
|
||||
{
|
||||
return objectTransaction(connection, obj, instId, TYPE_OBJ_ACK, timeoutMs);
|
||||
return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK, timeoutMs);
|
||||
}
|
||||
else
|
||||
{
|
||||
return objectTransaction(connection, obj, instId, TYPE_OBJ, timeoutMs);
|
||||
return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ, timeoutMs);
|
||||
}
|
||||
}
|
||||
|
||||
@ -180,18 +197,18 @@ int32_t UAVTalkSendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint1
|
||||
* \param[in] obj Object
|
||||
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
|
||||
* \param[in] type Transaction type
|
||||
* TYPE_OBJ: send object,
|
||||
* TYPE_OBJ_REQ: request object update
|
||||
* TYPE_OBJ_ACK: send object with an ack
|
||||
* UAVTALK_TYPE_OBJ: send object,
|
||||
* UAVTALK_TYPE_OBJ_REQ: request object update
|
||||
* UAVTALK_TYPE_OBJ_ACK: send object with an ack
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
static int32_t objectTransaction(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t type, int32_t timeoutMs)
|
||||
static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type, int32_t timeoutMs)
|
||||
{
|
||||
int32_t respReceived;
|
||||
|
||||
// Send object depending on if a response is needed
|
||||
if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ)
|
||||
if (type == UAVTALK_TYPE_OBJ_ACK || type == UAVTALK_TYPE_OBJ_REQ)
|
||||
{
|
||||
// Get transaction lock (will block if a transaction is pending)
|
||||
xSemaphoreTakeRecursive(connection->transLock, portMAX_DELAY);
|
||||
@ -220,10 +237,10 @@ static int32_t objectTransaction(UAVTalkConnection *connection, UAVObjHandle obj
|
||||
return 0;
|
||||
}
|
||||
}
|
||||
else if (type == TYPE_OBJ)
|
||||
else if (type == UAVTALK_TYPE_OBJ)
|
||||
{
|
||||
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
|
||||
sendObject(connection, obj, instId, TYPE_OBJ);
|
||||
sendObject(connection, obj, instId, UAVTALK_TYPE_OBJ);
|
||||
xSemaphoreGiveRecursive(connection->lock);
|
||||
return 0;
|
||||
}
|
||||
@ -240,8 +257,11 @@ static int32_t objectTransaction(UAVTalkConnection *connection, UAVObjHandle obj
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte)
|
||||
int32_t UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte)
|
||||
{
|
||||
UAVTalkConnectionData *connection;
|
||||
CHECKCONHANDLE(connectionHandle,connection,return -1);
|
||||
|
||||
UAVTalkInputProcessor *iproc = &connection->iproc;
|
||||
++connection->stats.rxBytes;
|
||||
|
||||
@ -252,7 +272,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte)
|
||||
switch (iproc->state)
|
||||
{
|
||||
case UAVTALK_STATE_SYNC:
|
||||
if (rxbyte != SYNC_VAL)
|
||||
if (rxbyte != UAVTALK_SYNC_VAL)
|
||||
break;
|
||||
|
||||
// Initialize and update the CRC
|
||||
@ -268,7 +288,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte)
|
||||
// update the CRC
|
||||
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte);
|
||||
|
||||
if ((rxbyte & TYPE_MASK) != TYPE_VER)
|
||||
if ((rxbyte & UAVTALK_TYPE_MASK) != UAVTALK_TYPE_VER)
|
||||
{
|
||||
iproc->state = UAVTALK_STATE_SYNC;
|
||||
break;
|
||||
@ -322,7 +342,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte)
|
||||
// exist, in which case we'll send a NACK
|
||||
|
||||
iproc->obj = UAVObjGetByID(iproc->objId);
|
||||
if (iproc->obj == 0 && iproc->type != TYPE_OBJ_REQ)
|
||||
if (iproc->obj == 0 && iproc->type != UAVTALK_TYPE_OBJ_REQ)
|
||||
{
|
||||
connection->stats.rxErrors++;
|
||||
iproc->state = UAVTALK_STATE_SYNC;
|
||||
@ -330,7 +350,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte)
|
||||
}
|
||||
|
||||
// Determine data length
|
||||
if (iproc->type == TYPE_OBJ_REQ || iproc->type == TYPE_ACK || iproc->type == TYPE_NACK)
|
||||
if (iproc->type == UAVTALK_TYPE_OBJ_REQ || iproc->type == UAVTALK_TYPE_ACK || iproc->type == UAVTALK_TYPE_NACK)
|
||||
iproc->length = 0;
|
||||
else
|
||||
iproc->length = UAVObjGetNumBytes(iproc->obj);
|
||||
@ -449,7 +469,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte)
|
||||
/**
|
||||
* Receive an object. This function process objects received through the telemetry stream.
|
||||
* \param[in] connection UAVTalkConnection to be used
|
||||
* \param[in] type Type of received message (TYPE_OBJ, TYPE_OBJ_REQ, TYPE_OBJ_ACK, TYPE_ACK, TYPE_NACK)
|
||||
* \param[in] type Type of received message (UAVTALK_TYPE_OBJ, UAVTALK_TYPE_OBJ_REQ, UAVTALK_TYPE_OBJ_ACK, UAVTALK_TYPE_ACK, UAVTALK_TYPE_NACK)
|
||||
* \param[in] objId ID of the object to work on
|
||||
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
|
||||
* \param[in] data Data buffer
|
||||
@ -457,7 +477,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte)
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length)
|
||||
static int32_t receiveObject(UAVTalkConnectionData *connection, uint8_t type, uint32_t objId, uint16_t instId, uint8_t* data, int32_t length)
|
||||
{
|
||||
UAVObjHandle obj;
|
||||
int32_t ret = 0;
|
||||
@ -468,7 +488,7 @@ static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32
|
||||
|
||||
// Process message type
|
||||
switch (type) {
|
||||
case TYPE_OBJ:
|
||||
case UAVTALK_TYPE_OBJ:
|
||||
// All instances, not allowed for OBJ messages
|
||||
if (instId != UAVOBJ_ALL_INSTANCES)
|
||||
{
|
||||
@ -482,7 +502,7 @@ static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32
|
||||
ret = -1;
|
||||
}
|
||||
break;
|
||||
case TYPE_OBJ_ACK:
|
||||
case UAVTALK_TYPE_OBJ_ACK:
|
||||
// All instances, not allowed for OBJ_ACK messages
|
||||
if (instId != UAVOBJ_ALL_INSTANCES)
|
||||
{
|
||||
@ -490,7 +510,7 @@ static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32
|
||||
if ( UAVObjUnpack(obj, instId, data) == 0 )
|
||||
{
|
||||
// Transmit ACK
|
||||
sendObject(connection, obj, instId, TYPE_ACK);
|
||||
sendObject(connection, obj, instId, UAVTALK_TYPE_ACK);
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -502,17 +522,17 @@ static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32
|
||||
ret = -1;
|
||||
}
|
||||
break;
|
||||
case TYPE_OBJ_REQ:
|
||||
case UAVTALK_TYPE_OBJ_REQ:
|
||||
// Send requested object if message is of type OBJ_REQ
|
||||
if (obj == 0)
|
||||
sendNack(connection, objId);
|
||||
else
|
||||
sendObject(connection, obj, instId, TYPE_OBJ);
|
||||
sendObject(connection, obj, instId, UAVTALK_TYPE_OBJ);
|
||||
break;
|
||||
case TYPE_NACK:
|
||||
case UAVTALK_TYPE_NACK:
|
||||
// Do nothing on flight side, let it time out.
|
||||
break;
|
||||
case TYPE_ACK:
|
||||
case UAVTALK_TYPE_ACK:
|
||||
// All instances, not allowed for ACK messages
|
||||
if (instId != UAVOBJ_ALL_INSTANCES)
|
||||
{
|
||||
@ -537,7 +557,7 @@ static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32
|
||||
* \param[in] obj Object
|
||||
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
|
||||
*/
|
||||
static void updateAck(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId)
|
||||
static void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId)
|
||||
{
|
||||
if (connection->respObj == obj && (connection->respInstId == instId || connection->respInstId == UAVOBJ_ALL_INSTANCES))
|
||||
{
|
||||
@ -555,7 +575,7 @@ static void updateAck(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
static int32_t sendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t type)
|
||||
static int32_t sendObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type)
|
||||
{
|
||||
uint32_t numInst;
|
||||
uint32_t n;
|
||||
@ -567,7 +587,7 @@ static int32_t sendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint1
|
||||
}
|
||||
|
||||
// Process message type
|
||||
if ( type == TYPE_OBJ || type == TYPE_OBJ_ACK )
|
||||
if ( type == UAVTALK_TYPE_OBJ || type == UAVTALK_TYPE_OBJ_ACK )
|
||||
{
|
||||
if (instId == UAVOBJ_ALL_INSTANCES)
|
||||
{
|
||||
@ -585,15 +605,15 @@ static int32_t sendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint1
|
||||
return sendSingleObject(connection, obj, instId, type);
|
||||
}
|
||||
}
|
||||
else if (type == TYPE_OBJ_REQ)
|
||||
else if (type == UAVTALK_TYPE_OBJ_REQ)
|
||||
{
|
||||
return sendSingleObject(connection, obj, instId, TYPE_OBJ_REQ);
|
||||
return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_OBJ_REQ);
|
||||
}
|
||||
else if (type == TYPE_ACK)
|
||||
else if (type == UAVTALK_TYPE_ACK)
|
||||
{
|
||||
if ( instId != UAVOBJ_ALL_INSTANCES )
|
||||
{
|
||||
return sendSingleObject(connection, obj, instId, TYPE_ACK);
|
||||
return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_ACK);
|
||||
}
|
||||
else
|
||||
{
|
||||
@ -615,7 +635,7 @@ static int32_t sendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint1
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
static int32_t sendSingleObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t type)
|
||||
static int32_t sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type)
|
||||
{
|
||||
int32_t length;
|
||||
int32_t dataOffset;
|
||||
@ -623,7 +643,7 @@ static int32_t sendSingleObject(UAVTalkConnection *connection, UAVObjHandle obj,
|
||||
|
||||
// Setup type and object id fields
|
||||
objId = UAVObjGetID(obj);
|
||||
connection->txBuffer[0] = SYNC_VAL; // sync byte
|
||||
connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte
|
||||
connection->txBuffer[1] = type;
|
||||
// data length inserted here below
|
||||
connection->txBuffer[4] = (uint8_t)(objId & 0xFF);
|
||||
@ -644,7 +664,7 @@ static int32_t sendSingleObject(UAVTalkConnection *connection, UAVObjHandle obj,
|
||||
}
|
||||
|
||||
// Determine data length
|
||||
if (type == TYPE_OBJ_REQ || type == TYPE_ACK)
|
||||
if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK)
|
||||
{
|
||||
length = 0;
|
||||
}
|
||||
@ -674,9 +694,17 @@ static int32_t sendSingleObject(UAVTalkConnection *connection, UAVObjHandle obj,
|
||||
|
||||
// Calculate checksum
|
||||
connection->txBuffer[dataOffset+length] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset+length);
|
||||
|
||||
// Send buffer
|
||||
if (connection->outStream!=NULL) (*connection->outStream)(connection->txBuffer, dataOffset+length+UAVTALK_CHECKSUM_LENGTH);
|
||||
|
||||
// Send buffer (partially if needed)
|
||||
uint32_t sent=0;
|
||||
while (sent < dataOffset+length+UAVTALK_CHECKSUM_LENGTH) {
|
||||
uint32_t sending = dataOffset+length+UAVTALK_CHECKSUM_LENGTH - sent;
|
||||
if ( sending > connection->txSize ) sending = connection->txSize;
|
||||
if ( connection->outStream != NULL ) {
|
||||
(*connection->outStream)(connection->txBuffer+sent, sending);
|
||||
}
|
||||
sent += sending;
|
||||
}
|
||||
|
||||
// Update stats
|
||||
++connection->stats.txObjects;
|
||||
@ -694,12 +722,12 @@ static int32_t sendSingleObject(UAVTalkConnection *connection, UAVObjHandle obj,
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
static int32_t sendNack(UAVTalkConnection *connection, uint32_t objId)
|
||||
static int32_t sendNack(UAVTalkConnectionData *connection, uint32_t objId)
|
||||
{
|
||||
int32_t dataOffset;
|
||||
|
||||
connection->txBuffer[0] = SYNC_VAL; // sync byte
|
||||
connection->txBuffer[1] = TYPE_NACK;
|
||||
connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte
|
||||
connection->txBuffer[1] = UAVTALK_TYPE_NACK;
|
||||
// data length inserted here below
|
||||
connection->txBuffer[4] = (uint8_t)(objId & 0xFF);
|
||||
connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF);
|
||||
|
||||
@ -34,6 +34,7 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template
|
||||
<<"uint16_t" << "uint32_t" << "float" << "uint8_t";
|
||||
|
||||
QString flightObjInit,objInc,objFileNames,objNames;
|
||||
qint32 sizeCalc;
|
||||
flightCodePath = QDir( templatepath + QString("flight/UAVObjects"));
|
||||
flightOutputPath = QDir( outputpath + QString("flight") );
|
||||
flightOutputPath.mkpath(flightOutputPath.absolutePath());
|
||||
@ -41,6 +42,7 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template
|
||||
flightCodeTemplate = readFile( flightCodePath.absoluteFilePath("uavobjecttemplate.c") );
|
||||
flightIncludeTemplate = readFile( flightCodePath.absoluteFilePath("inc/uavobjecttemplate.h") );
|
||||
flightInitTemplate = readFile( flightCodePath.absoluteFilePath("uavobjectsinittemplate.c") );
|
||||
flightInitIncludeTemplate = readFile( flightCodePath.absoluteFilePath("inc/uavobjectsinittemplate.h") );
|
||||
flightMakeTemplate = readFile( flightCodePath.absoluteFilePath("Makefiletemplate.inc") );
|
||||
|
||||
if ( flightCodeTemplate.isNull() || flightIncludeTemplate.isNull() || flightInitTemplate.isNull()) {
|
||||
@ -48,6 +50,7 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template
|
||||
return false;
|
||||
}
|
||||
|
||||
sizeCalc = 0;
|
||||
for (int objidx = 0; objidx < parser->getNumObjects(); ++objidx) {
|
||||
ObjectInfo* info=parser->getObjectByIndex(objidx);
|
||||
process_object(info);
|
||||
@ -57,6 +60,9 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template
|
||||
objInc.append("#include \"" + info->namelc + ".h\"\r\n");
|
||||
objFileNames.append(" " + info->namelc);
|
||||
objNames.append(" " + info->name);
|
||||
if (parser->getNumBytes(objidx)>sizeCalc) {
|
||||
sizeCalc = parser->getNumBytes(objidx);
|
||||
}
|
||||
}
|
||||
|
||||
// Write the flight object inialization files
|
||||
@ -65,7 +71,16 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template
|
||||
bool res = writeFileIfDiffrent( flightOutputPath.absolutePath() + "/uavobjectsinit.c",
|
||||
flightInitTemplate );
|
||||
if (!res) {
|
||||
cout << "Error: Could not write flight object init files" << endl;
|
||||
cout << "Error: Could not write flight object init file" << endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
// Write the flight object initialization header
|
||||
flightInitIncludeTemplate.replace( QString("$(SIZECALCULATION)"), QString().setNum(sizeCalc));
|
||||
res = writeFileIfDiffrent( flightOutputPath.absolutePath() + "/uavobjectsinit.h",
|
||||
flightInitIncludeTemplate );
|
||||
if (!res) {
|
||||
cout << "Error: Could not write flight object init header file" << endl;
|
||||
return false;
|
||||
}
|
||||
|
||||
|
||||
@ -34,7 +34,7 @@ class UAVObjectGeneratorFlight
|
||||
public:
|
||||
bool generate(UAVObjectParser* gen,QString templatepath,QString outputpath);
|
||||
QStringList fieldTypeStrC;
|
||||
QString flightCodeTemplate, flightIncludeTemplate, flightInitTemplate, flightMakeTemplate;
|
||||
QString flightCodeTemplate, flightIncludeTemplate, flightInitTemplate, flightInitIncludeTemplate, flightMakeTemplate;
|
||||
QDir flightCodePath;
|
||||
QDir flightOutputPath;
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user