mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-12 20:08:48 +01:00
8c8224c122
add code to shortcircuit formatting a packet if it will not fit.
379 lines
12 KiB
C
379 lines
12 KiB
C
/**
|
|
******************************************************************************
|
|
* @addtogroup OpenPilotModules OpenPilot Modules
|
|
* @{
|
|
* @addtogroup TelemetryModule Telemetry Module
|
|
* @brief Main telemetry module
|
|
* Starts three tasks (RX, TX, and priority TX) that watch event queues
|
|
* and handle all the telemetry of the UAVobjects
|
|
* @{
|
|
*
|
|
* @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"
|
|
#include "overosync.h"
|
|
#include "overosyncstats.h"
|
|
#include "systemstats.h"
|
|
|
|
// Private constants
|
|
#define OVEROSYNC_PACKET_SIZE 1024
|
|
#define MAX_QUEUE_SIZE 40
|
|
#define STACK_SIZE_BYTES 4096
|
|
#define TASK_PRIORITY (tskIDLE_PRIORITY + 0)
|
|
|
|
// Private types
|
|
|
|
// Private variables
|
|
static xQueueHandle queue;
|
|
static UAVTalkConnection uavTalkCon;
|
|
static xTaskHandle overoSyncTaskHandle;
|
|
volatile bool buffer_swap_failed;
|
|
volatile uint32_t buffer_swap_timeval;
|
|
|
|
// Private functions
|
|
static void overoSyncTask(void *parameters);
|
|
static int32_t packData(uint8_t * data, int32_t length);
|
|
static int32_t transmitData();
|
|
static void transmitDataDone(bool crc_ok, uint8_t crc_val);
|
|
static void registerObject(UAVObjHandle obj);
|
|
|
|
// External variables
|
|
extern int32_t pios_spi_overo_id;
|
|
|
|
struct dma_transaction {
|
|
uint8_t tx_buffer[OVEROSYNC_PACKET_SIZE] __attribute__ ((aligned(4)));
|
|
uint8_t rx_buffer[OVEROSYNC_PACKET_SIZE] __attribute__ ((aligned(4)));
|
|
};
|
|
|
|
struct overosync {
|
|
struct dma_transaction transactions[2];
|
|
uint32_t active_transaction_id;
|
|
uint32_t loading_transaction_id;
|
|
xSemaphoreHandle transaction_lock;
|
|
xSemaphoreHandle buffer_lock;
|
|
volatile bool transaction_done;
|
|
uint32_t sent_bytes;
|
|
uint32_t write_pointer;
|
|
uint32_t sent_objects;
|
|
uint32_t failed_objects;
|
|
uint32_t received_objects;
|
|
uint32_t framesync_error;
|
|
};
|
|
|
|
struct overosync *overosync;
|
|
|
|
static void PIOS_OVERO_IRQHandler();
|
|
|
|
static const struct pios_exti_cfg pios_exti_overo_cfg __exti_config = {
|
|
.vector = PIOS_OVERO_IRQHandler,
|
|
.line = EXTI_Line15,
|
|
.pin = {
|
|
.gpio = GPIOA,
|
|
.init = {
|
|
.GPIO_Pin = GPIO_Pin_15,
|
|
.GPIO_Speed = GPIO_Speed_100MHz,
|
|
.GPIO_Mode = GPIO_Mode_IN,
|
|
.GPIO_OType = GPIO_OType_OD,
|
|
.GPIO_PuPd = GPIO_PuPd_NOPULL,
|
|
},
|
|
},
|
|
.irq = {
|
|
.init = {
|
|
.NVIC_IRQChannel = EXTI15_10_IRQn,
|
|
.NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_MID,
|
|
.NVIC_IRQChannelSubPriority = 0,
|
|
.NVIC_IRQChannelCmd = ENABLE,
|
|
},
|
|
},
|
|
.exti = {
|
|
.init = {
|
|
.EXTI_Line = EXTI_Line15, // matches above GPIO pin
|
|
.EXTI_Mode = EXTI_Mode_Interrupt,
|
|
.EXTI_Trigger = EXTI_Trigger_Rising,
|
|
.EXTI_LineCmd = ENABLE,
|
|
},
|
|
},
|
|
};
|
|
|
|
/**
|
|
* On the rising edge of NSS schedule a new transaction. This cannot be
|
|
* done by the DMA complete because there is 150 us between that and the
|
|
* Overo deasserting the CS line. We don't want to spin that long in an
|
|
* isr
|
|
*/
|
|
void PIOS_OVERO_IRQHandler()
|
|
{
|
|
// transmitData must not block to get semaphore for when we get out of
|
|
// frame and transaction is still running here. -1 indicates the transaction
|
|
// semaphore is blocked and we are still in a transaction, thus a framesync
|
|
// error occurred. This shouldn't happen. Race condition?
|
|
if(transmitData() == -1)
|
|
overosync->framesync_error++;
|
|
}
|
|
|
|
/**
|
|
* Initialise the telemetry module
|
|
* \return -1 if initialisation failed
|
|
* \return 0 on success
|
|
*/
|
|
int32_t OveroSyncInitialize(void)
|
|
{
|
|
if(pios_spi_overo_id == 0)
|
|
return -1;
|
|
|
|
OveroSyncStatsInitialize();
|
|
|
|
PIOS_EXTI_Init(&pios_exti_overo_cfg);
|
|
// Create object queues
|
|
queue = xQueueCreate(MAX_QUEUE_SIZE, sizeof(UAVObjEvent));
|
|
|
|
// Initialise UAVTalk
|
|
uavTalkCon = UAVTalkInitialize(&packData);
|
|
|
|
return 0;
|
|
}
|
|
|
|
/**
|
|
* Initialise the telemetry module
|
|
* \return -1 if initialisation failed
|
|
* \return 0 on success
|
|
*/
|
|
int32_t OveroSyncStart(void)
|
|
{
|
|
if(pios_spi_overo_id == 0)
|
|
return -1;
|
|
|
|
overosync = (struct overosync *) pvPortMalloc(sizeof(*overosync));
|
|
if(overosync == NULL)
|
|
return -1;
|
|
|
|
overosync->transaction_lock = xSemaphoreCreateMutex();
|
|
if(overosync->transaction_lock == NULL)
|
|
return -1;
|
|
|
|
overosync->buffer_lock = xSemaphoreCreateMutex();
|
|
if(overosync->buffer_lock == NULL)
|
|
return -1;
|
|
|
|
overosync->active_transaction_id = 0;
|
|
overosync->loading_transaction_id = 0;
|
|
overosync->write_pointer = 0;
|
|
overosync->sent_bytes = 0;
|
|
overosync->framesync_error = 0;
|
|
|
|
// Process all registered objects and connect queue for updates
|
|
UAVObjIterate(®isterObject);
|
|
|
|
// Start telemetry tasks
|
|
xTaskCreate(overoSyncTask, (signed char *)"OveroSync", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY, &overoSyncTaskHandle);
|
|
|
|
TaskMonitorAdd(TASKINFO_RUNNING_OVEROSYNC, overoSyncTaskHandle);
|
|
|
|
return 0;
|
|
}
|
|
|
|
MODULE_INITCALL(OveroSyncInitialize, OveroSyncStart)
|
|
|
|
/**
|
|
* 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
|
|
*/
|
|
static void registerObject(UAVObjHandle obj)
|
|
{
|
|
int32_t eventMask;
|
|
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);
|
|
}
|
|
|
|
/**
|
|
* Telemetry transmit task, regular priority
|
|
*
|
|
* Logic: We need to double buffer the DMA transfers. Pack the buffer until either
|
|
* 1) it is full (and then we should record the number of missed events then)
|
|
* 2) the current transaction is done (we should immediately schedule since we are slave)
|
|
* when done packing the buffer we should call PIOS_SPI_TransferBlock, change the active buffer
|
|
* and then take the semaphrore
|
|
*/
|
|
static void overoSyncTask(void *parameters)
|
|
{
|
|
UAVObjEvent ev;
|
|
|
|
// Kick off SPI transfers (once one is completed another will automatically transmit)
|
|
overosync->transaction_done = true;
|
|
overosync->sent_objects = 0;
|
|
overosync->failed_objects = 0;
|
|
overosync->received_objects = 0;
|
|
|
|
portTickType lastUpdateTime = xTaskGetTickCount();
|
|
portTickType updateTime;
|
|
|
|
// Loop forever
|
|
while (1) {
|
|
// Wait for queue message
|
|
if (xQueueReceive(queue, &ev, portMAX_DELAY) == pdTRUE) {
|
|
|
|
// Check it will fit before packetizing
|
|
if ((overosync->write_pointer + UAVObjGetNumBytes(ev.obj) + 12) >=
|
|
sizeof(overosync->transactions[overosync->loading_transaction_id].tx_buffer)) {
|
|
overosync->failed_objects ++;
|
|
} else {
|
|
// Process event. This calls transmitData
|
|
UAVTalkSendObject(uavTalkCon, ev.obj, ev.instId, false, 0);
|
|
}
|
|
|
|
updateTime = xTaskGetTickCount();
|
|
if(((portTickType) (updateTime - lastUpdateTime)) > 1000) {
|
|
// Update stats. This will trigger a local send event too
|
|
OveroSyncStatsData syncStats;
|
|
syncStats.Send = overosync->sent_bytes;
|
|
syncStats.Received = 0;
|
|
syncStats.Connected = syncStats.Send > 500 ? OVEROSYNCSTATS_CONNECTED_TRUE : OVEROSYNCSTATS_CONNECTED_FALSE;
|
|
syncStats.DroppedUpdates = overosync->failed_objects;
|
|
OveroSyncStatsSet(&syncStats);
|
|
overosync->failed_objects = 0;
|
|
overosync->sent_bytes = 0;
|
|
lastUpdateTime = updateTime;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
static void transmitDataDone(bool crc_ok, uint8_t crc_val)
|
|
{
|
|
uint8_t *rx_buffer;
|
|
static signed portBASE_TYPE xHigherPriorityTaskWoken;
|
|
|
|
rx_buffer = overosync->transactions[overosync->active_transaction_id].rx_buffer;
|
|
|
|
// Release the semaphore and start another transaction (which grabs semaphore again but then
|
|
// returns instantly). Because this is called by the DMA ISR we need to be aware of context
|
|
// switches.
|
|
xSemaphoreGiveFromISR(overosync->transaction_lock, &xHigherPriorityTaskWoken);
|
|
portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
|
|
|
|
overosync->transaction_done = true;
|
|
|
|
// Parse the data from overo
|
|
for (uint32_t i = 0; rx_buffer[0] != 0 && i < sizeof(rx_buffer) ; i++)
|
|
UAVTalkProcessInputStream(uavTalkCon, rx_buffer[i]);
|
|
}
|
|
|
|
/**
|
|
* Transmit data buffer to the modem or USB port.
|
|
* \param[in] data Data buffer to send
|
|
* \param[in] length Length of buffer
|
|
* \return -1 on failure
|
|
* \return number of bytes transmitted on success
|
|
*/
|
|
uint32_t too_long = 0;
|
|
static int32_t packData(uint8_t * data, int32_t length)
|
|
{
|
|
uint8_t *tx_buffer;
|
|
|
|
portTickType tickTime = xTaskGetTickCount();
|
|
|
|
// Get the lock for manipulating the buffer
|
|
xSemaphoreTake(overosync->buffer_lock, portMAX_DELAY);
|
|
|
|
// Check this packet will fit
|
|
if ((overosync->write_pointer + length + sizeof(tickTime)) >
|
|
sizeof(overosync->transactions[overosync->loading_transaction_id].tx_buffer)) {
|
|
overosync->failed_objects ++;
|
|
xSemaphoreGive(overosync->buffer_lock);
|
|
return -1;
|
|
}
|
|
|
|
// Get offset into buffer and copy contents
|
|
tx_buffer = overosync->transactions[overosync->loading_transaction_id].tx_buffer +
|
|
overosync->write_pointer;
|
|
memcpy(tx_buffer, &tickTime, sizeof(tickTime));
|
|
memcpy(tx_buffer + sizeof(tickTime),data,length);
|
|
overosync->write_pointer += length + sizeof(tickTime);
|
|
overosync->sent_bytes += length;
|
|
overosync->sent_objects++;
|
|
|
|
xSemaphoreGive(overosync->buffer_lock);
|
|
|
|
// When the NSS line rises while we are packing data then a transaction doesn't start
|
|
// because that means we will be here very shortly afterwards (priority of task making that
|
|
// not always perfectly true) schedule the transaction here.
|
|
if (buffer_swap_failed && (PIOS_DELAY_DiffuS(buffer_swap_timeval) < 50)) {
|
|
buffer_swap_failed = false;
|
|
transmitData();
|
|
} else if (buffer_swap_failed) {
|
|
buffer_swap_failed = false;
|
|
too_long++;
|
|
}
|
|
|
|
return length;
|
|
}
|
|
|
|
static int32_t transmitData()
|
|
{
|
|
uint8_t *tx_buffer, *rx_buffer;
|
|
static signed portBASE_TYPE xHigherPriorityTaskWoken;
|
|
|
|
// Get this lock first so we don't swap buffers and then fail
|
|
// to start
|
|
if (xSemaphoreTake(overosync->transaction_lock, 0) == pdFALSE)
|
|
return -1;
|
|
|
|
// Get lock to manipulate buffers
|
|
if(xSemaphoreTake(overosync->buffer_lock, 0) == pdFALSE) {
|
|
xSemaphoreGiveFromISR(overosync->transaction_lock, &xHigherPriorityTaskWoken);
|
|
portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
|
|
buffer_swap_failed = true;
|
|
buffer_swap_timeval = PIOS_DELAY_GetRaw();
|
|
return -2;
|
|
}
|
|
|
|
overosync->transaction_done = false;
|
|
|
|
// Swap buffers
|
|
overosync->active_transaction_id = overosync->loading_transaction_id;
|
|
overosync->loading_transaction_id = (overosync->loading_transaction_id + 1) %
|
|
NELEMENTS(overosync->transactions);
|
|
|
|
tx_buffer = overosync->transactions[overosync->active_transaction_id].tx_buffer;
|
|
rx_buffer = overosync->transactions[overosync->active_transaction_id].rx_buffer;
|
|
|
|
// Prepare the new loading buffer
|
|
memset(overosync->transactions[overosync->loading_transaction_id].tx_buffer, 0xff,
|
|
sizeof(overosync->transactions[overosync->loading_transaction_id].tx_buffer));
|
|
overosync->write_pointer = 0;
|
|
|
|
xSemaphoreGiveFromISR(overosync->buffer_lock, &xHigherPriorityTaskWoken);
|
|
portEND_SWITCHING_ISR(xHigherPriorityTaskWoken);
|
|
|
|
return PIOS_SPI_TransferBlock(pios_spi_overo_id, (uint8_t *) tx_buffer, (uint8_t *) rx_buffer, sizeof(overosync->transactions[overosync->active_transaction_id].tx_buffer), &transmitDataDone) == 0 ? 0 : -3;
|
|
}
|
|
|
|
/**
|
|
* @}
|
|
* @}
|
|
*/
|