diff --git a/flight/Modules/Telemetry/telemetry.c b/flight/Modules/Telemetry/telemetry.c index 481641f76..3099120c7 100644 --- a/flight/Modules/Telemetry/telemetry.c +++ b/flight/Modules/Telemetry/telemetry.c @@ -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); diff --git a/flight/UAVObjects/inc/uavobjectsinit.h b/flight/UAVObjects/inc/uavobjectsinittemplate.h similarity index 93% rename from flight/UAVObjects/inc/uavobjectsinit.h rename to flight/UAVObjects/inc/uavobjectsinittemplate.h index ec946ba92..ec11c47ad 100644 --- a/flight/UAVObjects/inc/uavobjectsinit.h +++ b/flight/UAVObjects/inc/uavobjectsinittemplate.h @@ -29,4 +29,6 @@ void UAVObjectsInitializeAll(); +#define UAVOBJECTS_LARGEST $(SIZECALCULATION) + #endif // UAVOBJECTSINIT_H diff --git a/flight/UAVTalk/inc/uavtalk.h b/flight/UAVTalk/inc/uavtalk.h index 6d53f04f6..71114ce74 100644 --- a/flight/UAVTalk/inc/uavtalk.h +++ b/flight/UAVTalk/inc/uavtalk.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 /** diff --git a/flight/UAVTalk/inc/uavtalk_priv.h b/flight/UAVTalk/inc/uavtalk_priv.h new file mode 100644 index 000000000..1bcf01fd9 --- /dev/null +++ b/flight/UAVTalk/inc/uavtalk_priv.h @@ -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 +/** + * @} + * @} + */ diff --git a/flight/UAVTalk/uavtalk.c b/flight/UAVTalk/uavtalk.c index 36fdd9c12..236915b28 100644 --- a/flight/UAVTalk/uavtalk.c +++ b/flight/UAVTalk/uavtalk.c @@ -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); diff --git a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp index 8d3a30b30..3d9fd12b6 100644 --- a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp +++ b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.cpp @@ -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; } diff --git a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.h b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.h index 1050b2943..db728374b 100644 --- a/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.h +++ b/ground/uavobjgenerator/generators/flight/uavobjectgeneratorflight.h @@ -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;