mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2025-02-20 10:54:14 +01:00
Added in transmitting routine for telemetry.
git-svn-id: svn://svn.openpilot.org/OpenPilot/trunk@244 ebee16cc-31ac-478f-84a7-5cbb03baadba
This commit is contained in:
gussy
gussy
parent
09645920ee
commit
f683a5de3d
@ -23,7 +23,7 @@
|
||||
* 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
|
||||
*/
|
||||
|
||||
#include <stdlib.h> // for malloc
|
||||
#include "pios.h"
|
||||
#include "telemetry.h"
|
||||
#include "uavtalk.h"
|
||||
#include "uavobjectmanager.h"
|
||||
@ -47,20 +47,20 @@ xQueueHandle queue;
|
||||
xTaskHandle telemetryTaskHandle;
|
||||
|
||||
// Private functions
|
||||
void telemetryTask();
|
||||
void receiveTask();
|
||||
void periodicEventHandler(UAVObjEvent* ev);
|
||||
int32_t transmitData(uint8_t* data, int32_t length);
|
||||
void registerObject(UAVObjHandle obj);
|
||||
void updateObject(UAVObjHandle obj);
|
||||
int32_t addObject(UAVObjHandle obj);
|
||||
int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs);
|
||||
static void telemetryTask();
|
||||
static void receiveTask();
|
||||
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);
|
||||
|
||||
/**
|
||||
* Initialize the telemetry module
|
||||
* \return -1 if initialization failed
|
||||
* \return 0 on success
|
||||
*/
|
||||
* Initialize the telemetry module
|
||||
* \return -1 if initialization failed
|
||||
* \return 0 on success
|
||||
*/
|
||||
int32_t TelemetryInitialize()
|
||||
{
|
||||
// Create object queue
|
||||
@ -83,10 +83,10 @@ int32_t TelemetryInitialize()
|
||||
}
|
||||
|
||||
/**
|
||||
* 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
|
||||
*/
|
||||
* 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
|
||||
@ -98,7 +98,7 @@ void registerObject(UAVObjHandle obj)
|
||||
|
||||
/**
|
||||
* Update object's queue connections and timer, depending on object's settings
|
||||
* \param[in] obj Object to updates
|
||||
* \param[in] obj Object to updates
|
||||
*/
|
||||
void updateObject(UAVObjHandle obj)
|
||||
{
|
||||
@ -109,44 +109,34 @@ void updateObject(UAVObjHandle obj)
|
||||
UAVObjGetMetadata(obj, &metadata);
|
||||
|
||||
// Setup object depending on update mode
|
||||
if (metadata.telemetryUpdateMode == UPDATEMODE_PERIODIC)
|
||||
{
|
||||
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_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)
|
||||
{
|
||||
} 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_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)
|
||||
{
|
||||
} 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_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)
|
||||
{
|
||||
} else if(metadata.telemetryUpdateMode == UPDATEMODE_NEVER) {
|
||||
// Set update period
|
||||
setUpdatePeriod(obj, 0);
|
||||
// Disconnect queue
|
||||
@ -155,48 +145,39 @@ void updateObject(UAVObjHandle obj)
|
||||
}
|
||||
|
||||
/**
|
||||
* Telemetry task. Processes queue events and periodic updates. It does not return.
|
||||
* Telemetry task. Processes queue events and periodic updates. It does not return.
|
||||
*/
|
||||
void telemetryTask()
|
||||
static void telemetryTask()
|
||||
{
|
||||
UAVObjEvent ev;
|
||||
UAVObjMetadata metadata;
|
||||
int32_t retries;
|
||||
int32_t success;
|
||||
|
||||
|
||||
// Loop forever
|
||||
while (1)
|
||||
{
|
||||
while(1) {
|
||||
// Wait for queue message
|
||||
if ( xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE )
|
||||
{
|
||||
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)
|
||||
{
|
||||
if(ev.event == EV_UPDATED || ev.event == EV_UPDATED_MANUAL) {
|
||||
// Send update to GCS (with retries)
|
||||
retries = 0;
|
||||
while (retries < MAX_RETRIES && success == -1)
|
||||
{
|
||||
while(retries < MAX_RETRIES && success == -1) {
|
||||
success = UAVTalkSendObject(ev.obj, ev.instId, metadata.ackRequired, REQ_TIMEOUT_MS); // call blocks until ack is received or timeout
|
||||
++retries;
|
||||
}
|
||||
}
|
||||
else if (ev.event == EV_UPDATE_REQ)
|
||||
{
|
||||
} 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
|
||||
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 metadata object then make necessary telemetry updates
|
||||
if (UAVObjIsMetaobject(ev.obj))
|
||||
{
|
||||
if(UAVObjIsMetaobject(ev.obj)) {
|
||||
updateObject(UAVObjGetLinkedObj(ev.obj)); // linked object will be the actual object the metadata are for
|
||||
}
|
||||
}
|
||||
@ -209,18 +190,27 @@ void telemetryTask()
|
||||
/**
|
||||
* Transmit data buffer to the modem or USB port.
|
||||
* \param[in] data Data buffer to send
|
||||
* \param[in] length Length of buffer
|
||||
* \param[in] length Length of buffer
|
||||
* \return 0 Success
|
||||
*/
|
||||
int32_t transmitData(uint8_t* data, int32_t length)
|
||||
static int32_t transmitData(uint8_t* data, int32_t length)
|
||||
{
|
||||
// TODO: Send data to communication port
|
||||
return 0;
|
||||
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)
|
||||
* Event handler for periodic object updates (called by the event dispatcher)
|
||||
*/
|
||||
void periodicEventHandler(UAVObjEvent* ev)
|
||||
static void periodicEventHandler(UAVObjEvent* ev)
|
||||
{
|
||||
// Push event to the telemetry queue
|
||||
xQueueSend(queue, ev, 0); // do not wait if queue is full
|
||||
@ -232,7 +222,7 @@ void periodicEventHandler(UAVObjEvent* ev)
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t addObject(UAVObjHandle obj)
|
||||
static int32_t addObject(UAVObjHandle obj)
|
||||
{
|
||||
UAVObjEvent ev;
|
||||
|
||||
@ -250,7 +240,7 @@ int32_t addObject(UAVObjHandle obj)
|
||||
* \return 0 Success
|
||||
* \return -1 Failure
|
||||
*/
|
||||
int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
|
||||
static int32_t setUpdatePeriod(UAVObjHandle obj, int32_t updatePeriodMs)
|
||||
{
|
||||
UAVObjEvent ev;
|
||||
|
||||
|
||||
Loading…
x
Reference in New Issue
Block a user