From 1335ba263686ca5f4ce3b498ce7453452d21bcb7 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 12 Aug 2012 20:40:52 -0700 Subject: [PATCH 01/11] Turned off compile of osgearthview plugin. --- ground/openpilotgcs/src/plugins/plugins.pro | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/plugins.pro b/ground/openpilotgcs/src/plugins/plugins.pro index f441d52b3..4d7b1f11f 100644 --- a/ground/openpilotgcs/src/plugins/plugins.pro +++ b/ground/openpilotgcs/src/plugins/plugins.pro @@ -197,11 +197,11 @@ plugin_uavobjectutil.depends += plugin_uavobjects SUBDIRS += plugin_uavobjectutil # OSG Earth View plugin -plugin_osgearthview.subdir = osgearthview -plugin_osgearthview.depends = plugin_coreplugin -plugin_osgearthview.depends += plugin_uavobjects -plugin_osgearthview.depends += plugin_uavobjectwidgetutils -SUBDIRS += plugin_osgearthview +;; plugin_osgearthview.subdir = osgearthview +;; plugin_osgearthview.depends = plugin_coreplugin +;; plugin_osgearthview.depends += plugin_uavobjects +;; plugin_osgearthview.depends += plugin_uavobjectwidgetutils +;; SUBDIRS += plugin_osgearthview # Magic Waypoint gadget plugin_magicwaypoint.subdir = magicwaypoint From 171f91336c9062f9026a8c012d858b2343891ccb Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 12 Aug 2012 20:41:52 -0700 Subject: [PATCH 02/11] Removed assert that was no longer needef form PipX pios_board.c --- flight/PipXtreme/System/pios_board.c | 4 ---- 1 file changed, 4 deletions(-) diff --git a/flight/PipXtreme/System/pios_board.c b/flight/PipXtreme/System/pios_board.c index bcbe39ccd..3a2dd0544 100644 --- a/flight/PipXtreme/System/pios_board.c +++ b/flight/PipXtreme/System/pios_board.c @@ -188,10 +188,6 @@ void PIOS_Board_Init(void) { #if defined(PIOS_INCLUDE_USB_HID) - if (!usb_hid_present) { - PIOS_Assert(0); - } - /* Configure the usb HID port */ #if defined(PIOS_INCLUDE_COM) { From 2cd0733f7d6f22fd6165f98e66ec3507a12476aa Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Wed, 22 Aug 2012 06:45:44 -0700 Subject: [PATCH 03/11] Removed unecessary include of stm32f10x.h from aes.h --- flight/Libraries/inc/aes.h | 2 -- 1 file changed, 2 deletions(-) diff --git a/flight/Libraries/inc/aes.h b/flight/Libraries/inc/aes.h index 79e4f9211..725bc4414 100644 --- a/flight/Libraries/inc/aes.h +++ b/flight/Libraries/inc/aes.h @@ -2,8 +2,6 @@ #ifndef _AES_H_ #define _AES_H_ -#include "stm32f10x.h" - #define N_ROW 4 #define N_COL 4 #define N_BLOCK (N_ROW * N_COL) From 88c8cba0ff8c87b5cd6cdf4e3f57df2189563904 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Wed, 22 Aug 2012 06:46:51 -0700 Subject: [PATCH 04/11] Reduced size of temporaries to the correct size in rs.c --- flight/Libraries/rscode/rs.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/Libraries/rscode/rs.c b/flight/Libraries/rscode/rs.c index 5373b3ea0..39c014720 100644 --- a/flight/Libraries/rscode/rs.c +++ b/flight/Libraries/rscode/rs.c @@ -159,7 +159,7 @@ debug_check_syndrome (void) static void compute_genpoly (int nbytes, int genpoly[]) { - int i, tp[256], tp1[256]; + int i, tp[MAXDEG], tp1[MAXDEG]; /* multiply (x + a^n) for n = 1 to nbytes */ From 76907fe8f1cc5afe5d5a753ae01c75b264653b01 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Thu, 23 Aug 2012 07:02:51 -0700 Subject: [PATCH 05/11] Changed PipXStatus UAVO to update 1 / second. --- shared/uavobjectdefinition/pipxstatus.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/shared/uavobjectdefinition/pipxstatus.xml b/shared/uavobjectdefinition/pipxstatus.xml index c9dc2644b..3af8714c8 100755 --- a/shared/uavobjectdefinition/pipxstatus.xml +++ b/shared/uavobjectdefinition/pipxstatus.xml @@ -25,7 +25,7 @@ - + From 7ce0004c470c48025cc55be06aea0ee984dd104e Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Fri, 24 Aug 2012 19:25:02 -0700 Subject: [PATCH 06/11] Added include of stdint.h to aes.c --- flight/Libraries/aes.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/Libraries/aes.c b/flight/Libraries/aes.c index d381f157a..c21ce2f9e 100644 --- a/flight/Libraries/aes.c +++ b/flight/Libraries/aes.c @@ -1,7 +1,7 @@ // http://gladman.plushost.co.uk/oldsite/AES/index.php -//#include +#include #include "aes.h" From 34b433e074fd831f31ff903bde3f6bafc1d59064 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Fri, 24 Aug 2012 19:42:10 -0700 Subject: [PATCH 07/11] Added UAVTalkSendBuf function and added locking in UAVTalkSendAck and UAVTalkSendNack. --- flight/UAVTalk/inc/uavtalk.h | 1 + flight/UAVTalk/uavtalk.c | 52 ++++++++++++++++++++++++++++++++++-- 2 files changed, 51 insertions(+), 2 deletions(-) diff --git a/flight/UAVTalk/inc/uavtalk.h b/flight/UAVTalk/inc/uavtalk.h index ab47a1924..f7fc58cd1 100644 --- a/flight/UAVTalk/inc/uavtalk.h +++ b/flight/UAVTalk/inc/uavtalk.h @@ -56,6 +56,7 @@ int32_t UAVTalkSendObjectTimestamped(UAVTalkConnection connectionHandle, UAVObjH int32_t UAVTalkSendObjectRequest(UAVTalkConnection connection, UAVObjHandle obj, uint16_t instId, int32_t timeoutMs); int32_t UAVTalkSendAck(UAVTalkConnection connectionHandle, UAVObjHandle obj, uint16_t instId); int32_t UAVTalkSendNack(UAVTalkConnection connectionHandle, uint32_t objId); +int32_t UAVTalkSendBuf(UAVTalkConnection connectionHandle, uint8_t *buf, uint16_t len); UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connection, uint8_t rxbyte); UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connection, uint8_t rxbyte); void UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats); diff --git a/flight/UAVTalk/uavtalk.c b/flight/UAVTalk/uavtalk.c index 98a2081ac..b2f8515c8 100644 --- a/flight/UAVTalk/uavtalk.c +++ b/flight/UAVTalk/uavtalk.c @@ -570,7 +570,15 @@ int32_t UAVTalkSendAck(UAVTalkConnection connectionHandle, UAVObjHandle obj, uin UAVTalkConnectionData *connection; CHECKCONHANDLE(connectionHandle,connection,return -1); - return sendObject(connection, obj, instId, UAVTALK_TYPE_ACK); + // Lock + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + + int32_t ret = sendObject(connection, obj, instId, UAVTALK_TYPE_ACK); + + // Release lock + xSemaphoreGiveRecursive(connection->lock); + + return ret; } /** @@ -585,7 +593,47 @@ int32_t UAVTalkSendNack(UAVTalkConnection connectionHandle, uint32_t objId) UAVTalkConnectionData *connection; CHECKCONHANDLE(connectionHandle,connection,return -1); - return sendNack(connection, objId); + // Lock + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + + int32_t ret = sendNack(connection, objId); + + // Release lock + xSemaphoreGiveRecursive(connection->lock); + + return ret; +} + +/** + * Send a buffer containing a UAVTalk message through the telemetry link. + * This function locks the connection prior to sending. + * \param[in] connection UAVTalkConnection to be used + * \param[in] buf The data buffer containing the UAVTalk message + * \param[in] len The number of bytes to send from the data buffer + * \return 0 Success + * \return -1 Failure + */ +int32_t UAVTalkSendBuf(UAVTalkConnection connectionHandle, uint8_t *buf, uint16_t len) +{ + UAVTalkConnectionData *connection; + CHECKCONHANDLE(connectionHandle,connection, return -1); + + // Lock + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + + // Output the buffer + int32_t rc = (*connection->outStream)(buf, len); + + // Update stats + connection->stats.txBytes += len; + + // Release lock + xSemaphoreGiveRecursive(connection->lock); + + // Done + if (rc != len) + return -1; + return 0; } /** From fad32dcc29364d654fe6614683eac15a49bbfdea Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Fri, 24 Aug 2012 19:25:02 -0700 Subject: [PATCH 08/11] Split out the PipX radio interface from RadioComBridge module to a separate Radio module. Now the RadioComBridge module just routes messages between the com ports and handles sending/receiving the PipX UAVOs. --- flight/Libraries/inc/packet_handler.h | 12 +- flight/Libraries/packet_handler.c | 70 ++- flight/Modules/Radio/inc/radio.h | 39 ++ flight/Modules/Radio/radio.c | 539 ++++++++++++++++++ .../Modules/RadioComBridge/RadioComBridge.c | 409 ++----------- .../PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h | 38 +- flight/PiOS/Common/pios_rfm22b.c | 439 +++----------- flight/PiOS/inc/pios_rfm22b.h | 2 + flight/PiOS/inc/pios_rfm22b_priv.h | 2 + flight/PipXtreme/Makefile | 2 +- flight/PipXtreme/System/inc/pios_config.h | 5 + flight/PipXtreme/System/pios_board.c | 110 +--- .../board_hw_defs/pipxtreme/board_hw_defs.c | 64 ++- 13 files changed, 862 insertions(+), 869 deletions(-) create mode 100644 flight/Modules/Radio/inc/radio.h create mode 100644 flight/Modules/Radio/radio.c diff --git a/flight/Libraries/inc/packet_handler.h b/flight/Libraries/inc/packet_handler.h index 1e9f60807..1a4785842 100644 --- a/flight/Libraries/inc/packet_handler.h +++ b/flight/Libraries/inc/packet_handler.h @@ -30,6 +30,9 @@ #ifndef __PACKET_HANDLER_H__ #define __PACKET_HANDLER_H__ +#include +#include + // Public defines / macros #define PHPacketSize(p) ((uint8_t*)(p->data) + p->header.data_size - (uint8_t*)p) #define PHPacketSizeECC(p) ((uint8_t*)(p->data) + p->header.data_size + RS_ECC_NPARITY - (uint8_t*)p) @@ -71,7 +74,7 @@ typedef struct { #define PH_PPM_DATA_SIZE(p) ((uint8_t*)((p)->ecc) - (uint8_t*)(((PHPacketHandle)(p))->data)) typedef struct { PHPacketHeader header; - uint16_t channels[PIOS_RCVR_MAX_CHANNELS]; + uint16_t channels[GCSRECEIVER_CHANNEL_NUMELEM]; uint8_t ecc[RS_ECC_NPARITY]; } PHPpmPacket, *PHPpmPacketHandle; @@ -87,8 +90,10 @@ typedef struct { } PHStatusPacket, *PHStatusPacketHandle; typedef struct { - uint8_t winSize; - uint16_t maxConnections; + uint32_t default_destination_id; + uint32_t source_id; + uint16_t max_connections; + uint8_t win_size; } PacketHandlerConfig; typedef int32_t (*PHOutputStream)(PHPacketHandle packet); @@ -110,6 +115,7 @@ void PHReleaseRXPacket(PHInstHandle h, PHPacketHandle p); PHPacketHandle PHGetTXPacket(PHInstHandle h); void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p); uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p); +uint8_t PHTransmitData(PHInstHandle h, uint8_t *buf, uint16_t len); int32_t PHVerifyPacket(PHInstHandle h, PHPacketHandle p, uint16_t received_len); uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, bool rx_error); diff --git a/flight/Libraries/packet_handler.c b/flight/Libraries/packet_handler.c index f4e8e28d7..c417371d7 100644 --- a/flight/Libraries/packet_handler.c +++ b/flight/Libraries/packet_handler.c @@ -44,7 +44,6 @@ typedef struct { uint8_t rx_win_start; uint8_t rx_win_end; uint16_t tx_seq_id; - PHOutputStream stream; xSemaphoreHandle lock; PHOutputStream output_stream; PHDataHandler data_handler; @@ -75,13 +74,13 @@ PHInstHandle PHInitialize(PacketHandlerConfig *cfg) data->tx_seq_id = 0; // Allocate the packet windows - data->tx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.winSize); - data->rx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.winSize); + data->tx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.win_size); + data->rx_packets = pvPortMalloc(sizeof(PHPacket) * data->cfg.win_size); // Initialize the windows data->tx_win_start = data->tx_win_end = 0; data->rx_win_start = data->rx_win_end = 0; - for (uint8_t i = 0; i < data->cfg.winSize; ++i) + for (uint8_t i = 0; i < data->cfg.win_size; ++i) { data->tx_packets[i].header.type = PACKET_TYPE_NONE; data->rx_packets[i].header.type = PACKET_TYPE_NONE; @@ -93,6 +92,12 @@ PHInstHandle PHInitialize(PacketHandlerConfig *cfg) // Initialize the ECC library. initialize_ecc(); + // Initialize the handlers + data->output_stream = 0; + data->data_handler = 0; + data->status_handler = 0; + data->ppm_handler = 0; + // Return the structure. return (PHInstHandle)data; } @@ -172,7 +177,7 @@ PHPacketHandle PHGetTXPacket(PHInstHandle h) // Find a free packet. PHPacketHandle p = NULL; - for (uint8_t i = 0; i < data->cfg.winSize; ++i) + for (uint8_t i = 0; i < data->cfg.win_size; ++i) if (data->tx_packets[i].header.type == PACKET_TYPE_NONE) { p = data->tx_packets + i; @@ -205,7 +210,7 @@ void PHReleaseTXPacket(PHInstHandle h, PHPacketHandle p) // If this packet is at the start of the window, increment the start index. while ((data->tx_win_start != data->tx_win_end) && (data->tx_packets[data->tx_win_start].header.type == PACKET_TYPE_NONE)) - data->tx_win_start = (data->tx_win_start + 1) % data->cfg.winSize; + data->tx_win_start = (data->tx_win_start + 1) % data->cfg.win_size; // Release lock xSemaphoreGiveRecursive(data->lock); @@ -226,7 +231,7 @@ PHPacketHandle PHGetRXPacket(PHInstHandle h) // Find a free packet. PHPacketHandle p = NULL; - for (uint8_t i = 0; i < data->cfg.winSize; ++i) + for (uint8_t i = 0; i < data->cfg.win_size; ++i) if (data->rx_packets[i].header.type == PACKET_TYPE_NONE) { p = data->rx_packets + i; @@ -259,7 +264,7 @@ void PHReleaseRXPacket(PHInstHandle h, PHPacketHandle p) // If this packet is at the start of the window, increment the start index. while ((data->rx_win_start != data->rx_win_end) && (data->rx_packets[data->rx_win_start].header.type == PACKET_TYPE_NONE)) - data->rx_win_start = (data->rx_win_start + 1) % data->cfg.winSize; + data->rx_win_start = (data->rx_win_start + 1) % data->cfg.win_size; // Release lock xSemaphoreGiveRecursive(data->lock); @@ -280,19 +285,39 @@ uint8_t PHTransmitPacket(PHInstHandle h, PHPacketHandle p) if (!PHLTransmitPacket(data, p)) return 0; - // If this packet doesn't require an ACK, remove it from the TX window. - switch (p->header.type) { - case PACKET_TYPE_READY: - case PACKET_TYPE_NOTREADY: - case PACKET_TYPE_DATA: - case PACKET_TYPE_PPM: - PHReleaseTXPacket(h, p); - break; - } - return 1; } +/** + * Transmit a packet of data. + * \param[in] h The packet handler instance data pointer. + * \param[in] p A pointer to the data buffer. + * \param[in] len The length of the data buffer. + * \return 1 Success + * \return 0 Failure + */ +uint8_t PHTransmitData(PHInstHandle h, uint8_t *buf, uint16_t len) +{ + PHPacketDataHandle data = (PHPacketDataHandle)h; + + // Get a packet from the packet handler. + PHPacketHandle p = PHGetTXPacket(pios_packet_handler); + if (!p) + return 0; + + // Initialize the packet. + p->header.destination_id = data->cfg.default_destination_id; + p->header.source_id = data->cfg.source_id; + p->header.type = PACKET_TYPE_DATA; + p->header.data_size = len; + + // Copy the data into the packet. + memcpy(p->data, buf, len); + + // Send the packet. + return PHLTransmitPacket(data, p); +} + /** * Verify that a buffer contains a valid packet. * \param[in] h The packet handler instance data pointer. @@ -380,11 +405,11 @@ uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, bool rx_error) { // Find the packet ID in the TX buffer, and free it. unsigned int i = 0; - for (unsigned int i = 0; i < data->cfg.winSize; ++i) + for (unsigned int i = 0; i < data->cfg.win_size; ++i) if (data->tx_packets[i].header.tx_seq == p->header.rx_seq) PHReleaseTXPacket(h, data->tx_packets + i); #ifdef DEBUG_LEVEL - if (i == data->cfg.winSize) + if (i == data->cfg.win_size) DEBUG_PRINTF(1, "Error finding acked packet to release\n\r"); #endif } @@ -394,11 +419,11 @@ uint8_t PHReceivePacket(PHInstHandle h, PHPacketHandle p, bool rx_error) { // Resend the packet. unsigned int i = 0; - for ( ; i < data->cfg.winSize; ++i) + for ( ; i < data->cfg.win_size; ++i) if (data->tx_packets[i].header.tx_seq == p->header.rx_seq) PHLTransmitPacket(data, data->tx_packets + i); #ifdef DEBUG_LEVEL - if (i == data->cfg.winSize) + if (i == data->cfg.win_size) DEBUG_PRINTF(1, "Error finding acked packet to NACK\n\r"); DEBUG_PRINTF(1, "Resending after NACK\n\r"); #endif @@ -455,6 +480,7 @@ static uint8_t PHLTransmitPacket(PHPacketDataHandle data, PHPacketHandle p) encode_data((unsigned char*)p, PHPacketSize(p), (unsigned char*)p); // Transmit the packet using the output stream. + p->header.source_id = data->cfg.source_id; if(data->output_stream(p) == -1) return 0; diff --git a/flight/Modules/Radio/inc/radio.h b/flight/Modules/Radio/inc/radio.h new file mode 100644 index 000000000..58102e061 --- /dev/null +++ b/flight/Modules/Radio/inc/radio.h @@ -0,0 +1,39 @@ +/** + ****************************************************************************** + * @addtogroup OpenPilotModules OpenPilot Modules + * @{ + * @addtogroup Radio Input / Output Module + * @brief Read and Write packets from/to a radio device. + * @{ + * + * @file radio.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @brief Include file of the radio 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 + */ + +#ifndef RADIOMODULE_H +#define RADIOMODULE_H + +#endif // RADIOMODULE_H + +/** + * @} + * @} + */ diff --git a/flight/Modules/Radio/radio.c b/flight/Modules/Radio/radio.c new file mode 100644 index 000000000..6df334fae --- /dev/null +++ b/flight/Modules/Radio/radio.c @@ -0,0 +1,539 @@ +/** + ****************************************************************************** + * @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 +#include +#include +#include +#include +#include +#include +#include +#include + +// **************** +// 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)); + + // 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 */ + + // 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) +{ + PHStatusPacket status_packet; + + while (1) { + 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; + } + } + + // Delay until the next update period. + vTaskDelay(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); +} diff --git a/flight/Modules/RadioComBridge/RadioComBridge.c b/flight/Modules/RadioComBridge/RadioComBridge.c index eb674cbdd..572eacf4d 100644 --- a/flight/Modules/RadioComBridge/RadioComBridge.c +++ b/flight/Modules/RadioComBridge/RadioComBridge.c @@ -68,25 +68,13 @@ // **************** // 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 { uint32_t comPort; UAVTalkConnection UAVTalkCon; xQueueHandle sendQueue; xQueueHandle recvQueue; - xQueueHandle gcsQueue; uint16_t wdg; - bool checkHID; + bool isGCS; } UAVTalkComTaskParams; typedef struct { @@ -94,10 +82,7 @@ typedef struct { // The task handles. xTaskHandle GCSUAVTalkRecvTaskHandle; xTaskHandle UAVTalkRecvTaskHandle; - xTaskHandle radioReceiveTaskHandle; - xTaskHandle sendPacketTaskHandle; xTaskHandle UAVTalkSendTaskHandle; - xTaskHandle radioStatusTaskHandle; xTaskHandle transparentCommTaskHandle; xTaskHandle ppmInputTaskHandle; @@ -114,15 +99,8 @@ typedef struct { // Error statistics. uint32_t comTxErrors; uint32_t comTxRetries; - uint32_t comRxErrors; - uint32_t radioTxErrors; - uint32_t radioTxRetries; - uint32_t radioRxErrors; uint32_t UAVTalkErrors; - uint32_t packetErrors; uint32_t droppedPackets; - uint16_t txBytes; - uint16_t rxBytes; // The destination ID uint32_t destination_id; @@ -131,9 +109,6 @@ typedef struct { portTickType send_timeout; uint16_t min_packet_size; - // Track other radios that are in range. - PairStats pairStats[PIPXSTATUS_PAIRIDS_NUMELEM]; - // The RSSI of the last packet received. int8_t RSSI; @@ -155,19 +130,13 @@ typedef struct { // Private functions static void UAVTalkRecvTask(void *parameters); -static void radioReceiveTask(void *parameters); -static void sendPacketTask(void *parameters); static void UAVTalkSendTask(void *parameters); static void transparentCommTask(void * parameters); -static void radioStatusTask(void *parameters); static void ppmInputTask(void *parameters); static int32_t UAVTalkSendHandler(uint8_t * data, int32_t length); static int32_t GCSUAVTalkSendHandler(uint8_t * data, int32_t length); -static int32_t transmitPacket(PHPacketHandle packet); static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc); static void transmitData(uint32_t outputPort, uint8_t *buf, uint8_t len, bool checkHid); -static void StatusHandler(PHStatusPacketHandle p, int8_t rssi, int8_t afc); -static void PPMHandler(uint16_t *channels); static BufferedReadHandle BufferedReadInit(uint32_t com_port, uint16_t buffer_length); static bool BufferedRead(BufferedReadHandle h, uint8_t *value, uint32_t timeout_ms); static void BufferedReadSetCom(BufferedReadHandle h, uint32_t com_port); @@ -187,6 +156,11 @@ static RadioComBridgeData *data; static int32_t RadioComBridgeStart(void) { if(data) { + + // Register the callbacks with the packet handler + // This has to happen after the Radio module is initialized. + PHRegisterDataHandler(pios_packet_handler, receiveData); + // Start the primary tasks for receiving/sending UAVTalk packets from the GCS. xTaskCreate(UAVTalkRecvTask, (signed char *)"GCSUAVTalkRecvTask", STACK_SIZE_BYTES, (void*)&(data->gcs_uavtalk_params), TASK_PRIORITY + 2, &(data->GCSUAVTalkRecvTaskHandle)); xTaskCreate(UAVTalkSendTask, (signed char *)"GCSUAVTalkSendTask", STACK_SIZE_BYTES, (void*)&(data->gcs_uavtalk_params), TASK_PRIORITY+ 2, &(data->UAVTalkSendTaskHandle)); @@ -202,19 +176,15 @@ static int32_t RadioComBridgeStart(void) // Start the tasks if(PIOS_COM_TRANS_COM) xTaskCreate(transparentCommTask, (signed char *)"transparentComm", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->transparentCommTaskHandle)); - xTaskCreate(radioReceiveTask, (signed char *)"RadioReceive", STACK_SIZE_BYTES, NULL, TASK_PRIORITY+ 2, &(data->radioReceiveTaskHandle)); - xTaskCreate(sendPacketTask, (signed char *)"SendPacketTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->sendPacketTaskHandle)); - xTaskCreate(radioStatusTask, (signed char *)"RadioStatus", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioStatusTaskHandle)); if(PIOS_PPM_RECEIVER) xTaskCreate(ppmInputTask, (signed char *)"PPMInputTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY + 2, &(data->ppmInputTaskHandle)); + #ifdef PIOS_INCLUDE_WDG PIOS_WDG_RegisterFlag(PIOS_WDG_COMGCS); if(PIOS_COM_UAVTALK) PIOS_WDG_RegisterFlag(PIOS_WDG_COMUAVTALK); if(PIOS_COM_TRANS_COM) PIOS_WDG_RegisterFlag(PIOS_WDG_TRANSCOMM); - PIOS_WDG_RegisterFlag(PIOS_WDG_RADIORECEIVE); - //PIOS_WDG_RegisterFlag(PIOS_WDG_SENDPACKET); if(PIOS_PPM_RECEIVER) PIOS_WDG_RegisterFlag(PIOS_WDG_PPMINPUT); #endif @@ -247,6 +217,8 @@ static int32_t RadioComBridgeInitialize(void) data->GCSUAVTalkCon = UAVTalkInitialize(&GCSUAVTalkSendHandler); if (PIOS_COM_UAVTALK) data->UAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); + else + data->UAVTalkCon = 0; // Initialize the queues. data->ppmOutQueue = 0; @@ -261,62 +233,34 @@ static int32_t RadioComBridgeInitialize(void) } // Initialize the statistics. - data->radioTxErrors = 0; - data->radioTxRetries = 0; - data->radioRxErrors = 0; data->comTxErrors = 0; data->comTxRetries = 0; - data->comRxErrors = 0; data->UAVTalkErrors = 0; - data->packetErrors = 0; data->RSSI = -127; - // Register the callbacks with the packet handler - PHRegisterOutputStream(pios_packet_handler, transmitPacket); - PHRegisterDataHandler(pios_packet_handler, receiveData); - PHRegisterPPMHandler(pios_packet_handler, PPMHandler); - PHRegisterStatusHandler(pios_packet_handler, StatusHandler); - // Initialize the packet send timeout data->send_timeout = 25; // ms data->min_packet_size = 50; - // 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)); - // Configure our UAVObjects for updates. UAVObjConnectQueue(UAVObjGetByID(PIPXSTATUS_OBJID), data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); UAVObjConnectQueue(UAVObjGetByID(GCSRECEIVER_OBJID), data->uavtalkEventQueue ? data->uavtalkEventQueue : data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->gcsEventQueue, EV_UPDATED | EV_UPDATED_MANUAL); // Initialize the UAVTalk comm parameters. + data->gcs_uavtalk_params.isGCS = true; data->gcs_uavtalk_params.UAVTalkCon = data->GCSUAVTalkCon; - data->gcs_uavtalk_params.sendQueue = data->radioPacketQueue; + data->gcs_uavtalk_params.sendQueue = data->UAVTalkCon; data->gcs_uavtalk_params.recvQueue = data->gcsEventQueue; data->gcs_uavtalk_params.wdg = PIOS_WDG_COMGCS; - data->gcs_uavtalk_params.checkHID = true; data->gcs_uavtalk_params.comPort = PIOS_COM_GCS; if (PIOS_COM_UAVTALK) { - data->gcs_uavtalk_params.sendQueue = data->uavtalkEventQueue; + data->uavtalk_params.isGCS = false; data->uavtalk_params.UAVTalkCon = data->UAVTalkCon; - data->uavtalk_params.sendQueue = data->radioPacketQueue; + data->uavtalk_params.sendQueue = data->GCSUAVTalkCon; data->uavtalk_params.recvQueue = data->uavtalkEventQueue; - data->uavtalk_params.gcsQueue = data->gcsEventQueue; data->uavtalk_params.wdg = PIOS_WDG_COMUAVTALK; - data->uavtalk_params.checkHID = false; data->uavtalk_params.comPort = PIOS_COM_UAVTALK; } @@ -336,6 +280,8 @@ static void UAVTalkRecvTask(void *parameters) BufferedReadHandle f = BufferedReadInit(params->comPort, TEMP_BUFFER_SIZE); while (1) { + bool HID_available = false; + xQueueHandle sendQueue = 0; #ifdef PIOS_INCLUDE_WDG // Update the watchdog timer. @@ -343,13 +289,16 @@ static void UAVTalkRecvTask(void *parameters) PIOS_WDG_UpdateFlag(params->wdg); #endif /* PIOS_INCLUDE_WDG */ - // Receive from USB HID if available, otherwise UAVTalk com if it's available. #if defined(PIOS_INCLUDE_USB) - // Determine input port (USB takes priority over telemetry port) - if (params->checkHID && PIOS_USB_CheckAvailable(0)) + // Is USB plugged in? + if (PIOS_USB_CheckAvailable(0)) + HID_available = true; +#endif /* PIOS_INCLUDE_USB */ + + // Receive from USB HID if available, otherwise UAVTalk com if it's available. + if (params->isGCS && HID_available) BufferedReadSetCom(f, PIOS_COM_USB_HID); else -#endif /* PIOS_INCLUDE_USB */ { if (params->comPort) BufferedReadSetCom(f, params->comPort); @@ -360,9 +309,14 @@ static void UAVTalkRecvTask(void *parameters) } } + // Send packets to the UAVTalk port if this is a GCS connction and UAVTalk port is configured + // or to the GCS port if this is a UAVTalk connection and USB is plugged in. + if ((params->isGCS && data->UAVTalkCon) || (!params->isGCS && HID_available)) + sendQueue = params->sendQueue; + // Read the next byte uint8_t rx_byte; - if(!BufferedRead(f, &rx_byte, MAX_PORT_DELAY)) + if (!BufferedRead(f, &rx_byte, MAX_PORT_DELAY)) continue; // Get a TX packet from the packet handler if required. @@ -386,7 +340,6 @@ static void UAVTalkRecvTask(void *parameters) // Initialize the packet. p->header.destination_id = data->destination_id; - p->header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id); p->header.type = PACKET_TYPE_DATA; p->data[0] = rx_byte; p->header.data_size = 1; @@ -403,13 +356,6 @@ static void UAVTalkRecvTask(void *parameters) if (state == UAVTALK_STATE_COMPLETE) { - xQueueHandle sendQueue = params->sendQueue; -#if defined(PIOS_INCLUDE_USB) - if (params->gcsQueue) - if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID) - sendQueue = params->gcsQueue; -#endif /* PIOS_INCLUDE_USB */ - // Is this a local UAVObject? // We only generate GcsReceiver ojects, we don't consume them. if ((iproc->obj != NULL) && (iproc->objId != GCSRECEIVER_OBJID)) @@ -486,7 +432,10 @@ static void UAVTalkRecvTask(void *parameters) else { // Otherwise, queue the packet for transmission. - queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET); + if (sendQueue) + queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET); + else + PHTransmitPacket(PIOS_PACKET_HANDLER, p); } } else @@ -515,17 +464,14 @@ static void UAVTalkRecvTask(void *parameters) else { // Queue the packet for transmission. - queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET); + if (sendQueue) + queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET); + else + PHTransmitPacket(PIOS_PACKET_HANDLER, p); } p = NULL; } else if(state == UAVTALK_STATE_ERROR) { - xQueueHandle sendQueue = params->sendQueue; -#if defined(PIOS_INCLUDE_USB) - if (params->gcsQueue) - if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID) - sendQueue = params->gcsQueue; -#endif /* PIOS_INCLUDE_USB */ data->UAVTalkErrors++; // Send a NACK if required. @@ -540,84 +486,16 @@ static void UAVTalkRecvTask(void *parameters) else { // Transmit the packet anyway... - queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET); + if (sendQueue) + queueEvent(sendQueue, (void*)p, 0, EV_TRANSMIT_PACKET); + else + PHTransmitPacket(PIOS_PACKET_HANDLER, p); } p = NULL; } } } -/** - * The radio to com bridge task. - */ -static void radioReceiveTask(void *parameters) -{ - PHPacketHandle p = NULL; - - /* Handle radio -> usart/usb direction */ - while (1) { - uint32_t rx_bytes; - -#ifdef PIOS_INCLUDE_WDG - // Update the watchdog timer. - PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORECEIVE); -#endif /* PIOS_INCLUDE_WDG */ - - // Get a RX packet from the packet handler if required. - if (p == NULL) - p = PHGetRXPacket(pios_packet_handler); - - if(p == NULL) { - DEBUG_PRINTF(2, "RX Packet Unavailable.!\n\r"); - // 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. - if(PHVerifyPacket(pios_packet_handler, p, rx_bytes) > 0) { - UAVObjEvent ev; - ev.obj = (UAVObjHandle)p; - ev.event = EV_PACKET_RECEIVED; - xQueueSend(data->gcsEventQueue, &ev, portMAX_DELAY); - } else - { - data->packetErrors++; - PHReceivePacket(pios_packet_handler, p, true); - } - p = NULL; - } -} - -/** - * Send packets to the radio. - */ -static void sendPacketTask(void *parameters) -{ - UAVObjEvent ev; - - // 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, &ev, MAX_PORT_DELAY) == pdTRUE) { - PHPacketHandle p = (PHPacketHandle)ev.obj; - // Send the packet. - if(!PHTransmitPacket(pios_packet_handler, p)) - PHReleaseRXPacket(pios_packet_handler, p); - } - } -} - /** * Send packets to the com port. */ @@ -681,7 +559,7 @@ static void UAVTalkSendTask(void *parameters) { // Transmit the packet. PHPacketHandle p = (PHPacketHandle)ev.obj; - transmitData(params->comPort, p->data, p->header.data_size, params->checkHID); + UAVTalkSendBuf(params->UAVTalkCon, p->data, p->header.data_size); PHReleaseTXPacket(pios_packet_handler, p); } } @@ -721,7 +599,6 @@ static void transparentCommTask(void * parameters) // Initialize the packet. p->header.destination_id = data->destination_id; - p->header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id); //p->header.type = PACKET_TYPE_ACKED_DATA; p->header.type = PACKET_TYPE_DATA; p->header.data_size = 0; @@ -771,97 +648,6 @@ static void transparentCommTask(void * parameters) } } -/** - * The stats update task. - */ -static void radioStatusTask(void *parameters) -{ - PHStatusPacket status_packet; - - while (1) { - 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; - queueEvent(data->radioPacketQueue, (void*)sph, 0, EV_TRANSMIT_PACKET); - cntr = 0; - } - } - - // Delay until the next update period. - vTaskDelay(STATS_UPDATE_PERIOD_MS / portTICK_RATE_MS); - } -} - /** * The PPM input task. */ @@ -885,13 +671,21 @@ static void ppmInputTask(void *parameters) if (data->ppmOutQueue) { ppm_packet.header.destination_id = data->destination_id; - ppm_packet.header.source_id = PIOS_RFM22B_DeviceID(pios_rfm22b_id); ppm_packet.header.type = PACKET_TYPE_PPM; ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&ppm_packet); queueEvent(data->ppmOutQueue, (void*)pph, 0, EV_TRANSMIT_PACKET); } else - PPMHandler(ppm_packet.channels); + { + GCSReceiverData rcvr; + + // Copy the receiver channels into the GCSReceiver object. + for (uint8_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; ++i) + rcvr.Channel[i] = ppm_packet.channels[i]; + + // Set the GCSReceiverData object. + GCSReceiverSet(&rcvr); + } // Delay until the next update period. vTaskDelay(PIOS_PPM_PACKET_UPDATE_PERIOD_MS / portTICK_RATE_MS); @@ -910,7 +704,7 @@ static int32_t UAVTalkSend(UAVTalkComTaskParams *params, uint8_t *buf, int32_t l { uint32_t outputPort = params->comPort; #if defined(PIOS_INCLUDE_USB) - if (params->checkHID) + if (params->isGCS) { // Determine output port (USB takes priority over telemetry port) if (PIOS_USB_CheckAvailable(0) && PIOS_COM_USB_HID) @@ -947,19 +741,6 @@ static int32_t GCSUAVTalkSendHandler(uint8_t *buf, int32_t length) return UAVTalkSend(&(data->gcs_uavtalk_params), buf, length); } -/** - * 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) -{ - data->txBytes += PH_PACKET_SIZE(p); - return PIOS_COM_SendBuffer(PIOS_COM_RADIO, (uint8_t*)p, PH_PACKET_SIZE(p)); -} - /** * Receive a packet * \param[in] buf The received data buffer @@ -993,85 +774,13 @@ static void receiveData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc) data->RSSI = rssi; // Packet data should go to transparent com if it's configured, - // USB HID if it's connected, otherwise, UAVTalk com if it's configured. - uint32_t outputPort = PIOS_COM_TRANS_COM ? PIOS_COM_TRANS_COM : PIOS_COM_UAVTALK; - bool checkHid = (PIOS_COM_TRANS_COM == 0); - transmitData(outputPort, buf, len, checkHid); -} - -/** - * 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; - } -} - -/** - * 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); + // or just send it through the UAVTalk link. + if (PIOS_COM_TRANS_COM) + transmitData(PIOS_COM_TRANS_COM, buf, len, false); + else if (data->UAVTalkCon) + UAVTalkSendBuf(data->UAVTalkCon, buf, len); + else + UAVTalkSendBuf(data->GCSUAVTalkCon, buf, len); } static BufferedReadHandle BufferedReadInit(uint32_t com_port, uint16_t buffer_length) diff --git a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h index 71426d594..d500c86ea 100755 --- a/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_PIPXTREME_Rev1.h @@ -138,8 +138,6 @@ extern uint32_t pios_i2c_flexi_adapter_id; // See also pios_board.c //------------------------ #define PIOS_SPI_MAX_DEVS 1 -extern uint32_t pios_spi_port_id; -#define PIOS_SPI_PORT (pios_spi_port_id) //------------------------- // PIOS_USART @@ -270,30 +268,40 @@ extern uint32_t pios_ppm_rcvr_id; // RFM22 //------------------------- -extern uint32_t pios_rfm22b_id; +#if defined(PIOS_INCLUDE_RFM22B) +#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 256 +#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 256 +extern uint32_t pios_com_rfm22b_id; +#define PIOS_COM_RADIO (pios_com_rfm22b_id) +extern uint32_t pios_spi_rfm22b_id; +#define PIOS_RFM22_SPI_PORT (pios_spi_rfm22b_id) #define RFM22_EXT_INT_USE -#define RFM22_PIOS_SPI PIOS_SPI_PORT // SPIx +extern uint32_t pios_rfm22b_id; +#endif /* PIOS_INCLUDE_RFM22B */ -#if defined(RFM22_EXT_INT_USE) -#define PIOS_RFM22_EXTI_GPIO_PORT GPIOA -#define PIOS_RFM22_EXTI_GPIO_PIN GPIO_Pin_2 -#define PIOS_RFM22_EXTI_PORT_SOURCE GPIO_PortSourceGPIOA -#define PIOS_RFM22_EXTI_PIN_SOURCE GPIO_PinSource2 -#define PIOS_RFM22_EXTI_CLK RCC_APB2Periph_GPIOA -#define PIOS_RFM22_EXTI_LINE EXTI_Line2 -#define PIOS_RFM22_EXTI_IRQn EXTI2_IRQn -#define PIOS_RFM22_EXTI_PRIO PIOS_IRQ_PRIO_LOW -#endif +//------------------------- +// Packet Handler +//------------------------- +#if defined(PIOS_INCLUDE_PACKET_HANDLER) +extern uint32_t pios_packet_handler; +#define PIOS_PACKET_HANDLER (pios_packet_handler) +#define PIOS_PH_MAX_PACKET 255 +#define PIOS_PH_WIN_SIZE 3 +#define PIOS_PH_MAX_CONNECTIONS 1 +#define RS_ECC_NPARITY 4 +#endif /* PIOS_INCLUDE_PACKET_HANDLER */ //------------------------- // Packet Handler //------------------------- +#if defined(PIOS_INCLUDE_PACKET_HANDLER) uint32_t pios_packet_handler; -#define PIOS_INCLUDE_PACKET_HANDLER +#define PIOS_PACKET_HANDLER (pios_packet_handler) #define PIOS_PH_MAX_PACKET 255 #define PIOS_PH_WIN_SIZE 3 #define PIOS_PH_MAX_CONNECTIONS 1 +#endif /* PIOS_INCLUDE_PACKET_HANDLER */ //------------------------- // Reed-Solomon ECC diff --git a/flight/PiOS/Common/pios_rfm22b.c b/flight/PiOS/Common/pios_rfm22b.c index 1f65748ee..17c385c3e 100644 --- a/flight/PiOS/Common/pios_rfm22b.c +++ b/flight/PiOS/Common/pios_rfm22b.c @@ -51,13 +51,8 @@ #if defined(PIOS_INCLUDE_RFM22B) -#include // memmove - -#include -#include - +#include #include - #include /* Local Defines */ @@ -135,6 +130,17 @@ #define RFM22_DEFAULT_SS_RF_DATARATE 125 // 128bps +#ifndef RX_LED_ON +#define RX_LED_ON +#define RX_LED_OFF +#define TX_LED_ON +#define TX_LED_OFF +#define LINK_LED_ON +#define LINK_LED_OFF +#define USB_LED_ON +#define USB_LED_OFF +#endif + // ************************************ // Normal data streaming // GFSK modulation @@ -172,21 +178,20 @@ uint32_t random32 = 0x459ab8d8; /* Local function forwared declarations */ static void PIOS_RFM22B_Supervisor(uint32_t ppm_id); static void rfm22_processInt(void); -static void PIOS_RFM22_EXT_Int(void); static void rfm22_setTxMode(uint8_t mode); // SPI read/write functions void rfm22_startBurstWrite(uint8_t addr); inline void rfm22_burstWrite(uint8_t data) { - PIOS_SPI_TransferByte(RFM22_PIOS_SPI, data); + PIOS_SPI_TransferByte(PIOS_RFM22_SPI_PORT, data); } void rfm22_endBurstWrite(void); void rfm22_write(uint8_t addr, uint8_t data); void rfm22_startBurstRead(uint8_t addr); inline uint8_t rfm22_burstRead(void) { - return PIOS_SPI_TransferByte(RFM22_PIOS_SPI, 0xff); + return PIOS_SPI_TransferByte(PIOS_RFM22_SPI_PORT, 0xff); } void rfm22_endBurstRead(void); uint8_t rfm22_read(uint8_t addr); @@ -208,35 +213,6 @@ const struct pios_com_driver pios_rfm22b_com_driver = { .bind_rx_cb = PIOS_RFM22B_RegisterRxCallback, }; -// External interrupt configuration -static const struct pios_exti_cfg pios_exti_rfm22b_cfg __exti_config = { - .vector = PIOS_RFM22_EXT_Int, - .line = PIOS_RFM22_EXTI_LINE, - .pin = { - .gpio = PIOS_RFM22_EXTI_GPIO_PORT, - .init = { - .GPIO_Pin = PIOS_RFM22_EXTI_GPIO_PIN, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, - .irq = { - .init = { - .NVIC_IRQChannel = PIOS_RFM22_EXTI_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_RFM22_EXTI_PRIO, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .exti = { - .init = { - .EXTI_Line = PIOS_RFM22_EXTI_LINE, - .EXTI_Mode = EXTI_Mode_Interrupt, - .EXTI_Trigger = EXTI_Trigger_Falling, - .EXTI_LineCmd = ENABLE, - }, - }, -}; - // xtal 10 ppm, 434MHz #define LOOKUP_SIZE 14 const uint32_t data_rate[] = { 500, 1000, 2000, 4000, 8000, 9600, 16000, 19200, 24000, 32000, 64000, 128000, 192000, 256000}; @@ -337,17 +313,6 @@ volatile int32_t afc_correction_Hz; // afc correction reading (in Hz) volatile int16_t temperature_reg; // the temperature sensor reading -#if defined(RFM22_DEBUG) -volatile uint8_t prev_device_status; // just for debugging -volatile uint8_t prev_int_status1; // " " -volatile uint8_t prev_int_status2; // " " -volatile uint8_t prev_ezmac_status; // " " - -const char *debug_msg = ""; -const char *error_msg = ""; -static uint32_t debug_val = 0; -#endif - volatile uint8_t osc_load_cap; // xtal frequency calibration value volatile uint8_t rssi; // the current RSSI (register value) @@ -447,7 +412,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, const struct pios_rfm22b_cfg *cfg) // Bind the configuration to the device instance rfm22b_dev->cfg = *cfg; - + *rfm22b_id = (uint32_t)rfm22b_dev; // Initialize the TX pre-buffer pointer. @@ -469,6 +434,14 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, const struct pios_rfm22b_cfg *cfg) rfm22b_dev->supv_timer = PIOS_RFM22B_SUPERVISOR_TIMEOUT; rfm22b_dev->resets = 0; + // Initialize our SPI interface + if (PIOS_SPI_Init(&PIOS_RFM22_SPI_PORT, cfg->spi_cfg)) { + PIOS_Assert(0); + } + + // Initialize the external interrupt. + PIOS_EXTI_Init(cfg->exti_cfg); + // Initialize the radio device. int initval = rfm22_init_normal(rfm22b_dev->deviceID, cfg->minFrequencyHz, cfg->maxFrequencyHz, 50000); @@ -504,7 +477,7 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, const struct pios_rfm22b_cfg *cfg) rfm22_setFreqCalibration(cfg->RFXtalCap); rfm22_setNominalCarrierFrequency(cfg->frequencyHz); - rfm22_setDatarate(cfg->maxRFBandwidth, TRUE); + rfm22_setDatarate(cfg->maxRFBandwidth, true); rfm22_setTxPower(cfg->maxTxPower); DEBUG_PRINTF(2, "\n\r"); @@ -666,16 +639,6 @@ static void PIOS_RFM22B_Supervisor(uint32_t rfm22b_id) } } -static void rfm22_setDebug(const char* msg) -{ - debug_msg = msg; -} - -static void rfm22_setError(const char* msg) -{ - error_msg = msg; -} - // ************************************ // SPI read/write @@ -685,15 +648,15 @@ void rfm22_startBurstWrite(uint8_t addr) PIOS_DELAY_WaituS(1); // chip select line LOW - PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 0); + PIOS_SPI_RC_PinSet(PIOS_RFM22_SPI_PORT, 0, 0); - PIOS_SPI_TransferByte(RFM22_PIOS_SPI, 0x80 | addr); + PIOS_SPI_TransferByte(PIOS_RFM22_SPI_PORT, 0x80 | addr); } void rfm22_endBurstWrite(void) { // chip select line HIGH - PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 1); + PIOS_SPI_RC_PinSet(PIOS_RFM22_SPI_PORT, 0, 1); } void rfm22_write(uint8_t addr, uint8_t data) @@ -702,13 +665,13 @@ void rfm22_write(uint8_t addr, uint8_t data) PIOS_DELAY_WaituS(1); // chip select line LOW - PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 0); + PIOS_SPI_RC_PinSet(PIOS_RFM22_SPI_PORT, 0, 0); - PIOS_SPI_TransferByte(RFM22_PIOS_SPI, 0x80 | addr); - PIOS_SPI_TransferByte(RFM22_PIOS_SPI, data); + PIOS_SPI_TransferByte(PIOS_RFM22_SPI_PORT, 0x80 | addr); + PIOS_SPI_TransferByte(PIOS_RFM22_SPI_PORT, data); // chip select line HIGH - PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 1); + PIOS_SPI_RC_PinSet(PIOS_RFM22_SPI_PORT, 0, 1); } void rfm22_startBurstRead(uint8_t addr) @@ -717,15 +680,15 @@ void rfm22_startBurstRead(uint8_t addr) PIOS_DELAY_WaituS(1); // chip select line LOW - PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 0); + PIOS_SPI_RC_PinSet(PIOS_RFM22_SPI_PORT, 0, 0); - PIOS_SPI_TransferByte(RFM22_PIOS_SPI, addr & 0x7f); + PIOS_SPI_TransferByte(PIOS_RFM22_SPI_PORT, addr & 0x7f); } void rfm22_endBurstRead(void) { // chip select line HIGH - PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 1); + PIOS_SPI_RC_PinSet(PIOS_RFM22_SPI_PORT, 0, 1); } uint8_t rfm22_read(uint8_t addr) @@ -736,13 +699,13 @@ uint8_t rfm22_read(uint8_t addr) PIOS_DELAY_WaituS(1); // chip select line LOW - PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 0); + PIOS_SPI_RC_PinSet(PIOS_RFM22_SPI_PORT, 0, 0); - PIOS_SPI_TransferByte(RFM22_PIOS_SPI, addr & 0x7f); - rdata = PIOS_SPI_TransferByte(RFM22_PIOS_SPI, 0xff); + PIOS_SPI_TransferByte(PIOS_RFM22_SPI_PORT, addr & 0x7f); + rdata = PIOS_SPI_TransferByte(PIOS_RFM22_SPI_PORT, 0xff); // chip select line HIGH - PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 1); + PIOS_SPI_RC_PinSet(PIOS_RFM22_SPI_PORT, 0, 1); return rdata; } @@ -751,37 +714,10 @@ uint8_t rfm22_read(uint8_t addr) // external interrupt -static void PIOS_RFM22_EXT_Int(void) +void PIOS_RFM22_EXT_Int(void) { - rfm22_setDebug("Ext Int"); if (!exec_using_spi) rfm22_processInt(); - rfm22_setDebug("Ext Done"); -} - -void rfm22_disableExtInt(void) -{ -#if defined(RFM22_EXT_INT_USE) - rfm22_setDebug("Disable Int"); - // Configure the external interrupt - GPIO_EXTILineConfig(PIOS_RFM22_EXTI_PORT_SOURCE, PIOS_RFM22_EXTI_PIN_SOURCE); - EXTI_InitTypeDef EXTI_InitStructure = pios_exti_rfm22b_cfg.exti.init; - EXTI_InitStructure.EXTI_LineCmd = DISABLE; - EXTI_Init(&EXTI_InitStructure); - - EXTI_ClearFlag(PIOS_RFM22_EXTI_LINE); - rfm22_setDebug("Disable Int done"); -#endif -} - -void rfm22_enableExtInt(void) -{ -#if defined(RFM22_EXT_INT_USE) - rfm22_setDebug("Ensable Int"); - if (PIOS_EXTI_Init(&pios_exti_rfm22b_cfg)) - PIOS_Assert(0); - rfm22_setDebug("Ensable Int done"); -#endif } @@ -823,7 +759,7 @@ uint32_t rfm22_maxFrequency(void) void rfm22_setNominalCarrierFrequency(uint32_t frequency_hz) { - exec_using_spi = TRUE; + exec_using_spi = true; // ******* @@ -869,7 +805,7 @@ void rfm22_setNominalCarrierFrequency(uint32_t frequency_hz) // DEBUG_PRINTF(2, "rf setFreq frequency_step_size: %0.2f\n\r", frequency_step_size); #endif - exec_using_spi = FALSE; + exec_using_spi = false; } uint32_t rfm22_getNominalCarrierFrequency(void) @@ -914,7 +850,7 @@ uint32_t rfm22_freqHopSize(void) void rfm22_setDatarate(uint32_t datarate_bps, bool data_whitening) { - exec_using_spi = TRUE; + exec_using_spi = true; lookup_index = 0; while (lookup_index < (LOOKUP_SIZE - 1) && data_rate[lookup_index] < datarate_bps) @@ -1046,7 +982,7 @@ void rfm22_setDatarate(uint32_t datarate_bps, bool data_whitening) // ******* - exec_using_spi = FALSE; + exec_using_spi = false; } uint32_t rfm22_getDatarate(void) @@ -1059,7 +995,7 @@ uint32_t rfm22_getDatarate(void) void rfm22_setSSBandwidth(uint32_t bandwidth_index) { - exec_using_spi = TRUE; + exec_using_spi = true; ss_lookup_index = bandwidth_index; @@ -1095,14 +1031,14 @@ void rfm22_setSSBandwidth(uint32_t bandwidth_index) // ******* - exec_using_spi = FALSE; + exec_using_spi = false; } // ************************************ void rfm22_setRxMode(uint8_t mode, bool multi_packet_mode) { - exec_using_spi = TRUE; + exec_using_spi = true; // disable interrupts rfm22_write(RFM22_interrupt_enable1, 0x00); @@ -1151,7 +1087,7 @@ void rfm22_setRxMode(uint8_t mode, bool multi_packet_mode) // enable the receiver rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon | RFM22_opfc1_rxon); - exec_using_spi = FALSE; + exec_using_spi = false; } // ************************************ @@ -1175,14 +1111,14 @@ uint16_t rfm22_addHeader() uint8_t rfm22_txStart() { - if((tx_pre_buffer_size == 0) || (exec_using_spi == TRUE)) + if((tx_pre_buffer_size == 0) || (exec_using_spi == true)) { // Clear the TX buffer. tx_data_rd = tx_data_wr = 0; return 0; } - exec_using_spi = TRUE; + exec_using_spi = true; // Disable interrrupts. PIOS_IRQ_Disable(); @@ -1255,18 +1191,17 @@ uint8_t rfm22_txStart() TX_LED_ON; - exec_using_spi = FALSE; + exec_using_spi = false; return 1; } static void rfm22_setTxMode(uint8_t mode) { - rfm22_setDebug("setTxMode"); if (mode != TX_DATA_MODE && mode != TX_STREAM_MODE && mode != TX_CARRIER_MODE && mode != TX_PN_MODE) return; // invalid mode - exec_using_spi = TRUE; + exec_using_spi = true; // disable interrupts rfm22_write(RFM22_interrupt_enable1, 0x00); @@ -1356,9 +1291,8 @@ static void rfm22_setTxMode(uint8_t mode) // ******************* - exec_using_spi = FALSE; + exec_using_spi = false; - rfm22_setDebug("setTxMode end"); } // ************************************ @@ -1368,12 +1302,10 @@ void rfm22_processRxInt(void) { register uint8_t int_stat1 = int_status1; register uint8_t int_stat2 = int_status2; - rfm22_setDebug("processRxInt"); // FIFO under/over flow error. Restart RX mode. if (device_status & (RFM22_ds_ffunfl | RFM22_ds_ffovfl)) { - rfm22_setError("R_UNDER/OVERRUN"); rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false); return; } @@ -1383,7 +1315,6 @@ void rfm22_processRxInt(void) { rf_mode = RX_WAIT_SYNC_MODE; RX_LED_ON; - rfm22_setDebug("pream_det"); } // Sync word detected @@ -1391,7 +1322,6 @@ void rfm22_processRxInt(void) { rf_mode = RX_DATA_MODE; RX_LED_ON; - rfm22_setDebug("sync_det"); // read the 10-bit signed afc correction value // bits 9 to 2 @@ -1420,7 +1350,6 @@ void rfm22_processRxInt(void) // The received packet is going to be larger than the specified length if ((rx_buffer_wr + RX_FIFO_HI_WATERMARK) > len) { - rfm22_setError("r_size_error1"); rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false); return; } @@ -1428,7 +1357,6 @@ void rfm22_processRxInt(void) // Another packet length error. if (((rx_buffer_wr + RX_FIFO_HI_WATERMARK) >= len) && !(int_stat1 & RFM22_is1_ipkvalid)) { - rfm22_setError("r_size_error2"); rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false); return; } @@ -1452,7 +1380,6 @@ void rfm22_processRxInt(void) if (int_stat1 & RFM22_is1_icrerror) { rfm22_int_timer = 0; // reset the timer - rfm22_setError("CRC_ERR"); rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false); return; } @@ -1477,14 +1404,11 @@ void rfm22_processRxInt(void) if (rx_buffer_wr != len) { // we have a packet length error .. discard the packet - rfm22_setError("r_pack_len_error"); - debug_val = len; rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false); return; } // we have a valid received packet - rfm22_setDebug("VALID_R_PACKET"); if (rx_buffer_wr > 0) { @@ -1507,12 +1431,10 @@ void rfm22_processRxInt(void) if(!rfm22_txStart()) { // Switch to RX mode - rfm22_setDebug(" Set RX"); rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false); } } - rfm22_setDebug("processRxInt end"); } void rfm22_processTxInt(void) @@ -1525,7 +1447,6 @@ void rfm22_processTxInt(void) // FIFO under/over flow error. Back to RX mode. if (device_status & (RFM22_ds_ffunfl | RFM22_ds_ffovfl)) { - rfm22_setError("T_UNDER/OVERRUN"); rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false); return; } @@ -1544,7 +1465,6 @@ void rfm22_processTxInt(void) if (rfm22_int_timer >= 100) { rfm22_int_time_outs++; - rfm22_setError("T_TIMEOUT"); rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false); // back to rx mode return; } @@ -1564,31 +1484,27 @@ void rfm22_processTxInt(void) // Packet has been sent if (int_stat1 & RFM22_is1_ipksent) { - rfm22_setDebug(" T_Sent"); // Send another packet if it's available. if(!rfm22_txStart()) { // Switch to RX mode - rfm22_setDebug(" Set RX"); rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false); return; } } - rfm22_setDebug("ProcessTxInt done"); } static void rfm22_processInt(void) { - rfm22_setDebug("ProcessInt"); // this is called from the external interrupt handler if (!initialized || power_on_reset) // we haven't yet been initialized return; - exec_using_spi = TRUE; + exec_using_spi = true; // Reset the supervisor timer. rfm22b_dev_g->supv_timer = PIOS_RFM22B_SUPERVISOR_TIMEOUT; @@ -1619,9 +1535,8 @@ static void rfm22_processInt(void) // the RF module has gone and done a reset - we need to re-initialize the rf module if (int_status2 & RFM22_is2_ipor) { - initialized = FALSE; - power_on_reset = TRUE; - rfm22_setError("Reset"); + initialized = false; + power_on_reset = true; // Need to do something here! return; } @@ -1661,21 +1576,19 @@ static void rfm22_processInt(void) break; } - exec_using_spi = FALSE; - - rfm22_setDebug("ProcessInt done"); + exec_using_spi = false; } // ************************************ int8_t rfm22_getRSSI(void) { - exec_using_spi = TRUE; + exec_using_spi = true; rssi = rfm22_read(RFM22_rssi); // read rx signal strength .. 45 = -100dBm, 205 = -20dBm rssi_dBm = (int8_t)(rssi >> 1) - 122; // convert to dBm - exec_using_spi = FALSE; + exec_using_spi = false; return rssi_dBm; } @@ -1757,32 +1670,32 @@ int8_t rfm22_currentMode(void) return rf_mode; } -// return TRUE if we are transmitting +// return true if we are transmitting bool rfm22_transmitting(void) { return (rf_mode == TX_DATA_MODE || rf_mode == TX_STREAM_MODE || rf_mode == TX_CARRIER_MODE || rf_mode == TX_PN_MODE); } -// return TRUE if the channel is clear to transmit on +// return true if the channel is clear to transmit on bool rfm22_channelIsClear(void) { if (!initialized) // we haven't yet been initialized - return FALSE; + return false; if (rf_mode != RX_WAIT_PREAMBLE_MODE && rf_mode != RX_WAIT_SYNC_MODE) // we are receiving something or we are transmitting or we are scanning the spectrum - return FALSE; + return false; - return TRUE; + return true; } -// return TRUE if the transmiter is ready for use +// return true if the transmiter is ready for use bool rfm22_txReady(void) { if (!initialized) // we haven't yet been initialized - return FALSE; + return false; return (tx_data_rd == 0 && tx_data_wr == 0 && rf_mode != TX_DATA_MODE && rf_mode != TX_STREAM_MODE && rf_mode != TX_CARRIER_MODE && rf_mode != TX_PN_MODE && @@ -1807,11 +1720,11 @@ void rfm22_setFreqCalibration(uint8_t value) tx_data_rd = tx_data_wr = 0; } - exec_using_spi = TRUE; + exec_using_spi = true; rfm22_write(RFM22_xtal_osc_load_cap, osc_load_cap); - exec_using_spi = FALSE; + exec_using_spi = false; if (prev_rf_mode == TX_CARRIER_MODE || prev_rf_mode == TX_PN_MODE) rfm22_setTxMode(prev_rf_mode); @@ -1842,40 +1755,38 @@ int rfm22_resetModule(uint8_t mode, uint32_t min_frequency_hz, uint32_t max_freq { initialized = false; -#if defined(RFM22_EXT_INT_USE) - rfm22_disableExtInt(); -#endif - power_on_reset = false; // **************** - exec_using_spi = TRUE; + exec_using_spi = true; // **************** // setup the SPI port // chip select line HIGH - PIOS_SPI_RC_PinSet(RFM22_PIOS_SPI, 0, 1); + PIOS_SPI_RC_PinSet(PIOS_RFM22_SPI_PORT, 0, 1); // set SPI port SCLK frequency .. 4.5MHz - PIOS_SPI_SetClockSpeed(RFM22_PIOS_SPI, PIOS_SPI_PRESCALER_16); + PIOS_SPI_SetClockSpeed(PIOS_RFM22_SPI_PORT, PIOS_SPI_PRESCALER_16); // set SPI port SCLK frequency .. 2.25MHz - // PIOS_SPI_SetClockSpeed(RFM22_PIOS_SPI, PIOS_SPI_PRESCALER_32); + // PIOS_SPI_SetClockSpeed(PIOS_RFM22_SPI_PORT, PIOS_SPI_PRESCALER_32); // set SPI port SCLK frequency .. 285kHz .. purely for hardware fault finding - // PIOS_SPI_SetClockSpeed(RFM22_PIOS_SPI, PIOS_SPI_PRESCALER_256); + // PIOS_SPI_SetClockSpeed(PIOS_RFM22_SPI_PORT, PIOS_SPI_PRESCALER_256); // **************** // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B) rfm22_write(RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); // software reset the radio - PIOS_DELAY_WaitmS(26); // wait 26ms + // wait 26ms + PIOS_DELAY_WaitmS(26); for (int i = 50; i > 0; i--) { - PIOS_DELAY_WaitmS(1); // wait 1ms + // wait 1ms + PIOS_DELAY_WaitmS(1); // read the status registers int_status1 = rfm22_read(RFM22_interrupt_status1); @@ -1897,7 +1808,7 @@ int rfm22_resetModule(uint8_t mode, uint32_t min_frequency_hz, uint32_t max_freq // **************** - exec_using_spi = FALSE; + exec_using_spi = false; // **************** @@ -2019,197 +1930,6 @@ int rfm22_resetModule(uint8_t mode, uint32_t min_frequency_hz, uint32_t max_freq return 0; // OK } -// ************************************ - -int rfm22_init_scan_spectrum(uint32_t min_frequency_hz, uint32_t max_frequency_hz) -{ -#if defined(RFM22_DEBUG) - DEBUG_PRINTF(2, "\n\rRF init scan spectrum\n\r"); -#endif - - int res = rfm22_resetModule(RX_SCAN_SPECTRUM, min_frequency_hz, max_frequency_hz); - if (res < 0) - return res; - - // rfm22_setSSBandwidth(0); - rfm22_setSSBandwidth(1); - - // FIFO mode, GFSK modulation - uint8_t fd_bit = rfm22_read(RFM22_modulation_mode_control2) & RFM22_mmc2_fd; - rfm22_write(RFM22_modulation_mode_control2, RFM22_mmc2_trclk_clk_none | RFM22_mmc2_dtmod_fifo | fd_bit | RFM22_mmc2_modtyp_gfsk); - - rfm22_write(RFM22_cpu_output_clk, RFM22_coc_1MHz); // 1MHz clock output - - rfm22_write(RFM22_rssi_threshold_clear_chan_indicator, 0); - - rfm22_write(RFM22_preamble_detection_ctrl1, 31 << 3); // 31-nibbles rx preamble detection - - // avoid packet detection - rfm22_write(RFM22_data_access_control, RFM22_dac_enpacrx | RFM22_dac_encrc); - rfm22_write(RFM22_header_control1, 0x0f); - rfm22_write(RFM22_header_control2, 0x77); - - rfm22_write(RFM22_sync_word3, SYNC_BYTE_1); - rfm22_write(RFM22_sync_word2, SYNC_BYTE_2); - rfm22_write(RFM22_sync_word1, SYNC_BYTE_3 ^ 0xff); - rfm22_write(RFM22_sync_word0, SYNC_BYTE_4 ^ 0xff); - - // all the bits to be checked - rfm22_write(RFM22_header_enable3, 0xff); - rfm22_write(RFM22_header_enable2, 0xff); - rfm22_write(RFM22_header_enable1, 0xff); - rfm22_write(RFM22_header_enable0, 0xff); - - // set frequency hopping channel step size (multiples of 10kHz) - // rfm22_write(RFM22_frequency_hopping_step_size, 0); - - // set our nominal carrier frequency - rfm22_setNominalCarrierFrequency(min_frequency_hz); - - // set minimum tx power - rfm22_write(RFM22_tx_power, RFM22_tx_pwr_lna_sw | 0); - - rfm22_write(RFM22_agc_override1, RFM22_agc_ovr1_sgi | RFM22_agc_ovr1_agcen); - - // rfm22_write(RFM22_vco_current_trimming, 0x7f); - // rfm22_write(RFM22_vco_calibration_override, 0x40); - // rfm22_write(RFM22_chargepump_current_trimming_override, 0x80); - - // Enable RF module external interrupt - rfm22_enableExtInt(); - - rfm22_setRxMode(RX_SCAN_SPECTRUM, true); - - initialized = true; - - return 0; // OK -} - -// ************************************ - -int rfm22_init_tx_stream(uint32_t min_frequency_hz, uint32_t max_frequency_hz) -{ -#if defined(RFM22_DEBUG) - DEBUG_PRINTF(2, "\n\rRF init TX stream\n\r"); -#endif - - int res = rfm22_resetModule(TX_STREAM_MODE, min_frequency_hz, max_frequency_hz); - if (res < 0) - return res; - - frequency_hop_step_size_reg = 0; - - // set the RF datarate - rfm22_setDatarate(RFM22_DEFAULT_RF_DATARATE, FALSE); - - // FIFO mode, GFSK modulation - uint8_t fd_bit = rfm22_read(RFM22_modulation_mode_control2) & RFM22_mmc2_fd; - rfm22_write(RFM22_modulation_mode_control2, RFM22_mmc2_trclk_clk_none | RFM22_mmc2_dtmod_fifo | fd_bit | RFM22_mmc2_modtyp_gfsk); - - // disable the internal Tx & Rx packet handlers (without CRC) - rfm22_write(RFM22_data_access_control, 0); - - rfm22_write(RFM22_preamble_length, TX_PREAMBLE_NIBBLES); // x-nibbles tx preamble - rfm22_write(RFM22_preamble_detection_ctrl1, RX_PREAMBLE_NIBBLES << 3); // x-nibbles rx preamble detection - - rfm22_write(RFM22_header_control1, RFM22_header_cntl1_bcen_none | RFM22_header_cntl1_hdch_none); // header control - we are not using the header - rfm22_write(RFM22_header_control2, RFM22_header_cntl2_fixpklen | RFM22_header_cntl2_hdlen_none | RFM22_header_cntl2_synclen_32 | ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); // no header bytes, synchronization word length 3, 2 used, packet length not included in header (fixed packet length). - - rfm22_write(RFM22_sync_word3, SYNC_BYTE_1); // sync word - rfm22_write(RFM22_sync_word2, SYNC_BYTE_2); // - - // rfm22_write(RFM22_modem_test, 0x01); - - rfm22_write(RFM22_agc_override1, RFM22_agc_ovr1_agcen); - // rfm22_write(RFM22_agc_override1, RFM22_agc_ovr1_sgi | RFM22_agc_ovr1_agcen); - - rfm22_write(RFM22_frequency_hopping_step_size, frequency_hop_step_size_reg); // set frequency hopping channel step size (multiples of 10kHz) - - rfm22_setNominalCarrierFrequency((min_frequency_hz + max_frequency_hz) / 2); // set our nominal carrier frequency - - rfm22_write(RFM22_tx_power, RFM22_tx_pwr_papeaken | RFM22_tx_pwr_papeaklvl_0 | RFM22_tx_pwr_lna_sw | tx_power); // set the tx power - // rfm22_write(RFM22_tx_power, RFM22_tx_pwr_lna_sw | tx_power); // set the tx power - - // rfm22_write(RFM22_vco_current_trimming, 0x7f); - // rfm22_write(RFM22_vco_calibration_override, 0x40); - // rfm22_write(RFM22_chargepump_current_trimming_override, 0x80); - - rfm22_write(RFM22_tx_fifo_control1, TX_FIFO_HI_WATERMARK); // TX FIFO Almost Full Threshold (0 - 63) - rfm22_write(RFM22_tx_fifo_control2, TX_FIFO_LO_WATERMARK); // TX FIFO Almost Empty Threshold (0 - 63) - - // Enable RF module external interrupt - rfm22_enableExtInt(); - - initialized = true; - - return 0; // OK -} - -// ************************************ - -int rfm22_init_rx_stream(uint32_t min_frequency_hz, uint32_t max_frequency_hz) -{ -#if defined(RFM22_DEBUG) - DEBUG_PRINTF(2, "\n\rRF init RX stream\n\r"); -#endif - - int res = rfm22_resetModule(RX_WAIT_PREAMBLE_MODE, min_frequency_hz, max_frequency_hz); - if (res < 0) - return res; - - frequency_hop_step_size_reg = 0; - - // set the RF datarate - rfm22_setDatarate(RFM22_DEFAULT_RF_DATARATE, FALSE); - - // FIFO mode, GFSK modulation - uint8_t fd_bit = rfm22_read(RFM22_modulation_mode_control2) & RFM22_mmc2_fd; - rfm22_write(RFM22_modulation_mode_control2, RFM22_mmc2_trclk_clk_none | RFM22_mmc2_dtmod_fifo | fd_bit | RFM22_mmc2_modtyp_gfsk); - - // disable the internal Tx & Rx packet handlers (without CRC) - rfm22_write(RFM22_data_access_control, 0); - - rfm22_write(RFM22_preamble_length, TX_PREAMBLE_NIBBLES); // x-nibbles tx preamble - rfm22_write(RFM22_preamble_detection_ctrl1, RX_PREAMBLE_NIBBLES << 3); // x-nibbles rx preamble detection - - rfm22_write(RFM22_header_control1, RFM22_header_cntl1_bcen_none | RFM22_header_cntl1_hdch_none); // header control - we are not using the header - rfm22_write(RFM22_header_control2, RFM22_header_cntl2_fixpklen | RFM22_header_cntl2_hdlen_none | RFM22_header_cntl2_synclen_32 | ((TX_PREAMBLE_NIBBLES >> 8) & 0x01)); // no header bytes, synchronization word length 3, 2 used, packet length not included in header (fixed packet length). - - rfm22_write(RFM22_sync_word3, SYNC_BYTE_1); // sync word - rfm22_write(RFM22_sync_word2, SYNC_BYTE_2); // - - // no header bits to be checked - rfm22_write(RFM22_header_enable3, 0x00); - rfm22_write(RFM22_header_enable2, 0x00); - rfm22_write(RFM22_header_enable1, 0x00); - rfm22_write(RFM22_header_enable0, 0x00); - - // rfm22_write(RFM22_modem_test, 0x01); - - rfm22_write(RFM22_agc_override1, RFM22_agc_ovr1_agcen); - // rfm22_write(RFM22_agc_override1, RFM22_agc_ovr1_sgi | RFM22_agc_ovr1_agcen); - - rfm22_write(RFM22_frequency_hopping_step_size, frequency_hop_step_size_reg); // set frequency hopping channel step size (multiples of 10kHz) - - rfm22_setNominalCarrierFrequency((min_frequency_hz + max_frequency_hz) / 2); // set our nominal carrier frequency - - // rfm22_write(RFM22_vco_current_trimming, 0x7f); - // rfm22_write(RFM22_vco_calibration_override, 0x40); - // rfm22_write(RFM22_chargepump_current_trimming_override, 0x80); - - // RX FIFO Almost Full Threshold (0 - 63) - rfm22_write(RFM22_rx_fifo_control, RX_FIFO_HI_WATERMARK); - - // Enable RF module external interrupt - rfm22_enableExtInt(); - - rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false); - - initialized = true; - - return 0; // OK -} - // ************************************ // Initialise this hardware layer module and the rf module @@ -2225,7 +1945,7 @@ int rfm22_init_normal(uint32_t id, uint32_t min_frequency_hz, uint32_t max_frequ frequency_hop_step_size_reg = freq_hop_step_size; // set the RF datarate - rfm22_setDatarate(RFM22_DEFAULT_RF_DATARATE, TRUE); + rfm22_setDatarate(RFM22_DEFAULT_RF_DATARATE, true); // FIFO mode, GFSK modulation uint8_t fd_bit = rfm22_read(RFM22_modulation_mode_control2) & RFM22_mmc2_fd; @@ -2322,9 +2042,6 @@ int rfm22_init_normal(uint32_t id, uint32_t min_frequency_hz, uint32_t max_frequ // RX FIFO Almost Full Threshold (0 - 63) rfm22_write(RFM22_rx_fifo_control, RX_FIFO_HI_WATERMARK); - // Enable RF module external interrupt - rfm22_enableExtInt(); - rfm22_setRxMode(RX_WAIT_PREAMBLE_MODE, false); initialized = true; diff --git a/flight/PiOS/inc/pios_rfm22b.h b/flight/PiOS/inc/pios_rfm22b.h index aa00a835e..df9b6aa9e 100644 --- a/flight/PiOS/inc/pios_rfm22b.h +++ b/flight/PiOS/inc/pios_rfm22b.h @@ -33,6 +33,8 @@ /* Global Types */ struct pios_rfm22b_cfg { + const struct pios_spi_cfg * spi_cfg; /* Pointer to SPI interface configuration */ + const struct pios_exti_cfg * exti_cfg; /* Pointer to the EXTI configuration */ uint32_t frequencyHz; uint32_t minFrequencyHz; uint32_t maxFrequencyHz; diff --git a/flight/PiOS/inc/pios_rfm22b_priv.h b/flight/PiOS/inc/pios_rfm22b_priv.h index 792915546..4535a472e 100644 --- a/flight/PiOS/inc/pios_rfm22b_priv.h +++ b/flight/PiOS/inc/pios_rfm22b_priv.h @@ -591,6 +591,8 @@ typedef bool ( *t_rfm22_RxDataCallback ) (void *data, uint8_t len); // ************************************ +void PIOS_RFM22_EXT_Int(void); + uint32_t rfm22_minFrequency(void); uint32_t rfm22_maxFrequency(void); diff --git a/flight/PipXtreme/Makefile b/flight/PipXtreme/Makefile index 63f2e7907..e00278f36 100644 --- a/flight/PipXtreme/Makefile +++ b/flight/PipXtreme/Makefile @@ -66,7 +66,7 @@ FLASH_TOOL = OPENOCD # List of modules to include OPTMODULES = -MODULES = RadioComBridge +MODULES = Radio RadioComBridge # Paths OPSYSTEM = ./System diff --git a/flight/PipXtreme/System/inc/pios_config.h b/flight/PipXtreme/System/inc/pios_config.h index a94b2af89..254d92026 100755 --- a/flight/PipXtreme/System/inc/pios_config.h +++ b/flight/PipXtreme/System/inc/pios_config.h @@ -61,6 +61,8 @@ #define PIOS_INCLUDE_WDG #define PIOS_INCLUDE_BL_HELPER #define PIOS_INCLUDE_FLASH_EEPROM +#define PIOS_INCLUDE_RFM22B +#define PIOS_INCLUDE_PACKET_HANDLER /* Defaults for Logging */ #define LOG_FILENAME "PIOS.LOG" @@ -96,6 +98,9 @@ /* PIOS Initcall infrastructure */ #define PIOS_INCLUDE_INITCALL +/* Always include the radio module */ +#define RADIO_BUILTIN + #endif /* PIOS_CONFIG_H */ /** * @} diff --git a/flight/PipXtreme/System/pios_board.c b/flight/PipXtreme/System/pios_board.c index 3a2dd0544..84aa36d50 100644 --- a/flight/PipXtreme/System/pios_board.c +++ b/flight/PipXtreme/System/pios_board.c @@ -33,20 +33,17 @@ #include #include -#define PIOS_COM_SERIAL_RX_BUF_LEN 128 -#define PIOS_COM_SERIAL_TX_BUF_LEN 128 +#define PIOS_COM_SERIAL_RX_BUF_LEN 256 +#define PIOS_COM_SERIAL_TX_BUF_LEN 256 -#define PIOS_COM_FLEXI_RX_BUF_LEN 128 +#define PIOS_COM_FLEXI_RX_BUF_LEN 256 #define PIOS_COM_FLEXI_TX_BUF_LEN 128 -#define PIOS_COM_TELEM_USB_RX_BUF_LEN 128 -#define PIOS_COM_TELEM_USB_TX_BUF_LEN 128 +#define PIOS_COM_TELEM_USB_RX_BUF_LEN 256 +#define PIOS_COM_TELEM_USB_TX_BUF_LEN 256 -#define PIOS_COM_VCP_USB_RX_BUF_LEN 128 -#define PIOS_COM_VCP_USB_TX_BUF_LEN 128 - -#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 128 -#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 128 +#define PIOS_COM_VCP_USB_RX_BUF_LEN 256 +#define PIOS_COM_VCP_USB_TX_BUF_LEN 256 uint32_t pios_com_telem_usb_id = 0; uint32_t pios_com_telemetry_id; @@ -56,8 +53,6 @@ uint32_t pios_com_uavtalk_com_id = 0; uint32_t pios_com_gcs_com_id = 0; uint32_t pios_com_trans_com_id = 0; uint32_t pios_com_debug_id = 0; -uint32_t pios_com_rfm22b_id = 0; -uint32_t pios_rfm22b_id = 0; uint32_t pios_ppm_rcvr_id = 0; /** @@ -70,11 +65,6 @@ void PIOS_Board_Init(void) { /* Delay system */ PIOS_DELAY_Init(); - /* Set up the SPI interface to the serial flash */ - if (PIOS_SPI_Init(&pios_spi_port_id, &pios_spi_port_cfg)) { - PIOS_Assert(0); - } - /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); @@ -120,10 +110,6 @@ void PIOS_Board_Init(void) { PIOS_TIM_InitClock(&tim_4_cfg); #endif /* PIOS_INCLUDE_TIM */ -#if defined(PIOS_INCLUDE_PACKET_HANDLER) - pios_packet_handler = PHInitialize(&pios_ph_cfg); -#endif /* PIOS_INCLUDE_PACKET_HANDLER */ - #if defined(PIOS_INCLUDE_USB) /* Initialize board specific USB data */ PIOS_USB_BOARD_DATA_Init(); @@ -309,88 +295,6 @@ void PIOS_Board_Init(void) { break; } -#if defined(PIOS_INCLUDE_RFM22B) - struct pios_rfm22b_cfg pios_rfm22b_cfg = { - .frequencyHz = 434000000, - .minFrequencyHz = 434000000 - 2000000, - .maxFrequencyHz = 434000000 + 2000000, - .RFXtalCap = 0x7f, - .maxRFBandwidth = 128000, - .maxTxPower = RFM22_tx_pwr_txpow_7, // +20dBm .. 100mW - }; - - /* 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.RFSpeed) - { - 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); - } - } -#endif /* PIOS_INCLUDE_RFM22B */ - /* Remap AFIO pin */ GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); diff --git a/flight/board_hw_defs/pipxtreme/board_hw_defs.c b/flight/board_hw_defs/pipxtreme/board_hw_defs.c index 7df2c8132..5550b8ffd 100644 --- a/flight/board_hw_defs/pipxtreme/board_hw_defs.c +++ b/flight/board_hw_defs/pipxtreme/board_hw_defs.c @@ -72,7 +72,7 @@ void PIOS_SPI_port_irq_handler(void); void DMA1_Channel5_IRQHandler() __attribute__ ((alias ("PIOS_SPI_port_irq_handler"))); void DMA1_Channel4_IRQHandler() __attribute__ ((alias ("PIOS_SPI_port_irq_handler"))); -static const struct pios_spi_cfg pios_spi_port_cfg = +static const struct pios_spi_cfg pios_spi_rfm22b_cfg = { .regs = SPI1, @@ -176,15 +176,62 @@ static const struct pios_spi_cfg pios_spi_port_cfg = }, }; -uint32_t pios_spi_port_id; +uint32_t pios_spi_rfm22b_id; void PIOS_SPI_port_irq_handler(void) { /* Call into the generic code to handle the IRQ for this specific device */ - PIOS_SPI_IRQ_Handler(pios_spi_port_id); + PIOS_SPI_IRQ_Handler(pios_spi_rfm22b_id); } #endif /* PIOS_INCLUDE_SPI */ +#if defined(PIOS_INCLUDE_RFM22B) + +#include + +static const struct pios_exti_cfg pios_exti_rfm22b_cfg __exti_config = { + .vector = PIOS_RFM22_EXT_Int, + .line = EXTI_Line2, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_2, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI2_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line2, + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Falling, + .EXTI_LineCmd = ENABLE, + }, + }, +}; + +#include + +struct pios_rfm22b_cfg pios_rfm22b_cfg = { + .spi_cfg = &pios_spi_rfm22b_cfg, + .exti_cfg = &pios_exti_rfm22b_cfg, + .frequencyHz = 434000000, + .minFrequencyHz = 434000000 - 2000000, + .maxFrequencyHz = 434000000 + 2000000, + .RFXtalCap = 0x7f, + .maxRFBandwidth = 128000, + .maxTxPower = RFM22_tx_pwr_txpow_7, // +20dBm .. 100mW +}; + +#endif /* PIOS_INCLUDE_RFM22B */ + #if defined(PIOS_INCLUDE_ADC) /* @@ -537,14 +584,3 @@ const struct pios_eeprom_cfg pios_eeprom_cfg = { #include #endif /* PIOS_INCLUDE_RFM22B */ - -#if defined(PIOS_INCLUDE_PACKET_HANDLER) -#include - -// Initialize the packet handler -PacketHandlerConfig pios_ph_cfg = { - .winSize = PIOS_PH_WIN_SIZE, - .maxConnections = PIOS_PH_MAX_CONNECTIONS, -}; - -#endif /* PIOS_INCLUDE_PACKET_HANDLER */ From 415b86879c2b9d8c8f22b7ba449e5c948f971cca Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 25 Aug 2012 11:45:43 -0700 Subject: [PATCH 09/11] Added the RFM22B driver to the Revo to support an RFM22B radio on the external SPI port. --- flight/Modules/Telemetry/telemetry.c | 76 +++++--- flight/PiOS/Boards/STM32F4xx_Revolution.h | 33 ++++ flight/Revolution/Makefile | 9 + flight/Revolution/System/inc/pios_config.h | 2 + flight/Revolution/UAVObjects.inc | 2 + .../board_hw_defs/revolution/board_hw_defs.c | 175 ++++++++++++++++++ shared/uavobjectdefinition/hwsettings.xml | 2 +- 7 files changed, 275 insertions(+), 24 deletions(-) diff --git a/flight/Modules/Telemetry/telemetry.c b/flight/Modules/Telemetry/telemetry.c index 2b04e0545..a1cc19c34 100644 --- a/flight/Modules/Telemetry/telemetry.c +++ b/flight/Modules/Telemetry/telemetry.c @@ -35,6 +35,10 @@ #include "flighttelemetrystats.h" #include "gcstelemetrystats.h" #include "hwsettings.h" +#if defined(PIOS_PACKET_HANDLER) +#include "pipxstatus.h" +#include "packet_handler.h" +#endif // Private constants #define MAX_QUEUE_SIZE TELEM_QUEUE_SIZE @@ -80,6 +84,10 @@ static void processObjEvent(UAVObjEvent * ev); static void updateTelemetryStats(); static void gcsTelemetryStatsUpdated(); static void updateSettings(); +static uint32_t getComPort(); +#ifdef PIOS_PACKET_HANDLER +static void receivePacketData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc); +#endif /** * Initialise the telemetry module @@ -93,6 +101,13 @@ int32_t TelemetryStart(void) // Listen to objects of interest GCSTelemetryStatsConnectQueue(priorityQueue); + + // Register to receive data from the radio packet handler. + // This must be after the radio module is initialized. +#ifdef PIOS_PACKET_HANDLER + if (PIOS_PACKET_HANDLER) + PHRegisterDataHandler(PIOS_PACKET_HANDLER, receivePacketData); +#endif // Start telemetry tasks xTaskCreate(telemetryTxTask, (signed char *)"TelTx", STACK_SIZE_BYTES/4, NULL, TASK_PRIORITY_TX, &telemetryTxTaskHandle); @@ -247,6 +262,11 @@ static void processObjEvent(UAVObjEvent * ev) retries = 0; success = -1; if (ev->event == EV_UPDATED || ev->event == EV_UPDATED_MANUAL || ((ev->event == EV_UPDATED_PERIODIC) && (updateMode != UPDATEMODE_THROTTLED))) { +#ifdef PIOS_PACKET_HANDLER + // Don't send PipXStatus objects over the radio link. + if ((ev->obj == PipXStatusHandle()) && (getComPort() == 0) && PIOS_PACKET_HANDLER) + return; +#endif // Send update to GCS (with retries) while (retries < MAX_RETRIES && success == -1) { success = UAVTalkSendObject(uavTalkCon, ev->obj, ev->instId, UAVObjGetTelemetryAcked(&metadata), REQ_TIMEOUT_MS); // call blocks until ack is received or timeout @@ -322,19 +342,10 @@ static void telemetryTxPriTask(void *parameters) */ static void telemetryRxTask(void *parameters) { - uint32_t inputPort; // Task loop while (1) { -#if defined(PIOS_INCLUDE_USB) - // Determine input port (USB takes priority over telemetry port) - if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB) { - inputPort = PIOS_COM_TELEM_USB; - } else -#endif /* PIOS_INCLUDE_USB */ - { - inputPort = telemetryPort; - } + uint32_t inputPort = getComPort(); if (inputPort) { // Block until data are available @@ -362,23 +373,17 @@ static void telemetryRxTask(void *parameters) */ static int32_t transmitData(uint8_t * data, int32_t length) { - uint32_t outputPort; - - // Determine input port (USB takes priority over telemetry port) -#if defined(PIOS_INCLUDE_USB) - if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB) { - outputPort = PIOS_COM_TELEM_USB; - } else -#endif /* PIOS_INCLUDE_USB */ - { - outputPort = telemetryPort; - } + uint32_t outputPort = getComPort(); if (outputPort) { return PIOS_COM_SendBuffer(outputPort, data, length); - } else { - return -1; } +#ifdef PIOS_PACKET_HANDLER + if (PIOS_PACKET_HANDLER) + if (PHTransmitData(PIOS_PACKET_HANDLER, data, length)) + return 0; +#endif + return -1; } /** @@ -564,6 +569,31 @@ static void updateSettings() } } +/** + * Determine input/output com port (USB takes priority over telemetry port) + */ +static uint32_t getComPort() { +#if defined(PIOS_INCLUDE_USB) + if (PIOS_USB_CheckAvailable(0) && PIOS_COM_TELEM_USB) + return PIOS_COM_TELEM_USB; + else +#endif /* PIOS_INCLUDE_USB */ + return telemetryPort; +} + +#ifdef PIOS_PACKET_HANDLER +/** + * Receive a packet + * \param[in] buf The received data buffer + * \param[in] length Length of buffer + */ +static void receivePacketData(uint8_t *buf, uint8_t len, int8_t rssi, int8_t afc) +{ + for (uint8_t i = 0; i < len; ++i) + UAVTalkProcessInputStream(uavTalkCon, buf[i]); +} +#endif + /** * @} * @} diff --git a/flight/PiOS/Boards/STM32F4xx_Revolution.h b/flight/PiOS/Boards/STM32F4xx_Revolution.h index 370abd2e3..42163b995 100644 --- a/flight/PiOS/Boards/STM32F4xx_Revolution.h +++ b/flight/PiOS/Boards/STM32F4xx_Revolution.h @@ -32,6 +32,9 @@ #include +#define DEBUG_LEVEL 2 +#define DEBUG_PRINTF(level, ...) {if(level <= DEBUG_LEVEL && pios_com_aux_id > 0) { PIOS_COM_SendFormattedStringNonBlocking(pios_com_aux_id, __VA_ARGS__); }} + //------------------------ // Timers and Channels Used //------------------------ @@ -133,6 +136,26 @@ extern uint32_t pios_com_vcp_id; #define PIOS_COM_BRIDGE (pios_com_bridge_id) #define PIOS_COM_VCP (pios_com_vcp_id) #define PIOS_COM_DEBUG PIOS_COM_AUX +#if defined(PIOS_INCLUDE_RFM22B) +#define PIOS_COM_RFM22B_RF_RX_BUF_LEN 512 +#define PIOS_COM_RFM22B_RF_TX_BUF_LEN 512 +extern uint32_t pios_com_rfm22b_id; +#define PIOS_COM_RADIO (pios_com_rfm22b_id) +extern uint32_t pios_spi_rfm22b_id; +#define PIOS_RFM22_SPI_PORT (pios_spi_rfm22b_id) +#endif /* PIOS_INCLUDE_RFM22B */ + +//------------------------- +// Packet Handler +//------------------------- +#if defined(PIOS_INCLUDE_PACKET_HANDLER) +extern uint32_t pios_packet_handler; +#define PIOS_PACKET_HANDLER (pios_packet_handler) +#define PIOS_PH_MAX_PACKET 255 +#define PIOS_PH_WIN_SIZE 3 +#define PIOS_PH_MAX_CONNECTIONS 1 +#define RS_ECC_NPARITY 4 +#endif /* PIOS_INCLUDE_PACKET_HANDLER */ //------------------------ // TELEMETRY @@ -263,6 +286,16 @@ extern uint32_t pios_com_vcp_id; #define PIOS_USB_ENABLED 1 /* Should remove all references to this */ #define PIOS_USB_HID_MAX_DEVS 1 + +//------------------------- +// RFM22 +//------------------------- +#if defined(PIOS_INCLUDE_RFM22B) +extern uint32_t pios_rfm22b_id; +#define RFM22_EXT_INT_USE + +#endif /* PIOS_INCLUDE_RFM22B */ + #endif /* STM3210E_INS_H_ */ /** * @} diff --git a/flight/Revolution/Makefile b/flight/Revolution/Makefile index 88d154ba5..73462f033 100644 --- a/flight/Revolution/Makefile +++ b/flight/Revolution/Makefile @@ -57,6 +57,7 @@ MODULES += AltitudeHold VtolPathFollower FixedWingPathFollower PathPlanner MODULES += CameraStab MODULES += OveroSync MODULES += Telemetry +MODULES += Radio PYMODULES = #FlightPlan @@ -72,6 +73,7 @@ PIOSINC = $(PIOS)/inc OPMODULEDIR = ../Modules FLIGHTLIB = ../Libraries FLIGHTLIBINC = ../Libraries/inc +RSCODEINC = $(FLIGHTLIB)/rscode PIOSSTM32F4XX = $(PIOS)/STM32F4xx PIOSCOMMON = $(PIOS)/Common PIOSBOARDS = $(PIOS)/Boards @@ -141,6 +143,11 @@ SRC += $(FLIGHTLIB)/fifo_buffer.c SRC += $(FLIGHTLIB)/WorldMagModel.c SRC += $(FLIGHTLIB)/insgps13state.c SRC += $(FLIGHTLIB)/taskmonitor.c +SRC += $(FLIGHTLIB)/packet_handler.c +## The Reed-Solomon FEC library +SRC += $(FLIGHTLIB)/rscode/rs.c +SRC += $(FLIGHTLIB)/rscode/berlekamp.c +SRC += $(FLIGHTLIB)/rscode/galois.c ## PIOS Hardware (STM32F4xx) include $(PIOS)/STM32F4xx/library.mk @@ -162,6 +169,7 @@ SRC += $(PIOSCOMMON)/printf-stdarg.c SRC += $(PIOSCOMMON)/pios_usb_desc_hid_cdc.c SRC += $(PIOSCOMMON)/pios_usb_desc_hid_only.c SRC += $(PIOSCOMMON)/pios_usb_util.c +SRC += $(PIOSCOMMON)/pios_rfm22b.c include ./UAVObjects.inc SRC += $(UAVOBJSRC) @@ -203,6 +211,7 @@ EXTRAINCDIRS += $(OPUAVOBJ) EXTRAINCDIRS += $(OPUAVOBJINC) EXTRAINCDIRS += $(UAVOBJSYNTHDIR) EXTRAINCDIRS += $(FLIGHTLIBINC) +EXTRAINCDIRS += $(RSCODEINC) EXTRAINCDIRS += $(PIOSSTM32F4XX) EXTRAINCDIRS += $(PIOSCOMMON) EXTRAINCDIRS += $(PIOSBOARDS) diff --git a/flight/Revolution/System/inc/pios_config.h b/flight/Revolution/System/inc/pios_config.h index 4bb7be2a7..2886b6c09 100644 --- a/flight/Revolution/System/inc/pios_config.h +++ b/flight/Revolution/System/inc/pios_config.h @@ -73,6 +73,8 @@ #define PIOS_INCLUDE_COM_AUX #define PIOS_INCLUDE_COM_AUXSBUS #define PIOS_INCLUDE_COM_FLEXI +#define PIOS_INCLUDE_RFM22B +#define PIOS_INCLUDE_PACKET_HANDLER #define PIOS_INCLUDE_GPS #define PIOS_INCLUDE_GPS_NMEA_PARSER diff --git a/flight/Revolution/UAVObjects.inc b/flight/Revolution/UAVObjects.inc index 5b99daef4..6e1a513d8 100644 --- a/flight/Revolution/UAVObjects.inc +++ b/flight/Revolution/UAVObjects.inc @@ -91,6 +91,8 @@ UAVOBJSRCFILENAMES += altitudeholdsettings UAVOBJSRCFILENAMES += altitudeholddesired UAVOBJSRCFILENAMES += waypoint UAVOBJSRCFILENAMES += waypointactive +UAVOBJSRCFILENAMES += pipxsettings +UAVOBJSRCFILENAMES += pipxstatus UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/board_hw_defs/revolution/board_hw_defs.c b/flight/board_hw_defs/revolution/board_hw_defs.c index b54dd119d..91ca87ed8 100644 --- a/flight/board_hw_defs/revolution/board_hw_defs.c +++ b/flight/board_hw_defs/revolution/board_hw_defs.c @@ -447,6 +447,181 @@ void PIOS_SPI_flash_irq_handler(void) } #endif /* PIOS_FLASH_ON_ACCEL */ +#if defined(PIOS_INCLUDE_RFM22B) +#include +/* SPI3 Interface + * - Used for RFM22B radio + */ +void PIOS_SPI_rfm22b_irq_handler(void); +void DMA1_Stream0_IRQHandler(void) __attribute__((alias("PIOS_SPI_rfm22b_irq_handler"))); +void DMA1_Stream5_IRQHandler(void) __attribute__((alias("PIOS_SPI_rfm22b_irq_handler"))); +static const struct pios_spi_cfg pios_spi_rfm22b_cfg = { + .regs = SPI3, + .remap = GPIO_AF_SPI3, + .init = { + //.SPI_Mode = SPI_Mode_Slave, + .SPI_Mode = SPI_Mode_Master, + .SPI_Direction = SPI_Direction_2Lines_FullDuplex, + .SPI_DataSize = SPI_DataSize_8b, + //.SPI_NSS = SPI_NSS_Hard, + .SPI_NSS = SPI_NSS_Soft, + .SPI_FirstBit = SPI_FirstBit_MSB, + //.SPI_CRCPolynomial = 7, + .SPI_CRCPolynomial = 0, + //.SPI_CPOL = SPI_CPOL_High, + .SPI_CPOL = SPI_CPOL_Low, + //.SPI_CPHA = SPI_CPHA_2Edge, + .SPI_CPHA = SPI_CPHA_1Edge, + //.SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_2, + .SPI_BaudRatePrescaler = SPI_BaudRatePrescaler_256, + }, + .use_crc = false, + .dma = { + .irq = { + // Note this is the stream ID that triggers interrupts (in this case TX) + .flags = (DMA_IT_TCIF7), + //.flags = (DMA1_FLAG_TC2 | DMA1_FLAG_TE2 | DMA1_FLAG_HT2 | DMA1_FLAG_GL2), + .init = { + .NVIC_IRQChannel = DMA1_Stream7_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .channel = DMA1_Stream0, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t) & (SPI3->DR), + .DMA_DIR = DMA_DIR_PeripheralToMemory, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + //.DMA_Mode = DMA_Mode_Circular, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + //TODO: Enable FIFO + .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_Single, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + .tx = { + .channel = DMA1_Stream7, + .init = { + .DMA_Channel = DMA_Channel_0, + .DMA_PeripheralBaseAddr = (uint32_t) & (SPI3->DR), + .DMA_DIR = DMA_DIR_MemoryToPeripheral, + .DMA_PeripheralInc = DMA_PeripheralInc_Disable, + .DMA_MemoryInc = DMA_MemoryInc_Enable, + .DMA_PeripheralDataSize = DMA_PeripheralDataSize_Byte, + .DMA_MemoryDataSize = DMA_MemoryDataSize_Byte, + //.DMA_Mode = DMA_Mode_Circular, + .DMA_Mode = DMA_Mode_Normal, + .DMA_Priority = DMA_Priority_Medium, + .DMA_FIFOMode = DMA_FIFOMode_Disable, + .DMA_FIFOThreshold = DMA_FIFOThreshold_Full, + .DMA_MemoryBurst = DMA_MemoryBurst_Single, + .DMA_PeripheralBurst = DMA_PeripheralBurst_Single, + }, + }, + }, + .sclk = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .miso = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .mosi = { + .gpio = GPIOC, + .init = { + .GPIO_Pin = GPIO_Pin_12, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_NOPULL + }, + }, + .slave_count = 1, + .ssel = { { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_15, + .GPIO_Speed = GPIO_Speed_50MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + } }, +}; + +uint32_t pios_spi_rfm22b_id; +void PIOS_SPI_rfm22b_irq_handler(void) +{ + /* Call into the generic code to handle the IRQ for this specific device */ + PIOS_SPI_IRQ_Handler(pios_spi_rfm22b_id); +} + +static const struct pios_exti_cfg pios_exti_rfm22b_cfg __exti_config = { + .vector = PIOS_RFM22_EXT_Int, + .line = EXTI_Line0, + .pin = { + .gpio = GPIOD, + .init = { + .GPIO_Pin = GPIO_Pin_0, + .GPIO_Speed = GPIO_Speed_100MHz, + .GPIO_Mode = GPIO_Mode_IN, + .GPIO_OType = GPIO_OType_OD, + .GPIO_PuPd = GPIO_PuPd_NOPULL, + }, + }, + .irq = { + .init = { + .NVIC_IRQChannel = EXTI0_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_LOW, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .exti = { + .init = { + .EXTI_Line = EXTI_Line0, // matches above GPIO pin + .EXTI_Mode = EXTI_Mode_Interrupt, + .EXTI_Trigger = EXTI_Trigger_Falling, + .EXTI_LineCmd = ENABLE, + }, + }, +}; + +struct pios_rfm22b_cfg pios_rfm22b_cfg = { + .spi_cfg = &pios_spi_rfm22b_cfg, + .exti_cfg = &pios_exti_rfm22b_cfg, + .frequencyHz = 434000000, + .minFrequencyHz = 434000000 - 2000000, + .maxFrequencyHz = 434000000 + 2000000, + .RFXtalCap = 0x7f, + .maxRFBandwidth = 128000, + .maxTxPower = RFM22_tx_pwr_txpow_7, // +20dBm .. 100mW +}; + +#endif /* PIOS_INCLUDE_RFM22B */ + #endif /* PIOS_INCLUDE_SPI */ #if defined(PIOS_OVERO_SPI) diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 99d828452..862e8514a 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -18,7 +18,7 @@ - + From e602b73a6f2b5ddf80c37f5bf8087214ff4475da Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Fri, 24 Aug 2012 19:25:02 -0700 Subject: [PATCH 10/11] Split out the PipX radio interface from RadioComBridge module to a separate Radio module. Now the RadioComBridge module just routes messages between the com ports and handles sending/receiving the PipX UAVOs. Conflicts: flight/Modules/RadioComBridge/RadioComBridge.c flight/PipXtreme/System/pios_board.c Conflicts: flight/Modules/RadioComBridge/RadioComBridge.c --- .../Modules/RadioComBridge/RadioComBridge.c | 42 +++++++++++-------- 1 file changed, 25 insertions(+), 17 deletions(-) diff --git a/flight/Modules/RadioComBridge/RadioComBridge.c b/flight/Modules/RadioComBridge/RadioComBridge.c index 572eacf4d..02bf75f3f 100644 --- a/flight/Modules/RadioComBridge/RadioComBridge.c +++ b/flight/Modules/RadioComBridge/RadioComBridge.c @@ -55,8 +55,8 @@ #define BRIDGE_BUF_LEN 512 #define MAX_RETRIES 2 #define RETRY_TIMEOUT_MS 20 -#define STATS_UPDATE_PERIOD_MS 500 -#define RADIOSTATS_UPDATE_PERIOD_MS 250 +#define STATS_UPDATE_PERIOD_MS 1000 +#define RADIOSTATS_UPDATE_PERIOD_MS 500 #define MAX_LOST_CONTACT_TIME 4 #define PACKET_QUEUE_SIZE 10 #define MAX_PORT_DELAY 200 @@ -664,27 +664,35 @@ static void ppmInputTask(void *parameters) #endif /* PIOS_INCLUDE_WDG */ // Read the receiver. + bool valid_input_detected = false; for (uint8_t i = 1; i <= PIOS_PPM_NUM_INPUTS; ++i) + { ppm_packet.channels[i - 1] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i); - - // Send the PPM packet - if (data->ppmOutQueue) - { - ppm_packet.header.destination_id = data->destination_id; - ppm_packet.header.type = PACKET_TYPE_PPM; - ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&ppm_packet); - queueEvent(data->ppmOutQueue, (void*)pph, 0, EV_TRANSMIT_PACKET); + if(ppm_packet.channels[i - 1] != PIOS_RCVR_TIMEOUT) + valid_input_detected = true; } - else + + // Send the PPM packet if it's valid + if (valid_input_detected) { - GCSReceiverData rcvr; + if (data->ppmOutQueue) + { + ppm_packet.header.destination_id = data->destination_id; + ppm_packet.header.type = PACKET_TYPE_PPM; + ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&ppm_packet); + queueEvent(data->ppmOutQueue, (void*)pph, 0, EV_TRANSMIT_PACKET); + } + else + { + GCSReceiverData rcvr; - // Copy the receiver channels into the GCSReceiver object. - for (uint8_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; ++i) - rcvr.Channel[i] = ppm_packet.channels[i]; + // Copy the receiver channels into the GCSReceiver object. + for (uint8_t i = 0; i < GCSRECEIVER_CHANNEL_NUMELEM; ++i) + rcvr.Channel[i] = ppm_packet.channels[i]; - // Set the GCSReceiverData object. - GCSReceiverSet(&rcvr); + // Set the GCSReceiverData object. + GCSReceiverSet(&rcvr); + } } // Delay until the next update period. From b3dc6177022544879ec0bae31c8af2585ad26c3c Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 25 Aug 2012 12:52:06 -0700 Subject: [PATCH 11/11] Fixed PPM output from updated RadioComBridge module. --- .../Modules/RadioComBridge/RadioComBridge.c | 25 ++++++++----------- 1 file changed, 11 insertions(+), 14 deletions(-) diff --git a/flight/Modules/RadioComBridge/RadioComBridge.c b/flight/Modules/RadioComBridge/RadioComBridge.c index 02bf75f3f..8a92e27ee 100644 --- a/flight/Modules/RadioComBridge/RadioComBridge.c +++ b/flight/Modules/RadioComBridge/RadioComBridge.c @@ -91,10 +91,8 @@ typedef struct { UAVTalkConnection GCSUAVTalkCon; // Queue handles. - xQueueHandle radioPacketQueue; xQueueHandle gcsEventQueue; xQueueHandle uavtalkEventQueue; - xQueueHandle ppmOutQueue; // Error statistics. uint32_t comTxErrors; @@ -221,15 +219,12 @@ static int32_t RadioComBridgeInitialize(void) data->UAVTalkCon = 0; // Initialize the queues. - data->ppmOutQueue = 0; - data->radioPacketQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent)); data->gcsEventQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent)); if (PIOS_COM_UAVTALK) data->uavtalkEventQueue = xQueueCreate(PACKET_QUEUE_SIZE, sizeof(UAVObjEvent)); else { data->uavtalkEventQueue = 0; - data->ppmOutQueue = data->radioPacketQueue; } // Initialize the statistics. @@ -637,7 +632,7 @@ static void transparentCommTask(void * parameters) if (send_packet) { // Queue the packet for transmission. - queueEvent(data->radioPacketQueue, (void*)p, 0, EV_TRANSMIT_PACKET); + PHTransmitPacket(PIOS_PACKET_HANDLER, p); // Reset the timeout timeout = MAX_PORT_DELAY; @@ -675,14 +670,8 @@ static void ppmInputTask(void *parameters) // Send the PPM packet if it's valid if (valid_input_detected) { - if (data->ppmOutQueue) - { - ppm_packet.header.destination_id = data->destination_id; - ppm_packet.header.type = PACKET_TYPE_PPM; - ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&ppm_packet); - queueEvent(data->ppmOutQueue, (void*)pph, 0, EV_TRANSMIT_PACKET); - } - else + // Set the GCSReceiver UAVO if we're connected to the FC. + if (data->UAVTalkCon) { GCSReceiverData rcvr; @@ -693,6 +682,14 @@ static void ppmInputTask(void *parameters) // Set the GCSReceiverData object. GCSReceiverSet(&rcvr); } + else + { + // Otherwise, send a PPM packet over the radio link. + ppm_packet.header.destination_id = data->destination_id; + ppm_packet.header.type = PACKET_TYPE_PPM; + ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&ppm_packet); + PHTransmitPacket(PIOS_PACKET_HANDLER, pph); + } } // Delay until the next update period.