rooty/LibrePilot
1
0
Fork 0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-01 09:24:10 +01:00
Code Issues Releases Activity

Creation of UAVObject ,UAVTalk and Telemetry modules (compiles but untested!)

Browse Source 
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@196 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
vassilis 2010-02-22 02:18:23 +00:00 committed by vassilis
parent b536fc1669
commit b1b8fe4b37
15 changed files with 2081 additions and 6 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

349
flight/Libraries/utlist.h Normal file
View File

@ -0,0 +1,349 @@
/*
Copyright (c) 2007-2009, Troy D. Hanson
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:
* Redistributions of source code must retain the above copyright
notice, this list of conditions and the following disclaimer.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS
IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED
TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A
PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER
OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL,
EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO,
PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR
PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF
LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING
NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/
#ifndef UTLIST_H
#define UTLIST_H
#define UTLIST_VERSION 1.8
/*
* This file contains macros to manipulate singly and doubly-linked lists.
*
* 1. LL_ macros: singly-linked lists.
* 2. DL_ macros: doubly-linked lists.
* 3. CDL_ macros: circular doubly-linked lists.
*
* To use singly-linked lists, your structure must have a "next" pointer.
* To use doubly-linked lists, your structure must "prev" and "next" pointers.
* Either way, the pointer to the head of the list must be initialized to NULL.
*
* ----------------.EXAMPLE -------------------------
* struct item {
* int id;
* struct item *prev, *next;
* }
*
* struct item *list = NULL:
*
* int main() {
* struct item *item;
* ... allocate and populate item ...
* DL_APPEND(list, item);
* }
* --------------------------------------------------
*
* For doubly-linked lists, the append and delete macros are O(1)
* For singly-linked lists, append and delete are O(n) but prepend is O(1)
* The sort macro is O(n log(n)) for all types of single/double/circular lists.
*/
/******************************************************************************
* The sort macro is an adaptation of Simon Tatham's O(n log(n)) mergesort *
* Unwieldy variable names used here to avoid shadowing passed-in variables. *
*****************************************************************************/
#define LL_SORT(list, cmp) \
do { \
__typeof__(list) _ls_p, _ls_q, _ls_e, _ls_tail, _ls_oldhead; \
int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \
if (list) { \
_ls_insize = 1; \
_ls_looping = 1; \
while (_ls_looping) { \
_ls_p = list; \
_ls_oldhead = list; \
list = NULL; \
_ls_tail = NULL; \
_ls_nmerges = 0; \
while (_ls_p) { \
_ls_nmerges++; \
_ls_q = _ls_p; \
_ls_psize = 0; \
for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \
_ls_psize++; \
_ls_q = _ls_q->next; \
if (!_ls_q) break; \
} \
_ls_qsize = _ls_insize; \
while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \
if (_ls_psize == 0) { \
_ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \
} else if (_ls_qsize == 0 || !_ls_q) { \
_ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \
} else if (cmp(_ls_p,_ls_q) <= 0) { \
_ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \
} else { \
_ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \
} \
if (_ls_tail) { \
_ls_tail->next = _ls_e; \
} else { \
list = _ls_e; \
} \
_ls_tail = _ls_e; \
} \
_ls_p = _ls_q; \
} \
_ls_tail->next = NULL; \
if (_ls_nmerges <= 1) { \
_ls_looping=0; \
} \
_ls_insize *= 2; \
} \
} \
} while (0)
#define DL_SORT(list, cmp) \
do { \
__typeof__(list) _ls_p, _ls_q, _ls_e, _ls_tail, _ls_oldhead; \
int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \
if (list) { \
_ls_insize = 1; \
_ls_looping = 1; \
while (_ls_looping) { \
_ls_p = list; \
_ls_oldhead = list; \
list = NULL; \
_ls_tail = NULL; \
_ls_nmerges = 0; \
while (_ls_p) { \
_ls_nmerges++; \
_ls_q = _ls_p; \
_ls_psize = 0; \
for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \
_ls_psize++; \
_ls_q = _ls_q->next; \
if (!_ls_q) break; \
} \
_ls_qsize = _ls_insize; \
while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \
if (_ls_psize == 0) { \
_ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \
} else if (_ls_qsize == 0 || !_ls_q) { \
_ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \
} else if (cmp(_ls_p,_ls_q) <= 0) { \
_ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \
} else { \
_ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \
} \
if (_ls_tail) { \
_ls_tail->next = _ls_e; \
} else { \
list = _ls_e; \
} \
_ls_e->prev = _ls_tail; \
_ls_tail = _ls_e; \
} \
_ls_p = _ls_q; \
} \
list->prev = _ls_tail; \
_ls_tail->next = NULL; \
if (_ls_nmerges <= 1) { \
_ls_looping=0; \
} \
_ls_insize *= 2; \
} \
} \
} while (0)
#define CDL_SORT(list, cmp) \
do { \
__typeof__(list) _ls_p, _ls_q, _ls_e, _ls_tail, _ls_oldhead; \
int _ls_insize, _ls_nmerges, _ls_psize, _ls_qsize, _ls_i, _ls_looping; \
if (list) { \
_ls_insize = 1; \
_ls_looping = 1; \
while (_ls_looping) { \
_ls_p = list; \
_ls_oldhead = list; \
list = NULL; \
_ls_tail = NULL; \
_ls_nmerges = 0; \
while (_ls_p) { \
_ls_nmerges++; \
_ls_q = _ls_p; \
_ls_psize = 0; \
for (_ls_i = 0; _ls_i < _ls_insize; _ls_i++) { \
_ls_psize++; \
_ls_q = ((_ls_q->next == _ls_oldhead) ? NULL : _ls_q->next); \
if (!_ls_q) break; \
} \
_ls_qsize = _ls_insize; \
while (_ls_psize > 0 || (_ls_qsize > 0 && _ls_q)) { \
if (_ls_psize == 0) { \
_ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \
if (_ls_q == _ls_oldhead) { _ls_q = NULL; } \
} else if (_ls_qsize == 0 || !_ls_q) { \
_ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \
if (_ls_p == _ls_oldhead) { _ls_p = NULL; } \
} else if (cmp(_ls_p,_ls_q) <= 0) { \
_ls_e = _ls_p; _ls_p = _ls_p->next; _ls_psize--; \
if (_ls_p == _ls_oldhead) { _ls_p = NULL; } \
} else { \
_ls_e = _ls_q; _ls_q = _ls_q->next; _ls_qsize--; \
if (_ls_q == _ls_oldhead) { _ls_q = NULL; } \
} \
if (_ls_tail) { \
_ls_tail->next = _ls_e; \
} else { \
list = _ls_e; \
} \
_ls_e->prev = _ls_tail; \
_ls_tail = _ls_e; \
} \
_ls_p = _ls_q; \
} \
list->prev = _ls_tail; \
_ls_tail->next = list; \
if (_ls_nmerges <= 1) { \
_ls_looping=0; \
} \
_ls_insize *= 2; \
} \
} \
} while (0)
/******************************************************************************
* singly linked list macros (non-circular) *
*****************************************************************************/
#define LL_PREPEND(head,add) \
do { \
(add)->next = head; \
head = add; \
} while (0)
#define LL_APPEND(head,add) \
do { \
__typeof__(head) _tmp; \
(add)->next=NULL; \
if (head) { \
_tmp = head; \
while (_tmp->next) { _tmp = _tmp->next; } \
_tmp->next=(add); \
} else { \
(head)=(add); \
} \
} while (0)
#define LL_DELETE(head,del) \
do { \
__typeof__(head) _tmp; \
if ((head) == (del)) { \
(head)=(head)->next; \
} else { \
_tmp = head; \
while (_tmp->next && (_tmp->next != (del))) { \
_tmp = _tmp->next; \
} \
if (_tmp->next) { \
_tmp->next = ((del)->next); \
} \
} \
} while (0)
#define LL_FOREACH(head,el) \
for(el=head;el;el=el->next)
/******************************************************************************
* doubly linked list macros (non-circular) *
*****************************************************************************/
#define DL_PREPEND(head,add) \
do { \
(add)->next = head; \
if (head) { \
(add)->prev = (head)->prev; \
(head)->prev = (add); \
} else { \
(add)->prev = (add); \
} \
(head) = (add); \
} while (0)
#define DL_APPEND(head,add) \
do { \
if (head) { \
(add)->prev = (head)->prev; \
(head)->prev->next = (add); \
(head)->prev = (add); \
(add)->next = NULL; \
} else { \
(head)=(add); \
(head)->prev = (head); \
(head)->next = NULL; \
} \
} while (0);
#define DL_DELETE(head,del) \
do { \
if ((del)->prev == (del)) { \
(head)=NULL; \
} else if ((del)==(head)) { \
(del)->next->prev = (del)->prev; \
(head) = (del)->next; \
} else { \
(del)->prev->next = (del)->next; \
if ((del)->next) { \
(del)->next->prev = (del)->prev; \
} else { \
(head)->prev = (del)->prev; \
} \
} \
} while (0);
#define DL_FOREACH(head,el) \
for(el=head;el;el=el->next)
/******************************************************************************
* circular doubly linked list macros *
*****************************************************************************/
#define CDL_PREPEND(head,add) \
do { \
if (head) { \
(add)->prev = (head)->prev; \
(add)->next = (head); \
(head)->prev = (add); \
(add)->prev->next = (add); \
} else { \
(add)->prev = (add); \
(add)->next = (add); \
} \
(head)=(add); \
} while (0)
#define CDL_DELETE(head,del) \
do { \
if ( ((head)==(del)) && ((head)->next == (head))) { \
(head) = 0L; \
} else { \
(del)->next->prev = (del)->prev; \
(del)->prev->next = (del)->next; \
if ((del) == (head)) (head)=(del)->next; \
} \
} while (0);
#define CDL_FOREACH(head,el) \
for(el=head;el;el= (el->next==head ? 0L : el->next))
#endif /* UTLIST_H */

38
flight/Makefile
View File

@ -60,8 +60,16 @@ OUTDIR = Build
TARGET = OpenPilot
# Paths
OPENPILOTSYS = OpenPilot/System
OPENPILOTSYSINC = $(OPENPILOTSYS)/inc
OPSYSTEM = OpenPilot/System
OPSYSTEMINC = $(OPSYSTEM)/inc
OPUAVTALK = OpenPilot/UAVTalk
OPUAVTALKINC = $(OPUAVTALK)/inc
OPUAVOBJ = OpenPilot/UAVObjects
OPUAVOBJINC = $(OPUAVOBJ)/inc
OPSETTINGS = OpenPilot/Settings
OPSETTINGSINC = $(OPSETTINGS)/inc
MODTELEMETRY = Modules/Telemetry
MODTELEMETRYINC = $(MODTELEMETRY)/inc
PIOS = PiOS
PIOSINC = $(PIOS)/inc
PIOSSTM32F10X = $(PIOS)/STM32F10x
@ -87,9 +95,18 @@ DOXYGENDIR = Doc/Doxygen
# List C source files here. (C dependencies are automatically generated.)
# use file-extension c for "c-only"-files
## MODULES
SRC = $(MODTELEMETRY)/telemetry.c
## OPENPILOT:
SRC = $(OPENPILOTSYS)/openpilot.c
SRC += $(OPENPILOTSYS)/op_logging.c
SRC += $(OPSYSTEM)/openpilot.c
SRC += $(OPSYSTEM)/op_logging.c
SRC += $(OPUAVTALK)/uavtalk.c
SRC += $(OPUAVOBJ)/uavobjectutils.c
SRC += $(OPUAVOBJ)/uavobjectlist.c
SRC += $(OPUAVOBJ)/uavobjectsinit.c
## UAVOBJECTS
## PIOS:
SRC += $(PIOS)/pios.c
@ -197,8 +214,16 @@ ASRCARM =
# List any extra directories to look for include files here.
# Each directory must be seperated by a space.
EXTRAINCDIRS = $(OPENPILOTSYS)
EXTRAINCDIRS += $(OPENPILOTSYSINC)
EXTRAINCDIRS = $(OPSYSTEM)
EXTRAINCDIRS += $(OPSYSTEMINC)
EXTRAINCDIRS += $(OPUAVTALK)
EXTRAINCDIRS += $(OPUAVTALKINC)
EXTRAINCDIRS += $(OPUAVOBJ)
EXTRAINCDIRS += $(OPUAVOBJINC)
EXTRAINCDIRS += $(OPSETTINGS)
EXTRAINCDIRS += $(OPSETTINGSINC)
EXTRAINCDIRS += $(MODTELEMETRY)
EXTRAINCDIRS += $(MODTELEMETRYINC)
EXTRAINCDIRS += $(PIOS)
EXTRAINCDIRS += $(PIOSINC)
EXTRAINCDIRS += $(PIOSSTM32F10X)
@ -210,6 +235,7 @@ EXTRAINCDIRS += $(DOSFSDIR)
EXTRAINCDIRS += $(MSDDIR)
EXTRAINCDIRS += $(MININIDIR)
EXTRAINCDIRS += $(RTOSINCDIR)
EXTRAINCDIRS += $(APPLIBDIR)
EXTRAINCDIRS += $(RTOSSRCDIR)/portable/GCC/ARM_CM3

36
flight/Modules/Telemetry/inc/telemetry.h Normal file
View File

@ -0,0 +1,36 @@
/**
******************************************************************************
*
* @file telemetry.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Include file of the telemetry module.
* As with all modules only the initialize function is exposed all other
* interactions with the module take place through the event queue and
* objects.
* @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 TELEMETRY_H
#define TELEMETRY_H
#include <stdint.h>
int32_t TelemetryInitialize();
#endif // TELEMETRY_H

244
flight/Modules/Telemetry/telemetry.c Normal file
View File

@ -0,0 +1,244 @@
/**
******************************************************************************
*
* @file telemetry.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Telemetry module, handles telemetry and UAVObject updates
* @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
*/
#include "telemetry.h"
#include "uavtalk.h"
#include "uavobject.h"
#include "uavobjectlist.h"
#include "FreeRTOS.h"
#include "task.h"
#include "queue.h"
#include "semphr.h"
// Private constants
#define MAX_QUEUE_SIZE 20
#define STACK_SIZE 100
#define TASK_PRIORITY 100
#define REQ_TIMEOUT_MS 500
#define MAX_RETRIES 2
// Private variables
xQueueHandle queue;
xTaskHandle telemetryTaskHandle;
// Private functions
void telemetryTask();
void receiveTask();
int32_t transmitData(uint8_t* data, int32_t length);
void registerObject(UAVObject* obj);
void updateObject(UAVObject* obj);
/**
* Initialize the telemetry module
* \return -1 if initialization failed
* \return 0 on success
*/
int32_t TelemetryInitialize()
{
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjectQMsg));
// TODO: Get telemetry settings object
// TODO: Initialize communication ports
// Initialize UAVTalk
UAVTalkInitialize(&transmitData);
// Process all registered objects, register in UAVTalk and connect queue for updates
UAVObjectListIterate(&registerObject);
// Start tasks
xTaskCreate( telemetryTask, (signed char*)"Telemetry", STACK_SIZE, NULL, TASK_PRIORITY, &telemetryTaskHandle );
// TODO: Start receive task
return 0;
}
/**
* Register a new object, connects object to UAVTalk and connects the queue depending on the object's
* telemetry settings.
* \param[in] obj Object to connect
*/
void registerObject(UAVObject* obj)
{
// Register object in UAVTalk
UAVTalkConnectObject(obj->objectID, obj->pack, obj->unpack, 0);
// Setup object for telemetry updates
updateObject(obj);
}
/**
* Update object's queue connections and timer, depending on object's settings
* \param[in] obj Object to updates
*/
void updateObject(UAVObject* obj)
{
UAVObjectMetadata metadata;
int32_t eventMask;
// Get metadata
obj->getMetadata(&metadata);
// Setup object depending on update mode
if (metadata.telemetryUpdateMode == UPDATEMODE_PERIODIC)
{
// Set update period
UAVTalkSetUpdatePeriod(obj->objectID, metadata.telemetryUpdatePeriod);
// Connect queue
eventMask = QMSG_UPDATED_MANUAL|QMSG_UPDATE_REQ;
if (obj->isMetadata)
{
eventMask |= QMSG_UNPACKED; // we also need to act on remote updates (unpack events)
}
obj->connect(queue, eventMask);
}
else if (metadata.telemetryUpdateMode == UPDATEMODE_ONCHANGE)
{
// Set update period
UAVTalkSetUpdatePeriod(obj->objectID, 0);
// Connect queue
eventMask = QMSG_UPDATED|QMSG_UPDATED_MANUAL|QMSG_UPDATE_REQ;
if (obj->isMetadata)
{
eventMask |= QMSG_UNPACKED; // we also need to act on remote updates (unpack events)
}
obj->connect(queue, eventMask);
}
else if (metadata.telemetryUpdateMode == UPDATEMODE_MANUAL)
{
// Set update period
UAVTalkSetUpdatePeriod(obj->objectID, 0);
// Connect queue
eventMask = QMSG_UPDATED_MANUAL|QMSG_UPDATE_REQ;
if (obj->isMetadata)
{
eventMask |= QMSG_UNPACKED; // we also need to act on remote updates (unpack events)
}
obj->connect(queue, eventMask);
}
else if (metadata.telemetryUpdateMode == UPDATEMODE_NEVER)
{
// Set update period
UAVTalkSetUpdatePeriod(obj->objectID, 0);
// Disconnect queue
obj->disconnect(queue);
}
}
/**
* Telemetry task. Processes queue events and periodic updates. It does not return.
*/
void telemetryTask()
{
static int32_t timeToNextUpdateMs;
static int32_t delayMs;
static UAVObjectQMsg msg;
static UAVObjectMetadata metadata;
static int32_t retries;
static int32_t success;
// Initialize time
timeToNextUpdateMs = xTaskGetTickCount()*portTICK_RATE_MS;
// Loop forever
while (1)
{
// Calculate delay time
delayMs = timeToNextUpdateMs-(xTaskGetTickCount()*portTICK_RATE_MS);
if (delayMs < 0)
{
delayMs = 0;
}
// Wait for queue message
if ( xQueueReceive(queue, &msg, delayMs/portTICK_RATE_MS) == pdTRUE )
{
// Get object metadata
msg.obj->getMetadata(&metadata);
// Act on event
if (msg.event == QMSG_UPDATED || msg.event == QMSG_UPDATED_MANUAL)
{
// Send update to GCS (with retries)
retries = 0;
while (retries < MAX_RETRIES && success == -1)
{
success = UAVTalkSendObject(msg.obj->objectID, metadata.ackRequired, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
++retries;
}
}
else if (msg.event == QMSG_UPDATE_REQ)
{
// Request object update from GCS (with retries)
retries = 0;
while (retries < MAX_RETRIES && success == -1)
{
success = UAVTalkSendObjectRequest(msg.obj->objectID, REQ_TIMEOUT_MS); // call blocks until update is received or timeout
++retries;
}
}
// If this is a metadata object then make necessary telemetry updates
if (msg.obj->isMetadata)
{
updateObject(msg.obj->linkedObj); // linked object will be the actual object the metadata are for
}
}
// Process periodic updates
if ((xTaskGetTickCount()*portTICK_RATE_MS) >= timeToNextUpdateMs )
{
delayMs = UAVTalkProcessPeriodicUpdates();
timeToNextUpdateMs = xTaskGetTickCount()*portTICK_RATE_MS + delayMs;
}
}
}
/**
* Receive task. Processes received bytes (from the modem or USB) and passes them to
* UAVTalk for decodings. Does not return.
*/
void receiveTask()
{
// Main thread
while (1)
{
// TODO: Wait for bytes and pass to UAVTalk for processing
}
}
/**
* Transmit data buffer to the modem or USB port.
* \param[in] data Data buffer to send
* \param[in] length Length of buffer
*/
int32_t transmitData(uint8_t* data, int32_t length)
{
// TODO: Send data to communication port
return 0;
}

97
flight/OpenPilot/UAVObjects/inc/uavobject.h Normal file
View File

@ -0,0 +1,97 @@
/**
******************************************************************************
*
* @file uavobject.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Interface implemented by all UAVObjects.
* @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 UAVOBJECT_H
#define UAVOBJECT_H
#include <stdint.h>
#include "FreeRTOS.h"
#include "queue.h"
/**
* Object update mode, used by multiple modules (e.g. telemetry and logger)
*/
typedef enum {
UPDATEMODE_PERIODIC = 0, /** Automatically update object at periodic intervals */
UPDATEMODE_ONCHANGE, /** Only update object when its data changes */
UPDATEMODE_MANUAL, /** Manually update object, by calling the updated() function */
UPDATEMODE_NEVER /** Object is never updated */
} UAVObjectUpdateMode;
/**
* Object metadata, each object has a meta object that holds its metadata. The metadata define
* properties for each object and can be used by multiple modules (e.g. telemetry and logger)
*/
typedef struct {
int8_t ackRequired; /** Defines if an ack is required for the transactions of this object (1:acked, 0:not acked) */
UAVObjectUpdateMode telemetryUpdateMode; /** Update mode used by the telemetry module */
int32_t telemetryUpdatePeriod; /** Update period used by the telemetry module (only if telemetry mode is PERIODIC) */
UAVObjectUpdateMode gcsTelemetryUpdateMode; /** Update mode used by the GCS */
int32_t gcsTelemetryUpdatePeriod; /** Update period used by the GCS (only if telemetry mode is PERIODIC) */
UAVObjectUpdateMode loggingUpdateMode; /** Update mode used by the logging module */
int32_t loggingUpdatePeriod; /** Update period used by the logging module (only if logging mode is PERIODIC) */
} UAVObjectMetadata;
/**
* Interface implemented by all UAVObjects.
*/
struct UAVObjectStruct {
uint32_t objectID; /** Unique object ID */
uint32_t metadataID; /** ID of the metadata object */
int32_t isMetadata; /** Defines if this object is a meta object (1:meta, 0:regular object) */
int32_t numBytes; /** Number of bytes of object's data */
const char* name; /** Object name */
struct UAVObjectStruct* linkedObj; /** Link between regular objects and metaobject, for regular objects this is the metaobject, for metadata objects this is the parent object */
int32_t (*pack)(uint32_t objId, uint8_t* data, int32_t maxLength); /** Pack object data in a byte buffer */
int32_t (*unpack)(uint32_t objId, uint8_t* data, int32_t length); /** Unpack object data from a byte buffer */
int32_t (*initializeData)(const char* init); /** Initialize object data from a string, this is used by settings objects, the settings string is parsed and fields initialized */
void (*getMetadata)(UAVObjectMetadata* dataOut); /** Get the object's metadata */
void (*setMetadata)(const UAVObjectMetadata* dataIn); /** Set the object's metadata */
void (*requestUpdate)(); /** Request that this object is updated with the latest value from the GCS */
void (*updated)(); /** Trigger an update event, used when the update mode is set to UPDATEMODE_MANUAL */
int32_t (*connect)(xQueueHandle queue, int32_t eventMask); /** Connect an event queue (from a module) to this object, the eventMask defines which events are enabled (or'ed UAVObjectQMsgEvent or 0 for all events) */
int32_t (*disconnect)(xQueueHandle queue); /** Disconnect an event queue from this object */
};
typedef struct UAVObjectStruct UAVObject;
/**
* Event types generated by the objects.
*/
typedef enum {
QMSG_UNPACKED = 1, /** Object data updated by unpacking */
QMSG_UPDATED = 2, /** Object data updated by changing the data structure */
QMSG_UPDATED_MANUAL = 4, /** Object update event manually generated */
QMSG_UPDATE_REQ = 8 /** Request to update object data */
} UAVObjectQMsgEvent;
/**
* Event message, this structure is send in the event queue each time an event is generated
*/
typedef struct {
UAVObject* obj;
UAVObjectQMsgEvent event;
} UAVObjectQMsg;
#endif // UAVOBJECT_H

38
flight/OpenPilot/UAVObjects/inc/uavobjectlist.h Normal file
View File

@ -0,0 +1,38 @@
/**
******************************************************************************
*
* @file uavobjectlist.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Include files of the uavobjectlist 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 UAVOBJECTLIST_H
#define UAVOBJECTLIST_H
#include <stdint.h>
#include "uavobject.h"
int32_t UAVObjectListInitialize();
int32_t UAVObjectListRegister(UAVObject* obj);
UAVObject* UAVObjectListGetByID(int32_t id);
UAVObject* UAVObjectListGetByName(char* name);
void UAVObjectListIterate(void (*iterator)(UAVObject* obj));
#endif // UAVOBJECTLIST_H

32
flight/OpenPilot/UAVObjects/inc/uavobjectsinit.h Normal file
View File

@ -0,0 +1,32 @@
/**
******************************************************************************
*
* @file uavobjectsinit.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Initialize all objects.
* This file is automatically updated by the parser.
* @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 UAVOBJECTSINIT_H
#define UAVOBJECTSINIT_H
void UAVObjectsInitializeAll();
#endif // UAVOBJECTSINIT_H

50
flight/OpenPilot/UAVObjects/inc/uavobjecttemplate.h Normal file
View File

@ -0,0 +1,50 @@
/**
******************************************************************************
*
* @file uavobjecttemplate.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Template file for all objects. This file will not compile, it is used
* by the parser as a template to generate all other objects. All $(x) fields
* will be replaced by the parser with the actual object information.
* Each object has an meta object associated with it. The meta object
* contains information such as the telemetry and logging properties.
*
* @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 $(NAMEUC)_H
#define $(NAMEUC)_H
#include <stdint.h>
#include "uavobject.h"
#include "FreeRTOS.h"
#include "queue.h"
// Object data
typedef struct {
$(DATAFIELDS)
} __attribute__((packed)) $(NAME)Data;
// Generic interface functions
int32_t $(NAME)Initialize();
UAVObject* $(NAME)Get();
void $(NAME)GetData(TestObjectData* dataOut);
void $(NAME)SetData(const TestObjectData* dataIn);
#endif // $(NAME)_H

55
flight/OpenPilot/UAVObjects/inc/uavobjectutils.h Normal file
View File

@ -0,0 +1,55 @@
/**
******************************************************************************
*
* @file uavobjectutils.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Functions common to all objects, they are included in this file
* to avoid duplication on each generated object. The functions
* in this file should be called only by UAVObjects.
* @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 UAVOBJECTUTILS_H
#define UAVOBJECTUTILS_H
#include "uavobject.h"
#include "FreeRTOS.h"
#include "queue.h"
#include "semphr.h"
struct ObjectQueueListStruct {
xQueueHandle queue;
int32_t eventMask;
struct ObjectQueueListStruct* next;
};
typedef struct ObjectQueueListStruct ObjectQueueList;
typedef struct {
UAVObject obj;
xSemaphoreHandle mutex;
ObjectQueueList* queues;
} ObjectContext;
int32_t UAVObjUtilsConnect(ObjectContext* context, xQueueHandle queue, int32_t eventMask);
int32_t UAVObjUtilsDisconnect(ObjectContext* context, xQueueHandle queue);
void UAVObjUtilsSendEvent(ObjectContext* context, UAVObjectQMsgEvent event);
void UAVObjUtilsRequestUpdate(ObjectContext* context);
void UAVObjUtilsUpdated(ObjectContext* context);
#endif //UAVOBJECTUTILS_H

176
flight/OpenPilot/UAVObjects/uavobjectlist.c Normal file
View File

@ -0,0 +1,176 @@
/**
******************************************************************************
*
* @file uavobjectlist.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Object list library. This library holds a collection of all objects.
* It can be used by all modules/libraries to find an object reference.
* @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
*/
#include <stdlib.h> // for malloc
#include "string.h" // for strcmp
#include "uavobjectlist.h"
#include "utlist.h"
#include "FreeRTOS.h"
#include "queue.h"
#include "semphr.h"
// Private types
struct ObjectListStruct {
UAVObject* obj;
struct ObjectListStruct* next;
};
typedef struct ObjectListStruct ObjectList;
// Private variables
ObjectList* objList;
xSemaphoreHandle mutex;
/**
* Initialize the object list
* \return 0 Success
* \return -1 Failure
*/
int32_t UAVObjectListInitialize()
{
// Initialize object list
objList = NULL;
// Create mutex
mutex = xSemaphoreCreateRecursiveMutex();
if (mutex == NULL)
return -1;
// Done
return 0;
}
/**
* Register an object to the list
* \param[in] obj Object to register
* \return 0 Success
* \return -1 Failure
*/
int32_t UAVObjectListRegister(UAVObject* obj)
{
ObjectList* objEntry;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Check that the object is not already registred
LL_FOREACH(objList, objEntry)
{
if (objList->obj == obj)
{
// Already registered, ignore
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
// Create and append entry
objEntry = (ObjectList*)malloc(sizeof(ObjectList));
objEntry->obj = obj;
LL_APPEND(objList, objEntry);
// Release lock
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Retrieve an object from the list given its id
* \param[in] The object ID
* \return The object or NULL if not found.
*/
UAVObject* UAVObjectListGetByID(int32_t id)
{
ObjectList* objEntry;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Look for object
LL_FOREACH(objList, objEntry)
{
if (objEntry->obj->objectID == id)
{
// Release lock
xSemaphoreGiveRecursive(mutex);
// Done, object found
return objEntry->obj;
}
}
// Object not found, release lock and return error
xSemaphoreGiveRecursive(mutex);
return NULL;
}
/**
* Retrieve an object from the list given its name
* \param[in] name The name of the object
* \return 0 Success
* \return -1 Failure
*/
UAVObject* UAVObjectListGetByName(char* name)
{
ObjectList* objEntry;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Look for object
LL_FOREACH(objList, objEntry)
{
if (strcmp(objEntry->obj->name, name) == 0)
{
// Release lock
xSemaphoreGiveRecursive(mutex);
// Done, object found
return objEntry->obj;
}
}
// Object not found, release lock and return error
xSemaphoreGiveRecursive(mutex);
return NULL;
}
/**
* Iterate through all objects in the list.
* \param iterator This function will be called once for each object,
* the object will be passed as a parameter
*/
void UAVObjectsIterate(void (*iterator)(UAVObject* obj))
{
ObjectList* objEntry;
// Get lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Iterate through the list and invoke iterator for each object
LL_FOREACH(objList, objEntry)
{
(*iterator)(objEntry->obj);
}
// Release lock
xSemaphoreGiveRecursive(mutex);
}

36
flight/OpenPilot/UAVObjects/uavobjectsinit.c Normal file
View File

@ -0,0 +1,36 @@
/**
******************************************************************************
*
* @file uavobjectsinit.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Initialize all objects.
* This file is automatically updated by the parser.
* @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
*/
#include "uavobjectsinit.h"
/**
* Initialize all objects.
* This function is updated by the parser each time a new object is created.
*/
void UAVObjectsInitializeAll()
{
}

268
flight/OpenPilot/UAVObjects/uavobjecttemplate.c Normal file
View File

@ -0,0 +1,268 @@
/**
******************************************************************************
*
* @file uavobjecttemplate.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Template file for all objects. This file will not compile, it is used
* by the parser as a template to generate all other objects. All $(x) fields
* will be replaced by the parser with the actual object information.
* Each object has a meta object associated with it. The meta object
* contains information such as the telemetry and logging properties.
*
* @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
*/
#define $(METAOBJECT)
#include <string.h> // for memcpy
#include "$(NAMELC).h"
#ifndef METAOBJECT
#include "$(NAMELC)meta.h"
#endif //METAOBJECT
#include "uavobjectlist.h"
#include "uavobjectutils.h"
#include "FreeRTOS.h"
#include "semphr.h"
// Private constants
#define OBJECT_ID $(OBJID)
#define METADATA_ID $(METAID)
#define NAME $(NAMESTR)
// Private variables
ObjectContext context;
$(NAME)Data data;
uint32_t objectID;
#ifdef METAOBJECT
UAVObjectMetadata metaData;
#endif //METAOBJECT
// Private functions
int32_t pack(uint32_t objId, uint8_t* data, int32_t maxLength);
int32_t unpack(uint32_t objId, uint8_t* data, int32_t length);
int32_t initializeData(const char* init);
void getMetadata(UAVObjectMetadata* dataOut);
void setMetadata(const UAVObjectMetadata* dataIn);
void requestUpdate();
void updated();
int32_t connect(xQueueHandle queue, int32_t ignoreUnpack);
int32_t disconnect(xQueueHandle queue);
int32_t getFieldIndexByName(char* name);
/**
* Initialize object.
* \return 0 Success
* \return -1 Failure
*/
int32_t $(NAME)Initialize()
{
// Create mutex
context.mutex = xSemaphoreCreateRecursiveMutex();
if (context.mutex == NULL)
return -1;
// Setup UAVObject table
context.obj.objectID = OBJECT_ID;
context.obj.metadataID = METADATA_ID;
#ifndef METAOBJECT
context.obj.isMetadata = 0;
#else
context.obj.isMetadata = 1;
#endif //METAOBJECT
context.obj.name = NAME;
context.obj.numBytes = sizeof($(NAME)Data);
context.obj.pack = &pack;
context.obj.unpack = &unpack;
context.obj.initializeData = initializeData;
context.obj.getMetadata = &getMetadata;
context.obj.setMetadata = &setMetadata;
context.obj.requestUpdate = &requestUpdate;
context.obj.updated = &updated;
context.obj.connect = &connect;
context.obj.disconnect = &disconnect;
// Initialise connected queue list
context.queues = NULL;
objectID = OBJECT_ID;
// Initialise linked object
#ifndef METAOBJECT
$(NAME)MetaInitialize();
context.obj.linkedObj = $(NAME)MetaGet();
context.obj.linkedObj->linkedObj = &context.obj;
#else
context.obj.linkedObj = NULL;
#endif //METAOBJECT
// Register object with object list
UAVObjectListRegister(&context.obj);
return 0;
}
/**
* See UAVObject.h description for more details.
*/
int32_t pack(uint32_t objId, uint8_t* dataOut, int32_t maxLength)
{
int32_t res;
// Lock
xSemaphoreTakeRecursive(context.mutex, portMAX_DELAY);
// Pack data
if (sizeof($(NAME)Data) <= maxLength)
{
memcpy(dataOut, &data, sizeof($(NAME)Data));
res = sizeof($(NAME)Data);
}
else
{
res = -1;
}
// Unlock and return
xSemaphoreGiveRecursive(context.mutex);
return sizeof($(NAME)Data);
}
/**
* See UAVObject.h description for more details.
*/
int32_t unpack(uint32_t objId, uint8_t* dataIn, int32_t length)
{
// Lock
xSemaphoreTakeRecursive(context.mutex, portMAX_DELAY);
// Unpack data
memcpy(&data, dataIn, sizeof($(NAME)Data));
// Trigger event
UAVObjUtilsSendEvent(&context, QMSG_UNPACKED);
// Unlock and return
xSemaphoreGiveRecursive(context.mutex);
return 0;
}
/**
* See UAVObject.h description for more details.
*/
int32_t initializeData(const char* init)
{
return -1;
}
/**
* See UAVObject.h description for more details.
*/
void getMetadata(UAVObjectMetadata* dataOut)
{
// Get lock
xSemaphoreTakeRecursive(context.mutex, portMAX_DELAY);
// Get data
#ifndef METAOBJECT
$(NAME)MetaGetData(dataOut);
#else
memcpy(dataOut, &metaData, sizeof(UAVObjectMetadata));
#endif //METAOBJECT
// Release lock
xSemaphoreGiveRecursive(context.mutex);
}
/**
* See UAVObject.h description for more details.
*/
void setMetadata(const UAVObjectMetadata* dataIn)
{
// Get lock
xSemaphoreTakeRecursive(context.mutex, portMAX_DELAY);
// Get data
#ifndef METAOBJECT
$(NAME)MetaSetData(dataIn);
#else
memcpy(&metaData, dataIn, sizeof(UAVObjectMetadata));
#endif //METAOBJECT
// Release lock
xSemaphoreGiveRecursive(context.mutex);
}
/**
* See UAVObject.h description for more details.
*/
void requestUpdate()
{
UAVObjUtilsRequestUpdate(&context);
}
/**
* See UAVObject.h description for more details.
*/
void updated()
{
UAVObjUtilsUpdated(&context);
}
/**
* See UAVObject.h description for more details.
*/
int32_t connect(xQueueHandle queue, int32_t eventMask)
{
return UAVObjUtilsConnect(&context, queue, eventMask);
}
/**
* See UAVObject.h description for more details.
*/
int32_t disconnect(xQueueHandle queue)
{
return UAVObjUtilsDisconnect(&context, queue);
}
/**
* Get object's data structure.
* \param[out] dataOut Data structure to copy data into
*/
void $(NAME)GetData($(NAME)Data* dataOut)
{
// Get lock
xSemaphoreTakeRecursive(context.mutex, portMAX_DELAY);
// Copy data
memcpy(dataOut, &data, sizeof($(NAME)Data));
// Release lock
xSemaphoreGiveRecursive(context.mutex);
}
/**
* Set object's data structure.
* \param[in] dataIn Data structure to copy data from.
*/
void $(NAME)SetData(const $(NAME)Data* dataIn)
{
// Get lock
xSemaphoreTakeRecursive(context.mutex, portMAX_DELAY);
// Copy data
memcpy(&data, dataIn, sizeof($(NAME)Data));
// Send notification for updates
UAVObjUtilsSendEvent(&context, QMSG_UPDATED);
// Release lock
xSemaphoreGiveRecursive(context.mutex);
}

133
flight/OpenPilot/UAVObjects/uavobjectutils.c Normal file
View File

@ -0,0 +1,133 @@
/**
******************************************************************************
*
* @file uavobjectutils.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief Functions common to all objects, they are included in this file
* to avoid duplication on each generated object. The functions
* in this file should be called only by UAVObjects.
* @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
*/
#include <stdlib.h> // for malloc
#include <string.h> // for memcpy
#include "uavobjectutils.h"
#include "uavobject.h"
#include "utlist.h"
/**
* See UAVObject.h description for more details.
*/
int32_t UAVObjUtilsConnect(ObjectContext* context, xQueueHandle queue, int32_t eventMask)
{
ObjectQueueList* queueHandle;
// Lock
xSemaphoreTakeRecursive(context->mutex, portMAX_DELAY);
// Check that the queue is not already connected, if it is simply update event mask
LL_FOREACH(context->queues, queueHandle)
{
if (queueHandle->queue == queue)
{
// Already connected, update event mask and return
queueHandle->eventMask = eventMask;
xSemaphoreGiveRecursive(context->mutex);
return 0;
}
}
// Add queue to list
queueHandle = (ObjectQueueList*)malloc(sizeof(ObjectQueueList));
queueHandle->queue = queue;
queueHandle->eventMask = eventMask;
LL_APPEND(context->queues, queueHandle);
// Done
xSemaphoreGiveRecursive(context->mutex);
return 0;
}
/**
* See UAVObject.h description for more details.
*/
int32_t UAVObjUtilsDisconnect(ObjectContext* context, xQueueHandle queue)
{
ObjectQueueList* queueHandle;
// Lock
xSemaphoreTakeRecursive(context->mutex, portMAX_DELAY);
// Find queue and remove it
LL_FOREACH(context->queues, queueHandle)
{
if (queueHandle->queue == queue)
{
LL_DELETE(context->queues, queueHandle);
xSemaphoreGiveRecursive(context->mutex);
return 0;
}
}
// If this point is reached the queue was not found
xSemaphoreGiveRecursive(context->mutex);
return -1;
}
/**
* See UAVObject.h description for more details.
*/
void UAVObjUtilsSendEvent(ObjectContext* context, UAVObjectQMsgEvent event)
{
ObjectQueueList* queueHandle;
UAVObjectQMsg msg;
// Setup event
msg.obj = &context->obj;
msg.event = event;
// Go through each object and push the object ID in the queue (if event is activated for the queue)
LL_FOREACH(context->queues, queueHandle)
{
if ( queueHandle->eventMask == 0 || (queueHandle->eventMask & event) != 0 )
{
xQueueSend(queueHandle->queue, &msg, 0); // do not wait if queue is full
}
}
}
/**
* See UAVObject.h description for more details.
*/
void UAVObjUtilsRequestUpdate(ObjectContext* context)
{
xSemaphoreTakeRecursive(context->mutex, portMAX_DELAY);
UAVObjUtilsSendEvent(context, QMSG_UPDATE_REQ);
xSemaphoreGiveRecursive(context->mutex);
}
/**
* See UAVObject.h description for more details.
*/
void UAVObjUtilsUpdated(ObjectContext* context)
{
xSemaphoreTakeRecursive(context->mutex, portMAX_DELAY);
UAVObjUtilsSendEvent(context, QMSG_UPDATED_MANUAL);
xSemaphoreGiveRecursive(context->mutex);
}

51
flight/OpenPilot/UAVTalk/inc/uavtalk.h Normal file
View File

@ -0,0 +1,51 @@
/**
******************************************************************************
*
* @file uavtalk.h
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief 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_C_H
#define UAVTALK_C_H
#include <stdint.h>
// Public constants
#define UAVTALK_WAITFOREVER -1
#define UAVTALK_NOWAIT 0
// Public types
typedef int32_t (*UAVTalkUnpackCb)(uint32_t objId, uint8_t* data, int32_t length);
typedef int32_t (*UAVTalkPackCb)(uint32_t objId, uint8_t* data, int32_t maxLength);
typedef int32_t (*UAVTalkOutputStream)(uint8_t* data, int32_t length);
// Public functions
int32_t UAVTalkInitialize(UAVTalkOutputStream outputStream);
int32_t UAVTalkConnectObject(uint32_t objectId, UAVTalkPackCb packCb, UAVTalkUnpackCb unpackCb, int32_t updatePeriodMs);
int32_t UAVTalkSetUpdatePeriod(uint32_t objectId, int32_t updatePeriodMs);
int32_t UAVTalkSetOutputStream(UAVTalkOutputStream outputStream);
int32_t UAVTalkSendObject(uint32_t objectId, uint8_t acked, int32_t timeoutMs);
int32_t UAVTalkSendObjectRequest(uint32_t objectId, int32_t timeoutMs);
int32_t UAVTalkProcessInputStream(uint8_t rxbyte);
int32_t UAVTalkProcessPeriodicUpdates(void);
#endif // UAVTALK_C_H

484
flight/OpenPilot/UAVTalk/uavtalk.c Normal file
View File

@ -0,0 +1,484 @@
/**
******************************************************************************
*
* @file uavtalk.c
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010.
* @brief UAVTalk library, implements to telemetry protocol. See the wiki for more details.
* This library should not be called directly by the application, it is only used by the
* Telemetry module.
* @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
*/
#include <stdlib.h> // for malloc
#include "uavtalk.h"
#include "utlist.h"
#include "FreeRTOS.h"
#include "semphr.h"
// Private constants
#define TYPE_MASK 0xFC
#define TYPE_BASE 0x50
#define TYPE_OBJ (TYPE_BASE | 0x00)
#define TYPE_OBJ_REQ (TYPE_BASE | 0x01)
#define TYPE_OBJ_ACK (TYPE_BASE | 0x02)
#define TYPE_ACK (TYPE_BASE | 0x03)
#define HEADER_LENGTH 6 // type (1), object ID (4), length (1)
#define CHECKSUM_LENGTH 2
#define MAX_PAYLOAD_LENGTH 256
#define MAX_PACKET_LENGTH (HEADER_LENGTH+MAX_PAYLOAD_LENGTH+CHECKSUM_LENGTH)
#define MAX_UPDATE_PERIOD_MS 1000
#define MIN_UPDATE_PERIOD_MS 1
// Private types
struct ObjectHandleStruct {
uint32_t objectId;
UAVTalkUnpackCb packCb;
UAVTalkUnpackCb unpackCb;
xSemaphoreHandle sema;
uint32_t waitingResp;
int32_t updatePeriodMs;
int32_t timeToNextUpdateMs;
struct ObjectHandleStruct* next;
};
typedef struct ObjectHandleStruct ObjectHandle;
typedef enum {STATE_SYNC, STATE_OBJID, STATE_LENGTH, STATE_DATA, STATE_CS} RxState;
// Private variables
UAVTalkOutputStream outStream;
ObjectHandle* objects;
int32_t timeToNextUpdateMs;
xSemaphoreHandle mutex;
uint8_t rxBuffer[MAX_PACKET_LENGTH];
uint8_t txBuffer[MAX_PACKET_LENGTH];
// Private functions
uint16_t updateChecksum(uint16_t cs, uint8_t* data, int32_t length);
ObjectHandle* findObject(uint32_t objId);
int32_t objectTransaction(uint32_t objectId, uint8_t type, int32_t timeout);
int32_t sendObject(ObjectHandle* obj, uint8_t type);
int32_t receiveObject(uint8_t type, ObjectHandle* obj, uint8_t* data, int32_t length);
/**
* Initialize the UAVTalk library
* \param[in] outputStream Function pointer that is called to send a data buffer
* \return 0 Success
* \return -1 Failure
*/
int32_t UAVTalkInitialize(UAVTalkOutputStream outputStream) {
outStream = outputStream;
mutex = xSemaphoreCreateRecursiveMutex();
timeToNextUpdateMs = 0;
objects = NULL;
return 0;
}
/**
* Connect an object to the UAVTalk library. All objects needs to be registered, this is needed
* so that the library knows how to call the pack and unpack functions of the object.
* \param[in] objectId ID of the object
* \param[in] packCb Callback function that is used to pack the object, called each time the object needs to be sent.
* \param[in] unpackCb Callback function that is used to unpack the object, called each time the object is received.
* \return 0 Success
* \return -1 Failure
*/
int32_t UAVTalkConnectObject(uint32_t objectId, UAVTalkPackCb packCb, UAVTalkUnpackCb unpackCb, int32_t updatePeriodMs) {
ObjectHandle* obj;
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Check that the object is not already connected
LL_FOREACH(objects, obj)
{
if (obj->objectId == objectId)
{
// Already registered, ignore
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
// Create handle
obj = (ObjectHandle*)malloc(sizeof(ObjectHandle));
obj->objectId = objectId;
obj->packCb = packCb;
obj->unpackCb = unpackCb;
vSemaphoreCreateBinary(obj->sema);
obj->waitingResp = 0;
obj->updatePeriodMs = updatePeriodMs;
obj->timeToNextUpdateMs = 0;
// Add to list
LL_APPEND(objects, obj);
// Done
xSemaphoreGiveRecursive(mutex);
return 0;
}
/**
* Setup object for periodic updates.
* \param[in] objectId ID of the object to update
* \param[in] updatePeriodMs The update period in ms, if zero then periodic updates are disabled
* \return 0 Success
* \return -1 Failure
*/
int32_t UAVTalkSetUpdatePeriod(uint32_t objectId, int32_t updatePeriodMs) {
ObjectHandle* obj;
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Get and update object
obj = findObject(objectId);
if (obj != 0) {
obj->updatePeriodMs = updatePeriodMs;
obj->timeToNextUpdateMs = 0;
xSemaphoreGiveRecursive(mutex);
return 0;
}
else {
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
/**
* Request an update for the specified object, on success the object data would have been
* updated by the GCS.
* \param[in] objectId ID of the object to update
* \param[in] timeout Time to wait for the response, when zero it will return immediately
* \return 0 Success
* \return -1 Failure
*/
int32_t UAVTalkSendObjectRequest(uint32_t objectId, int32_t timeout) {
return objectTransaction(objectId, TYPE_OBJ_REQ, timeout);
}
/**
* Send the specified object through the telemetry link.
* \param[in] objectId ID of the object to send
* \param[in] acked Selects if an ack is required (1:ack required, 0: ack not required)
* \param[in] timeoutMs Time to wait for the ack, when zero it will return immediately
* \return 0 Success
* \return -1 Failure
*/
int32_t UAVTalkSendObject(uint32_t objectId, uint8_t acked, int32_t timeoutMs) {
if (acked == 1) {
return objectTransaction(objectId, TYPE_OBJ_ACK, timeoutMs);
} else {
return objectTransaction(objectId, TYPE_OBJ, timeoutMs);
}
}
/**
* Execute the requested transaction on an object.
* \param[in] objectId ID of object
* \param[in] type Transaction type
* TYPE_OBJ: send object,
* TYPE_OBJ_REQ: request object update
* TYPE_OBJ_ACK: send object with an ack
* \return 0 Success
* \return -1 Failure
*/
int32_t objectTransaction(uint32_t objectId, uint8_t type, int32_t timeoutMs) {
ObjectHandle* obj;
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Find object
obj = findObject(objectId);
if (obj == 0) {
xSemaphoreGiveRecursive(mutex);
return -1;
}
// Send object depending on if a response is needed
if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) {
sendObject(obj, type);
xSemaphoreGiveRecursive(mutex); // need to release lock since the next call will block until a response is received
xSemaphoreTake(obj->sema, 0); // the semaphore needs to block on the next call, here we make sure the value is zero (binary sema)
xSemaphoreTake(obj->sema, timeoutMs/portTICK_RATE_MS); // lock on object until a response is received (or timeout)
xSemaphoreTakeRecursive(mutex, portMAX_DELAY); // complete transaction
// Check if a response was received
if (obj->waitingResp == 1) {
obj->waitingResp = 0;
xSemaphoreGiveRecursive(mutex);
return -1;
} else {
xSemaphoreGiveRecursive(mutex);
return 0;
}
} else if (type == TYPE_OBJ) {
sendObject(obj, TYPE_OBJ);
xSemaphoreGiveRecursive(mutex);
return 0;
} else {
xSemaphoreGiveRecursive(mutex);
return -1;
}
}
/**
* Handle periodic updates for all objects.
* \return The time to wait until the next update (in ms)
* \return 0 Success
* \return -1 Failure
*/
int32_t UAVTalkProcessPeriodicUpdates(void) {
ObjectHandle* obj;
int32_t minDelay = MAX_UPDATE_PERIOD_MS;
// Lock
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
// Iterate through each object and update its timer, if zero then transmit object.
// Also calculate smallest delay to next update (will be used for setting timeToNextUpdateMs)
LL_FOREACH(objects, obj) {
// If object is configured for periodic updates
if (obj->updatePeriodMs > 0) {
obj->timeToNextUpdateMs -= timeToNextUpdateMs;
// Check if time for the next update
if (obj->timeToNextUpdateMs <= 0) {
// Reset timer
obj->timeToNextUpdateMs = obj->updatePeriodMs;
// Send object
sendObject(obj, TYPE_OBJ);
}
// Update minimum delay
if (obj->timeToNextUpdateMs < minDelay) {
minDelay = obj->timeToNextUpdateMs;
}
}
}
// Check if delay for the next update is too short
if (minDelay < MIN_UPDATE_PERIOD_MS) {
minDelay = MIN_UPDATE_PERIOD_MS;
}
// Done
timeToNextUpdateMs = minDelay;
xSemaphoreGiveRecursive(mutex);
return timeToNextUpdateMs;
}
/**
* Process an byte from the telemetry stream.
* \param[in] rxbyte Received byte
* \return 0 Success
* \return -1 Failure
*/
int32_t UAVTalkProcessInputStream(uint8_t rxbyte) {
static uint8_t tmpBuffer[4];
static ObjectHandle* obj;
static uint8_t type;
static uint32_t objId;
static uint8_t length;
static uint16_t cs, csRx;
static int32_t rxCount;
static RxState state = STATE_SYNC;
// Receive state machine
switch (state) {
case STATE_SYNC:
if ((rxbyte & TYPE_MASK) == TYPE_BASE ) {
cs = rxbyte;
type = rxbyte;
state = STATE_OBJID;
rxCount = 0;
}
break;
case STATE_OBJID:
tmpBuffer[rxCount++] = rxbyte;
if (rxCount == 4) {
// Search for object, if not found reset state machine
objId = (tmpBuffer[3] << 24) | (tmpBuffer[2] << 16) | (tmpBuffer[1] << 8) | (tmpBuffer[0]);
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
obj = findObject(objId);
xSemaphoreGiveRecursive(mutex);
if (obj == 0) {
state = STATE_SYNC;
} else {
cs = updateChecksum(cs, tmpBuffer, 4);
state = STATE_LENGTH;
rxCount = 0;
}
}
break;
case STATE_LENGTH:
length = (int32_t)rxbyte;
if (length > MAX_PAYLOAD_LENGTH ||
((type == TYPE_OBJ_REQ || type == TYPE_ACK) && length != 0)) {
state = STATE_SYNC;
} else {
cs = updateChecksum(cs, &length, 1);
rxCount = 0;
if (length > 0) {
state = STATE_DATA;
} else {
state = STATE_CS;
}
}
break;
case STATE_DATA:
rxBuffer[rxCount++] = rxbyte;
if (rxCount == length) {
cs = updateChecksum(cs, rxBuffer, length);
state = STATE_CS;
rxCount = 0;
}
break;
case STATE_CS:
tmpBuffer[rxCount++] = rxbyte;
if (rxCount == 2) {
csRx = (tmpBuffer[1] << 8) | (tmpBuffer[0]);
if (csRx == cs) {
xSemaphoreTakeRecursive(mutex, portMAX_DELAY);
receiveObject(type, obj, rxBuffer, length);
xSemaphoreGiveRecursive(mutex);
}
state = STATE_SYNC;
}
break;
default:
state = STATE_SYNC;
}
// Done
return 0;
}
/**
* Receive an object. This function process objects received through the telemetry stream.
* \param[in] type Type of received message (TYPE_OBJ, TYPE_OBJ_REQ, TYPE_OBJ_ACK, TYPE_ACK)
* \param[in] obj Handle of the received object
* \param[in] data Data buffer
* \param[in] length Buffer length
* \return 0 Success
* \return -1 Failure
*/
int32_t receiveObject(uint8_t type, ObjectHandle* obj, uint8_t* data, int32_t length) {
// Unpack object if the message is of type OBJ or OBJ_ACK
if (type == TYPE_OBJ || type == TYPE_OBJ_ACK) {
(obj->unpackCb)(obj->objectId, data, length);
}
// Send requested object if message is of type OBJ_REQ
if (type == TYPE_OBJ_REQ) {
sendObject(obj, TYPE_OBJ);
}
// Send ACK if message is of type OBJ_ACK
if (type == TYPE_OBJ_ACK) {
sendObject(obj, TYPE_ACK);
}
// If a response was pending on the object, unblock any waiting tasks
if (type == TYPE_ACK || type == TYPE_OBJ) {
if (obj->waitingResp == 1) {
obj->waitingResp = 0;
xSemaphoreGive(obj->sema);
}
}
// Done
return 0;
}
/**
* Send an object through the telemetry link.
* \param[in] obj Object handle to send
* \param[in] type Transaction type
* \return 0 Success
* \return -1 Failure
*/
int32_t sendObject(ObjectHandle* obj, uint8_t type) {
int32_t length;
uint16_t cs = 0;
// Check for valid packet type
if (type != TYPE_OBJ && type != TYPE_OBJ_ACK && type != TYPE_OBJ_REQ && type != TYPE_ACK) {
return -1;
}
// If a response is expected, set the flag
if (type == TYPE_OBJ_ACK || type == TYPE_OBJ_REQ) {
obj->waitingResp = 1;
}
// Setup type and object id fields
txBuffer[0] = type;
txBuffer[1] = (uint8_t)(obj->objectId & 0xFF);
txBuffer[2] = (uint8_t)((obj->objectId >> 8) & 0xFF);
txBuffer[3] = (uint8_t)((obj->objectId >> 16) & 0xFF);
txBuffer[4] = (uint8_t)((obj->objectId >> 24) & 0xFF);
// Setup length and data field (if one)
if (type == TYPE_ACK || type == TYPE_OBJ_REQ) {
length = 0;
} else {
// Pack object
length = (obj->packCb)(obj->objectId, &txBuffer[HEADER_LENGTH], MAX_PAYLOAD_LENGTH);
// Check length
if (length > MAX_PAYLOAD_LENGTH || length <= 0) {
return -1;
}
}
txBuffer[5] = (uint8_t)length;
// Calculate checksum
cs = 0;
cs = updateChecksum(cs, txBuffer, HEADER_LENGTH+length);
txBuffer[HEADER_LENGTH+length] = (uint8_t)(cs & 0xFF);
txBuffer[HEADER_LENGTH+length+1] = (uint8_t)((cs >> 8) & 0xFF);
// Send buffer
if (outStream!=NULL) (*outStream)(txBuffer, HEADER_LENGTH+length+CHECKSUM_LENGTH);
// Done
return 0;
}
/**
* Update checksum.
* TODO: Replace with CRC-16
* \param[in] data Data buffer to update checksum on
* \param[in] length Length of buffer
* \return Updated checksum
*/
uint16_t updateChecksum(uint16_t cs, uint8_t* data, int32_t length) {
int32_t n;
for (n = 0; n < length; ++n) {
cs += (uint16_t)data[n];
}
return cs;
}
/**
* Find an object handle given the object ID
* \param[in] objId Object ID
* \return The object handle or NULL if not found
*/
ObjectHandle* findObject(uint32_t objId) {
ObjectHandle* obj;
LL_FOREACH(objects, obj) {
if (obj->objectId == objId) {
return obj;
}
}
return NULL;
}