mirror of
https://bitbucket.org/librepilot/librepilot.git
synced 2024-12-01 09:24:10 +01:00
d9ec6454c2
Previously it was polling too fast and the PipXStatus was incorrect as a result. Now make sure the stats loop only runs at 500 ms.
553 lines
16 KiB
C
553 lines
16 KiB
C
/**
|
|
******************************************************************************
|
|
* @addtogroup OpenPilotModules OpenPilot Modules
|
|
* @{
|
|
* @addtogroup Radio Input / Output Module
|
|
* @brief Read and Write packets from/to a radio device.
|
|
* @{
|
|
*
|
|
* @file radio.c
|
|
* @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012.
|
|
* @brief Bridges selected Com Port to the COM VCP emulated serial port
|
|
* @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 <gcsreceiver.h>
|
|
#include <hwsettings.h>
|
|
#include <pipxsettings.h>
|
|
#include <pipxstatus.h>
|
|
#include <packet_handler.h>
|
|
#include <pios_com_priv.h>
|
|
#include <pios_rfm22b_priv.h>
|
|
#include <radio.h>
|
|
|
|
// ****************
|
|
// Private constants
|
|
|
|
#define STACK_SIZE_BYTES 150
|
|
#define TASK_PRIORITY (tskIDLE_PRIORITY + 2)
|
|
#define PACKET_QUEUE_SIZE PIOS_PH_WIN_SIZE
|
|
#define MAX_PORT_DELAY 200
|
|
#define STATS_UPDATE_PERIOD_MS 500
|
|
#define RADIOSTATS_UPDATE_PERIOD_MS 250
|
|
#define MAX_LOST_CONTACT_TIME 4
|
|
|
|
#ifndef LINK_LED_ON
|
|
#define LINK_LED_ON
|
|
#define LINK_LED_OFF
|
|
#endif
|
|
|
|
// ****************
|
|
// Private types
|
|
|
|
typedef struct {
|
|
uint32_t pairID;
|
|
uint16_t retries;
|
|
uint16_t errors;
|
|
uint16_t uavtalk_errors;
|
|
uint16_t resets;
|
|
uint16_t dropped;
|
|
int8_t rssi;
|
|
uint8_t lastContact;
|
|
} PairStats;
|
|
|
|
typedef struct {
|
|
|
|
// The task handles.
|
|
xTaskHandle radioReceiveTaskHandle;
|
|
xTaskHandle radioStatusTaskHandle;
|
|
xTaskHandle sendPacketTaskHandle;
|
|
|
|
// Queue handles.
|
|
xQueueHandle radioPacketQueue;
|
|
|
|
// Error statistics.
|
|
uint32_t radioTxErrors;
|
|
uint32_t radioRxErrors;
|
|
uint32_t packetErrors;
|
|
uint16_t txBytes;
|
|
uint16_t rxBytes;
|
|
|
|
// External error statistics
|
|
uint32_t droppedPackets;
|
|
uint32_t comTxRetries;
|
|
uint32_t UAVTalkErrors;
|
|
|
|
// The destination ID
|
|
uint32_t destination_id;
|
|
|
|
// Track other radios that are in range.
|
|
PairStats pairStats[PIPXSTATUS_PAIRIDS_NUMELEM];
|
|
|
|
// The RSSI of the last packet received.
|
|
int8_t RSSI;
|
|
|
|
} RadioData;
|
|
|
|
// ****************
|
|
// Private functions
|
|
|
|
static void radioReceiveTask(void *parameters);
|
|
static void radioStatusTask(void *parameters);
|
|
static void sendPacketTask(void *parameters);
|
|
static void StatusHandler(PHStatusPacketHandle p, int8_t rssi, int8_t afc);
|
|
static int32_t transmitPacket(PHPacketHandle packet);
|
|
static void PPMHandler(uint16_t *channels);
|
|
|
|
// ****************
|
|
// Private variables
|
|
|
|
static RadioData *data = 0;
|
|
|
|
// ****************
|
|
// Global variables
|
|
uint32_t pios_rfm22b_id = 0;
|
|
uint32_t pios_com_rfm22b_id = 0;
|
|
uint32_t pios_packet_handler = 0;
|
|
extern struct pios_rfm22b_cfg pios_rfm22b_cfg;
|
|
|
|
|
|
/**
|
|
* Start the module
|
|
* \return -1 if initialisation failed
|
|
* \return 0 on success
|
|
*/
|
|
static int32_t RadioStart(void)
|
|
{
|
|
if (!data)
|
|
return -1;
|
|
|
|
// Start the tasks.
|
|
xTaskCreate(radioReceiveTask, (signed char *)"RadioReceive", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioReceiveTaskHandle));
|
|
xTaskCreate(radioStatusTask, (signed char *)"RadioStatus", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioStatusTaskHandle));
|
|
xTaskCreate(sendPacketTask, (signed char *)"SendPacket", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->sendPacketTaskHandle));
|
|
|
|
// Install the monitors
|
|
TaskMonitorAdd(TASKINFO_RUNNING_MODEMRX, data->radioReceiveTaskHandle);
|
|
TaskMonitorAdd(TASKINFO_RUNNING_MODEMTX, data->sendPacketTaskHandle);
|
|
TaskMonitorAdd(TASKINFO_RUNNING_MODEMSTAT, data->radioStatusTaskHandle);
|
|
|
|
// Register the watchdog timers.
|
|
#ifdef PIOS_WDG_RADIORECEIVE
|
|
PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORECEIVE);
|
|
#endif /* PIOS_WDG_RADIORECEIVE */
|
|
|
|
return 0;
|
|
}
|
|
|
|
|
|
/**
|
|
* Initialise the module
|
|
* \return -1 if initialisation failed
|
|
* \return 0 on success
|
|
*/
|
|
static int32_t RadioInitialize(void)
|
|
{
|
|
|
|
// See if this module is enabled.
|
|
#ifndef RADIO_BUILTIN
|
|
HwSettingsInitialize();
|
|
uint8_t optionalModules[HWSETTINGS_OPTIONALMODULES_NUMELEM];
|
|
HwSettingsOptionalModulesGet(optionalModules);
|
|
if (optionalModules[HWSETTINGS_OPTIONALMODULES_RADIO] != HWSETTINGS_OPTIONALMODULES_ENABLED)
|
|
return -1;
|
|
#endif
|
|
|
|
// Initalize out UAVOs
|
|
PipXSettingsInitialize();
|
|
PipXStatusInitialize();
|
|
|
|
PipXSettingsData pipxSettings;
|
|
PipXSettingsGet(&pipxSettings);
|
|
|
|
/* Retrieve hardware settings. */
|
|
pios_rfm22b_cfg.frequencyHz = pipxSettings.Frequency;
|
|
pios_rfm22b_cfg.RFXtalCap = pipxSettings.FrequencyCalibration;
|
|
switch (pipxSettings.RFSpeed)
|
|
{
|
|
case PIPXSETTINGS_RFSPEED_2400:
|
|
pios_rfm22b_cfg.maxRFBandwidth = 2000;
|
|
break;
|
|
case PIPXSETTINGS_RFSPEED_4800:
|
|
pios_rfm22b_cfg.maxRFBandwidth = 4000;
|
|
break;
|
|
case PIPXSETTINGS_RFSPEED_9600:
|
|
pios_rfm22b_cfg.maxRFBandwidth = 9600;
|
|
break;
|
|
case PIPXSETTINGS_RFSPEED_19200:
|
|
pios_rfm22b_cfg.maxRFBandwidth = 19200;
|
|
break;
|
|
case PIPXSETTINGS_RFSPEED_38400:
|
|
pios_rfm22b_cfg.maxRFBandwidth = 32000;
|
|
break;
|
|
case PIPXSETTINGS_RFSPEED_57600:
|
|
pios_rfm22b_cfg.maxRFBandwidth = 64000;
|
|
break;
|
|
case PIPXSETTINGS_RFSPEED_115200:
|
|
pios_rfm22b_cfg.maxRFBandwidth = 128000;
|
|
break;
|
|
}
|
|
switch (pipxSettings.MaxRFPower)
|
|
{
|
|
case PIPXSETTINGS_MAXRFPOWER_125:
|
|
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_0;
|
|
break;
|
|
case PIPXSETTINGS_MAXRFPOWER_16:
|
|
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_1;
|
|
break;
|
|
case PIPXSETTINGS_MAXRFPOWER_316:
|
|
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_2;
|
|
break;
|
|
case PIPXSETTINGS_MAXRFPOWER_63:
|
|
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_3;
|
|
break;
|
|
case PIPXSETTINGS_MAXRFPOWER_126:
|
|
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_4;
|
|
break;
|
|
case PIPXSETTINGS_MAXRFPOWER_25:
|
|
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_5;
|
|
break;
|
|
case PIPXSETTINGS_MAXRFPOWER_50:
|
|
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_6;
|
|
break;
|
|
case PIPXSETTINGS_MAXRFPOWER_100:
|
|
pios_rfm22b_cfg.maxTxPower = RFM22_tx_pwr_txpow_7;
|
|
break;
|
|
}
|
|
|
|
/* Initalize the RFM22B radio COM device. */
|
|
{
|
|
if (PIOS_RFM22B_Init(&pios_rfm22b_id, &pios_rfm22b_cfg)) {
|
|
PIOS_Assert(0);
|
|
}
|
|
uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_RX_BUF_LEN);
|
|
uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_RFM22B_RF_TX_BUF_LEN);
|
|
PIOS_Assert(rx_buffer);
|
|
PIOS_Assert(tx_buffer);
|
|
if (PIOS_COM_Init(&pios_com_rfm22b_id, &pios_rfm22b_com_driver, pios_rfm22b_id,
|
|
rx_buffer, PIOS_COM_RFM22B_RF_RX_BUF_LEN,
|
|
tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) {
|
|
PIOS_Assert(0);
|
|
}
|
|
}
|
|
|
|
// Initialize the packet handler
|
|
PacketHandlerConfig pios_ph_cfg = {
|
|
.default_destination_id = 0xffffffff, // Broadcast
|
|
.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id),
|
|
.win_size = PIOS_PH_WIN_SIZE,
|
|
.max_connections = PIOS_PH_MAX_CONNECTIONS,
|
|
};
|
|
pios_packet_handler = PHInitialize(&pios_ph_cfg);
|
|
|
|
// allocate and initialize the static data storage only if module is enabled
|
|
data = (RadioData *)pvPortMalloc(sizeof(RadioData));
|
|
if (!data)
|
|
return -1;
|
|
|
|
// Initialize the statistics.
|
|
data->radioTxErrors = 0;
|
|
data->radioRxErrors = 0;
|
|
data->packetErrors = 0;
|
|
data->droppedPackets = 0;
|
|
data->comTxRetries = 0;
|
|
data->UAVTalkErrors = 0;
|
|
data->RSSI = -127;
|
|
|
|
// Initialize the detected device statistics.
|
|
for (uint8_t i = 0; i < PIPXSTATUS_PAIRIDS_NUMELEM; ++i)
|
|
{
|
|
data->pairStats[i].pairID = 0;
|
|
data->pairStats[i].rssi = -127;
|
|
data->pairStats[i].retries = 0;
|
|
data->pairStats[i].errors = 0;
|
|
data->pairStats[i].uavtalk_errors = 0;
|
|
data->pairStats[i].resets = 0;
|
|
data->pairStats[i].dropped = 0;
|
|
data->pairStats[i].lastContact = 0;
|
|
}
|
|
// The first slot is reserved for our current pairID
|
|
PipXSettingsPairIDGet(&(data->pairStats[0].pairID));
|
|
data->destination_id = data->pairStats[0].pairID ? data->pairStats[0].pairID : 0xffffffff;
|
|
|
|
// Create the packet queue.
|
|
data->radioPacketQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(PHPacketHandle));
|
|
|
|
// Register the callbacks with the packet handler
|
|
PHRegisterStatusHandler(pios_packet_handler, StatusHandler);
|
|
PHRegisterOutputStream(pios_packet_handler, transmitPacket);
|
|
PHRegisterPPMHandler(pios_packet_handler, PPMHandler);
|
|
|
|
return 0;
|
|
}
|
|
MODULE_INITCALL(RadioInitialize, RadioStart)
|
|
|
|
/**
|
|
* The task that receives packets from the radio.
|
|
*/
|
|
static void radioReceiveTask(void *parameters)
|
|
{
|
|
PHPacketHandle p = NULL;
|
|
|
|
/* Handle radio -> usart/usb direction */
|
|
while (1) {
|
|
uint32_t rx_bytes;
|
|
|
|
#ifdef PIOS_WDG_RADIORECEIVE
|
|
// Update the watchdog timer.
|
|
PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORECEIVE);
|
|
#endif /* PIOS_INCLUDE_WDG */
|
|
|
|
PIOS_RFM22_processPendingISR(5);
|
|
|
|
// Get a RX packet from the packet handler if required.
|
|
if (p == NULL)
|
|
p = PHGetRXPacket(pios_packet_handler);
|
|
|
|
if(p == NULL) {
|
|
// Wait a bit for a packet to come available.
|
|
vTaskDelay(5);
|
|
continue;
|
|
}
|
|
|
|
// Receive data from the radio port
|
|
rx_bytes = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, (uint8_t*)p, PIOS_PH_MAX_PACKET, MAX_PORT_DELAY);
|
|
if(rx_bytes == 0)
|
|
continue;
|
|
data->rxBytes += rx_bytes;
|
|
|
|
// Verify that the packet is valid and pass it on.
|
|
bool rx_error = PHVerifyPacket(pios_packet_handler, p, rx_bytes) < 0;
|
|
if(rx_error)
|
|
data->packetErrors++;
|
|
PHReceivePacket(pios_packet_handler, p, rx_error);
|
|
p = NULL;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Send packets to the radio.
|
|
*/
|
|
static void sendPacketTask(void *parameters)
|
|
{
|
|
PHPacketHandle p;
|
|
|
|
// Loop forever
|
|
while (1) {
|
|
#ifdef PIOS_INCLUDE_WDG
|
|
// Update the watchdog timer.
|
|
//PIOS_WDG_UpdateFlag(PIOS_WDG_SENDPACKET);
|
|
#endif /* PIOS_INCLUDE_WDG */
|
|
// Wait for a packet on the queue.
|
|
if (xQueueReceive(data->radioPacketQueue, &p, MAX_PORT_DELAY) == pdTRUE) {
|
|
PIOS_COM_SendBuffer(PIOS_COM_RADIO, (uint8_t*)p, PH_PACKET_SIZE(p));
|
|
PHReleaseTXPacket(pios_packet_handler, p);
|
|
}
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Transmit a packet to the radio port.
|
|
* \param[in] buf Data buffer to send
|
|
* \param[in] length Length of buffer
|
|
* \return -1 on failure
|
|
* \return number of bytes transmitted on success
|
|
*/
|
|
static int32_t transmitPacket(PHPacketHandle p)
|
|
{
|
|
uint16_t len = PH_PACKET_SIZE(p);
|
|
data->txBytes += len;
|
|
if (xQueueSend(data->radioPacketQueue, &p, portMAX_DELAY) != pdTRUE)
|
|
return -1;
|
|
return len;
|
|
}
|
|
|
|
/**
|
|
* Receive a status packet
|
|
* \param[in] status The status structure
|
|
*/
|
|
static void StatusHandler(PHStatusPacketHandle status, int8_t rssi, int8_t afc)
|
|
{
|
|
uint32_t id = status->header.source_id;
|
|
bool found = false;
|
|
// Have we seen this device recently?
|
|
uint8_t id_idx = 0;
|
|
for ( ; id_idx < PIPXSTATUS_PAIRIDS_NUMELEM; ++id_idx)
|
|
if(data->pairStats[id_idx].pairID == id)
|
|
{
|
|
found = true;
|
|
break;
|
|
}
|
|
|
|
// If we have seen it, update the RSSI and reset the last contact couter
|
|
if(found)
|
|
{
|
|
data->pairStats[id_idx].rssi = rssi;
|
|
data->pairStats[id_idx].retries = status->retries;
|
|
data->pairStats[id_idx].errors = status->errors;
|
|
data->pairStats[id_idx].uavtalk_errors = status->uavtalk_errors;
|
|
data->pairStats[id_idx].resets = status->resets;
|
|
data->pairStats[id_idx].dropped = status->dropped;
|
|
data->pairStats[id_idx].lastContact = 0;
|
|
}
|
|
|
|
// If we haven't seen it, find a slot to put it in.
|
|
if (!found)
|
|
{
|
|
uint32_t pairID;
|
|
PipXSettingsPairIDGet(&pairID);
|
|
|
|
uint8_t min_idx = 0;
|
|
if(id != pairID)
|
|
{
|
|
int8_t min_rssi = data->pairStats[0].rssi;
|
|
for (id_idx = 1; id_idx < PIPXSTATUS_PAIRIDS_NUMELEM; ++id_idx)
|
|
{
|
|
if(data->pairStats[id_idx].rssi < min_rssi)
|
|
{
|
|
min_rssi = data->pairStats[id_idx].rssi;
|
|
min_idx = id_idx;
|
|
}
|
|
}
|
|
}
|
|
data->pairStats[min_idx].pairID = id;
|
|
data->pairStats[min_idx].rssi = rssi;
|
|
data->pairStats[min_idx].retries = status->retries;
|
|
data->pairStats[min_idx].errors = status->errors;
|
|
data->pairStats[min_idx].uavtalk_errors = status->uavtalk_errors;
|
|
data->pairStats[min_idx].resets = status->resets;
|
|
data->pairStats[min_idx].dropped = status->dropped;
|
|
data->pairStats[min_idx].lastContact = 0;
|
|
}
|
|
}
|
|
|
|
/**
|
|
* The stats update task.
|
|
*/
|
|
static void radioStatusTask(void *parameters)
|
|
{
|
|
static portTickType lastSysTime;
|
|
PHStatusPacket status_packet;
|
|
while (1) {
|
|
lastSysTime = xTaskGetTickCount();
|
|
|
|
PipXStatusData pipxStatus;
|
|
uint32_t pairID;
|
|
|
|
// Get object data
|
|
PipXStatusGet(&pipxStatus);
|
|
PipXSettingsPairIDGet(&pairID);
|
|
|
|
// Update the status
|
|
pipxStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id);
|
|
pipxStatus.Retries = data->comTxRetries;
|
|
pipxStatus.Errors = data->packetErrors;
|
|
pipxStatus.UAVTalkErrors = data->UAVTalkErrors;
|
|
pipxStatus.Dropped = data->droppedPackets;
|
|
pipxStatus.Resets = PIOS_RFM22B_Resets(pios_rfm22b_id);
|
|
pipxStatus.TXRate = (uint16_t)((float)(data->txBytes * 1000) / STATS_UPDATE_PERIOD_MS);
|
|
data->txBytes = 0;
|
|
pipxStatus.RXRate = (uint16_t)((float)(data->rxBytes * 1000) / STATS_UPDATE_PERIOD_MS);
|
|
data->rxBytes = 0;
|
|
pipxStatus.LinkState = PIPXSTATUS_LINKSTATE_DISCONNECTED;
|
|
pipxStatus.RSSI = data->RSSI;
|
|
LINK_LED_OFF;
|
|
|
|
// Update the potential pairing contacts
|
|
for (uint8_t i = 0; i < PIPXSTATUS_PAIRIDS_NUMELEM; ++i)
|
|
{
|
|
pipxStatus.PairIDs[i] = data->pairStats[i].pairID;
|
|
pipxStatus.PairSignalStrengths[i] = data->pairStats[i].rssi;
|
|
data->pairStats[i].lastContact++;
|
|
// Remove this device if it's stale.
|
|
if(data->pairStats[i].lastContact > MAX_LOST_CONTACT_TIME)
|
|
{
|
|
data->pairStats[i].pairID = 0;
|
|
data->pairStats[i].rssi = -127;
|
|
data->pairStats[i].retries = 0;
|
|
data->pairStats[i].errors = 0;
|
|
data->pairStats[i].uavtalk_errors = 0;
|
|
data->pairStats[i].resets = 0;
|
|
data->pairStats[i].dropped = 0;
|
|
data->pairStats[i].lastContact = 0;
|
|
}
|
|
// Add the paired devices statistics to ours.
|
|
if(pairID && (data->pairStats[i].pairID == pairID) && (data->pairStats[i].rssi > -127))
|
|
{
|
|
pipxStatus.Retries += data->pairStats[i].retries;
|
|
pipxStatus.Errors += data->pairStats[i].errors;
|
|
pipxStatus.UAVTalkErrors += data->pairStats[i].uavtalk_errors;
|
|
pipxStatus.Dropped += data->pairStats[i].dropped;
|
|
pipxStatus.Resets += data->pairStats[i].resets;
|
|
pipxStatus.Dropped += data->pairStats[i].dropped;
|
|
pipxStatus.LinkState = PIPXSTATUS_LINKSTATE_CONNECTED;
|
|
LINK_LED_ON;
|
|
}
|
|
}
|
|
|
|
// Update the object
|
|
PipXStatusSet(&pipxStatus);
|
|
|
|
// Broadcast the status.
|
|
{
|
|
static uint16_t cntr = 0;
|
|
if(cntr++ == RADIOSTATS_UPDATE_PERIOD_MS / STATS_UPDATE_PERIOD_MS)
|
|
{
|
|
// Queue the status message
|
|
status_packet.header.destination_id = 0xffffffff;
|
|
status_packet.header.type = PACKET_TYPE_STATUS;
|
|
status_packet.header.data_size = PH_STATUS_DATA_SIZE(&status_packet);
|
|
status_packet.header.source_id = pipxStatus.DeviceID;
|
|
status_packet.retries = data->comTxRetries;
|
|
status_packet.errors = data->packetErrors;
|
|
status_packet.uavtalk_errors = data->UAVTalkErrors;
|
|
status_packet.dropped = data->droppedPackets;
|
|
status_packet.resets = PIOS_RFM22B_Resets(pios_rfm22b_id);
|
|
PHPacketHandle sph = (PHPacketHandle)&status_packet;
|
|
PHTransmitPacket(PIOS_PACKET_HANDLER, sph);
|
|
cntr = 0;
|
|
}
|
|
}
|
|
|
|
portTickType timeSinceUpdate;
|
|
do {
|
|
PIOS_RFM22_processPendingISR(5);
|
|
timeSinceUpdate = xTaskGetTickCount() - lastSysTime;
|
|
}
|
|
while(timeSinceUpdate < STATS_UPDATE_PERIOD_MS / portTICK_RATE_MS);
|
|
}
|
|
}
|
|
|
|
/**
|
|
* Receive a ppm packet
|
|
* \param[in] channels The ppm channels
|
|
*/
|
|
static void PPMHandler(uint16_t *channels)
|
|
{
|
|
GCSReceiverData rcvr;
|
|
|
|
// Copy the receiver channels into the GCSReceiver object.
|
|
for (uint8_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; ++i)
|
|
rcvr.Channel[i] = channels[i];
|
|
|
|
// Set the GCSReceiverData object.
|
|
GCSReceiverSet(&rcvr);
|
|
}
|