1
0
mirror of https://bitbucket.org/librepilot/librepilot.git synced 2024-12-11 19:24:10 +01:00
LibrePilot/flight/OpenPilot/Modules/Telemetry/telemetry.c
vassilis a64cdc64ea Updates on UAVObjects required by the object generator
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@396 ebee16cc-31ac-478f-84a7-5cbb03baadba
2010-03-27 03:19:56 +00:00

273 lines
7.7 KiB
C

/**
******************************************************************************
*
* @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 "openpilot.h"
// Private constants
#define MAX_QUEUE_SIZE 20
#define STACK_SIZE 100
#define TASK_PRIORITY (tskIDLE_PRIORITY + 2)
#define REQ_TIMEOUT_MS 500
#define MAX_RETRIES 3
// Private types
// Private variables
static COMPortTypeDef telemetryPort;
static xQueueHandle queue;
static xTaskHandle telemetryTaskHandle;
// Private functions
static void telemetryTask(void* parameters);
static void periodicEventHandler(UAVObjEvent* ev);
static int32_t transmitData(uint8_t* data, int32_t length);
static void registerObject(UAVObjHandle obj);
static void updateObject(UAVObjHandle obj);
static int32_t addObject(UAVObjHandle obj);
static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs);
/**
* Initialise the telemetry module
* \return -1 if initialisation failed
* \return 0 on success
*/
int32_t TelemetryInitialize(void)
{
// Create object queue
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
// TODO: Get telemetry settings object
telemetryPort = COM_USART1;
// Initialise UAVTalk
UAVTalkInitialize(&transmitData);
// Process all registered objects and connect queue for updates
UAVObjIterate(&registerObject);
// Start telemetry task
xTaskCreate(telemetryTask, (signed char*)"Telemetry", STACK_SIZE, NULL, TASK_PRIORITY, &telemetryTaskHandle);
return 0;
}
/**
* Register a new object, adds object to local list and connects the queue depending on the object's
* telemetry settings.
* \param[in] obj Object to connect
*/
void registerObject(UAVObjHandle obj)
{
// Setup object for periodic updates
addObject(obj);
// 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(UAVObjHandle obj)
{
UAVObjMetadata metadata;
int32_t eventMask;
// Get metadata
UAVObjGetMetadata(obj, &metadata);
// Setup object depending on update mode
if(metadata.telemetryUpdateMode == UPDATEMODE_PERIODIC)
{
// Set update period
setUpdatePeriod(obj, metadata.telemetryUpdatePeriod);
// Connect queue
eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
if(UAVObjIsMetaobject(obj))
{
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
}
UAVObjConnectQueue(obj, queue, eventMask);
}
else if(metadata.telemetryUpdateMode == UPDATEMODE_ONCHANGE)
{
// Set update period
setUpdatePeriod(obj, 0);
// Connect queue
eventMask = EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ;
if(UAVObjIsMetaobject(obj))
{
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
}
UAVObjConnectQueue(obj, queue, eventMask);
}
else if(metadata.telemetryUpdateMode == UPDATEMODE_MANUAL)
{
// Set update period
setUpdatePeriod(obj, 0);
// Connect queue
eventMask = EV_UPDATED_MANUAL | EV_UPDATE_REQ;
if(UAVObjIsMetaobject(obj))
{
eventMask |= EV_UNPACKED; // we also need to act on remote updates (unpack events)
}
UAVObjConnectQueue(obj, queue, eventMask);
}
else if(metadata.telemetryUpdateMode == UPDATEMODE_NEVER)
{
// Set update period
setUpdatePeriod(obj, 0);
// Disconnect queue
UAVObjDisconnectQueue(obj, queue);
}
}
/**
* Telemetry task. Processes queue events and periodic updates. It does not return.
*/
static void telemetryTask(void* parameters)
{
UAVObjEvent ev;
UAVObjMetadata metadata;
int32_t retries;
int32_t success;
// Loop forever
while(1)
{
// Wait for queue message
if(xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE)
{
// Get object metadata
UAVObjGetMetadata(ev.obj, &metadata);
// Act on event
if(ev.event == EV_UPDATED || ev.event == EV_UPDATED_MANUAL)
{
// Send update to GCS (with retries)
retries = 0;
while(retries < MAX_RETRIES && success == -1)
{
success = UAVTalkSendObject(ev.obj, ev.instId, metadata.telemetryAcked, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
++retries;
}
}
else if(ev.event == EV_UPDATE_REQ)
{
// Request object update from GCS (with retries)
retries = 0;
while(retries < MAX_RETRIES && success == -1)
{
success = UAVTalkSendObjectRequest(ev.obj, ev.instId, REQ_TIMEOUT_MS); // call blocks until update is received or timeout
++retries;
}
}
// If this is a metaobject then make necessary telemetry updates
if(UAVObjIsMetaobject(ev.obj))
{
updateObject(UAVObjGetLinkedObj(ev.obj)); // linked object will be the actual object the metadata are for
}
}
/* This blocks the task until there is something on the buffer */
if(PIOS_COM_ReceiveBufferUsed(telemetryPort) > 0)
{
UAVTalkProcessInputStream(PIOS_COM_ReceiveBuffer(telemetryPort));
}
else if(PIOS_COM_ReceiveBufferUsed(COM_USB_HID) > 0)
{
UAVTalkProcessInputStream(PIOS_COM_ReceiveBuffer(COM_USB_HID));
}
}
}
/**
* Transmit data buffer to the modem or USB port.
* \param[in] data Data buffer to send
* \param[in] length Length of buffer
* \return 0 Success
*/
static int32_t transmitData(uint8_t* data, int32_t length)
{
COMPortTypeDef OutputPort;
/* If USB HID transfer is possible */
if(!PIOS_USB_HID_CheckAvailable())
{
OutputPort = COM_USART1;
}
else
{
OutputPort = COM_USB_HID;
}
return PIOS_COM_SendBuffer(OutputPort, data, length);
}
/**
* Event handler for periodic object updates (called by the event dispatcher)
*/
static void periodicEventHandler(UAVObjEvent* ev)
{
// Push event to the telemetry queue
xQueueSend(queue, ev, 0); // do not wait if queue is full
}
/**
* Setup object for periodic updates.
* \param[in] obj The object to update
* \return 0 Success
* \return -1 Failure
*/
static int32_t addObject(UAVObjHandle obj)
{
UAVObjEvent ev;
// Add object for periodic updates
ev.obj = obj;
ev.instId = UAVOBJ_ALL_INSTANCES;
ev.event = EV_UPDATED_MANUAL;
return EventPeriodicCreate(&ev, &periodicEventHandler, 0);
}
/**
* Set update period of object (it must be already setup for periodic updates)
* \param[in] obj 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
*/
static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
{
UAVObjEvent ev;
// Add object for periodic updates
ev.obj = obj;
ev.instId = UAVOBJ_ALL_INSTANCES;
ev.event = EV_UPDATED_MANUAL;
return EventPeriodicUpdate(&ev, &periodicEventHandler, updatePeriodMs);
}