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

VTalk (flight): Buffers allocated dynamically with size calculated by UAVObjGenerator. Support for tx packet sizes < object size.i

Browse Source
This commit is contained in:
Corvus Corax 2011-06-24 02:08:20 +02:00
parent 926246b402
commit f52f6d30c6
7 changed files with 239 additions and 122 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

12
flight/Modules/Telemetry/telemetry.c
View File

@ -103,7 +103,7 @@ int32_t TelemetryInitialize(void)
updateSettings(); updateSettings();
// Initialise UAVTalk // Initialise UAVTalk
UAVTalkInitialize(&uavTalkCon, &transmitData); uavTalkCon = UAVTalkInitialize(&transmitData,256);
// Process all registered objects and connect queue for updates // Process all registered objects and connect queue for updates
UAVObjIterate(&registerObject); UAVObjIterate(&registerObject);
@ -222,7 +222,7 @@ static void processObjEvent(UAVObjEvent * ev)
if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL) { if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL) {
// Send update to GCS (with retries) // Send update to GCS (with retries)
while (retries < MAX_RETRIES && success == -1) { 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; ++retries;
} }
// Update stats // Update stats
@ -233,7 +233,7 @@ static void processObjEvent(UAVObjEvent * ev)
} else if (ev->event == EV_UPDATE_REQ) { } else if (ev->event == EV_UPDATE_REQ) {
// Request object update from GCS (with retries) // Request object update from GCS (with retries)
while (retries < MAX_RETRIES && success == -1) { 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; ++retries;
} }
// Update stats // 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 // TODO: Currently we periodically check the buffer for data, update once the PIOS_COM is made blocking
len = PIOS_COM_ReceiveBufferUsed(inputPort); len = PIOS_COM_ReceiveBufferUsed(inputPort);
for (int32_t n = 0; n < len; ++n) { 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 vTaskDelay(5); // <- remove when blocking calls are implemented
@ -404,8 +404,8 @@ static void updateTelemetryStats()
uint32_t timeNow; uint32_t timeNow;
// Get stats // Get stats
UAVTalkGetStats(&uavTalkCon, &utalkStats); UAVTalkGetStats(uavTalkCon, &utalkStats);
UAVTalkResetStats(&uavTalkCon); UAVTalkResetStats(uavTalkCon);
// Get object data // Get object data
FlightTelemetryStatsGet(&flightStats); FlightTelemetryStatsGet(&flightStats);

2
flight/UAVObjects/inc/uavobjectsinit.h → flight/UAVObjects/inc/uavobjectsinittemplate.h
View File

@ -29,4 +29,6 @@
void UAVObjectsInitializeAll(); void UAVObjectsInitializeAll();
#define UAVOBJECTS_LARGEST $(SIZECALCULATION)
#endif // UAVOBJECTSINIT_H #endif // UAVOBJECTSINIT_H

57
flight/UAVTalk/inc/uavtalk.h
View File

@ -29,24 +29,9 @@
#ifndef UAVTALK_H #ifndef UAVTALK_H
#define 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 // Public types
typedef int32_t (*UAVTalkOutputStream)(uint8_t* data, int32_t length); 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 { typedef struct {
uint32_t txBytes; uint32_t txBytes;
uint32_t rxBytes; uint32_t rxBytes;
@ -58,41 +43,17 @@ typedef struct {
uint32_t rxErrors; uint32_t rxErrors;
} UAVTalkStats; } UAVTalkStats;
typedef struct { typedef void* UAVTalkConnection;
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;
// Public functions // Public functions
int32_t UAVTalkInitialize(UAVTalkConnection *connection, UAVTalkOutputStream outputStream); UAVTalkConnection UAVTalkInitialize(UAVTalkOutputStream outputStream, uint32_t maxPacketSize);
int32_t UAVTalkSetOutputStream(UAVTalkConnection *connection, UAVTalkOutputStream outputStream); int32_t UAVTalkSetOutputStream(UAVTalkConnection connection, UAVTalkOutputStream outputStream);
UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection *connection); UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection connection);
int32_t UAVTalkSendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t instId, uint8_t acked, int32_t timeoutMs); 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 UAVTalkSendObjectRequest(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs);
int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte); int32_t UAVTalkProcessInputStream(UAVTalkConnection connection, uint8_t rxbyte);
void UAVTalkGetStats(UAVTalkConnection *connection, UAVTalkStats* stats); void UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats);
void UAVTalkResetStats(UAVTalkConnection *connection); void UAVTalkResetStats(UAVTalkConnection connection);
#endif // UAVTALK_H #endif // UAVTALK_H
/** /**

111
flight/UAVTalk/inc/uavtalk_priv.h Normal file
View 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
/**
* @}
* @}
*/

158
flight/UAVTalk/uavtalk.c
View File

@ -30,26 +30,16 @@
*/ */
#include "openpilot.h" #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 // Private functions
static int32_t objectTransaction(UAVTalkConnection *connection, UAVObjHandle objectId, uint16_t instId, uint8_t type, int32_t timeout); static int32_t objectTransaction(UAVTalkConnectionData *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 sendObject(UAVTalkConnectionData *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 sendSingleObject(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId, uint8_t type);
static int32_t sendNack(UAVTalkConnection *connection, uint32_t objId); static int32_t sendNack(UAVTalkConnectionData *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 int32_t receiveObject(UAVTalkConnectionData *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 void updateAck(UAVTalkConnectionData *connection, UAVObjHandle obj, uint16_t instId);
/** /**
* Initialize the UAVTalk library * Initialize the UAVTalk library
@ -58,17 +48,28 @@ static void updateAck(UAVTalkConnection *connection, UAVObjHandle obj, uint16_t
* \return 0 Success * \return 0 Success
* \return -1 Failure * \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.rxPacketLength = 0;
connection->iproc.state = UAVTALK_STATE_SYNC; connection->iproc.state = UAVTALK_STATE_SYNC;
connection->outStream = outputStream; connection->outStream = outputStream;
connection->lock = xSemaphoreCreateRecursiveMutex(); connection->lock = xSemaphoreCreateRecursiveMutex();
connection->transLock = 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); vSemaphoreCreateBinary(connection->respSema);
xSemaphoreTake(connection->respSema, 0); // reset to zero xSemaphoreTake(connection->respSema, 0); // reset to zero
UAVTalkResetStats(connection); UAVTalkResetStats( (UAVTalkConnection) connection );
return 0; return (UAVTalkConnection) connection;
} }
/** /**
@ -78,8 +79,12 @@ int32_t UAVTalkInitialize(UAVTalkConnection *connection, UAVTalkOutputStream out
* \return 0 Success * \return 0 Success
* \return -1 Failure * \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 // Lock
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
@ -98,8 +103,10 @@ int32_t UAVTalkSetOutputStream(UAVTalkConnection *connection, UAVTalkOutputStrea
* \param[in] connection UAVTalkConnection to be used * \param[in] connection UAVTalkConnection to be used
* @return UAVTarlkOutputStream the output stream 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; return connection->outStream;
} }
@ -108,8 +115,11 @@ UAVTalkOutputStream UAVTalkGetOutputStream(UAVTalkConnection *connection)
* \param[in] connection UAVTalkConnection to be used * \param[in] connection UAVTalkConnection to be used
* @param[out] statsOut Statistics counters * @param[out] statsOut Statistics counters
*/ */
void UAVTalkGetStats(UAVTalkConnection *connection, UAVTalkStats* statsOut) void UAVTalkGetStats(UAVTalkConnection connectionHandle, UAVTalkStats* statsOut)
{ {
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle,connection,return );
// Lock // Lock
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
@ -124,8 +134,11 @@ void UAVTalkGetStats(UAVTalkConnection *connection, UAVTalkStats* statsOut)
* Reset the statistics counters. * Reset the statistics counters.
* \param[in] connection UAVTalkConnection to be used * \param[in] connection UAVTalkConnection to be used
*/ */
void UAVTalkResetStats(UAVTalkConnection *connection) void UAVTalkResetStats(UAVTalkConnection connectionHandle)
{ {
UAVTalkConnectionData *connection;
CHECKCONHANDLE(connectionHandle,connection,return);
// Lock // Lock
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
@ -146,9 +159,11 @@ void UAVTalkResetStats(UAVTalkConnection *connection)
* \return 0 Success * \return 0 Success
* \return -1 Failure * \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 0 Success
* \return -1 Failure * \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 // Send object
if (acked == 1) if (acked == 1)
{ {
return objectTransaction(connection, obj, instId, TYPE_OBJ_ACK, timeoutMs); return objectTransaction(connection, obj, instId, UAVTALK_TYPE_OBJ_ACK, timeoutMs);
} }
else 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] obj Object
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
* \param[in] type Transaction type * \param[in] type Transaction type
* TYPE_OBJ: send object, * UAVTALK_TYPE_OBJ: send object,
* TYPE_OBJ_REQ: request object update * UAVTALK_TYPE_OBJ_REQ: request object update
* TYPE_OBJ_ACK: send object with an ack * UAVTALK_TYPE_OBJ_ACK: send object with an ack
* \return 0 Success * \return 0 Success
* \return -1 Failure * \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; int32_t respReceived;
// Send object depending on if a response is needed // 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) // Get transaction lock (will block if a transaction is pending)
xSemaphoreTakeRecursive(connection->transLock, portMAX_DELAY); xSemaphoreTakeRecursive(connection->transLock, portMAX_DELAY);
@ -220,10 +237,10 @@ static int32_t objectTransaction(UAVTalkConnection *connection, UAVObjHandle obj
return 0; return 0;
} }
} }
else if (type == TYPE_OBJ) else if (type == UAVTALK_TYPE_OBJ)
{ {
xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY);
sendObject(connection, obj, instId, TYPE_OBJ); sendObject(connection, obj, instId, UAVTALK_TYPE_OBJ);
xSemaphoreGiveRecursive(connection->lock); xSemaphoreGiveRecursive(connection->lock);
return 0; return 0;
} }
@ -240,8 +257,11 @@ static int32_t objectTransaction(UAVTalkConnection *connection, UAVObjHandle obj
* \return 0 Success * \return 0 Success
* \return -1 Failure * \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; UAVTalkInputProcessor *iproc = &connection->iproc;
++connection->stats.rxBytes; ++connection->stats.rxBytes;
@ -252,7 +272,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte)
switch (iproc->state) switch (iproc->state)
{ {
case UAVTALK_STATE_SYNC: case UAVTALK_STATE_SYNC:
if (rxbyte != SYNC_VAL) if (rxbyte != UAVTALK_SYNC_VAL)
break; break;
// Initialize and update the CRC // Initialize and update the CRC
@ -268,7 +288,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte)
// update the CRC // update the CRC
iproc->cs = PIOS_CRC_updateByte(iproc->cs, rxbyte); 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; iproc->state = UAVTALK_STATE_SYNC;
break; break;
@ -322,7 +342,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte)
// exist, in which case we'll send a NACK // exist, in which case we'll send a NACK
iproc->obj = UAVObjGetByID(iproc->objId); 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++; connection->stats.rxErrors++;
iproc->state = UAVTALK_STATE_SYNC; iproc->state = UAVTALK_STATE_SYNC;
@ -330,7 +350,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte)
} }
// Determine data length // 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; iproc->length = 0;
else else
iproc->length = UAVObjGetNumBytes(iproc->obj); 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. * Receive an object. This function process objects received through the telemetry stream.
* \param[in] connection UAVTalkConnection to be used * \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] objId ID of the object to work on
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. * \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances.
* \param[in] data Data buffer * \param[in] data Data buffer
@ -457,7 +477,7 @@ int32_t UAVTalkProcessInputStream(UAVTalkConnection *connection, uint8_t rxbyte)
* \return 0 Success * \return 0 Success
* \return -1 Failure * \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; UAVObjHandle obj;
int32_t ret = 0; int32_t ret = 0;
@ -468,7 +488,7 @@ static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32
// Process message type // Process message type
switch (type) { switch (type) {
case TYPE_OBJ: case UAVTALK_TYPE_OBJ:
// All instances, not allowed for OBJ messages // All instances, not allowed for OBJ messages
if (instId != UAVOBJ_ALL_INSTANCES) if (instId != UAVOBJ_ALL_INSTANCES)
{ {
@ -482,7 +502,7 @@ static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32
ret = -1; ret = -1;
} }
break; break;
case TYPE_OBJ_ACK: case UAVTALK_TYPE_OBJ_ACK:
// All instances, not allowed for OBJ_ACK messages // All instances, not allowed for OBJ_ACK messages
if (instId != UAVOBJ_ALL_INSTANCES) 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 ) if ( UAVObjUnpack(obj, instId, data) == 0 )
{ {
// Transmit ACK // Transmit ACK
sendObject(connection, obj, instId, TYPE_ACK); sendObject(connection, obj, instId, UAVTALK_TYPE_ACK);
} }
else else
{ {
@ -502,17 +522,17 @@ static int32_t receiveObject(UAVTalkConnection *connection, uint8_t type, uint32
ret = -1; ret = -1;
} }
break; break;
case TYPE_OBJ_REQ: case UAVTALK_TYPE_OBJ_REQ:
// Send requested object if message is of type OBJ_REQ // Send requested object if message is of type OBJ_REQ
if (obj == 0) if (obj == 0)
sendNack(connection, objId); sendNack(connection, objId);
else else
sendObject(connection, obj, instId, TYPE_OBJ); sendObject(connection, obj, instId, UAVTALK_TYPE_OBJ);
break; break;
case TYPE_NACK: case UAVTALK_TYPE_NACK:
// Do nothing on flight side, let it time out. // Do nothing on flight side, let it time out.
break; break;
case TYPE_ACK: case UAVTALK_TYPE_ACK:
// All instances, not allowed for ACK messages // All instances, not allowed for ACK messages
if (instId != UAVOBJ_ALL_INSTANCES) 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] obj Object
* \param[in] instId The instance ID of UAVOBJ_ALL_INSTANCES for all instances. * \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)) 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 0 Success
* \return -1 Failure * \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 numInst;
uint32_t n; uint32_t n;
@ -567,7 +587,7 @@ static int32_t sendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint1
} }
// Process message type // 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) if (instId == UAVOBJ_ALL_INSTANCES)
{ {
@ -585,15 +605,15 @@ static int32_t sendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint1
return sendSingleObject(connection, obj, instId, type); 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 ) if ( instId != UAVOBJ_ALL_INSTANCES )
{ {
return sendSingleObject(connection, obj, instId, TYPE_ACK); return sendSingleObject(connection, obj, instId, UAVTALK_TYPE_ACK);
} }
else else
{ {
@ -615,7 +635,7 @@ static int32_t sendObject(UAVTalkConnection *connection, UAVObjHandle obj, uint1
* \return 0 Success * \return 0 Success
* \return -1 Failure * \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 length;
int32_t dataOffset; int32_t dataOffset;
@ -623,7 +643,7 @@ static int32_t sendSingleObject(UAVTalkConnection *connection, UAVObjHandle obj,
// Setup type and object id fields // Setup type and object id fields
objId = UAVObjGetID(obj); objId = UAVObjGetID(obj);
connection->txBuffer[0] = SYNC_VAL; // sync byte connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte
connection->txBuffer[1] = type; connection->txBuffer[1] = type;
// data length inserted here below // data length inserted here below
connection->txBuffer[4] = (uint8_t)(objId & 0xFF); connection->txBuffer[4] = (uint8_t)(objId & 0xFF);
@ -644,7 +664,7 @@ static int32_t sendSingleObject(UAVTalkConnection *connection, UAVObjHandle obj,
} }
// Determine data length // Determine data length
if (type == TYPE_OBJ_REQ || type == TYPE_ACK) if (type == UAVTALK_TYPE_OBJ_REQ || type == UAVTALK_TYPE_ACK)
{ {
length = 0; length = 0;
} }
@ -675,8 +695,16 @@ static int32_t sendSingleObject(UAVTalkConnection *connection, UAVObjHandle obj,
// Calculate checksum // Calculate checksum
connection->txBuffer[dataOffset+length] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset+length); connection->txBuffer[dataOffset+length] = PIOS_CRC_updateCRC(0, connection->txBuffer, dataOffset+length);
// Send buffer // Send buffer (partially if needed)
if (connection->outStream!=NULL) (*connection->outStream)(connection->txBuffer, dataOffset+length+UAVTALK_CHECKSUM_LENGTH); 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 // Update stats
++connection->stats.txObjects; ++connection->stats.txObjects;
@ -694,12 +722,12 @@ static int32_t sendSingleObject(UAVTalkConnection *connection, UAVObjHandle obj,
* \return 0 Success * \return 0 Success
* \return -1 Failure * \return -1 Failure
*/ */
static int32_t sendNack(UAVTalkConnection *connection, uint32_t objId) static int32_t sendNack(UAVTalkConnectionData *connection, uint32_t objId)
{ {
int32_t dataOffset; int32_t dataOffset;
connection->txBuffer[0] = SYNC_VAL; // sync byte connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte
connection->txBuffer[1] = TYPE_NACK; connection->txBuffer[1] = UAVTALK_TYPE_NACK;
// data length inserted here below // data length inserted here below
connection->txBuffer[4] = (uint8_t)(objId & 0xFF); connection->txBuffer[4] = (uint8_t)(objId & 0xFF);
connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF); connection->txBuffer[5] = (uint8_t)((objId >> 8) & 0xFF);

17
ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp
View File

@ -34,6 +34,7 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template
<<"uint16_t" << "uint32_t" << "float" << "uint8_t"; <<"uint16_t" << "uint32_t" << "float" << "uint8_t";
QString flightObjInit,objInc,objFileNames,objNames; QString flightObjInit,objInc,objFileNames,objNames;
qint32 sizeCalc;
flightCodePath = QDir( templatepath + QString("flight/UAVObjects")); flightCodePath = QDir( templatepath + QString("flight/UAVObjects"));
flightOutputPath = QDir( outputpath + QString("flight") ); flightOutputPath = QDir( outputpath + QString("flight") );
flightOutputPath.mkpath(flightOutputPath.absolutePath()); flightOutputPath.mkpath(flightOutputPath.absolutePath());
@ -41,6 +42,7 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template
flightCodeTemplate = readFile( flightCodePath.absoluteFilePath("uavobjecttemplate.c") ); flightCodeTemplate = readFile( flightCodePath.absoluteFilePath("uavobjecttemplate.c") );
flightIncludeTemplate = readFile( flightCodePath.absoluteFilePath("inc/uavobjecttemplate.h") ); flightIncludeTemplate = readFile( flightCodePath.absoluteFilePath("inc/uavobjecttemplate.h") );
flightInitTemplate = readFile( flightCodePath.absoluteFilePath("uavobjectsinittemplate.c") ); flightInitTemplate = readFile( flightCodePath.absoluteFilePath("uavobjectsinittemplate.c") );
flightInitIncludeTemplate = readFile( flightCodePath.absoluteFilePath("inc/uavobjectsinittemplate.h") );
flightMakeTemplate = readFile( flightCodePath.absoluteFilePath("Makefiletemplate.inc") ); flightMakeTemplate = readFile( flightCodePath.absoluteFilePath("Makefiletemplate.inc") );
if ( flightCodeTemplate.isNull() || flightIncludeTemplate.isNull() || flightInitTemplate.isNull()) { if ( flightCodeTemplate.isNull() || flightIncludeTemplate.isNull() || flightInitTemplate.isNull()) {
@ -48,6 +50,7 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template
return false; return false;
} }
sizeCalc = 0;
for (int objidx = 0; objidx < parser->getNumObjects(); ++objidx) { for (int objidx = 0; objidx < parser->getNumObjects(); ++objidx) {
ObjectInfo* info=parser->getObjectByIndex(objidx); ObjectInfo* info=parser->getObjectByIndex(objidx);
process_object(info); process_object(info);
@ -57,6 +60,9 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template
objInc.append("#include \"" + info->namelc + ".h\"\r\n"); objInc.append("#include \"" + info->namelc + ".h\"\r\n");
objFileNames.append(" " + info->namelc); objFileNames.append(" " + info->namelc);
objNames.append(" " + info->name); objNames.append(" " + info->name);
if (parser->getNumBytes(objidx)>sizeCalc) {
sizeCalc = parser->getNumBytes(objidx);
}
} }
// Write the flight object inialization files // Write the flight object inialization files
@ -65,7 +71,16 @@ bool UAVObjectGeneratorFlight::generate(UAVObjectParser* parser,QString template
bool res = writeFileIfDiffrent( flightOutputPath.absolutePath() + "/uavobjectsinit.c", bool res = writeFileIfDiffrent( flightOutputPath.absolutePath() + "/uavobjectsinit.c",
flightInitTemplate ); flightInitTemplate );
if (!res) { 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; return false;
} }

2
ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.h
View File

@ -34,7 +34,7 @@ class UAVObjectGeneratorFlight
public: public:
bool generate(UAVObjectParser* gen,QString templatepath,QString outputpath); bool generate(UAVObjectParser* gen,QString templatepath,QString outputpath);
QStringList fieldTypeStrC; QStringList fieldTypeStrC;
QString flightCodeTemplate, flightIncludeTemplate, flightInitTemplate, flightMakeTemplate; QString flightCodeTemplate, flightIncludeTemplate, flightInitTemplate, flightInitIncludeTemplate, flightMakeTemplate;
QDir flightCodePath; QDir flightCodePath;
QDir flightOutputPath; QDir flightOutputPath;