From 9090365ca7b094b6f470b020e56cbf6a0a2f4bbe Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Wed, 26 Jun 2013 13:14:19 +0300 Subject: [PATCH 01/28] Update CREDITS.txt --- CREDITS.txt | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/CREDITS.txt b/CREDITS.txt index c39940316..3b3897b02 100644 --- a/CREDITS.txt +++ b/CREDITS.txt @@ -26,10 +26,11 @@ Joe Hlebasko Andy Honecker Ryan Hunt Mark James -Sami Korhonen -Thorsten Klose Ricky King +Thorsten Klose +Sami Korhonen Hallvard Kristiansen +Alan Krum Edouard Lafargue Mike Labranche Fredrik Larsson From 5a9cee51b9212290e2a9165c97ed595f1faa4ea5 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Wed, 26 Jun 2013 20:29:04 +0300 Subject: [PATCH 02/28] Fix indentation in a .no-auto-format source file --- ground/openpilotgcs/src/plugins/ophid/hidapi/windows/hid.c | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/ophid/hidapi/windows/hid.c b/ground/openpilotgcs/src/plugins/ophid/hidapi/windows/hid.c index 099e52944..0b561a2b2 100644 --- a/ground/openpilotgcs/src/plugins/ophid/hidapi/windows/hid.c +++ b/ground/openpilotgcs/src/plugins/ophid/hidapi/windows/hid.c @@ -219,7 +219,7 @@ static HANDLE open_device(const char *path, BOOL enumerate) { HANDLE handle; DWORD desired_access = (enumerate)? 0: (GENERIC_WRITE | GENERIC_READ); - DWORD share_mode = FILE_SHARE_READ|FILE_SHARE_WRITE; + DWORD share_mode = FILE_SHARE_READ | FILE_SHARE_WRITE; handle = CreateFileA(path, desired_access, @@ -228,7 +228,7 @@ static HANDLE open_device(const char *path, BOOL enumerate) OPEN_EXISTING, FILE_FLAG_OVERLAPPED,/*FILE_ATTRIBUTE_NORMAL,*/ 0); - DWORD error = GetLastError(); + DWORD error = GetLastError(); return handle; } From ac75af55c1334da52354db360b203940cd9d9a6c Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Thu, 6 Jun 2013 17:51:13 -0700 Subject: [PATCH 03/28] OP-932 Moves the PIOS_PPM_OUT_Set function from pios_ppm_out_priv.h to pios_ppm_out.h --- flight/pios/inc/pios_ppm_out.h | 41 +++++++++++++++++++++++++++++ flight/pios/inc/pios_ppm_out_priv.h | 1 - 2 files changed, 41 insertions(+), 1 deletion(-) create mode 100644 flight/pios/inc/pios_ppm_out.h diff --git a/flight/pios/inc/pios_ppm_out.h b/flight/pios/inc/pios_ppm_out.h new file mode 100644 index 000000000..5efc0d416 --- /dev/null +++ b/flight/pios/inc/pios_ppm_out.h @@ -0,0 +1,41 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_PPM PPM Functions + * @brief PIOS interface to write to ppm port + * @{ + * + * @file pios_ppm_out_priv.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @brief ppm private structures. + * @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 PIOS_PPM_OUT_H +#define PIOS_PPM_OUT_H + +extern void PIOS_PPM_OUT_Set(uint32_t ppm_out_id, uint8_t servo, uint16_t position); + +#endif /* PIOS_PPM_H */ + +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_ppm_out_priv.h b/flight/pios/inc/pios_ppm_out_priv.h index 013599765..19e73d66b 100644 --- a/flight/pios/inc/pios_ppm_out_priv.h +++ b/flight/pios/inc/pios_ppm_out_priv.h @@ -39,7 +39,6 @@ struct pios_ppm_out_cfg { }; extern int32_t PIOS_PPM_Out_Init(uint32_t *ppm_out_id, const struct pios_ppm_out_cfg *cfg); -extern void PIOS_PPM_OUT_Set(uint32_t ppm_out_id, uint8_t servo, uint16_t position); #endif /* PIOS_PPM_PRIV_H */ From 7c10623615e314c656ec7e55cc77081adbf69227 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Thu, 6 Jun 2013 18:08:45 -0700 Subject: [PATCH 04/28] OP-932 Adds a semaphore to PPM input to signal when a new frame has been received. This can be used to process a new frame with as low a latency as possible. --- flight/pios/common/pios_rcvr.c | 32 +++++++++++++++++++++ flight/pios/inc/pios_ppm_priv.h | 3 -- flight/pios/inc/pios_rcvr.h | 2 ++ flight/pios/stm32f10x/pios_ppm.c | 49 ++++++++++++++++++++++++++++++-- flight/pios/stm32f4xx/pios_ppm.c | 46 +++++++++++++++++++++++++++++- 5 files changed, 126 insertions(+), 6 deletions(-) diff --git a/flight/pios/common/pios_rcvr.c b/flight/pios/common/pios_rcvr.c index 5e2e856da..db46b1fd7 100644 --- a/flight/pios/common/pios_rcvr.c +++ b/flight/pios/common/pios_rcvr.c @@ -113,6 +113,38 @@ int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel) return rcvr_dev->driver->read(rcvr_dev->lower_id, channel); } +/** + * @brief Get a semaphore that signals when a new sample is available. + * @param[in] rcvr_id driver to read from + * @param[in] channel channel to read + * @returns The semaphore, or NULL if not supported. + */ +xSemaphoreHandle PIOS_RCVR_GetSemaphore(uint32_t rcvr_id, uint8_t channel) +{ + // Publicly facing API uses channel 1 for first channel + if (channel == 0) { + return NULL; + } else { + channel--; + } + + if (rcvr_id == 0) { + return NULL; + } + + struct pios_rcvr_dev *rcvr_dev = (struct pios_rcvr_dev *)rcvr_id; + + if (!PIOS_RCVR_validate(rcvr_dev)) { + /* Undefined RCVR port for this board (see pios_board.c) */ + PIOS_Assert(0); + } + + if (rcvr_dev->driver->get_semaphore) { + return rcvr_dev->driver->get_semaphore(rcvr_dev->lower_id, channel); + } + return NULL; +} + #endif /* PIOS_INCLUDE_RCVR */ /** diff --git a/flight/pios/inc/pios_ppm_priv.h b/flight/pios/inc/pios_ppm_priv.h index 66ccd6eb7..f319b78c0 100644 --- a/flight/pios/inc/pios_ppm_priv.h +++ b/flight/pios/inc/pios_ppm_priv.h @@ -31,9 +31,6 @@ #ifndef PIOS_PPM_PRIV_H #define PIOS_PPM_PRIV_H -#include -#include - struct pios_ppm_cfg { TIM_ICInitTypeDef tim_ic_init; const struct pios_tim_channel *channels; diff --git a/flight/pios/inc/pios_rcvr.h b/flight/pios/inc/pios_rcvr.h index 450e04628..74fac086f 100644 --- a/flight/pios/inc/pios_rcvr.h +++ b/flight/pios/inc/pios_rcvr.h @@ -34,10 +34,12 @@ struct pios_rcvr_driver { void (*init)(uint32_t id); int32_t (*read)(uint32_t id, uint8_t channel); + xSemaphoreHandle (*get_semaphore)(uint32_t id, uint8_t channel); }; /* Public Functions */ extern int32_t PIOS_RCVR_Read(uint32_t rcvr_id, uint8_t channel); +extern xSemaphoreHandle PIOS_RCVR_GetSemaphore(uint32_t rcvr_id, uint8_t channel); /*! Define error codes for PIOS_RCVR_Get */ enum PIOS_RCVR_errors { diff --git a/flight/pios/stm32f10x/pios_ppm.c b/flight/pios/stm32f10x/pios_ppm.c index 39bb2618d..47cf034c0 100644 --- a/flight/pios/stm32f10x/pios_ppm.c +++ b/flight/pios/stm32f10x/pios_ppm.c @@ -32,14 +32,19 @@ #ifdef PIOS_INCLUDE_PPM -#include "pios_ppm_priv.h" +#include +#include "pios_ppm_priv.h" /* Provide a RCVR driver */ static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel); +static xSemaphoreHandle PIOS_PPM_Get_Semaphore(uint32_t rcvr_id, uint8_t channel); const struct pios_rcvr_driver pios_ppm_rcvr_driver = { - .read = PIOS_PPM_Get, + .read = PIOS_PPM_Get, +#if defined(PIOS_INCLUDE_FREERTOS) + .get_semaphore = PIOS_PPM_Get_Semaphore +#endif }; #define PIOS_PPM_IN_MIN_NUM_CHANNELS 4 @@ -76,6 +81,10 @@ struct pios_ppm_dev { uint8_t supv_timer; bool Tracking; bool Fresh; + +#ifdef PIOS_INCLUDE_FREERTOS + xSemaphoreHandle new_sample_semaphores[PIOS_PPM_IN_MIN_NUM_CHANNELS]; +#endif /* PIOS_INCLUDE_FREERTOS */ }; static bool PIOS_PPM_validate(struct pios_ppm_dev *ppm_dev) @@ -93,6 +102,11 @@ static struct pios_ppm_dev *PIOS_PPM_alloc(void) return NULL; } + // Initialize the semaphores to 0. + for (uint8_t i = 0; i < PIOS_PPM_IN_MIN_NUM_CHANNELS; ++i) { + ppm_dev->new_sample_semaphores[i] = 0; + } + ppm_dev->magic = PIOS_PPM_DEV_MAGIC; return ppm_dev; } @@ -199,6 +213,28 @@ out_fail: return -1; } +#if defined(PIOS_INCLUDE_FREERTOS) +static xSemaphoreHandle PIOS_PPM_Get_Semaphore(uint32_t rcvr_id, uint8_t channel) +{ + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)rcvr_id; + + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return 0; + } + + if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) { + /* Channel out of range */ + return 0; + } + + if (ppm_dev->new_sample_semaphores[channel] == 0) { + vSemaphoreCreateBinary(ppm_dev->new_sample_semaphores[channel]); + } + return ppm_dev->new_sample_semaphores[channel]; +} +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ + /** * Get the value of an input channel * \param[in] channel Number of the channel desired (zero based) @@ -296,6 +332,15 @@ static void PIOS_PPM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; } +#if defined(PIOS_INCLUDE_FREERTOS) + /* Signal that a new sample is ready on this channel. */ + if (ppm_dev->new_sample_semaphores[chan_idx] != 0) { + signed portBASE_TYPE pxHigherPriorityTaskWoken = pdFALSE; + if (xSemaphoreGiveFromISR(ppm_dev->new_sample_semaphores[chan_idx], &pxHigherPriorityTaskWoken) == pdTRUE) { + portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); /* FIXME: is this the right place for this? */ + } + } +#endif /* USE_FREERTOS */ } ppm_dev->Fresh = TRUE; diff --git a/flight/pios/stm32f4xx/pios_ppm.c b/flight/pios/stm32f4xx/pios_ppm.c index 91c5d2ea8..c5f9534f4 100644 --- a/flight/pios/stm32f4xx/pios_ppm.c +++ b/flight/pios/stm32f4xx/pios_ppm.c @@ -36,9 +36,13 @@ /* Provide a RCVR driver */ static int32_t PIOS_PPM_Get(uint32_t rcvr_id, uint8_t channel); +static xSemaphoreHandle PIOS_PPM_Get_Semaphore(uint32_t rcvr_id, uint8_t channel); const struct pios_rcvr_driver pios_ppm_rcvr_driver = { - .read = PIOS_PPM_Get, + .read = PIOS_PPM_Get, +#if defined(PIOS_INCLUDE_FREERTOS) + .get_semaphore = PIOS_PPM_Get_Semaphore +#endif }; #define PIOS_PPM_IN_MIN_NUM_CHANNELS 4 @@ -72,6 +76,10 @@ struct pios_ppm_dev { uint8_t supv_timer; bool Tracking; bool Fresh; + +#ifdef PIOS_INCLUDE_FREERTOS + xSemaphoreHandle new_sample_semaphores[PIOS_PPM_IN_MIN_NUM_CHANNELS]; +#endif /* PIOS_INCLUDE_FREERTOS */ }; static bool PIOS_PPM_validate(struct pios_ppm_dev *ppm_dev) @@ -89,6 +97,11 @@ static struct pios_ppm_dev *PIOS_PPM_alloc(void) return NULL; } + // Initialize the semaphores to 0. + for (uint8_t i = 0; i < PIOS_PPM_IN_MIN_NUM_CHANNELS; ++i) { + ppm_dev->new_sample_semaphores[i] = 0; + } + ppm_dev->magic = PIOS_PPM_DEV_MAGIC; return ppm_dev; } @@ -194,6 +207,28 @@ out_fail: return -1; } +#if defined(PIOS_INCLUDE_FREERTOS) +static xSemaphoreHandle PIOS_PPM_Get_Semaphore(uint32_t rcvr_id, uint8_t channel) +{ + struct pios_ppm_dev *ppm_dev = (struct pios_ppm_dev *)rcvr_id; + + if (!PIOS_PPM_validate(ppm_dev)) { + /* Invalid device specified */ + return 0; + } + + if (channel >= PIOS_PPM_IN_MAX_NUM_CHANNELS) { + /* Channel out of range */ + return 0; + } + + if (ppm_dev->new_sample_semaphores[channel] == 0) { + vSemaphoreCreateBinary(ppm_dev->new_sample_semaphores[channel]); + } + return ppm_dev->new_sample_semaphores[channel]; +} +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ + /** * Get the value of an input channel * \param[in] channel Number of the channel desired (zero based) @@ -286,6 +321,15 @@ static void PIOS_PPM_tim_edge_cb(__attribute__((unused)) uint32_t tim_id, uint32 i < PIOS_PPM_IN_MAX_NUM_CHANNELS; i++) { ppm_dev->CaptureValue[i] = PIOS_RCVR_TIMEOUT; } +#if defined(PIOS_INCLUDE_FREERTOS) + /* Signal that a new sample is ready on this channel. */ + if (ppm_dev->new_sample_semaphores[chan_idx] != 0) { + signed portBASE_TYPE pxHigherPriorityTaskWoken = pdFALSE; + if (xSemaphoreGiveFromISR(ppm_dev->new_sample_semaphores[chan_idx], &pxHigherPriorityTaskWoken) == pdTRUE) { + portEND_SWITCHING_ISR(pxHigherPriorityTaskWoken); /* FIXME: is this the right place for his? */ + } + } +#endif /* USE_FREERTOS */ } ppm_dev->Fresh = true; From 08efc8d15211f9e82b598fa8c98ec350e1d698ed Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Thu, 6 Jun 2013 17:43:47 -0700 Subject: [PATCH 05/28] OP-932: Adds an oplink receiver device, which is based on the gcs receiver device, but uses the OPLinkReceiver UAVObject. The OPLinkReceiver packets are generated on the OPLink modem. --- flight/pios/common/pios_oplinkrcvr.c | 194 ++++++++++++++++++ flight/pios/inc/pios_oplinkrcvr_priv.h | 45 ++++ .../coptercontrol/firmware/inc/pios_config.h | 1 + .../oplinkmini/firmware/inc/pios_config.h | 3 +- .../boards/osd/firmware/inc/pios_config.h | 1 + .../revolution/firmware/inc/pios_config.h | 1 + .../revoproto/firmware/inc/pios_config.h | 1 + .../simposix/firmware/inc/pios_config.h | 3 +- make/apps-defs.mk | 1 + shared/uavobjectdefinition/oplinkreceiver.xml | 10 + 10 files changed, 258 insertions(+), 2 deletions(-) create mode 100644 flight/pios/common/pios_oplinkrcvr.c create mode 100644 flight/pios/inc/pios_oplinkrcvr_priv.h create mode 100644 shared/uavobjectdefinition/oplinkreceiver.xml diff --git a/flight/pios/common/pios_oplinkrcvr.c b/flight/pios/common/pios_oplinkrcvr.c new file mode 100644 index 000000000..51574406f --- /dev/null +++ b/flight/pios/common/pios_oplinkrcvr.c @@ -0,0 +1,194 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_OPLinkRCVR OPLink Receiver Input Functions + * @brief Code to read the channels within the OPLinkReceiver UAVObject + * @{ + * + * @file pios_opinkrcvr.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @brief GCS Input functions (STM32 dependent) + * @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 + +#ifdef PIOS_INCLUDE_OPLINKRCVR + +#include +#include +#include + +static OPLinkReceiverData oplinkreceiverdata; + +/* Provide a RCVR driver */ +static int32_t PIOS_OPLinkRCVR_Get(uint32_t rcvr_id, uint8_t channel); +static void PIOS_oplinkrcvr_Supervisor(uint32_t ppm_id); + +const struct pios_rcvr_driver pios_oplinkrcvr_rcvr_driver = { + .read = PIOS_OPLinkRCVR_Get, +}; + +/* Local Variables */ +enum pios_oplinkrcvr_dev_magic { + PIOS_OPLINKRCVR_DEV_MAGIC = 0x07ab9e2544cf5029, +}; + +struct pios_oplinkrcvr_dev { + enum pios_oplinkrcvr_dev_magic magic; + + uint8_t supv_timer; + bool Fresh; +}; + +static struct pios_oplinkrcvr_dev *global_oplinkrcvr_dev; + +static bool PIOS_oplinkrcvr_validate(struct pios_oplinkrcvr_dev *oplinkrcvr_dev) +{ + return oplinkrcvr_dev->magic == PIOS_OPLINKRCVR_DEV_MAGIC; +} + +#if defined(PIOS_INCLUDE_FREERTOS) +static struct pios_oplinkrcvr_dev *PIOS_oplinkrcvr_alloc(void) +{ + struct pios_oplinkrcvr_dev *oplinkrcvr_dev; + + oplinkrcvr_dev = (struct pios_oplinkrcvr_dev *)pvPortMalloc(sizeof(*oplinkrcvr_dev)); + if (!oplinkrcvr_dev) { + return NULL; + } + + oplinkrcvr_dev->magic = PIOS_OPLINKRCVR_DEV_MAGIC; + oplinkrcvr_dev->Fresh = false; + oplinkrcvr_dev->supv_timer = 0; + + /* The update callback cannot receive the device pointer, so set it in a global */ + global_oplinkrcvr_dev = oplinkrcvr_dev; + + return oplinkrcvr_dev; +} +#else +static struct pios_oplinkrcvr_dev pios_oplinkrcvr_devs[PIOS_OPLINKRCVR_MAX_DEVS]; +static uint8_t pios_oplinkrcvr_num_devs; +static struct pios_oplinkrcvr_dev *PIOS_oplinkrcvr_alloc(void) +{ + struct pios_oplinkrcvr_dev *oplinkrcvr_dev; + + if (pios_oplinkrcvr_num_devs >= PIOS_OPLINKRCVR_MAX_DEVS) { + return NULL; + } + + oplinkrcvr_dev = &pios_oplinkrcvr_devs[pios_oplinkrcvr_num_devs++]; + oplinkrcvr_dev->magic = PIOS_OPLINKRCVR_DEV_MAGIC; + oplinkrcvr_dev->Fresh = false; + oplinkrcvr_dev->supv_timer = 0; + + global_oplinkrcvr_dev = oplinkrcvr_dev; + + return oplinkrcvr_dev; +} +#endif /* if defined(PIOS_INCLUDE_FREERTOS) */ + +static void oplinkreceiver_updated(UAVObjEvent *ev) +{ + struct pios_oplinkrcvr_dev *oplinkrcvr_dev = global_oplinkrcvr_dev; + + if (ev->obj == OPLinkReceiverHandle()) { + OPLinkReceiverGet(&oplinkreceiverdata); + oplinkrcvr_dev->Fresh = true; + } +} + +extern int32_t PIOS_OPLinkRCVR_Init(__attribute__((unused)) uint32_t *oplinkrcvr_id) +{ + struct pios_oplinkrcvr_dev *oplinkrcvr_dev; + + /* Allocate the device structure */ + oplinkrcvr_dev = (struct pios_oplinkrcvr_dev *)PIOS_oplinkrcvr_alloc(); + if (!oplinkrcvr_dev) { + return -1; + } + + for (uint8_t i = 0; i < OPLINKRECEIVER_CHANNEL_NUMELEM; i++) { + /* Flush channels */ + oplinkreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT; + } + + /* Register uavobj callback */ + OPLinkReceiverConnectCallback(oplinkreceiver_updated); + + /* Register the failsafe timer callback. */ + if (!PIOS_RTC_RegisterTickCallback(PIOS_oplinkrcvr_Supervisor, (uint32_t)oplinkrcvr_dev)) { + PIOS_DEBUG_Assert(0); + } + + return 0; +} + +/** + * Get the value of an input channel + * \param[in] channel Number of the channel desired (zero based) + * \output PIOS_RCVR_INVALID channel not available + * \output PIOS_RCVR_TIMEOUT failsafe condition or missing receiver + * \output >=0 channel value + */ +static int32_t PIOS_OPLinkRCVR_Get(__attribute__((unused)) uint32_t rcvr_id, uint8_t channel) +{ + if (channel >= OPLINKRECEIVER_CHANNEL_NUMELEM) { + /* channel is out of range */ + return PIOS_RCVR_INVALID; + } + + return oplinkreceiverdata.Channel[channel]; +} + +static void PIOS_oplinkrcvr_Supervisor(uint32_t oplinkrcvr_id) +{ + /* Recover our device context */ + struct pios_oplinkrcvr_dev *oplinkrcvr_dev = (struct pios_oplinkrcvr_dev *)oplinkrcvr_id; + + if (!PIOS_oplinkrcvr_validate(oplinkrcvr_dev)) { + /* Invalid device specified */ + return; + } + + /* + * RTC runs at 625Hz. + */ + if (++(oplinkrcvr_dev->supv_timer) < (PIOS_OPLINK_RCVR_TIMEOUT_MS * 1000 / 625)) { + return; + } + oplinkrcvr_dev->supv_timer = 0; + + if (!oplinkrcvr_dev->Fresh) { + for (int32_t i = 0; i < OPLINKRECEIVER_CHANNEL_NUMELEM; i++) { + oplinkreceiverdata.Channel[i] = PIOS_RCVR_TIMEOUT; + } + } + + oplinkrcvr_dev->Fresh = false; +} + +#endif /* PIOS_INCLUDE_OPLINKRCVR */ + +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_oplinkrcvr_priv.h b/flight/pios/inc/pios_oplinkrcvr_priv.h new file mode 100644 index 000000000..84c46b0bf --- /dev/null +++ b/flight/pios/inc/pios_oplinkrcvr_priv.h @@ -0,0 +1,45 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_OPLinkRCVR OPLink Receiver Functions + * @brief PIOS interface to read from OPLink receiver port + * @{ + * + * @file pios_oplinkrcvr_priv.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2013. + * @brief OPLINK receiver private functions + * @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 PIOS_OPLINKRCVR_PRIV_H +#define PIOS_OPLINKRCVR_PRIV_H + +#include + +extern const struct pios_rcvr_driver pios_oplinkrcvr_rcvr_driver; + +extern int32_t PIOS_OPLinkRCVR_Init(uint32_t *oplinkrcvr_id); + +#endif /* PIOS_OPLINKRCVR_PRIV_H */ + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index 7d53b585e..26527d716 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -93,6 +93,7 @@ #define PIOS_INCLUDE_DSM #define PIOS_INCLUDE_SBUS #define PIOS_INCLUDE_GCSRCVR +/* #define PIOS_INCLUDE_OPLINKRCVR */ /* PIOS abstract receiver interface */ #define PIOS_INCLUDE_RCVR diff --git a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h index 69ab3ee05..992cb67f3 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h @@ -92,7 +92,8 @@ /* #define PIOS_INCLUDE_PPM_FLEXI */ /* #define PIOS_INCLUDE_DSM */ /* #define PIOS_INCLUDE_SBUS */ -#define PIOS_INCLUDE_GCSRCVR +/* #define PIOS_INCLUDE_GCSRCVR */ +/* #define PIOS_INCLUDE_OPLINKRCVR */ /* PIOS abstract receiver interface */ #define PIOS_INCLUDE_RCVR diff --git a/flight/targets/boards/osd/firmware/inc/pios_config.h b/flight/targets/boards/osd/firmware/inc/pios_config.h index a0c7d6362..f4fbdf4be 100644 --- a/flight/targets/boards/osd/firmware/inc/pios_config.h +++ b/flight/targets/boards/osd/firmware/inc/pios_config.h @@ -93,6 +93,7 @@ /* #define PIOS_INCLUDE_DSM */ /* #define PIOS_INCLUDE_SBUS */ /* #define PIOS_INCLUDE_GCSRCVR */ +/* #define PIOS_INCLUDE_OPLINKRCVR */ /* PIOS abstract receiver interface */ /* #define PIOS_INCLUDE_RCVR */ diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index f170eebfe..ab74c31a0 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -93,6 +93,7 @@ #define PIOS_INCLUDE_DSM #define PIOS_INCLUDE_SBUS #define PIOS_INCLUDE_GCSRCVR +#define PIOS_INCLUDE_OPLINKRCVR /* PIOS abstract receiver interface */ #define PIOS_INCLUDE_RCVR diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config.h b/flight/targets/boards/revoproto/firmware/inc/pios_config.h index 30be6bb7a..8c5268885 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_config.h @@ -93,6 +93,7 @@ #define PIOS_INCLUDE_DSM #define PIOS_INCLUDE_SBUS #define PIOS_INCLUDE_GCSRCVR +/* #define PIOS_INCLUDE_OPLINKRCVR */ /* PIOS abstract receiver interface */ #define PIOS_INCLUDE_RCVR diff --git a/flight/targets/boards/simposix/firmware/inc/pios_config.h b/flight/targets/boards/simposix/firmware/inc/pios_config.h index 70e3ed2b6..ebdcf654d 100644 --- a/flight/targets/boards/simposix/firmware/inc/pios_config.h +++ b/flight/targets/boards/simposix/firmware/inc/pios_config.h @@ -84,7 +84,8 @@ // #define PIOS_INCLUDE_SBUS #define PIOS_INCLUDE_PPM #define PIOS_INCLUDE_PWM -// #define PIOS_INCLUDE_GCSRCVR +/* #define PIOS_INCLUDE_GCSRCVR */ +/* #define PIOS_INCLUDE_OPLINKRCVR */ #define PIOS_INCLUDE_IAP #define PIOS_INCLUDE_BL_HELPER diff --git a/make/apps-defs.mk b/make/apps-defs.mk index 0b764e898..dd7c3dc77 100644 --- a/make/apps-defs.mk +++ b/make/apps-defs.mk @@ -70,6 +70,7 @@ SRC += $(PIOSCOMMON)/pios_l3gd20.c SRC += $(PIOSCOMMON)/pios_mpu6000.c SRC += $(PIOSCOMMON)/pios_mpxv.c SRC += $(PIOSCOMMON)/pios_ms5611.c +SRC += $(PIOSCOMMON)/pios_oplinkrcvr.c SRC += $(PIOSCOMMON)/pios_video.c SRC += $(PIOSCOMMON)/pios_wavplay.c diff --git a/shared/uavobjectdefinition/oplinkreceiver.xml b/shared/uavobjectdefinition/oplinkreceiver.xml new file mode 100644 index 000000000..96e3187af --- /dev/null +++ b/shared/uavobjectdefinition/oplinkreceiver.xml @@ -0,0 +1,10 @@ + + + A receiver channel group carried over the OPLink radio. + + + + + + + From 12c8ef2e3a6614a1983d50a3a7dc5704f88829ef Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 15 Jun 2013 07:31:29 -0700 Subject: [PATCH 06/28] OP-932 Changes the UAVTalkRelayInputStream function to UAVTalkRelayPacket to allow for not relaying a packet on error, etc. Also adds a function to get the object ID out of the current packet (UAVTalkGetPacketObjId). These functions are used by the OPLink. --- flight/uavtalk/inc/uavtalk.h | 4 +- flight/uavtalk/uavtalk.c | 204 +++++++++++++++++++---------------- 2 files changed, 116 insertions(+), 92 deletions(-) diff --git a/flight/uavtalk/inc/uavtalk.h b/flight/uavtalk/inc/uavtalk.h index cfbeef471..7b35606ff 100644 --- a/flight/uavtalk/inc/uavtalk.h +++ b/flight/uavtalk/inc/uavtalk.h @@ -59,10 +59,12 @@ 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); -UAVTalkRxState UAVTalkRelayInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte); +UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle); +int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle); void UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats); void UAVTalkResetStats(UAVTalkConnection connection); void UAVTalkGetLastTimestamp(UAVTalkConnection connection, uint16_t *timestamp); +uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connection); #endif // UAVTALK_H /** diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index 5ab2dee04..ae6031feb 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -287,7 +287,7 @@ static int32_t objectTransaction(UAVTalkConnectionData *connection, UAVObjHandle /** * Process an byte from the telemetry stream. - * \param[in] connection UAVTalkConnection to be used + * \param[in] connectionHandle UAVTalkConnection to be used * \param[in] rxbyte Received byte * \return UAVTalkRxState */ @@ -534,83 +534,117 @@ UAVTalkRxState UAVTalkProcessInputStream(UAVTalkConnection connectionHandle, uin UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbyte); if (state == UAVTALK_STATE_COMPLETE) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle, connection, return -1); - UAVTalkInputProcessor *iproc = &connection->iproc; - - xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); - receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length); - xSemaphoreGiveRecursive(connection->lock); + UAVTalkReceiveObject(connectionHandle); } return state; } /** - * Process an byte from the telemetry stream, sending the packet out the output stream when it's complete - * This allows the interlieving of packets on an output UAVTalk stream, and is used by the OPLink device to - * relay packets from an input com port to a different output com port without sending one packet in the middle - * of another packet. Because this uses both the receive buffer and transmit buffer, it should only be used - * for relaying a packet, not for the standard sending and receiving of packets. + * Send a parsed packet received on one connection handle out on a different connection handle. + * The packet must be in a complete state, meaning it is completed parsing. + * The packet is re-assembled from the component parts into a complete message and sent. + * This can be used to relay packets from one UAVTalk connection to another. * \param[in] connection UAVTalkConnection to be used * \param[in] rxbyte Received byte - * \return UAVTalkRxState + * \return 0 Success + * \return -1 Failure */ -UAVTalkRxState UAVTalkRelayInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte) +UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle) { - UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(connectionHandle, rxbyte); + UAVTalkConnectionData *inConnection; - if (state == UAVTALK_STATE_COMPLETE) { - UAVTalkConnectionData *connection; - CHECKCONHANDLE(connectionHandle, connection, return -1); - UAVTalkInputProcessor *iproc = &connection->iproc; + CHECKCONHANDLE(inConnectionHandle, inConnection, return -1); + UAVTalkInputProcessor *inIproc = &inConnection->iproc; - if (!connection->outStream) { - return -1; - } - - // Setup type and object id fields - connection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte - connection->txBuffer[1] = iproc->type; - // data length inserted here below - connection->txBuffer[4] = (uint8_t)(iproc->objId & 0xFF); - connection->txBuffer[5] = (uint8_t)((iproc->objId >> 8) & 0xFF); - connection->txBuffer[6] = (uint8_t)((iproc->objId >> 16) & 0xFF); - connection->txBuffer[7] = (uint8_t)((iproc->objId >> 24) & 0xFF); - - // Setup instance ID if one is required - int32_t dataOffset = 8; - if (iproc->instanceLength > 0) { - connection->txBuffer[8] = (uint8_t)(iproc->instId & 0xFF); - connection->txBuffer[9] = (uint8_t)((iproc->instId >> 8) & 0xFF); - dataOffset = 10; - } - - // Add timestamp when the transaction type is appropriate - if (iproc->type & UAVTALK_TIMESTAMPED) { - portTickType time = xTaskGetTickCount(); - connection->txBuffer[dataOffset] = (uint8_t)(time & 0xFF); - connection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF); - dataOffset += 2; - } - - // Copy data (if any) - memcpy(&connection->txBuffer[dataOffset], connection->rxBuffer, iproc->length); - - // Store the packet length - connection->txBuffer[2] = (uint8_t)((dataOffset + iproc->length) & 0xFF); - connection->txBuffer[3] = (uint8_t)(((dataOffset + iproc->length) >> 8) & 0xFF); - - // Copy the checksum - connection->txBuffer[dataOffset + iproc->length] = iproc->cs; - - // Send the buffer. - if (UAVTalkSendBuf(connectionHandle, connection->txBuffer, iproc->rxPacketLength) < 0) { - return UAVTALK_STATE_ERROR; - } + // The input packet must be completely parsed. + if (inIproc->state != UAVTALK_STATE_COMPLETE) { + return -1; } - return state; + UAVTalkConnectionData *outConnection; + CHECKCONHANDLE(outConnectionHandle, outConnection, return -1); + + if (!outConnection->outStream) { + return -1; + } + + // Lock + xSemaphoreTakeRecursive(outConnection->lock, portMAX_DELAY); + + // Setup type and object id fields + outConnection->txBuffer[0] = UAVTALK_SYNC_VAL; // sync byte + outConnection->txBuffer[1] = inIproc->type; + // data length inserted here below + int32_t dataOffset = 8; + if (inIproc->objId != 0) { + outConnection->txBuffer[4] = (uint8_t)(inIproc->objId & 0xFF); + outConnection->txBuffer[5] = (uint8_t)((inIproc->objId >> 8) & 0xFF); + outConnection->txBuffer[6] = (uint8_t)((inIproc->objId >> 16) & 0xFF); + outConnection->txBuffer[7] = (uint8_t)((inIproc->objId >> 24) & 0xFF); + + // Setup instance ID if one is required + if (inIproc->instanceLength > 0) { + outConnection->txBuffer[8] = (uint8_t)(inIproc->instId & 0xFF); + outConnection->txBuffer[9] = (uint8_t)((inIproc->instId >> 8) & 0xFF); + dataOffset = 10; + } + } else { + dataOffset = 4; + } + + // Add timestamp when the transaction type is appropriate + if (inIproc->type & UAVTALK_TIMESTAMPED) { + portTickType time = xTaskGetTickCount(); + outConnection->txBuffer[dataOffset] = (uint8_t)(time & 0xFF); + outConnection->txBuffer[dataOffset + 1] = (uint8_t)((time >> 8) & 0xFF); + dataOffset += 2; + } + + // Copy data (if any) + memcpy(&outConnection->txBuffer[dataOffset], inConnection->rxBuffer, inIproc->length); + + // Store the packet length + outConnection->txBuffer[2] = (uint8_t)((dataOffset + inIproc->length) & 0xFF); + outConnection->txBuffer[3] = (uint8_t)(((dataOffset + inIproc->length) >> 8) & 0xFF); + + // Copy the checksum + outConnection->txBuffer[dataOffset + inIproc->length] = inIproc->cs; + + // Send the buffer. + int32_t rc = (*outConnection->outStream)(outConnection->txBuffer, inIproc->rxPacketLength); + + // Update stats + outConnection->stats.txBytes += rc; + + // Release lock + xSemaphoreGiveRecursive(outConnection->lock); + + // Done + if (rc != inIproc->rxPacketLength) { + return -1; + } + + return 0; +} + +/** + * Complete receiving a UAVTalk packet. This will cause the packet to be unpacked, acked, etc. + * \param[in] connectionHandle UAVTalkConnection to be used + * \return 0 Success + * \return -1 Failure + */ +int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle) +{ + UAVTalkConnectionData *connection; + + CHECKCONHANDLE(connectionHandle, connection, return -1); + UAVTalkInputProcessor *iproc = &connection->iproc; + if (iproc->state != UAVTALK_STATE_COMPLETE) { + return -1; + } + receiveObject(connection, iproc->type, iproc->objId, iproc->instId, connection->rxBuffer, iproc->length); + return 0; } /** @@ -662,37 +696,17 @@ int32_t UAVTalkSendNack(UAVTalkConnection connectionHandle, uint32_t objId) } /** - * 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 + * Get the object ID of the current packet. + * \param[in] connectionHandle UAVTalkConnection to be used + * \param[in] objId Object ID to send a NACK for + * \return The object ID, or 0 on error. */ -int32_t UAVTalkSendBuf(UAVTalkConnection connectionHandle, uint8_t *buf, uint16_t len) +uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connectionHandle) { 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; + CHECKCONHANDLE(connectionHandle, connection, return 0); + return connection->iproc.objId; } /** @@ -716,6 +730,9 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, UAVObjHandle obj; int32_t ret = 0; + // Lock + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + // Get the handle to the Object. Will be zero // if object does not exist. obj = UAVObjGetByID(objId); @@ -734,6 +751,7 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, ret = -1; } break; + case UAVTALK_TYPE_OBJ_ACK: case UAVTALK_TYPE_OBJ_ACK_TS: // All instances, not allowed for OBJ_ACK messages @@ -772,6 +790,10 @@ static int32_t receiveObject(UAVTalkConnectionData *connection, default: ret = -1; } + + // Unlock + xSemaphoreGiveRecursive(connection->lock); + // Done return ret; } From 8c370df3190d1b05f5070136c2c4a508a4af4f5e Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 15 Jun 2013 13:50:13 -0700 Subject: [PATCH 07/28] OP-932 A significant re-work of the OPLink and configuration of the OPLink. Auto-configuration is removed, and remote modems are bound the coordinator, rather than the other way around. Timing of the radio link is also significantly changed. The rfm22b code is also simplified so that it only sends packet, and has no notion about what those packets are, so the rfm22b is converteded to simply a com type device. --- flight/libraries/inc/packet_handler.h | 111 - flight/modules/OPLink/oplinkmod.c | 4 +- .../modules/RadioComBridge/RadioComBridge.c | 484 ++-- flight/modules/System/systemmod.c | 53 +- flight/pios/common/pios_rfm22b.c | 1162 +++------- flight/pios/common/pios_rfm22b_rcvr.c | 2 + flight/pios/inc/pios_rfm22b.h | 19 +- flight/pios/inc/pios_rfm22b_priv.h | 128 +- .../oplinkmini/firmware/inc/pios_config.h | 2 +- .../boards/oplinkmini/firmware/pios_board.c | 121 +- .../boards/revolution/firmware/UAVObjects.inc | 5 +- .../revolution/firmware/inc/pios_config.h | 2 +- .../boards/revolution/firmware/pios_board.c | 61 +- flight/targets/boards/revolution/pios_board.h | 1 + .../plugins/config/configpipxtremewidget.cpp | 153 +- .../plugins/config/configpipxtremewidget.h | 10 +- .../src/plugins/config/configrevohwwidget.cpp | 13 +- .../src/plugins/config/configrevohwwidget.ui | 240 +- .../src/plugins/config/pipxtreme.ui | 2039 +++++------------ .../src/plugins/uavobjects/uavobjects.pro | 2 + shared/uavobjectdefinition/hwsettings.xml | 2 - shared/uavobjectdefinition/oplinksettings.xml | 18 +- shared/uavobjectdefinition/oplinkstatus.xml | 2 +- 23 files changed, 1507 insertions(+), 3127 deletions(-) delete mode 100644 flight/libraries/inc/packet_handler.h diff --git a/flight/libraries/inc/packet_handler.h b/flight/libraries/inc/packet_handler.h deleted file mode 100644 index 3df4f8609..000000000 --- a/flight/libraries/inc/packet_handler.h +++ /dev/null @@ -1,111 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup OpenPilotSystem OpenPilot System - * @{ - * @addtogroup OpenPilotLibraries OpenPilot System Libraries - * @{ - * - * @file packet_handler.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief A packet handler for handeling radio packet transmission. - * @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 __PACKET_HANDLER_H__ -#define __PACKET_HANDLER_H__ - -#include -#include -#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) - -// Public types -typedef enum { - PACKET_TYPE_NONE = 0, - PACKET_TYPE_STATUS, // broadcasts status of this modem - PACKET_TYPE_CON_REQUEST, // request a connection to another modem - PACKET_TYPE_DATA, // data packet (packet contains user data) - PACKET_TYPE_DUPLICATE_DATA, // a duplicate data packet - PACKET_TYPE_PPM, // PPM relay values - PACKET_TYPE_ACK, // Acknowlege the receipt of a packet - PACKET_TYPE_NACK, // Acknowlege the receipt of an uncorrectable packet -} PHPacketType; - -typedef struct { - uint32_t destination_id; - portTickType prev_tx_time; - uint16_t seq_num; - uint8_t type; - uint8_t data_size; -} PHPacketHeader; - -#define PH_MAX_DATA (PIOS_PH_MAX_PACKET - sizeof(PHPacketHeader) - RS_ECC_NPARITY) -#define PH_PACKET_SIZE(p) ((p)->data + (p)->header.data_size - (uint8_t *)(p) + RS_ECC_NPARITY) -typedef struct { - PHPacketHeader header; - uint8_t data[PH_MAX_DATA + RS_ECC_NPARITY]; -} PHPacket, *PHPacketHandle; - -#define PH_ACK_NACK_DATA_SIZE(p) ((uint8_t *)((p)->ecc) - (uint8_t *)(((PHPacketHandle)(p))->data)) -typedef struct { - PHPacketHeader header; - uint8_t ecc[RS_ECC_NPARITY]; -} PHAckNackPacket, *PHAckNackPacketHandle; - -#define PH_PPM_DATA_SIZE(p) ((uint8_t *)((p)->ecc) - (uint8_t *)(((PHPacketHandle)(p))->data)) -typedef struct { - PHPacketHeader header; - int16_t channels[PIOS_RFM22B_RCVR_MAX_CHANNELS]; - uint8_t ecc[RS_ECC_NPARITY]; -} PHPpmPacket, *PHPpmPacketHandle; - -#define PH_STATUS_DATA_SIZE(p) ((uint8_t *)((p)->ecc) - (uint8_t *)(((PHPacketHandle)(p))->data)) -typedef struct { - PHPacketHeader header; - uint32_t source_id; - uint8_t link_quality; - int8_t received_rssi; - uint8_t ecc[RS_ECC_NPARITY]; -} PHStatusPacket, *PHStatusPacketHandle; - -#define PH_CONNECTION_DATA_SIZE(p) ((uint8_t *)((p)->ecc) - (uint8_t *)(((PHPacketHandle)(p))->data)) -typedef struct { - PHPacketHeader header; - uint32_t source_id; - uint32_t min_frequency; - uint32_t max_frequency; - uint32_t channel_spacing; - portTickType status_rx_time; - OPLinkSettingsMainPortOptions main_port; - OPLinkSettingsFlexiPortOptions flexi_port; - OPLinkSettingsVCPPortOptions vcp_port; - OPLinkSettingsComSpeedOptions com_speed; - uint8_t ecc[RS_ECC_NPARITY]; -} PHConnectionPacket, *PHConnectionPacketHandle; - -#endif // __PACKET_HANDLER_H__ - -/** - * @} - * @} - */ diff --git a/flight/modules/OPLink/oplinkmod.c b/flight/modules/OPLink/oplinkmod.c index 9b2b068a2..2c976ac16 100644 --- a/flight/modules/OPLink/oplinkmod.c +++ b/flight/modules/OPLink/oplinkmod.c @@ -38,10 +38,12 @@ * 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA */ +#include + +#include #include #include -#include #include #include diff --git a/flight/modules/RadioComBridge/RadioComBridge.c b/flight/modules/RadioComBridge/RadioComBridge.c index cbf3608ed..9724f6e3f 100644 --- a/flight/modules/RadioComBridge/RadioComBridge.c +++ b/flight/modules/RadioComBridge/RadioComBridge.c @@ -7,7 +7,7 @@ * @{ * * @file RadioComBridge.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012-2013. * @brief Bridges selected Com Port to the COM VCP emulated serial port * @see The GNU Public License (GPL) Version 3 * @@ -32,8 +32,6 @@ #include #include -#include -#include #include #include #include @@ -46,12 +44,6 @@ #include -// External functions -void PIOS_InitUartMainPort(); -void PIOS_InitUartFlexiPort(); -void PIOS_InitPPMMainPort(bool input); -void PIOS_InitPPMFlexiPort(bool input); - // **************** // Private constants @@ -75,12 +67,12 @@ typedef struct { xTaskHandle radioRxTaskHandle; // The UAVTalk connection on the com side. - UAVTalkConnection outUAVTalkCon; - UAVTalkConnection inUAVTalkCon; + UAVTalkConnection telemUAVTalkCon; + UAVTalkConnection radioUAVTalkCon; // Queue handles. - xQueueHandle gcsEventQueue; xQueueHandle uavtalkEventQueue; + xQueueHandle radioEventQueue; // Error statistics. uint32_t comTxErrors; @@ -91,9 +83,6 @@ typedef struct { // Should we parse UAVTalk? bool parseUAVTalk; - // We can only configure the hardware once. - bool configured; - // The current configured uart speed OPLinkSettingsComSpeedOptions comSpeed; } RadioComBridgeData; @@ -107,11 +96,9 @@ static void radioTxTask(void *parameters); static void radioRxTask(void *parameters); static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length); static int32_t RadioSendHandler(uint8_t *buf, int32_t length); -static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte); -static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEventType type); -static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, - OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed); -static void updateSettings(OPLinkSettingsData *oplinkSettings); +static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte); +static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte); +static void objectPersistenceUpdatedCb(UAVObjEvent *objEv); // **************** // Private variables @@ -119,7 +106,7 @@ static void updateSettings(OPLinkSettingsData *oplinkSettings); static RadioComBridgeData *data; /** - * Start the module + * @brief Start the module * * @return -1 if initialisation failed, 0 on success */ @@ -130,26 +117,10 @@ static int32_t RadioComBridgeStart(void) OPLinkSettingsData oplinkSettings; OPLinkSettingsGet(&oplinkSettings); - // Set the baudrates, etc. - bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id); - if (is_coordinator) { - // Set the frequency range. - PIOS_RFM22B_SetFrequencyRange(pios_rfm22b_id, oplinkSettings.MinFrequency, oplinkSettings.MaxFrequency, oplinkSettings.ChannelSpacing); - - // Set the com baud rates. - updateSettings(&oplinkSettings); - - // Reinitilize the modem. - PIOS_RFM22B_Reinit(pios_rfm22b_id); - - // We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port). - data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) && - (oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) && - (oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL)); - } else { - // Configure the com port configuration callback on the remote modem. - PIOS_RFM22B_SetComConfigCallback(pios_rfm22b_id, &configureComCallback); - } + // We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port). + data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) && + (oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) && + (oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL)); // Set the maximum radio RF power. switch (oplinkSettings.MaxRFPower) { @@ -182,8 +153,9 @@ static int32_t RadioComBridgeStart(void) break; } - // Set the initial frequency. - PIOS_RFM22B_SetInitialFrequency(pios_rfm22b_id, oplinkSettings.InitFrequency); + // Configure our UAVObjects for updates. + UAVObjConnectQueue(UAVObjGetByID(OPLINKSTATUS_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); + UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL); // Start the primary tasks for receiving/sending UAVTalk packets from the GCS. xTaskCreate(telemetryTxTask, (signed char *)"telemetryTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->telemetryTxTaskHandle)); @@ -205,7 +177,7 @@ static int32_t RadioComBridgeStart(void) } /** - * Initialise the module + * @brief Initialise the module * * @return -1 if initialisation failed on success */ @@ -221,12 +193,16 @@ static int32_t RadioComBridgeInitialize(void) OPLinkStatusInitialize(); ObjectPersistenceInitialize(); + // Configure the UAVObject callbacks + ObjectPersistenceConnectCallback(&objectPersistenceUpdatedCb); + // Initialise UAVTalk - data->outUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); - data->inUAVTalkCon = UAVTalkInitialize(&RadioSendHandler); + data->telemUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); + data->radioUAVTalkCon = UAVTalkInitialize(&RadioSendHandler); // Initialize the queues. data->uavtalkEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); + data->radioEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); // Configure our UAVObjects for updates. UAVObjConnectQueue(UAVObjGetByID(OPLINKSTATUS_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); @@ -240,7 +216,6 @@ static int32_t RadioComBridgeInitialize(void) data->comTxRetries = 0; data->UAVTalkErrors = 0; data->parseUAVTalk = true; - data->configured = false; data->comSpeed = OPLINKSETTINGS_COMSPEED_9600; PIOS_COM_RADIO = PIOS_COM_RFM22B; @@ -249,7 +224,7 @@ static int32_t RadioComBridgeInitialize(void) MODULE_INITCALL(RadioComBridgeInitialize, RadioComBridgeStart); /** - * Telemetry transmit task, regular priority + * @brief Telemetry transmit task, regular priority * * @param[in] parameters The task parameters */ @@ -269,7 +244,7 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters) uint32_t retries = 0; int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendObject(data->outUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0; + success = UAVTalkSendObject(data->telemUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0; if (!success) { ++retries; } @@ -280,7 +255,7 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters) uint32_t retries = 0; int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendAck(data->outUAVTalkCon, ev.obj, ev.instId) == 0; + success = UAVTalkSendAck(data->telemUAVTalkCon, ev.obj, ev.instId) == 0; if (!success) { ++retries; } @@ -291,7 +266,7 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters) uint32_t retries = 0; int32_t success = -1; while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendNack(data->outUAVTalkCon, UAVObjGetID(ev.obj)) == 0; + success = UAVTalkSendNack(data->telemUAVTalkCon, UAVObjGetID(ev.obj)) == 0; if (!success) { ++retries; } @@ -303,7 +278,7 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters) } /** - * Radio tx task. Receive data packets from the com port and send to the radio. + * @brief Radio tx task. Receive data packets from the com port and send to the radio. * * @param[in] parameters The task parameters */ @@ -315,23 +290,51 @@ static void radioTxTask(__attribute__((unused)) void *parameters) PIOS_WDG_UpdateFlag(PIOS_WDG_RADIOTX); #endif - // Wait until the com port is available. - if (data->parseUAVTalk || !PIOS_COM_TELEMETRY) { - vTaskDelay(5); - continue; - } + // Process the radio event queue, sending UAVObjects over the radio link as necessary. + UAVObjEvent ev; - // Read from the com port. - uint8_t serial_data[1]; - uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_TELEMETRY, serial_data, sizeof(serial_data), MAX_PORT_DELAY); - if (bytes_to_process > 0) { - PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, serial_data, bytes_to_process); + // Wait for queue message + if (xQueueReceive(data->radioEventQueue, &ev, MAX_PORT_DELAY) == pdTRUE) { + if ((ev.event == EV_UPDATED) || (ev.event == EV_UPDATE_REQ)) { + // Send update (with retries) + uint32_t retries = 0; + int32_t success = -1; + while (retries < MAX_RETRIES && success == -1) { + success = UAVTalkSendObject(data->radioUAVTalkCon, ev.obj, 0, 0, RETRY_TIMEOUT_MS) == 0; + if (!success) { + ++retries; + } + } + data->comTxRetries += retries; + } else if (ev.event == EV_SEND_ACK) { + // Send the ACK + uint32_t retries = 0; + int32_t success = -1; + while (retries < MAX_RETRIES && success == -1) { + success = UAVTalkSendAck(data->radioUAVTalkCon, ev.obj, ev.instId) == 0; + if (!success) { + ++retries; + } + } + data->comTxRetries += retries; + } else if (ev.event == EV_SEND_NACK) { + // Send the NACK + uint32_t retries = 0; + int32_t success = -1; + while (retries < MAX_RETRIES && success == -1) { + success = UAVTalkSendNack(data->radioUAVTalkCon, UAVObjGetID(ev.obj)) == 0; + if (!success) { + ++retries; + } + } + data->comTxRetries += retries; + } } } } /** - * Radio rx task. Receive data packets from the radio and pass them on. + * @brief Radio rx task. Receive data packets from the radio and pass them on. * * @param[in] parameters The task parameters */ @@ -345,22 +348,16 @@ static void radioRxTask(__attribute__((unused)) void *parameters) uint8_t serial_data[1]; uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, serial_data, sizeof(serial_data), MAX_PORT_DELAY); if (bytes_to_process > 0) { - // Either pass the data through the UAVTalk parser, or just send it to the radio (if we're doing raw comms). - if (data->parseUAVTalk) { - for (uint8_t i = 0; i < bytes_to_process; i++) { - if (UAVTalkRelayInputStream(data->outUAVTalkCon, serial_data[i]) == UAVTALK_STATE_ERROR) { - data->UAVTalkErrors++; - } - } - } else if (PIOS_COM_TELEMETRY) { - PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEMETRY, serial_data, bytes_to_process); + // Pass the data through the UAVTalk parser. + for (uint8_t i = 0; i < bytes_to_process; i++) { + ProcessRadioStream(data->radioUAVTalkCon, data->telemUAVTalkCon, serial_data[i]); } } } } /** - * Receive telemetry from the USB/COM port. + * @brief Receive telemetry from the USB/COM port. * * @param[in] parameters The task parameters */ @@ -383,7 +380,7 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, serial_data, sizeof(serial_data), MAX_PORT_DELAY); if (bytes_to_process > 0) { for (uint8_t i = 0; i < bytes_to_process; i++) { - ProcessInputStream(data->inUAVTalkCon, serial_data[i]); + ProcessTelemetryStream(data->telemUAVTalkCon, data->radioUAVTalkCon, serial_data[i]); } } } else { @@ -393,7 +390,7 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) } /** - * Transmit data buffer to the com port. + * @brief Transmit data buffer to the com port. * * @param[in] buf Data buffer to send * @param[in] length Length of buffer @@ -439,272 +436,109 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length) } /** - * Process a byte of data received + * @brief Process a byte of data received on the telemetry stream * - * @param[in] connectionHandle The UAVTalk connection handle + * @param[in] inConnectionHandle The UAVTalk connection handle on the telemetry port + * @param[in] outConnectionHandle The UAVTalk connection handle on the radio port. * @param[in] rxbyte The received byte. */ -static void ProcessInputStream(UAVTalkConnection connectionHandle, uint8_t rxbyte) +static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte) { // Keep reading until we receive a completed packet. - UAVTalkRxState state = UAVTalkRelayInputStream(connectionHandle, rxbyte); - UAVTalkConnectionData *connection = (UAVTalkConnectionData *)(connectionHandle); - UAVTalkInputProcessor *iproc = &(connection->iproc); + UAVTalkRxState state = UAVTalkProcessInputStream(inConnectionHandle, rxbyte); - if (state == UAVTALK_STATE_COMPLETE) { - // Is this a local UAVObject? - // We only generate GcsReceiver ojects, we don't consume them. - if ((iproc->obj != NULL) && (iproc->objId != GCSRECEIVER_OBJID)) { - // We treat the ObjectPersistence object differently - if (iproc->objId == OBJECTPERSISTENCE_OBJID) { - // Unpack object, if the instance does not exist it will be created! - UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer); - - // Get the ObjectPersistence object. - ObjectPersistenceData obj_per; - ObjectPersistenceGet(&obj_per); - - // Is this concerning or setting object? - if (obj_per.ObjectID == OPLINKSETTINGS_OBJID) { - // Queue up the ACK. - queueEvent(data->uavtalkEventQueue, (void *)iproc->obj, iproc->instId, EV_SEND_ACK); - - // Is this a save, load, or delete? - bool success = true; - switch (obj_per.Operation) { - case OBJECTPERSISTENCE_OPERATION_LOAD: - { -#if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS) - // Load the settings. - void *obj = UAVObjGetByID(obj_per.ObjectID); - if (obj == 0) { - success = false; - } else { - // Load selected instance - success = (UAVObjLoad(obj, obj_per.InstanceID) == 0); - } -#endif - break; - } - case OBJECTPERSISTENCE_OPERATION_SAVE: - { -#if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS) - void *obj = UAVObjGetByID(obj_per.ObjectID); - if (obj == 0) { - success = false; - } else { - // Save selected instance - success = UAVObjSave(obj, obj_per.InstanceID) == 0; - } -#endif - break; - } - case OBJECTPERSISTENCE_OPERATION_DELETE: - { -#if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS) - void *obj = UAVObjGetByID(obj_per.ObjectID); - if (obj == 0) { - success = false; - } else { - // Save selected instance - success = UAVObjDelete(obj, obj_per.InstanceID) == 0; - } -#endif - break; - } - default: - break; - } - if (success == true) { - obj_per.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED; - ObjectPersistenceSet(&obj_per); - } - } - } else { - switch (iproc->type) { - case UAVTALK_TYPE_OBJ: - // Unpack object, if the instance does not exist it will be created! - UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer); - break; - case UAVTALK_TYPE_OBJ_REQ: - // Queue up an object send request. - queueEvent(data->uavtalkEventQueue, (void *)iproc->obj, iproc->instId, EV_UPDATE_REQ); - break; - case UAVTALK_TYPE_OBJ_ACK: - if (UAVObjUnpack(iproc->obj, iproc->instId, connection->rxBuffer) == 0) { - // Queue up an ACK - queueEvent(data->uavtalkEventQueue, (void *)iproc->obj, iproc->instId, EV_SEND_ACK); - } - break; - } - } - } - } else if (state == UAVTALK_STATE_ERROR) { + if (state == UAVTALK_STATE_ERROR) { data->UAVTalkErrors++; + } else if (state == UAVTALK_STATE_COMPLETE) { + UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle); + } +} - // Send a NACK if required. - if ((iproc->obj) && (iproc->type == UAVTALK_TYPE_OBJ_ACK)) { - // Queue up a NACK - queueEvent(data->uavtalkEventQueue, iproc->obj, iproc->instId, EV_SEND_NACK); +/** + * @brief Process a byte of data received on the radio data stream. + * + * @param[in] inConnectionHandle The UAVTalk connection handle on the radio port. + * @param[in] outConnectionHandle The UAVTalk connection handle on the telemetry port. + * @param[in] rxbyte The received byte. + */ +static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte) +{ + // Keep reading until we receive a completed packet. + UAVTalkRxState state = UAVTalkProcessInputStreamQuiet(inConnectionHandle, rxbyte); + + if (state == UAVTALK_STATE_ERROR) { + data->UAVTalkErrors++; + } else if (state == UAVTALK_STATE_COMPLETE) { + // We only want to unpack certain objects from the remote modem. + uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle); + if (objId != OPLINKSTATUS_OBJID) { + UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle); } } } /** - * Queue and event into an event queue. - * - * @param[in] queue The event queue - * @param[in] obj The data pointer - * @param[in] type The event type + * @brief Callback that is called when the ObjectPersistence UAVObject is changed. + * @param[in] objEv The event that precipitated the callback. */ -static void queueEvent(xQueueHandle queue, void *obj, uint16_t instId, UAVObjEventType type) +static void objectPersistenceUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) { - UAVObjEvent ev; + // Get the ObjectPersistence object. + ObjectPersistenceData obj_per; - ev.obj = (UAVObjHandle)obj; - ev.instId = instId; - ev.event = type; - xQueueSend(queue, &ev, portMAX_DELAY); -} + ObjectPersistenceGet(&obj_per); -/** - * Configure the output port based on a configuration event from the remote coordinator. - * - * @param[in] com_port The com port to configure - * @param[in] com_speed The com port speed - */ -static void configureComCallback(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, - OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed) -{ - // Update the com baud rate - data->comSpeed = com_speed; - - // Get the settings. - OPLinkSettingsData oplinkSettings; - OPLinkSettingsGet(&oplinkSettings); - - switch (main_port) { - case OPLINKSETTINGS_REMOTEMAINPORT_DISABLED: - oplinkSettings.MainPort = OPLINKSETTINGS_MAINPORT_DISABLED; - break; - case OPLINKSETTINGS_REMOTEMAINPORT_SERIAL: - oplinkSettings.MainPort = OPLINKSETTINGS_MAINPORT_SERIAL; - break; - case OPLINKSETTINGS_REMOTEMAINPORT_PPM: - oplinkSettings.MainPort = OPLINKSETTINGS_MAINPORT_PPM; - break; - } - - switch (flexi_port) { - case OPLINKSETTINGS_REMOTEFLEXIPORT_DISABLED: - oplinkSettings.FlexiPort = OPLINKSETTINGS_FLEXIPORT_DISABLED; - break; - case OPLINKSETTINGS_REMOTEFLEXIPORT_SERIAL: - oplinkSettings.FlexiPort = OPLINKSETTINGS_FLEXIPORT_SERIAL; - break; - case OPLINKSETTINGS_REMOTEFLEXIPORT_PPM: - oplinkSettings.FlexiPort = OPLINKSETTINGS_FLEXIPORT_PPM; - break; - } - - switch (vcp_port) { - case OPLINKSETTINGS_REMOTEVCPPORT_DISABLED: - oplinkSettings.VCPPort = OPLINKSETTINGS_VCPPORT_DISABLED; - break; - case OPLINKSETTINGS_REMOTEVCPPORT_SERIAL: - oplinkSettings.VCPPort = OPLINKSETTINGS_VCPPORT_SERIAL; - break; - } - - // We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port). - data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) && - (oplinkSettings.FlexiPort != OPLINKSETTINGS_FLEXIPORT_SERIAL) && - (oplinkSettings.VCPPort != OPLINKSETTINGS_VCPPORT_SERIAL)); - - // Update the OPLinkSettings object. - OPLinkSettingsSet(&oplinkSettings); - - // Perform the update. - updateSettings(&oplinkSettings); -} - -/** - * Update the oplink settings. - */ -static void updateSettings(OPLinkSettingsData *oplinkSettings) -{ - // We can only configure the hardware once. - if (data->configured) { - return; - } - data->configured = true; - - // Configure the main port - bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id); - switch (oplinkSettings->MainPort) { - case OPLINKSETTINGS_MAINPORT_TELEMETRY: - case OPLINKSETTINGS_MAINPORT_SERIAL: - /* Configure the main port for uart serial */ - PIOS_InitUartMainPort(); - PIOS_COM_TELEMETRY = PIOS_COM_TELEM_UART_MAIN; - break; - case OPLINKSETTINGS_MAINPORT_PPM: - PIOS_InitPPMMainPort(is_coordinator); - break; - case OPLINKSETTINGS_MAINPORT_DISABLED: - break; - } - - // Configure the flexi port - switch (oplinkSettings->FlexiPort) { - case OPLINKSETTINGS_FLEXIPORT_TELEMETRY: - case OPLINKSETTINGS_FLEXIPORT_SERIAL: - /* Configure the flexi port as uart serial */ - PIOS_InitUartFlexiPort(); - PIOS_COM_TELEMETRY = PIOS_COM_TELEM_UART_FLEXI; - break; - case OPLINKSETTINGS_FLEXIPORT_PPM: - PIOS_InitPPMFlexiPort(is_coordinator); - break; - case OPLINKSETTINGS_FLEXIPORT_DISABLED: - break; - } - - // Configure the USB VCP port - switch (oplinkSettings->VCPPort) { - case OPLINKSETTINGS_VCPPORT_SERIAL: - PIOS_COM_TELEMETRY = PIOS_COM_TELEM_USB_VCP; - break; - case OPLINKSETTINGS_VCPPORT_DISABLED: - break; - } - - // Update the com baud rate. - uint32_t comBaud = 9600; - switch (data->comSpeed) { - case OPLINKSETTINGS_COMSPEED_2400: - comBaud = 2400; - break; - case OPLINKSETTINGS_COMSPEED_4800: - comBaud = 4800; - break; - case OPLINKSETTINGS_COMSPEED_9600: - comBaud = 9600; - break; - case OPLINKSETTINGS_COMSPEED_19200: - comBaud = 19200; - break; - case OPLINKSETTINGS_COMSPEED_38400: - comBaud = 38400; - break; - case OPLINKSETTINGS_COMSPEED_57600: - comBaud = 57600; - break; - case OPLINKSETTINGS_COMSPEED_115200: - comBaud = 115200; - break; - } - if (PIOS_COM_TELEMETRY) { - PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, comBaud); + // Is this concerning or setting object? + if (obj_per.ObjectID == OPLINKSETTINGS_OBJID) { + // Is this a save, load, or delete? + bool success = true; + switch (obj_per.Operation) { + case OBJECTPERSISTENCE_OPERATION_LOAD: + { +#if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS) + // Load the settings. + void *obj = UAVObjGetByID(obj_per.ObjectID); + if (obj == 0) { + success = false; + } else { + // Load selected instance + success = (UAVObjLoad(obj, obj_per.InstanceID) == 0); + } +#endif + break; + } + case OBJECTPERSISTENCE_OPERATION_SAVE: + { +#if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS) + void *obj = UAVObjGetByID(obj_per.ObjectID); + if (obj == 0) { + success = false; + } else { + // Save selected instance + success = UAVObjSave(obj, obj_per.InstanceID) == 0; + } +#endif + break; + } + case OBJECTPERSISTENCE_OPERATION_DELETE: + { +#if defined(PIOS_INCLUDE_FLASH_LOGFS_SETTINGS) + void *obj = UAVObjGetByID(obj_per.ObjectID); + if (obj == 0) { + success = false; + } else { + // Save selected instance + success = UAVObjDelete(obj, obj_per.InstanceID) == 0; + } +#endif + break; + } + default: + break; + } + if (success == true) { + obj_per.Operation = OBJECTPERSISTENCE_OPERATION_COMPLETED; + ObjectPersistenceSet(&obj_per); + } } } diff --git a/flight/modules/System/systemmod.c b/flight/modules/System/systemmod.c index 9ff5dd7e0..874331d68 100644 --- a/flight/modules/System/systemmod.c +++ b/flight/modules/System/systemmod.c @@ -54,6 +54,9 @@ #include #include #include +#if defined(PIOS_INCLUDE_RFM22B) +#include +#endif // Flight Libraries #include @@ -80,7 +83,7 @@ #if defined(PIOS_SYSTEM_STACK_SIZE) #define STACK_SIZE_BYTES PIOS_SYSTEM_STACK_SIZE #else -#define STACK_SIZE_BYTES 924 +#define STACK_SIZE_BYTES 1024 #endif #define TASK_PRIORITY (tskIDLE_PRIORITY + 1) @@ -249,6 +252,54 @@ static void systemTask(__attribute__((unused)) void *parameters) UAVObjEvent ev; int delayTime = SYSTEM_UPDATE_PERIOD_MS / portTICK_RATE_MS / (LED_BLINK_RATE_HZ * 2); +#if defined(PIOS_INCLUDE_RFM22B) + // Update the OPLinkStatus UAVO + OPLinkStatusData oplinkStatus; + OPLinkStatusGet(&oplinkStatus); + + // Get the other device stats. + PIOS_RFM2B_GetPairStats(pios_rfm22b_id, oplinkStatus.PairIDs, oplinkStatus.PairSignalStrengths, OPLINKSTATUS_PAIRIDS_NUMELEM); + + // Get the stats from the radio device + struct rfm22b_stats radio_stats; + PIOS_RFM22B_GetStats(pios_rfm22b_id, &radio_stats); + + // Update the OPLInk status + static bool first_time = true; + static uint16_t prev_tx_count = 0; + static uint16_t prev_rx_count = 0; + oplinkStatus.HeapRemaining = xPortGetFreeHeapSize(); + oplinkStatus.DeviceID = PIOS_RFM22B_DeviceID(pios_rfm22b_id); + oplinkStatus.RxGood = radio_stats.rx_good; + oplinkStatus.RxCorrected = radio_stats.rx_corrected; + oplinkStatus.RxErrors = radio_stats.rx_error; + oplinkStatus.RxMissed = radio_stats.rx_missed; + oplinkStatus.RxFailure = radio_stats.rx_failure; + oplinkStatus.TxDropped = radio_stats.tx_dropped; + oplinkStatus.TxResent = radio_stats.tx_resent; + oplinkStatus.TxFailure = radio_stats.tx_failure; + oplinkStatus.Resets = radio_stats.resets; + oplinkStatus.Timeouts = radio_stats.timeouts; + oplinkStatus.RSSI = radio_stats.rssi; + oplinkStatus.LinkQuality = radio_stats.link_quality; + if (first_time) { + first_time = false; + } else { + uint16_t tx_count = radio_stats.tx_byte_count; + uint16_t rx_count = radio_stats.rx_byte_count; + uint16_t tx_bytes = (tx_count < prev_tx_count) ? (0xffff - prev_tx_count + tx_count) : (tx_count - prev_tx_count); + uint16_t rx_bytes = (rx_count < prev_rx_count) ? (0xffff - prev_rx_count + rx_count) : (rx_count - prev_rx_count); + oplinkStatus.TXRate = (uint16_t)((float)(tx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS); + oplinkStatus.RXRate = (uint16_t)((float)(rx_bytes * 1000) / SYSTEM_UPDATE_PERIOD_MS); + prev_tx_count = tx_count; + prev_rx_count = rx_count; + } + oplinkStatus.TXSeq = radio_stats.tx_seq; + oplinkStatus.RXSeq = radio_stats.rx_seq; + oplinkStatus.LinkState = radio_stats.link_state; + OPLinkStatusSet(&oplinkStatus); +#endif /* if defined(PIOS_INCLUDE_RFM22B) */ + if (xQueueReceive(objectPersistenceQueue, &ev, delayTime) == pdTRUE) { // If object persistence is updated call the callback objectUpdatedCb(&ev); diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index 242f17b21..94538607d 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -51,12 +51,8 @@ #ifdef PIOS_INCLUDE_RFM22B #include -#include -#if defined(PIOS_INCLUDE_GCSRCVR) -#include -#endif #include -#include +#include #include /* Local Defines */ @@ -66,32 +62,24 @@ #define EVENT_QUEUE_SIZE 5 #define RFM22B_DEFAULT_RX_DATARATE RFM22_datarate_9600 #define RFM22B_DEFAULT_TX_POWER RFM22_tx_pwr_txpow_0 -#define RFM22B_LINK_QUALITY_THRESHOLD 20 #define RFM22B_NOMINAL_CARRIER_FREQUENCY 430000000 -#define RFM22B_MAXIMUM_FREQUENCY 440000000 -#define RFM22B_DEFAULT_FREQUENCY 433000000 -#define RFM22B_FREQUENCY_HOP_STEP_SIZE 75000 - -// The maximum amount of time since the last message received to consider the connection broken. -#define DISCONNECT_TIMEOUT_MS 1000 // ms +#define RFM22B_LINK_QUALITY_THRESHOLD 20 +#define RFM22B_DEFAULT_NUM_CHANNELS 1 +#define RFM22B_DEFAULT_MIN_CHANNEL 0 +#define RFM22B_DEFAULT_MAX_CHANNEL 250 +#define RFM22B_DEFAULT_CHANNEL_SET 24 +#define RFM22B_DEFAULT_PACKET_PERIOD 15 // The maximum amount of time without activity before initiating a reset. #define PIOS_RFM22B_SUPERVISOR_TIMEOUT 100 // ms -// The time between updates for sending stats the radio link. -#define RADIOSTATS_UPDATE_PERIOD_MS 250 - -// The number of stats updates that a modem can miss before it's considered disconnected -#define MAX_RADIOSTATS_MISS_COUNT 3 - -// The time between PPM updates -#define PPM_UPDATE_PERIOD_MS 30 - // this is too adjust the RF module so that it is on frequency #define OSC_LOAD_CAP 0x7F // cap = 12.5pf .. default #define TX_PREAMBLE_NIBBLES 12 // 7 to 511 (number of nibbles) #define RX_PREAMBLE_NIBBLES 6 // 5 to 31 (number of nibbles) +#define SYNC_BYTES 4 +#define HEADER_BYTES 4 // the size of the rf modules internal FIFO buffers #define FIFO_SIZE 64 @@ -155,24 +143,19 @@ static const uint8_t OUT_FF[64] = { 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF }; +// The randomized channel list. +static const uint8_t channel_list[] = { 68, 34, 2, 184, 166, 94, 204, 18, 47, 118, 239, 176, 5, 213, 218, 186, 104, 160, 199, 209, 231, 197, 92, 191, 88, 129, 40, 19, 93, 200, 156, 14, 247, 182, 193, 194, 208, 210, 248, 76, 244, 48, 179, 105, 25, 74, 155, 203, 39, 97, 195, 81, 83, 180, 134, 172, 235, 132, 198, 119, 207, 154, 0, 61, 140, 171, 245, 26, 95, 3, 22, 62, 169, 55, 127, 144, 45, 33, 170, 91, 158, 167, 63, 201, 41, 21, 190, 51, 103, 49, 189, 205, 240, 89, 181, 149, 6, 157, 249, 230, 115, 72, 163, 17, 29, 99, 28, 117, 219, 73, 78, 53, 69, 216, 161, 124, 110, 242, 214, 145, 13, 11, 220, 113, 138, 58, 54, 162, 237, 37, 152, 187, 232, 77, 126, 85, 38, 238, 173, 23, 188, 100, 131, 226, 31, 9, 114, 106, 221, 42, 233, 139, 4, 241, 96, 211, 8, 98, 121, 147, 24, 217, 27, 87, 122, 125, 135, 148, 178, 71, 206, 57, 141, 35, 30, 246, 159, 16, 32, 15, 229, 20, 12, 223, 150, 101, 79, 56, 102, 111, 174, 236, 137, 143, 52, 225, 64, 224, 112, 168, 243, 130, 108, 202, 123, 146, 228, 75, 46, 153, 7, 192, 175, 151, 222, 59, 82, 90, 1, 65, 109, 44, 165, 84, 43, 36, 128, 196, 67, 80, 136, 86, 70, 234, 66, 185, 10, 164, 177, 116, 50, 107, 183, 215, 212, 60, 227, 133, 120, 142 }; /* Local function forwared declarations */ static void pios_rfm22_task(void *parameters); static bool pios_rfm22_readStatus(struct pios_rfm22b_dev *rfm22b_dev); -static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening); +static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev); static void pios_rfm22_inject_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_radio_event event, bool inISR); static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev); -static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *rfm22b_dev); -static enum pios_radio_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev); -static enum pios_radio_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev); -static enum pios_radio_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev); -static enum pios_radio_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev); -static enum pios_radio_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev); -static enum pios_radio_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_radio_event radio_setRxMode(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_radio_event radio_rxData(struct pios_rfm22b_dev *rfm22b_dev); -static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, PHPacketHandle p, uint16_t rx_len); +static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *rfm22b_dev, uint8_t *p, uint16_t rx_len); static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_radio_event radio_txData(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_radio_event rfm22_txFailure(struct pios_rfm22b_dev *rfm22b_dev); @@ -181,17 +164,19 @@ static void rfm22_process_event(struct pios_rfm22b_dev *rfm22b_dev, enum pios_ra static enum pios_radio_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_radio_event rfm22_error(struct pios_rfm22b_dev *rfm22b_dev); static enum pios_radio_event rfm22_fatal_error(struct pios_rfm22b_dev *rfm22b_dev); -static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev); -static void rfm22_sendPPM(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_rfm22b_rx_packet_status status); -static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t min_frequency, uint32_t max_frequency, uint32_t step_size); +static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint8_t init_chan); static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel); +static void rfm22_updatePairStatus(struct pios_rfm22b_dev *radio_dev); static void rfm22_calculateLinkQuality(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev); -static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev); +static bool rfm22_isCoordinator(struct pios_rfm22b_dev *rfm22b_dev); +static uint32_t rfm22_destinationID(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev); +static void rfm22_synchronizeClock(struct pios_rfm22b_dev *rfm22b_dev); static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks); -static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev); +static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t index); +static uint8_t rfm22_calcChannelFromClock(struct pios_rfm22b_dev *rfm22b_dev); static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev); static void rfm22_clearLEDs(); @@ -212,110 +197,50 @@ static uint8_t rfm22_read(struct pios_rfm22b_dev *rfm22b_dev, uint8_t addr); /* The state transition table */ static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_STATES] = { // Initialization thread - [RADIO_STATE_UNINITIALIZED] = { + [RADIO_STATE_UNINITIALIZED] = { .entry_fn = 0, - .next_state = { + .next_state = { [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, }, }, - [RADIO_STATE_INITIALIZING] = { + [RADIO_STATE_INITIALIZING] = { .entry_fn = rfm22_init, - .next_state = { + .next_state = { [RADIO_EVENT_INITIALIZED] = RADIO_STATE_RX_MODE, [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_REQUESTING_CONNECTION] = { - .entry_fn = rfm22_requestConnection, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, - [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR - }, - }, - [RADIO_STATE_ACCEPTING_CONNECTION] = { - .entry_fn = rfm22_acceptConnection, - .next_state = { - [RADIO_EVENT_DEFAULT] = RADIO_STATE_SENDING_ACK, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, - [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, - }, - }, - [RADIO_STATE_RX_MODE] = { + [RADIO_STATE_RX_MODE] = { .entry_fn = radio_setRxMode, - .next_state = { + .next_state = { [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_ACK_TIMEOUT] = RADIO_STATE_RECEIVING_NACK, [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA, - [RADIO_EVENT_FAILURE] = RADIO_STATE_RX_FAILURE, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, - [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, - }, - }, - [RADIO_STATE_RX_DATA] = { - .entry_fn = radio_rxData, - .next_state = { - [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA, - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_ACK_TIMEOUT] = RADIO_STATE_RECEIVING_NACK, - [RADIO_EVENT_RX_COMPLETE] = RADIO_STATE_SENDING_ACK, - [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, - [RADIO_EVENT_STATUS_RECEIVED] = RADIO_STATE_RECEIVING_STATUS, - [RADIO_EVENT_CONNECTION_REQUESTED] = RADIO_STATE_ACCEPTING_CONNECTION, - [RADIO_EVENT_PACKET_ACKED] = RADIO_STATE_RECEIVING_ACK, - [RADIO_EVENT_PACKET_NACKED] = RADIO_STATE_RECEIVING_NACK, - [RADIO_EVENT_FAILURE] = RADIO_STATE_RX_FAILURE, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, - [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, - }, - }, - [RADIO_STATE_RECEIVING_ACK] = { - .entry_fn = rfm22_receiveAck, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, - [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, - }, - }, - [RADIO_STATE_RECEIVING_NACK] = { - .entry_fn = rfm22_receiveNack, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, - [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, - }, - }, - [RADIO_STATE_RECEIVING_STATUS] = { - .entry_fn = rfm22_receiveStatus, - .next_state = { - [RADIO_EVENT_RX_COMPLETE] = RADIO_STATE_TX_START, - [RADIO_EVENT_REQUEST_CONNECTION] = RADIO_STATE_REQUESTING_CONNECTION, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, + }, + }, + [RADIO_STATE_RX_DATA] = { + .entry_fn = radio_rxData, + .next_state = { + [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA, + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_RX_COMPLETE] = RADIO_STATE_TX_START, + [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_TX_START] = { + [RADIO_STATE_TX_START] = { .entry_fn = radio_txStart, - .next_state = { + .next_state = { [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_TX_DATA, [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, @@ -324,21 +249,20 @@ static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_ST [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_TX_DATA] = { + [RADIO_STATE_TX_DATA] = { .entry_fn = radio_txData, - .next_state = { + .next_state = { [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_TX_DATA, [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, - [RADIO_EVENT_FAILURE] = RADIO_STATE_TX_FAILURE, [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_TX_FAILURE] = { + [RADIO_STATE_TX_FAILURE] = { .entry_fn = rfm22_txFailure, - .next_state = { + .next_state = { [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, @@ -346,29 +270,9 @@ static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_ST [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_SENDING_ACK] = { - .entry_fn = rfm22_sendAck, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, - [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, - }, - }, - [RADIO_STATE_SENDING_NACK] = { - .entry_fn = rfm22_sendNack, - .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, - [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, - }, - }, - [RADIO_STATE_TIMEOUT] = { + [RADIO_STATE_TIMEOUT] = { .entry_fn = rfm22_timeout, - .next_state = { + .next_state = { [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, @@ -376,16 +280,16 @@ static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_ST [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_ERROR] = { + [RADIO_STATE_ERROR] = { .entry_fn = rfm22_error, - .next_state = { + .next_state = { [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, - [RADIO_STATE_FATAL_ERROR] = { + [RADIO_STATE_FATAL_ERROR] = { .entry_fn = rfm22_fatal_error, - .next_state = {}, + .next_state = {}, }, }; @@ -451,12 +355,12 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->spi_id = spi_id; // Initialize our configuration parameters - rfm22b_dev->send_ppm = false; rfm22b_dev->datarate = RFM22B_DEFAULT_RX_DATARATE; rfm22b_dev->tx_power = RFM22B_DEFAULT_TX_POWER; + rfm22b_dev->coordinator = false; + rfm22b_dev->coordinatorID = 0; // Initialize the com callbacks. - rfm22b_dev->com_config_cb = NULL; rfm22b_dev->rx_in_cb = NULL; rfm22b_dev->tx_out_cb = NULL; @@ -476,18 +380,11 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->stats.rx_seq = 0; rfm22b_dev->stats.tx_failure = 0; - // Initialize the frequencies. - PIOS_RFM22B_SetInitialFrequency(*rfm22b_id, RFM22B_DEFAULT_FREQUENCY); - PIOS_RFM22B_SetFrequencyRange(*rfm22b_id, RFM22B_DEFAULT_FREQUENCY, RFM22B_DEFAULT_FREQUENCY, RFM22B_FREQUENCY_HOP_STEP_SIZE); - - // Initialize the bindings. - for (uint32_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { - rfm22b_dev->bindings[i].pairID = 0; - } - rfm22b_dev->coordinator = false; + // Initialize the channels. + PIOS_RFM22B_SetChannelConfig(*rfm22b_id, RFM22B_DEFAULT_NUM_CHANNELS, RFM22B_DEFAULT_MIN_CHANNEL, RFM22B_DEFAULT_MAX_CHANNEL, RFM22B_DEFAULT_CHANNEL_SET, RFM22B_DEFAULT_PACKET_PERIOD, false); // Create the event queue - rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_radio_event)); + rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_radio_event)); // Bind the configuration to the device instance rfm22b_dev->cfg = *cfg; @@ -508,11 +405,6 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->deviceID = crcs[0] | crcs[1] << 8 | crcs[2] << 16 | crcs[3] << 24; DEBUG_PRINTF(2, "RF device ID: %x\n\r", rfm22b_dev->deviceID); -#if defined(PIOS_INCLUDE_GCSRCVR) - // Initialize the GCSReceive object - GCSReceiverInitialize(); -#endif - // Initialize the external interrupt. PIOS_EXTI_Init(cfg->exti_cfg); @@ -580,6 +472,16 @@ uint32_t PIOS_RFM22B_DeviceID(uint32_t rfm22b_id) return 0; } +/** + * Are we connected to the remote modem? + * + * @param[in] rfm22b_dev The device structure + */ +static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev) +{ + return (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) || (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTING); +} + /** * Returns true if the modem is configured as a coordinator. * @@ -591,7 +493,7 @@ bool PIOS_RFM22B_IsCoordinator(uint32_t rfm22b_id) struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; if (PIOS_RFM22B_Validate(rfm22b_dev)) { - return rfm22b_dev->coordinator; + return rfm22_isCoordinator(rfm22b_dev); } return false; } @@ -629,85 +531,68 @@ void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr) } /** - * Sets the radio frequency range and initial frequency + * Sets the range and number of channels to use for the radio. + * The channels are 0 to 255 divided across the 430-440 MHz range. + * The number of channels configured will be spread across the selected channel range. + * The channel spacing is 10MHz / 250 = 40kHz * * @param[in] rfm22b_id The RFM22B device index. - * @param[in] min_freq The minimum frequency - * @param[in] max_freq The maximum frequency - * @param[in] step_size The channel step size + * @param[in] num_channels The number of channels to use for frequency hopping. + * @param[in] min_chan The minimum channel. + * @param[in] max_chan The maximum channel. + * @param[in] chan_set The "seed" for selecting a channel sequence. + * @param[in] packet_period The fixed time alloted to sending a single packet + * @param[in] oneway Only the coordinator can send packets if true. */ -void PIOS_RFM22B_SetFrequencyRange(uint32_t rfm22b_id, uint32_t min_freq, uint32_t max_freq, uint32_t step_size) +void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, uint8_t num_chan, uint8_t min_chan, uint8_t max_chan, uint8_t chan_set, uint8_t packet_period, bool oneway) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } - rfm22b_dev->con_packet.min_frequency = min_freq; - rfm22b_dev->con_packet.max_frequency = max_freq; - rfm22b_dev->con_packet.channel_spacing = step_size; + rfm22b_dev->packet_period = packet_period; + rfm22b_dev->num_channels = num_chan; + rfm22b_dev->one_way_link = oneway; + + // Find the first N channels that meet the min/max criteria out of the random channel list. + uint8_t num_found = 0; + for (uint16_t i = 0; (i < RFM22B_NUM_CHANNELS) && (num_found < num_chan); ++i) { + uint8_t idx = (i + chan_set) % RFM22B_NUM_CHANNELS; + uint8_t chan = channel_list[idx]; + if ((chan >= min_chan) && (chan <= max_chan)) { + rfm22b_dev->channels[num_found++] = chan; + } + } } /** - * Sets the initial radio frequency range + * Set a modem to be a coordinator or not. * - * @param[in] rfm22b_id The RFM22B device index. - * @param[in] init_freq The initial frequency + * @param[in] rfm22b_id The RFM22B device index. + * @param[in] coordinator If true, this modem will be configured as a coordinator. */ -void PIOS_RFM22B_SetInitialFrequency(uint32_t rfm22b_id, uint32_t init_freq) +extern void PIOS_RFM22B_SetCoordinator(uint32_t rfm22b_id, bool coordinator) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if (!PIOS_RFM22B_Validate(rfm22b_dev)) { - return; + if (PIOS_RFM22B_Validate(rfm22b_dev)) { + rfm22b_dev->coordinator = coordinator; } - rfm22b_dev->init_frequency = init_freq; } /** - * Set the com port configuration callback (to receive com configuration over the air) + * Sets the device coordinator ID. * - * @param[in] rfm22b_id The rfm22b device. - * @param[in] cb A pointer to the callback function + * @param[in] rfm22b_id The RFM22B device index. + * @param[in] coord_id The coordinator ID. */ -void PIOS_RFM22B_SetComConfigCallback(uint32_t rfm22b_id, PIOS_RFM22B_ComConfigCallback cb) +void PIOS_RFM22B_SetCoordinatorID(uint32_t rfm22b_id, uint32_t coord_id) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - if (!PIOS_RFM22B_Validate(rfm22b_dev)) { - return; - } - rfm22b_dev->com_config_cb = cb; -} - -/** - * Set the list of modems that this modem will bind with. - * - * @param[in] rfm22b_id The rfm22b device. - * @param[in] bindingPairIDs The array of binding IDs. - * @param[in] mainPortSettings The array of main com port configurations. - * @param[in] flexiPortSettings The array of flexi com port configurations. - * @param[in] vcpPortSettings The array of VCP com port configurations. - * @param[in] comSpeeds The array of com port speeds. - */ -void PIOS_RFM22B_SetBindings(uint32_t rfm22b_id, const uint32_t bindingPairIDs[], const uint8_t mainPortSettings[], - const uint8_t flexiPortSettings[], const uint8_t vcpPortSettings[], const uint8_t comSpeeds[]) -{ - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - - if (!PIOS_RFM22B_Validate(rfm22b_dev)) { - return; - } - - // This modem will be considered a coordinator if any bindings have been set. - rfm22b_dev->coordinator = false; - for (uint32_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { - rfm22b_dev->bindings[i].pairID = bindingPairIDs[i]; - rfm22b_dev->bindings[i].main_port = mainPortSettings[i]; - rfm22b_dev->bindings[i].flexi_port = flexiPortSettings[i]; - rfm22b_dev->bindings[i].vcp_port = vcpPortSettings[i]; - rfm22b_dev->bindings[i].com_speed = comSpeeds[i]; - rfm22b_dev->coordinator |= (rfm22b_dev->bindings[i].pairID != 0); + if (PIOS_RFM22B_Validate(rfm22b_dev)) { + rfm22b_dev->coordinatorID = coord_id; } } @@ -728,17 +613,7 @@ void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats) // Calculate the current link quality rfm22_calculateLinkQuality(rfm22b_dev); - // We are connected if our destination ID is in the pair stats. - if (rfm22b_dev->destination_id != 0xffffffff) { - for (uint8_t i = 0; i < OPLINKSTATUS_PAIRIDS_NUMELEM; ++i) { - if ((rfm22b_dev->pair_stats[i].pairID == rfm22b_dev->destination_id) && - (rfm22b_dev->pair_stats[i].rssi > -127)) { - rfm22b_dev->stats.rssi = rfm22b_dev->pair_stats[i].rssi; - rfm22b_dev->stats.afc_correction = rfm22b_dev->pair_stats[i].afc_correction; - break; - } - } - } + // Return the stats. *stats = rfm22b_dev->stats; } @@ -780,7 +655,7 @@ bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id) if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return false; } - return rfm22_isConnected(rfm22b_dev) && (rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD); + return rfm22b_dev->stats.link_quality > RFM22B_LINK_QUALITY_THRESHOLD; } /** @@ -790,7 +665,7 @@ bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id) * @param[in] p The packet to receive into. * @return true if Rx mode was entered sucessfully. */ -bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p) +bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, uint8_t *p) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; @@ -855,7 +730,7 @@ bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p) * @param[in] p The packet to transmit. * @return true if there if the packet was queued for transmission. */ -bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) +bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, uint8_t *p, uint8_t len) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; @@ -863,8 +738,9 @@ bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) return false; } - rfm22b_dev->tx_packet = p; - rfm22b_dev->packet_start_ticks = xTaskGetTickCount(); + rfm22b_dev->tx_packet_handle = p; + rfm22b_dev->stats.tx_byte_count += len; + rfm22b_dev->packet_start_ticks = xTaskGetTickCount(); if (rfm22b_dev->packet_start_ticks == 0) { rfm22b_dev->packet_start_ticks = 1; } @@ -877,23 +753,23 @@ bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) rfm22_write(rfm22b_dev, RFM22_interrupt_enable2, 0x00); // set the tx power + rfm22b_dev->tx_power = 0x7; rfm22_write(rfm22b_dev, RFM22_tx_power, RFM22_tx_pwr_lna_sw | rfm22b_dev->tx_power); // TUNE mode rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_pllon); // Queue the data up for sending - rfm22b_dev->tx_data_wr = PH_PACKET_SIZE(rfm22b_dev->tx_packet); + rfm22b_dev->tx_data_wr = len; RX_LED_OFF; // Set the destination address in the transmit header. - // The destination address is the first 4 bytes of the message. - uint8_t *tx_buffer = (uint8_t *)(rfm22b_dev->tx_packet); - rfm22_write(rfm22b_dev, RFM22_transmit_header0, tx_buffer[0]); - rfm22_write(rfm22b_dev, RFM22_transmit_header1, tx_buffer[1]); - rfm22_write(rfm22b_dev, RFM22_transmit_header2, tx_buffer[2]); - rfm22_write(rfm22b_dev, RFM22_transmit_header3, tx_buffer[3]); + uint32_t id = rfm22_destinationID(rfm22b_dev); + rfm22_write(rfm22b_dev, RFM22_transmit_header0, id & 0xff); + rfm22_write(rfm22b_dev, RFM22_transmit_header1, (id >> 8) & 0xff); + rfm22_write(rfm22b_dev, RFM22_transmit_header2, (id >> 16) & 0xff); + rfm22_write(rfm22b_dev, RFM22_transmit_header3, (id >> 24) & 0xff); // FIFO mode, GFSK modulation uint8_t fd_bit = rfm22_read(rfm22b_dev, RFM22_modulation_mode_control2) & RFM22_mmc2_fd; @@ -904,9 +780,10 @@ bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) rfm22_write(rfm22b_dev, RFM22_op_and_func_ctrl2, 0x00); // Set the total number of data bytes we are going to transmit. - rfm22_write(rfm22b_dev, RFM22_transmit_packet_length, rfm22b_dev->tx_data_wr); + rfm22_write(rfm22b_dev, RFM22_transmit_packet_length, len); // Add some data to the chips TX FIFO before enabling the transmitter + uint8_t *tx_buffer = rfm22b_dev->tx_packet_handle; rfm22_assertCs(rfm22b_dev); PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access | 0x80); int bytes_to_write = (rfm22b_dev->tx_data_wr - rfm22b_dev->tx_data_rd); @@ -929,6 +806,10 @@ bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p) TX_LED_ON; +#ifdef PIOS_RFM22B_DEBUG_ON_TELEM + D1_LED_ON; +#endif + return true; } @@ -951,11 +832,10 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id) return PIOS_RFM22B_INT_FAILURE; } - // TX FIFO almost empty, it needs filling up if (rfm22b_dev->status_regs.int_status_1.tx_fifo_almost_empty) { // Add data to the TX FIFO buffer - uint8_t *tx_buffer = (uint8_t *)(rfm22b_dev->tx_packet); + uint8_t *tx_buffer = rfm22b_dev->tx_packet_handle; uint16_t max_bytes = FIFO_SIZE - TX_FIFO_LO_WATERMARK - 1; rfm22_claimBus(rfm22b_dev); rfm22_assertCs(rfm22b_dev); @@ -971,7 +851,6 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id) } else if (rfm22b_dev->status_regs.int_status_1.packet_sent_interrupt) { // Transition out of Tx mode. rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION; - return PIOS_RFM22B_TX_COMPLETE; } @@ -991,7 +870,7 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return PIOS_RFM22B_INT_FAILURE; } - uint8_t *rx_buffer = (uint8_t *)(rfm22b_dev->rx_packet_handle); + uint8_t *rx_buffer = rfm22b_dev->rx_packet_handle; pios_rfm22b_int_result ret = PIOS_RFM22B_INT_SUCCESS; // Read the device status registers @@ -1015,7 +894,14 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) // read the total length of the packet data uint32_t len = rfm22_read(rfm22b_dev, RFM22_received_packet_length); - // their must still be data in the RX FIFO we need to get + // The received packet is going to be larger than the receive buffer + if (len > rfm22b_dev->max_packet_len) { + rfm22_releaseBus(rfm22b_dev); + rfm22_rxFailure(rfm22b_dev); + return PIOS_RFM22B_INT_FAILURE; + } + + // there must still be data in the RX FIFO we need to get if (rfm22b_dev->rx_buffer_wr < len) { int32_t bytes_to_read = len - rfm22b_dev->rx_buffer_wr; // Fetch the data from the RX FIFO @@ -1026,6 +912,12 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) rfm22_deassertCs(rfm22b_dev); } + // Read the packet header (destination ID) + rfm22b_dev->rx_destination_id = rfm22_read(rfm22b_dev, RFM22_received_header0); + rfm22b_dev->rx_destination_id |= (rfm22_read(rfm22b_dev, RFM22_received_header1) << 8); + rfm22b_dev->rx_destination_id |= (rfm22_read(rfm22b_dev, RFM22_received_header2) << 16); + rfm22b_dev->rx_destination_id |= (rfm22_read(rfm22b_dev, RFM22_received_header3) << 24); + // Release the SPI bus. rfm22_releaseBus(rfm22b_dev); @@ -1038,6 +930,9 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) // Increment the total byte received count. rfm22b_dev->stats.rx_byte_count += rfm22b_dev->rx_buffer_wr; + // Update the pair status with this packet. + rfm22_updatePairStatus(rfm22b_dev); + // We're finished with Rx mode rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION; @@ -1059,6 +954,13 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) return PIOS_RFM22B_INT_FAILURE; } + // The received packet is going to be larger than the receive buffer + if ((rfm22b_dev->rx_buffer_wr + RX_FIFO_HI_WATERMARK) > rfm22b_dev->max_packet_len) { + rfm22_releaseBus(rfm22b_dev); + rfm22_rxFailure(rfm22b_dev); + return PIOS_RFM22B_INT_FAILURE; + } + // Fetch the data from the RX FIFO rfm22_assertCs(rfm22b_dev); PIOS_SPI_TransferByte(rfm22b_dev->spi_id, RFM22_fifo_access & 0x7F); @@ -1074,15 +976,19 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) } else if (rfm22b_dev->status_regs.int_status_2.valid_preamble_detected) { // Valid preamble detected RX_LED_ON; + + // Sync word detected #ifdef PIOS_RFM22B_DEBUG_ON_TELEM D2_LED_ON; #endif // PIOS_RFM22B_DEBUG_ON_TELEM + rfm22b_dev->packet_start_ticks = xTaskGetTickCount(); + if (rfm22b_dev->packet_start_ticks == 0) { + rfm22b_dev->packet_start_ticks = 1; + } // We detected the preamble, now wait for sync. rfm22b_dev->rfm22b_state = RFM22B_STATE_RX_WAIT_SYNC; } else if (rfm22b_dev->status_regs.int_status_2.sync_word_detected) { - // Sync word detected - // Claim the SPI bus. rfm22_claimBus(rfm22b_dev); @@ -1112,15 +1018,6 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) return PIOS_RFM22B_INT_FAILURE; } - // Set the packet start time if necessary. - if ((rfm22b_dev->packet_start_ticks == 0) && - ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT_SYNC))) { - rfm22b_dev->packet_start_ticks = xTaskGetTickCount(); - if (rfm22b_dev->packet_start_ticks == 0) { - rfm22b_dev->packet_start_ticks = 1; - } - } - return ret; } @@ -1151,9 +1048,7 @@ static void pios_rfm22_task(void *parameters) if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } - portTickType lastEventTicks = xTaskGetTickCount(); - portTickType lastStatusTicks = lastEventTicks; - portTickType lastPPMTicks = lastEventTicks; + portTickType lastEventTicks = xTaskGetTickCount(); while (1) { #if defined(PIOS_INCLUDE_WDG) && defined(PIOS_WDG_RFM22B) @@ -1191,41 +1086,13 @@ static void pios_rfm22_task(void *parameters) } // Change channels if necessary. - if (PIOS_RFM22B_InRxWait((uint32_t)rfm22b_dev)) { - rfm22_changeChannel(rfm22b_dev); - } + rfm22_changeChannel(rfm22b_dev); portTickType curTicks = xTaskGetTickCount(); - uint32_t last_rec_ms = (rfm22b_dev->rx_complete_ticks == 0) ? 0 : pios_rfm22_time_difference_ms(rfm22b_dev->rx_complete_ticks, curTicks); // Have we been sending / receiving this packet too long? if ((rfm22b_dev->packet_start_ticks > 0) && - (pios_rfm22_time_difference_ms(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->max_packet_time * 3))) { + (pios_rfm22_time_difference_ms(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->packet_period * 3))) { rfm22_process_event(rfm22b_dev, RADIO_EVENT_TIMEOUT); - } else if (last_rec_ms > DISCONNECT_TIMEOUT_MS) { - // Has it been too long since we received a packet - rfm22_process_event(rfm22b_dev, RADIO_EVENT_ERROR); - } else { - // Are we waiting for an ACK? - if (rfm22b_dev->prev_tx_packet) { - // Should we resend the packet? - if ((pios_rfm22_time_difference_ms(rfm22b_dev->tx_complete_ticks, curTicks) > rfm22b_dev->max_ack_delay) && - PIOS_RFM22B_InRxWait((uint32_t)rfm22b_dev)) { - rfm22b_dev->tx_complete_ticks = curTicks; - rfm22_process_event(rfm22b_dev, RADIO_EVENT_ACK_TIMEOUT); - } - } else { - // Queue up a PPM packet if it's time. - if (pios_rfm22_time_difference_ms(lastPPMTicks, curTicks) > PPM_UPDATE_PERIOD_MS) { - rfm22_sendPPM(rfm22b_dev); - lastPPMTicks = curTicks; - } - - // Queue up a status packet if it's time. - if (pios_rfm22_time_difference_ms(lastStatusTicks, curTicks) > RADIOSTATS_UPDATE_PERIOD_MS) { - rfm22_sendStatus(rfm22b_dev); - lastStatusTicks = curTicks; - } - } } // Send a packet if it's our time slice @@ -1237,7 +1104,19 @@ static void pios_rfm22_task(void *parameters) D4_LED_OFF; } #endif + + // Start transmitting a packet if it's time. if (time_to_send && PIOS_RFM22B_InRxWait((uint32_t)rfm22b_dev)) { + // If the on_sync_channel flag is set, it means that we were on the sync channel, but no packet was received, so transition to a non-connected state. + if (rfm22b_dev->on_sync_channel) { + rfm22b_dev->on_sync_channel = false; + if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) { + rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTING; + } else { + rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; + } + } + rfm22_process_event(rfm22b_dev, RADIO_EVENT_TX_START); } } @@ -1371,28 +1250,23 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) } // Initialize the state - rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; - rfm22b_dev->destination_id = 0xffffffff; - rfm22b_dev->send_status = false; - rfm22b_dev->send_connection_request = false; - rfm22b_dev->send_ack_nack = false; - rfm22b_dev->resend_packet = false; + rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; // Initialize the packets. - rfm22b_dev->rx_packet_len = 0; - rfm22b_dev->tx_packet = NULL; - rfm22b_dev->prev_tx_packet = NULL; - rfm22b_dev->data_packet.header.data_size = 0; + rfm22b_dev->rx_packet_len = 0; + rfm22b_dev->rx_destination_id = 0; + rfm22b_dev->tx_packet_handle = NULL; // Initialize the devide state - rfm22b_dev->rx_buffer_wr = 0; - rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; - rfm22b_dev->frequency_hop_channel = 0; - rfm22b_dev->afc_correction_Hz = 0; - rfm22b_dev->packet_start_ticks = 0; - rfm22b_dev->tx_complete_ticks = 0; - rfm22b_dev->rx_complete_ticks = 0; - rfm22b_dev->rfm22b_state = RFM22B_STATE_INITIALIZING; + rfm22b_dev->rx_buffer_wr = 0; + rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; + rfm22b_dev->channel = 0; + rfm22b_dev->channel_index = 0; + rfm22b_dev->afc_correction_Hz = 0; + rfm22b_dev->packet_start_ticks = 0; + rfm22b_dev->tx_complete_ticks = 0; + rfm22b_dev->rfm22b_state = RFM22B_STATE_INITIALIZING; + rfm22b_dev->on_sync_channel = false; // software reset the RF chip .. following procedure according to Si4x3x Errata (rev. B) rfm22_write_claim(rfm22b_dev, RFM22_op_and_func_ctrl1, RFM22_opfc1_swres); @@ -1522,13 +1396,14 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) RFM22_header_cntl1_hdch_1 | RFM22_header_cntl1_hdch_2 | RFM22_header_cntl1_hdch_3); - // Check all bit of all bytes of the header - rfm22_write(rfm22b_dev, RFM22_header_enable0, 0xff); - rfm22_write(rfm22b_dev, RFM22_header_enable1, 0xff); - rfm22_write(rfm22b_dev, RFM22_header_enable2, 0xff); - rfm22_write(rfm22b_dev, RFM22_header_enable3, 0xff); - // Set the ID to be checked - uint32_t id = rfm22b_dev->deviceID; + // Check all bit of all bytes of the header, unless we're an unbound modem. + uint8_t header_mask = (rfm22_destinationID(rfm22b_dev) == 0xffffffff) ? 0 : 0xff; + rfm22_write(rfm22b_dev, RFM22_header_enable0, header_mask); + rfm22_write(rfm22b_dev, RFM22_header_enable1, header_mask); + rfm22_write(rfm22b_dev, RFM22_header_enable2, header_mask); + rfm22_write(rfm22b_dev, RFM22_header_enable3, header_mask); + // The destination ID and receive ID should be the same. + uint32_t id = rfm22_destinationID(rfm22b_dev); rfm22_write(rfm22b_dev, RFM22_check_header0, id & 0xff); rfm22_write(rfm22b_dev, RFM22_check_header1, (id >> 8) & 0xff); rfm22_write(rfm22b_dev, RFM22_check_header2, (id >> 16) & 0xff); @@ -1561,8 +1436,8 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) rfm22_releaseBus(rfm22b_dev); // Initialize the frequency and datarate to te default. - rfm22_setNominalCarrierFrequency(rfm22b_dev, rfm22b_dev->init_frequency, rfm22b_dev->init_frequency, RFM22B_FREQUENCY_HOP_STEP_SIZE); - pios_rfm22_setDatarate(rfm22b_dev, RFM22B_DEFAULT_RX_DATARATE, true); + rfm22_setNominalCarrierFrequency(rfm22b_dev, 0); + pios_rfm22_setDatarate(rfm22b_dev); return RADIO_EVENT_INITIALIZED; } @@ -1580,15 +1455,19 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) * @param[in] datarate The air datarate. * @param[in] data_whitening Is data whitening desired? */ -static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm22b_datarate datarate, bool data_whitening) +static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev) { - uint32_t datarate_bps = data_rate[datarate]; + enum rfm22b_datarate datarate = rfm22b_dev->datarate; + bool data_whitening = true; + uint32_t datarate_bps = data_rate[datarate]; - rfm22b_dev->max_packet_time = (uint16_t)((float)(PIOS_PH_MAX_PACKET * 8 * 1000) / (float)(datarate_bps) + 0.5f); + // Calculate the maximum packet length from the datarate. + float bytes_per_period = (float)datarate_bps * (float)(rfm22b_dev->packet_period) / 8000; - // Generate a pseudo-random number from 0-8 to add to the delay - uint8_t random = PIOS_CRC_updateByte(0, (uint8_t)(xTaskGetTickCount() & 0xff)) & 0x03; - rfm22b_dev->max_ack_delay = (uint16_t)((float)((sizeof(PHAckNackPacket) * 8 + TX_PREAMBLE_NIBBLES * 4) * 1000) / (float)(datarate_bps) + 0.5f) * 4 + 4 + random; + rfm22b_dev->max_packet_len = bytes_per_period - TX_PREAMBLE_NIBBLES / 2 - SYNC_BYTES - HEADER_BYTES - 5; + if (rfm22b_dev->max_packet_len > RFM22B_MAX_PACKET_LEN) { + rfm22b_dev->max_packet_len = RFM22B_MAX_PACKET_LEN; + } // Claim the SPI bus. rfm22_claimBus(rfm22b_dev); @@ -1651,16 +1530,17 @@ static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev, enum rfm2 } /** - * Set the nominal carrier frequency and channel step size. + * Set the nominal carrier frequency, channel step size, and initial channel * * @param[in] rfm33b_dev The device structure pointer. - * @param[in] min_frequency The minimum frequenc to transmit on (in Hz). - * @param[in] max_frequency The maximum frequenc to transmit on (in Hz). - * @param[in] step_size The channel spacing (in Hz). + * @param[in] init_chan The initial channel to tune to. */ -static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint32_t min_frequency, uint32_t max_frequency, uint32_t step_size) +static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, uint8_t init_chan) { - uint32_t frequency_hz = min_frequency; + // Set the frequency channels to start at 430MHz + uint32_t frequency_hz = RFM22B_NOMINAL_CARRIER_FREQUENCY; + // The step size is 10MHz / 250 channels = 40khz, and the step size is specified in 10khz increments. + uint8_t freq_hop_step_size = 4; // holds the hbsel (1 or 2) uint8_t hbsel; @@ -1681,22 +1561,15 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, // Claim the SPI bus. rfm22_claimBus(rfm22b_dev); - // Calculate the number of frequency hopping channels. - rfm22b_dev->num_channels = (step_size == 0) ? 1 : (uint16_t)((max_frequency - min_frequency) / step_size); - - // initialize the frequency hopping step size (specified in 10khz increments). - uint32_t freq_hop_step_size = step_size / 10000; - if (freq_hop_step_size > 255) { - freq_hop_step_size = 255; - } - rfm22_write(rfm22b_dev, RFM22_frequency_hopping_step_size, (uint8_t)freq_hop_step_size); + // Setthe frequency hopping step size. + rfm22_write(rfm22b_dev, RFM22_frequency_hopping_step_size, freq_hop_step_size); // frequency hopping channel (0-255) - rfm22b_dev->frequency_step_size = 156.25f * hbsel; + rfm22b_dev->frequency_step_size = 156.25f * hbsel; // frequency hopping channel (0-255) - rfm22b_dev->frequency_hop_channel = 0; - rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, 0); + rfm22b_dev->channel = init_chan; + rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, init_chan); // no frequency offset rfm22_write(rfm22b_dev, RFM22_frequency_offset1, 0); @@ -1720,14 +1593,14 @@ static void rfm22_setNominalCarrierFrequency(struct pios_rfm22b_dev *rfm22b_dev, static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t channel) { // set the frequency hopping channel - if (rfm22b_dev->frequency_hop_channel == channel) { + if (rfm22b_dev->channel == channel) { return false; } #ifdef PIOS_RFM22B_DEBUG_ON_TELEM D3_LED_TOGGLE; #endif // PIOS_RFM22B_DEBUG_ON_TELEM - rfm22b_dev->frequency_hop_channel = channel; - rfm22_write_claim(rfm22b_dev, RFM22_frequency_hopping_channel_select, channel); + rfm22b_dev->channel = channel; + rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, channel); return true; } @@ -1777,10 +1650,6 @@ static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->rx_buffer_wr = 0; rfm22b_dev->packet_start_ticks = 0; rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION; - rfm22b_dev->rx_complete_ticks = xTaskGetTickCount(); - if (rfm22b_dev->rx_complete_ticks == 0) { - rfm22b_dev->rx_complete_ticks = 1; - } } @@ -1796,94 +1665,45 @@ static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev) */ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev) { - PHPacketHandle p = NULL; + uint8_t *p = radio_dev->rx_packet; + uint8_t len = 0; + uint8_t max_data_len = radio_dev->max_packet_len - RS_ECC_NPARITY; // Don't send if it's not our turn, or if we're receiving a packet. if (!rfm22_timeToSend(radio_dev) || !PIOS_RFM22B_InRxWait((uint32_t)radio_dev)) { return RADIO_EVENT_RX_MODE; } -#ifdef PIOS_PPM_RECEIVER - // Send a PPM packet? - if (radio_dev->send_ppm) { - p = (PHPacketHandle) & (radio_dev->ppm_packet); - radio_dev->send_ppm = false; - } -#endif - - // Send an ACK/NACK? - if (!p && radio_dev->send_ack_nack) { - p = (PHPacketHandle) & (radio_dev->ack_nack_packet); - radio_dev->send_ack_nack = false; - } - - // Resend a packet? - if (!p && radio_dev->resend_packet) { - p = radio_dev->prev_tx_packet; - radio_dev->prev_tx_packet = NULL; - radio_dev->resend_packet = false; - } - - // Don't send another packet if we're waiting for an ACK. - if (!p && radio_dev->prev_tx_packet) { + // Don't send anything if we're bound to a coordinator and not yet connected. + if (!rfm22_isCoordinator(radio_dev) && !rfm22_isConnected(radio_dev)) { return RADIO_EVENT_RX_MODE; } - // Send a connection request? - if (!p && radio_dev->send_connection_request) { - p = (PHPacketHandle) & (radio_dev->con_packet); - radio_dev->send_connection_request = false; - } - // Try to get some data to send - if (!p) { + if (radio_dev->tx_out_cb) { bool need_yield = false; - p = &radio_dev->data_packet; - p->header.type = PACKET_TYPE_DATA; - p->header.destination_id = radio_dev->destination_id; - if (radio_dev->tx_out_cb && (p->header.data_size == 0)) { - p->header.data_size = (radio_dev->tx_out_cb)(radio_dev->tx_out_context, p->data, PH_MAX_DATA, NULL, &need_yield); - } - - // Don't send any data until we're connected. - if (!rfm22_isConnected(radio_dev)) { - p->header.data_size = 0; - } - if (p->header.data_size == 0) { - p = NULL; - } + len = (radio_dev->tx_out_cb)(radio_dev->tx_out_context, p, max_data_len, NULL, &need_yield); } - // Send status? - if (!p && radio_dev->send_status) { - p = (PHPacketHandle) & (radio_dev->status_packet); - radio_dev->send_status = false; - } - - if (!p) { + // Always send a packet on the sync channel if this modem is a coordinator. + if ((len == 0) && ((radio_dev->channel_index != 0) || !rfm22_isCoordinator(radio_dev))) { return RADIO_EVENT_RX_MODE; } -#ifdef PIOS_RFM22B_DEBUG_ON_TELEM - D1_LED_ON; -#endif + // Increment the packet sequence number. + radio_dev->stats.tx_seq++; - // Add the packet sequence number. - p->header.seq_num = radio_dev->stats.tx_seq++; - - // Pass the time of the previous transmitted packet to use for synchronizing the clocks. - p->header.prev_tx_time = radio_dev->tx_complete_ticks; - - // Change the channel if necessary, but not when ACKing the connection request message. - if ((p->header.type != PACKET_TYPE_ACK) || (radio_dev->rx_packet.header.type != PACKET_TYPE_CON_REQUEST)) { - rfm22_changeChannel(radio_dev); - } + // Change the channel. + rfm22_changeChannel(radio_dev); // Add the error correcting code. - encode_data((unsigned char *)p, PHPacketSize(p), (unsigned char *)p); + if (len != 0) { + encode_data((unsigned char *)p, len, (unsigned char *)p); + } + len += RS_ECC_NPARITY; // Transmit the packet. - PIOS_RFM22B_TransmitPacket((uint32_t)radio_dev, p); + PIOS_RFM22B_TransmitPacket((uint32_t)radio_dev, p, len); return RADIO_EVENT_NUM_EVENTS; } @@ -1901,25 +1721,11 @@ static enum pios_radio_event radio_txData(struct pios_rfm22b_dev *radio_dev) // Is the transmition complete if (res == PIOS_RFM22B_TX_COMPLETE) { - radio_dev->stats.tx_byte_count += PH_PACKET_SIZE(radio_dev->tx_packet); - radio_dev->tx_complete_ticks = xTaskGetTickCount(); + radio_dev->tx_complete_ticks = xTaskGetTickCount(); // Is this an ACK? - bool is_ack = (radio_dev->tx_packet->header.type == PACKET_TYPE_ACK); ret_event = RADIO_EVENT_RX_MODE; - if (is_ack) { - // If this is an ACK for a connection request message we need to - // configure this modem from the connection request message. - if (radio_dev->rx_packet.header.type == PACKET_TYPE_CON_REQUEST) { - rfm22_setConnectionParameters(radio_dev); - } - } else if ((radio_dev->tx_packet->header.type != PACKET_TYPE_NACK) && - (radio_dev->tx_packet->header.type != PACKET_TYPE_PPM) && - (radio_dev->tx_packet->header.type != PACKET_TYPE_STATUS)) { - // We need to wait for an ACK if this packet it not an ACK, NACK, PPM, or status packet. - radio_dev->prev_tx_packet = radio_dev->tx_packet; - } - radio_dev->tx_packet = 0; + radio_dev->tx_packet_handle = 0; radio_dev->tx_data_wr = radio_dev->tx_data_rd = 0; // Start a new transaction radio_dev->packet_start_ticks = 0; @@ -1940,7 +1746,7 @@ static enum pios_radio_event radio_txData(struct pios_rfm22b_dev *radio_dev) */ static enum pios_radio_event radio_setRxMode(struct pios_rfm22b_dev *rfm22b_dev) { - if (!PIOS_RFM22B_ReceivePacket((uint32_t)rfm22b_dev, &(rfm22b_dev->rx_packet))) { + if (!PIOS_RFM22B_ReceivePacket((uint32_t)rfm22b_dev, rfm22b_dev->rx_packet)) { return RADIO_EVENT_NUM_EVENTS; } rfm22b_dev->packet_start_ticks = 0; @@ -1957,19 +1763,22 @@ static enum pios_radio_event radio_setRxMode(struct pios_rfm22b_dev *rfm22b_dev) * @param[in] rc_len The number of bytes received. * @return enum pios_radio_event The next event to inject */ -static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_dev, PHPacketHandle p, uint16_t rx_len) +static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_dev, uint8_t *p, uint16_t rx_len) { - portTickType curTicks = xTaskGetTickCount(); - + uint8_t data_len = rx_len - RS_ECC_NPARITY; // Attempt to correct any errors in the packet. - decode_data((unsigned char *)p, rx_len); - - bool good_packet = check_syndrome() == 0; + bool good_packet = true; bool corrected_packet = false; - // We have an error. Try to correct it. - if (!good_packet && (correct_errors_erasures((unsigned char *)p, rx_len, 0, 0) != 0)) { - // We corrected it - corrected_packet = true; + + if (data_len > 0) { + decode_data((unsigned char *)p, rx_len); + + good_packet = check_syndrome() == 0; + // We have an error. Try to correct it. + if (!good_packet && (correct_errors_erasures((unsigned char *)p, rx_len, 0, 0) != 0)) { + // We corrected it + corrected_packet = true; + } } // Set the packet status @@ -1985,101 +1794,22 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d enum pios_radio_event ret_event = RADIO_EVENT_RX_COMPLETE; if (good_packet || corrected_packet) { - switch (p->header.type) { - case PACKET_TYPE_STATUS: - ret_event = RADIO_EVENT_STATUS_RECEIVED; - break; - case PACKET_TYPE_CON_REQUEST: - ret_event = RADIO_EVENT_CONNECTION_REQUESTED; - break; - case PACKET_TYPE_DATA: - { - // Send the data to the com port - bool rx_need_yield; - if (radio_dev->rx_in_cb) { - (radio_dev->rx_in_cb)(radio_dev->rx_in_context, p->data, p->header.data_size, NULL, &rx_need_yield); - } - break; - } - case PACKET_TYPE_DUPLICATE_DATA: - break; - case PACKET_TYPE_ACK: - ret_event = RADIO_EVENT_PACKET_ACKED; - break; - case PACKET_TYPE_NACK: - ret_event = RADIO_EVENT_PACKET_NACKED; - break; - case PACKET_TYPE_PPM: - { -#if defined(PIOS_INCLUDE_GCSRCVR) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)) || defined(PIOS_INCLUDE_RFM22B_RCVR) - PHPpmPacketHandle ppmp = (PHPpmPacketHandle)p; -#if defined(PIOS_INCLUDE_GCSRCVR) || (defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT)) - bool ppm_output = false; -#endif -#endif -#if defined(PIOS_INCLUDE_RFM22B_RCVR) - ppm_output = true; - radio_dev->ppm_fresh = true; - for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) { - radio_dev->ppm_channel[i] = ppmp->channels[i]; - } -#endif -#if defined(PIOS_INCLUDE_PPM_OUT) && defined(PIOS_PPM_OUTPUT) - if (PIOS_PPM_OUTPUT) { - ppm_output = true; - for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) { - PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, ppmp->channels[i]); - } - } -#endif -#if defined(PIOS_INCLUDE_GCSRCVR) - if (!ppm_output) { - GCSReceiverData gcsRcvr; - for (uint8_t i = 0; (i < PIOS_RFM22B_RCVR_MAX_CHANNELS) && (i < GCSRECEIVER_CHANNEL_NUMELEM); ++i) { - gcsRcvr.Channel[i] = ppmp->channels[i]; - } - GCSReceiverSet(&gcsRcvr); - } -#endif - break; - } - default: - break; + // Send the data to the com port + bool rx_need_yield; + if (radio_dev->rx_in_cb) { + (radio_dev->rx_in_cb)(radio_dev->rx_in_context, p, data_len, NULL, &rx_need_yield); } - uint16_t seq_num = radio_dev->rx_packet.header.seq_num; - if (rfm22_isConnected(radio_dev)) { - // Adjust the clock syncronization if this is the remote modem. - // The coordinator sends the time that the previous packet was finised transmitting, - // which should match the time that the last packet was received. - uint16_t prev_seq_num = radio_dev->stats.rx_seq; - if (seq_num == (prev_seq_num + 1)) { - portTickType local_rx_time = radio_dev->rx_complete_ticks; - portTickType remote_tx_time = radio_dev->rx_packet.header.prev_tx_time; - portTickType time_delta = remote_tx_time - local_rx_time; - portTickType time_delta_delta = (time_delta > radio_dev->time_delta) ? (time_delta - radio_dev->time_delta) : (radio_dev->time_delta - time_delta); - if (time_delta_delta <= 2) { - radio_dev->time_delta = remote_tx_time - local_rx_time; - } - } else if (seq_num > prev_seq_num) { - // Add any missed packets into the stats. - uint16_t missed_packets = seq_num - prev_seq_num - 1; - radio_dev->stats.rx_missed += missed_packets; - } + // We only synchronize the clock on packets from our coordinator on the sync channel. + if (!rfm22_isCoordinator(radio_dev) && (radio_dev->rx_destination_id == rfm22_destinationID(radio_dev)) && (radio_dev->channel_index == 0)) { + rfm22_synchronizeClock(radio_dev); + radio_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED; + radio_dev->on_sync_channel = false; } - - // Update the sequence number - radio_dev->stats.rx_seq = seq_num; } else { ret_event = RADIO_EVENT_RX_COMPLETE; } - // Log the time that the packet was received. - radio_dev->rx_complete_ticks = curTicks; - if (radio_dev->rx_complete_ticks == 0) { - radio_dev->rx_complete_ticks = 1; - } - return ret_event; } @@ -2122,210 +1852,19 @@ static enum pios_radio_event radio_rxData(struct pios_rfm22b_dev *radio_dev) } /***************************************************************************** -* Packet Transmition Functions +* Link Statistics Functions *****************************************************************************/ /** - * Send a radio status message. - * - * @param[in] rfm22b_dev The device structure - */ -static void rfm22_sendStatus(struct pios_rfm22b_dev *rfm22b_dev) -{ - // Don't send if a status is already queued or if we're the coordinator and not connected. - if (rfm22b_dev->send_status || (rfm22b_dev->coordinator && !rfm22_isConnected(rfm22b_dev))) { - return; - } - - // Update the link quality metric. - rfm22_calculateLinkQuality(rfm22b_dev); - - // Queue the status message - if (rfm22_isConnected(rfm22b_dev)) { - rfm22b_dev->status_packet.header.destination_id = rfm22b_dev->destination_id; - } else if (rfm22b_dev->coordinator) { - return; - } else { - rfm22b_dev->status_packet.header.destination_id = 0xffffffff; // Broadcast - } - rfm22b_dev->status_packet.header.type = PACKET_TYPE_STATUS; - rfm22b_dev->status_packet.header.data_size = PH_STATUS_DATA_SIZE(&(rfm22b_dev->status_packet)); - rfm22b_dev->status_packet.source_id = rfm22b_dev->deviceID; - rfm22b_dev->status_packet.link_quality = rfm22b_dev->stats.link_quality; - rfm22b_dev->status_packet.received_rssi = rfm22b_dev->rssi_dBm; - rfm22b_dev->send_status = true; -} - -/** - * Send a PPM packet. - * - * @param[in] rfm22b_dev The device structure - */ -static void rfm22_sendPPM(__attribute__((unused)) struct pios_rfm22b_dev *rfm22b_dev) -{ -#ifdef PIOS_PPM_RECEIVER - // Only send PPM if we're connected - if (!rfm22_isConnected(rfm22b_dev)) { - return; - } - - // Just return if the PPM receiver is not configured. - if (PIOS_PPM_RECEIVER == 0) { - return; - } - - // See if we have any valid channels. - bool valid_input_detected = false; - for (uint8_t i = 0; i < PIOS_PPM_NUM_INPUTS; ++i) { - rfm22b_dev->ppm_packet.channels[i] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i + 1); - if ((rfm22b_dev->ppm_packet.channels[i] != PIOS_RCVR_INVALID) && (rfm22b_dev->ppm_packet.channels[i] != PIOS_RCVR_TIMEOUT)) { - valid_input_detected = true; - } - } - - // Send the PPM packet if it's valid - if (valid_input_detected) { - rfm22b_dev->ppm_packet.header.destination_id = rfm22b_dev->destination_id; - rfm22b_dev->ppm_packet.header.type = PACKET_TYPE_PPM; - rfm22b_dev->ppm_packet.header.data_size = PH_PPM_DATA_SIZE(&(rfm22b_dev->ppm_packet)); - rfm22b_dev->send_ppm = true; - } -#endif /* ifdef PIOS_PPM_RECEIVER */ -} - -/** - * Send an ACK to a received packet. + * Update the modem pair status. * * @param[in] rfm22b_dev The device structure - * @return enum pios_radio_event The next event to inject */ -static enum pios_radio_event rfm22_sendAck(struct pios_rfm22b_dev *rfm22b_dev) +static void rfm22_updatePairStatus(struct pios_rfm22b_dev *radio_dev) { - // We don't ACK PPM or status packets. - if ((rfm22b_dev->rx_packet.header.type != PACKET_TYPE_PPM) && (rfm22b_dev->rx_packet.header.type != PACKET_TYPE_STATUS)) { - PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet)); - aph->header.destination_id = rfm22b_dev->destination_id; - aph->header.type = PACKET_TYPE_ACK; - aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph); - rfm22b_dev->send_ack_nack = true; - } - return RADIO_EVENT_TX_START; -} - -/** - * Send an NACK to a received packet. - * - * @param[in] rfm22b_dev The device structure - * @return enum pios_radio_event The next event to inject - */ -static enum pios_radio_event rfm22_sendNack(struct pios_rfm22b_dev *rfm22b_dev) -{ - PHAckNackPacketHandle aph = (PHAckNackPacketHandle)(&(rfm22b_dev->ack_nack_packet)); - - // We don't NACK PPM or status packets. - if ((rfm22b_dev->rx_packet.header.type != PACKET_TYPE_PPM) && (rfm22b_dev->rx_packet.header.type != PACKET_TYPE_STATUS)) { - aph->header.destination_id = rfm22b_dev->destination_id; - aph->header.type = PACKET_TYPE_NACK; - aph->header.data_size = PH_ACK_NACK_DATA_SIZE(aph); - rfm22b_dev->send_ack_nack = true; - } - return RADIO_EVENT_TX_START; -} - -/** - * Send a connection request message. - * - * @param[in] rfm22b_dev The device structure - * @return enum pios_radio_event The next event to inject - */ -static enum pios_radio_event rfm22_requestConnection(struct pios_rfm22b_dev *rfm22b_dev) -{ - PHConnectionPacketHandle cph = &(rfm22b_dev->con_packet); - - // Set our connection state to requesting connection. - rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTING; - - // Fill in the connection request - rfm22b_dev->destination_id = rfm22b_dev->bindings[rfm22b_dev->cur_binding].pairID; - cph->header.destination_id = rfm22b_dev->destination_id; - cph->header.type = PACKET_TYPE_CON_REQUEST; - cph->header.data_size = PH_CONNECTION_DATA_SIZE(&(rfm22b_dev->con_packet)); - cph->source_id = rfm22b_dev->deviceID; - cph->status_rx_time = rfm22b_dev->rx_complete_ticks; - cph->main_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].main_port; - cph->flexi_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].flexi_port; - cph->vcp_port = rfm22b_dev->bindings[rfm22b_dev->cur_binding].vcp_port; - cph->com_speed = rfm22b_dev->bindings[rfm22b_dev->cur_binding].com_speed; - rfm22b_dev->send_connection_request = true; - rfm22b_dev->prev_tx_packet = NULL; - - return RADIO_EVENT_TX_START; -} - - -/***************************************************************************** -* Packet Receipt Functions -*****************************************************************************/ - -/** - * Receive an ACK. - * - * @param[in] rfm22b_dev The device structure - * @return enum pios_radio_event The next event to inject - */ -static enum pios_radio_event rfm22_receiveAck(struct pios_rfm22b_dev *rfm22b_dev) -{ - PHPacketHandle prev = rfm22b_dev->prev_tx_packet; - - // Clear the previous TX packet. - rfm22b_dev->prev_tx_packet = NULL; - - // Was this a connection request? - switch (prev->header.type) { - case PACKET_TYPE_CON_REQUEST: - rfm22_setConnectionParameters(rfm22b_dev); - break; - case PACKET_TYPE_DATA: - rfm22b_dev->data_packet.header.data_size = 0; - break; - } - - // Should we try to start another TX? - return RADIO_EVENT_TX_START; -} - -/** - * Receive an MACK. - * - * @param[in] rfm22b_dev The device structure - * @return enum pios_radio_event The next event to inject - */ -static enum pios_radio_event rfm22_receiveNack(struct pios_rfm22b_dev *rfm22b_dev) -{ - // Resend the previous TX packet. - rfm22b_dev->tx_packet = rfm22b_dev->prev_tx_packet; - rfm22b_dev->prev_tx_packet = NULL; - - // Increment the reset packet counter if we're connected. - if (rfm22_isConnected(rfm22b_dev)) { - rfm22b_add_rx_status(rfm22b_dev, RADIO_RESENT_TX_PACKET); - } - return RADIO_EVENT_TX_START; -} - -/** - * Receive a status packet - * - * @param[in] rfm22b_dev The device structure - * @return enum pios_radio_event The next event to inject - */ -static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *radio_dev) -{ - PHStatusPacketHandle status = (PHStatusPacketHandle) & (radio_dev->rx_packet); - int8_t rssi = radio_dev->rssi_dBm; - int8_t afc = radio_dev->afc_correction_Hz; - uint32_t id = status->source_id; - enum pios_radio_event ret_event = RADIO_EVENT_RX_COMPLETE; + int8_t rssi = radio_dev->rssi_dBm; + int8_t afc = radio_dev->afc_correction_Hz; + uint32_t id = radio_dev->rx_destination_id; // Have we seen this device recently? bool found = false; @@ -2338,7 +1877,7 @@ static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *radio_d } } - // If we have seen it, update the RSSI and reset the last contact couter + // If we have seen it, update the RSSI and reset the last contact counter if (found) { radio_dev->pair_stats[id_idx].rssi = rssi; radio_dev->pair_stats[id_idx].afc_correction = afc; @@ -2358,26 +1897,8 @@ static enum pios_radio_event rfm22_receiveStatus(struct pios_rfm22b_dev *radio_d radio_dev->pair_stats[min_idx].afc_correction = afc; radio_dev->pair_stats[min_idx].lastContact = 0; } - - // Send a connection request message if we're not connected, and this is a status message from a modem that we're bound to. - if (radio_dev->coordinator && !rfm22_isConnected(radio_dev)) { - for (uint8_t i = 0; i < OPLINKSETTINGS_BINDINGS_NUMELEM; ++i) { - if (radio_dev->bindings[i].pairID == id) { - radio_dev->cur_binding = i; - ret_event = RADIO_EVENT_REQUEST_CONNECTION; - break; - } - } - } - - return ret_event; } - -/***************************************************************************** -* Link Statistics Functions -*****************************************************************************/ - /** * Calculate the link quality from the packet receipt, tranmittion statistics. * @@ -2438,87 +1959,30 @@ static void rfm22b_add_rx_status(struct pios_rfm22b_dev *rfm22b_dev, enum pios_r *****************************************************************************/ /** - * Are we connected to the remote modem? + * Are we a coordinator modem? * * @param[in] rfm22b_dev The device structure */ -static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev) +static bool rfm22_isCoordinator(struct pios_rfm22b_dev *rfm22b_dev) { - return rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED; + return rfm22b_dev->coordinator; } /** - * Set the connection parameters from a connection request message. + * Returns the destination ID to send packets to. * - * @param[in] rfm22b_dev The device structure + * @param[in] rfm22b_id The RFM22B device index. + * @return The destination ID */ -static void rfm22_setConnectionParameters(struct pios_rfm22b_dev *rfm22b_dev) +uint32_t rfm22_destinationID(struct pios_rfm22b_dev *rfm22b_dev) { - PHConnectionPacketHandle cph = &(rfm22b_dev->con_packet); - - // Set our connection state to connected - rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTED; - - // Call the com port configuration function - if (rfm22b_dev->com_config_cb) { - rfm22b_dev->com_config_cb(cph->main_port, cph->flexi_port, cph->vcp_port, cph->com_speed); + if (rfm22_isCoordinator(rfm22b_dev)) { + return rfm22b_dev->deviceID; + } else if (rfm22b_dev->coordinatorID) { + return rfm22b_dev->coordinatorID; + } else { + return 0xffffffff; } - - // Configure this modem min an max frequency. - rfm22_setNominalCarrierFrequency(rfm22b_dev, cph->min_frequency, cph->max_frequency, cph->channel_spacing); - - // Configure the modem datarate. - rfm22b_dev->datarate = RFM22_datarate_64000; - switch (cph->com_speed) { - case OPLINKSETTINGS_COMSPEED_2400: - rfm22b_dev->datarate = RFM22_datarate_8000; - break; - case OPLINKSETTINGS_COMSPEED_4800: - rfm22b_dev->datarate = RFM22_datarate_8000; - break; - case OPLINKSETTINGS_COMSPEED_9600: - rfm22b_dev->datarate = RFM22_datarate_16000; - break; - case OPLINKSETTINGS_COMSPEED_19200: - rfm22b_dev->datarate = RFM22_datarate_32000; - break; - case OPLINKSETTINGS_COMSPEED_38400: - rfm22b_dev->datarate = RFM22_datarate_57600; - break; - case OPLINKSETTINGS_COMSPEED_57600: - rfm22b_dev->datarate = RFM22_datarate_128000; - break; - case OPLINKSETTINGS_COMSPEED_115200: - rfm22b_dev->datarate = RFM22_datarate_192000; - break; - } - pios_rfm22_setDatarate(rfm22b_dev, rfm22b_dev->datarate, true); -} - -/** - * Accept a connection request. - * - * @param[in] rfm22b_dev The device structure - * @return enum pios_radio_event The next event to inject - */ -static enum pios_radio_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm22b_dev) -{ - // Copy the connection packet - PHConnectionPacketHandle cph = (PHConnectionPacketHandle)(&(rfm22b_dev->rx_packet)); - PHConnectionPacketHandle lcph = (PHConnectionPacketHandle)(&(rfm22b_dev->con_packet)); - - memcpy((uint8_t *)lcph, (uint8_t *)cph, PH_PACKET_SIZE((PHPacketHandle)cph)); - - // Set the destination ID to the source of the connection request message. - rfm22b_dev->destination_id = cph->source_id; - - // The remote modem sets the time delta between the two modems using the differene between the clock - // on the local modem when it sent the status packet and the time on the coordinator modem when it was received. - portTickType local_tx_time = rfm22b_dev->tx_complete_ticks; - portTickType remote_rx_time = cph->status_rx_time; - rfm22b_dev->time_delta = remote_rx_time - local_tx_time; - - return RADIO_EVENT_DEFAULT; } @@ -2526,6 +1990,23 @@ static enum pios_radio_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm2 * Frequency Hopping Functions *****************************************************************************/ +/** + * Synchronize the clock after a packet receive from our coordinator on the syncronization channel. + * This function should be called when a packet is received on the synchronization channel. + * + * @param[in] rfm22b_dev The device structure + */ +static void rfm22_synchronizeClock(struct pios_rfm22b_dev *rfm22b_dev) +{ + portTickType start_time = rfm22b_dev->packet_start_ticks; + + // This packet was transmitted on channel 0, calculate the time delta that will force us to transmit on channel 0 at the time this packet started. + uint16_t frequency_hop_cycle_time = rfm22b_dev->packet_period * rfm22b_dev->num_channels; + uint16_t time_delta = start_time % frequency_hop_cycle_time; + + rfm22b_dev->time_delta = frequency_hop_cycle_time - time_delta + 1; +} + /** * Return the extimated current clock ticks count on the coordinator modem. * This is the master clock used for all synchronization. @@ -2534,7 +2015,7 @@ static enum pios_radio_event rfm22_acceptConnection(struct pios_rfm22b_dev *rfm2 */ static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, portTickType ticks) { - if (rfm22b_dev->coordinator) { + if (rfm22_isCoordinator(rfm22b_dev)) { return ticks; } return ticks + rfm22b_dev->time_delta; @@ -2549,9 +2030,30 @@ static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev) { portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); - // Divide time into 8ms blocks. Coordinator sends in first 1 ms, and remote send in 4th. - // Channel changes occurs in the 7th. - return (rfm22b_dev->coordinator) ? ((time & 0x7) == 0) : ((time & 0x7) == 3); + if (!rfm22_isCoordinator(rfm22b_dev)) { + time += rfm22b_dev->packet_period - 1; + } else { + time -= 1; + } + return (time % (rfm22b_dev->packet_period * 2)) == 0; +} + +/** + * Calculate the nth channel index. + * + * @param[in] rfm22b_dev The device structure + * @param[in] index The channel index to calculate + */ +static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t index) +{ + // Make sure we don't index outside of the range. + uint8_t idx = index % rfm22b_dev->num_channels; + + rfm22b_dev->channel_index = idx; + if (idx == 0) { + rfm22b_dev->on_sync_channel = true; + } + return rfm22b_dev->channels[idx]; } /** @@ -2559,14 +2061,14 @@ static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev) * * @param[in] rfm22b_dev The device structure */ -static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev) +static uint8_t rfm22_calcChannelFromClock(struct pios_rfm22b_dev *rfm22b_dev) { portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); // Divide time into 8ms blocks. Coordinator sends in first 2 ms, and remote send in 5th and 6th ms. // Channel changes occur in the last 2 ms. - uint8_t n = (((time + 2) >> 3) & 0xff); + uint8_t n = (time / rfm22b_dev->packet_period) % rfm22b_dev->num_channels; - return PIOS_CRC_updateByte(0, n) % rfm22b_dev->num_channels; + return rfm22_calcChannel(rfm22b_dev, n); } /** @@ -2576,10 +2078,12 @@ static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev) */ static bool rfm22_changeChannel(struct pios_rfm22b_dev *rfm22b_dev) { - if (rfm22_isConnected(rfm22b_dev)) { - return rfm22_setFreqHopChannel(rfm22b_dev, rfm22_calcChannel(rfm22b_dev)); + // A disconnected non-coordinator modem should sit on the sync channel until connected. + if (!rfm22_isCoordinator(rfm22b_dev) && !rfm22_isConnected(rfm22b_dev)) { + return rfm22_setFreqHopChannel(rfm22b_dev, rfm22_calcChannel(rfm22b_dev, 0)); + } else { + return rfm22_setFreqHopChannel(rfm22b_dev, rfm22_calcChannelFromClock(rfm22b_dev)); } - return false; } @@ -2612,7 +2116,7 @@ static enum pios_radio_event rfm22_timeout(struct pios_rfm22b_dev *rfm22b_dev) rfm22b_dev->stats.timeouts++; rfm22b_dev->packet_start_ticks = 0; // Release the Tx packet if it's set. - if (rfm22b_dev->tx_packet != 0) { + if (rfm22b_dev->tx_packet_handle != 0) { rfm22b_dev->tx_data_rd = rfm22b_dev->tx_data_wr = 0; } rfm22b_dev->rfm22b_state = RFM22B_STATE_TRANSITION; diff --git a/flight/pios/common/pios_rfm22b_rcvr.c b/flight/pios/common/pios_rfm22b_rcvr.c index 81f43efa8..f0dbc3bc4 100644 --- a/flight/pios/common/pios_rfm22b_rcvr.c +++ b/flight/pios/common/pios_rfm22b_rcvr.c @@ -34,6 +34,8 @@ #include "pios_rfm22b_priv.h" +#include + /* Provide a RCVR driver */ static int32_t PIOS_RFM22B_RCVR_Get(uint32_t rcvr_id, uint8_t channel); static void PIOS_RFM22B_RCVR_Supervisor(uint32_t rcvr_id); diff --git a/flight/pios/inc/pios_rfm22b.h b/flight/pios/inc/pios_rfm22b.h index c6417cc80..b28f8ff6b 100644 --- a/flight/pios/inc/pios_rfm22b.h +++ b/flight/pios/inc/pios_rfm22b.h @@ -31,7 +31,7 @@ #ifndef PIOS_RFM22B_H #define PIOS_RFM22B_H -#include +#include #include /* Constant definitions */ @@ -104,28 +104,21 @@ struct rfm22b_stats { uint8_t link_state; }; -/* Callback function prototypes */ -typedef void (*PIOS_RFM22B_ComConfigCallback)(OPLinkSettingsRemoteMainPortOptions main_port, OPLinkSettingsRemoteFlexiPortOptions flexi_port, - OPLinkSettingsRemoteVCPPortOptions vcp_port, OPLinkSettingsComSpeedOptions com_speed); - /* Public Functions */ extern int32_t PIOS_RFM22B_Init(uint32_t *rfb22b_id, uint32_t spi_id, uint32_t slave_num, const struct pios_rfm22b_cfg *cfg); extern void PIOS_RFM22B_Reinit(uint32_t rfb22b_id); extern void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr); -extern void PIOS_RFM22B_SetFrequencyRange(uint32_t rfm22b_id, uint32_t min_freq, uint32_t max_freq, uint32_t step_size); -extern void PIOS_RFM22B_SetInitialFrequency(uint32_t rfm22b_id, uint32_t init_freq); -extern void PIOS_RFM22B_SetDestinationId(uint32_t rfm22b_id, uint32_t dest_id); -extern void PIOS_RFM22B_SetComConfigCallback(uint32_t rfm22b_id, PIOS_RFM22B_ComConfigCallback cb); -extern void PIOS_RFM22B_SetBindings(uint32_t rfm22b_id, const uint32_t bindingPairIDs[], const uint8_t mainPortSettings[], - const uint8_t flexiPortSettings[], const uint8_t vcpPortSettings[], const uint8_t comSpeeds[]); +extern void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, uint8_t num_chan, uint8_t min_chan, uint8_t max_chan, uint8_t chan_set, uint8_t packet_period, bool oneway); +extern void PIOS_RFM22B_SetCoordinator(uint32_t rfm22b_id, bool coordinator); +extern void PIOS_RFM22B_SetCoordinatorID(uint32_t rfm22b_id, uint32_t coord_id); extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id); extern bool PIOS_RFM22B_IsCoordinator(uint32_t rfb22b_id); extern void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats); extern uint8_t PIOS_RFM2B_GetPairStats(uint32_t rfm22b_id, uint32_t *device_ids, int8_t *RSSIs, uint8_t max_pairs); extern bool PIOS_RFM22B_InRxWait(uint32_t rfb22b_id); extern bool PIOS_RFM22B_LinkStatus(uint32_t rfm22b_id); -extern bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, PHPacketHandle p); -extern bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, PHPacketHandle p); +extern bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, uint8_t *p); +extern bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, uint8_t *p, uint8_t len); extern pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id); extern pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id); diff --git a/flight/pios/inc/pios_rfm22b_priv.h b/flight/pios/inc/pios_rfm22b_priv.h index 8e274ef44..ad21e68a9 100644 --- a/flight/pios/inc/pios_rfm22b_priv.h +++ b/flight/pios/inc/pios_rfm22b_priv.h @@ -40,14 +40,14 @@ // ************************************ -#define RFM22_DEVICE_VERSION_V2 0x02 -#define RFM22_DEVICE_VERSION_A0 0x04 -#define RFM22_DEVICE_VERSION_B1 0x06 +#define RFM22B_MAX_PACKET_LEN 64 +#define RFM22B_NUM_CHANNELS 250 // ************************************ -#define RFM22_MIN_CARRIER_FREQUENCY_HZ 240000000ul -#define RFM22_MAX_CARRIER_FREQUENCY_HZ 930000000ul +#define RFM22_DEVICE_VERSION_V2 0x02 +#define RFM22_DEVICE_VERSION_A0 0x04 +#define RFM22_DEVICE_VERSION_B1 0x06 // ************************************ @@ -534,19 +534,12 @@ enum pios_rfm22b_dev_magic { enum pios_radio_state { RADIO_STATE_UNINITIALIZED, RADIO_STATE_INITIALIZING, - RADIO_STATE_REQUESTING_CONNECTION, - RADIO_STATE_ACCEPTING_CONNECTION, RADIO_STATE_RX_MODE, RADIO_STATE_RX_DATA, RADIO_STATE_RX_FAILURE, - RADIO_STATE_RECEIVING_STATUS, RADIO_STATE_TX_START, RADIO_STATE_TX_DATA, RADIO_STATE_TX_FAILURE, - RADIO_STATE_SENDING_ACK, - RADIO_STATE_SENDING_NACK, - RADIO_STATE_RECEIVING_ACK, - RADIO_STATE_RECEIVING_NACK, RADIO_STATE_TIMEOUT, RADIO_STATE_ERROR, RADIO_STATE_FATAL_ERROR, @@ -559,16 +552,9 @@ enum pios_radio_event { RADIO_EVENT_INT_RECEIVED, RADIO_EVENT_INITIALIZE, RADIO_EVENT_INITIALIZED, - RADIO_EVENT_REQUEST_CONNECTION, - RADIO_EVENT_CONNECTION_REQUESTED, - RADIO_EVENT_PACKET_ACKED, - RADIO_EVENT_PACKET_NACKED, - RADIO_EVENT_ACK_TIMEOUT, RADIO_EVENT_RX_MODE, RADIO_EVENT_RX_COMPLETE, - RADIO_EVENT_STATUS_RECEIVED, RADIO_EVENT_TX_START, - RADIO_EVENT_FAILURE, RADIO_EVENT_TIMEOUT, RADIO_EVENT_ERROR, RADIO_EVENT_FATAL_ERROR, @@ -603,14 +589,6 @@ typedef struct { uint8_t lastContact; } rfm22b_pair_stats; -typedef struct { - uint32_t pairID; - OPLinkSettingsRemoteMainPortOptions main_port; - OPLinkSettingsRemoteFlexiPortOptions flexi_port; - OPLinkSettingsRemoteVCPPortOptions vcp_port; - OPLinkSettingsComSpeedOptions com_speed; -} rfm22b_binding; - enum pios_rfm22b_chip_power_state { RFM22B_IDLE_STATE = 0x00, RFM22B_RX_STATE = 0x01, @@ -683,27 +661,22 @@ typedef struct { rfm22b_int_status_2 int_status_2; } rfm22b_device_status; - struct pios_rfm22b_dev { enum pios_rfm22b_dev_magic magic; struct pios_rfm22b_cfg cfg; // The SPI bus information - uint32_t spi_id; - uint32_t slave_num; + uint32_t spi_id; + uint32_t slave_num; + + // Should this modem ack as a coordinator. + bool coordinator; // The device ID - uint32_t deviceID; + uint32_t deviceID; - // The destination ID - uint32_t destination_id; - - // The list of bound radios. - rfm22b_binding bindings[OPLINKSETTINGS_BINDINGS_NUMELEM]; - uint8_t cur_binding; - - // Is this device a coordinator? - bool coordinator; + // The coodinator ID (0 if this modem is a coordinator). + uint32_t coordinatorID; // The task handle xTaskHandle taskHandle; @@ -714,9 +687,6 @@ struct pios_rfm22b_dev { // ISR pending semaphore xSemaphoreHandle isrPending; - // The com configuration callback - PIOS_RFM22B_ComConfigCallback com_config_cb; - // The COM callback functions. pios_com_callback rx_in_cb; uint32_t rx_in_context; @@ -749,77 +719,57 @@ struct pios_rfm22b_dev { struct rfm22b_stats stats; // Stats - uint16_t errors; + uint16_t errors; // RSSI in dBm - int8_t rssi_dBm; + int8_t rssi_dBm; // The tx data packet - PHPacket data_packet; + uint8_t tx_packet[RFM22B_MAX_PACKET_LEN]; // The current tx packet - PHPacketHandle tx_packet; - // The previous tx packet (waiting for an ACK) - PHPacketHandle prev_tx_packet; + uint8_t *tx_packet_handle; // The tx data read index - uint16_t tx_data_rd; + uint16_t tx_data_rd; // The tx data write index - uint16_t tx_data_wr; + uint16_t tx_data_wr; // The tx packet sequence number - uint16_t tx_seq; + uint16_t tx_seq; // The rx data packet - PHPacket rx_packet; + uint8_t rx_packet[RFM22B_MAX_PACKET_LEN]; // The rx data packet - PHPacketHandle rx_packet_handle; + uint8_t *rx_packet_handle; // The receive buffer write index - uint16_t rx_buffer_wr; + uint16_t rx_buffer_wr; // The receive buffer write index - uint16_t rx_packet_len; + uint16_t rx_packet_len; + // The id that the packet was received from + uint32_t rx_destination_id; + // The maximum packet length (including header, etc.) + uint8_t max_packet_len; + // The time alloted for transmission of a packet. + uint8_t packet_period; + // Do all packets originate from the coordinator modem? + bool one_way_link; - // The status packet - PHStatusPacket status_packet; - - // The ACK/NACK packet - PHAckNackPacket ack_nack_packet; - -#ifdef PIOS_PPM_RECEIVER - // The PPM packet - PHPpmPacket ppm_packet; -#endif - - // The connection packet. - PHConnectionPacket con_packet; - - // Send flags - bool send_status; - bool send_ppm; - bool send_connection_request; - bool send_ack_nack; - bool resend_packet; - - // The initial frequency - uint32_t init_frequency; + // The channel list + uint8_t channels[RFM22B_NUM_CHANNELS]; // The number of frequency hopping channels. - uint16_t num_channels; - + uint8_t num_channels; // The frequency hopping step size float frequency_step_size; // current frequency hop channel - uint8_t frequency_hop_channel; + uint8_t channel; + // current frequency hop channel index + uint8_t channel_index; // afc correction reading (in Hz) int8_t afc_correction_Hz; // The packet timers. portTickType packet_start_ticks; portTickType tx_complete_ticks; - portTickType rx_complete_ticks; portTickType time_delta; - - // The maximum time (ms) that it should take to transmit / receive a packet. - uint32_t max_packet_time; - - // The maximum time to wait for an ACK. - uint8_t max_ack_delay; + bool on_sync_channel; #ifdef PIOS_INCLUDE_RFM22B_RCVR // The PPM channel values diff --git a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h index 992cb67f3..da5b7d039 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h @@ -118,7 +118,7 @@ #define PIOS_INCLUDE_RFM22B_COM /* #define PIOS_INCLUDE_RFM22B_RCVR */ #define PIOS_INCLUDE_PPM_OUT -/* #define PIOS_RFM22B_DEBUG_ON_TELEM */ +#define PIOS_RFM22B_DEBUG_ON_TELEM /* PIOS misc peripherals */ /* #define PIOS_INCLUDE_VIDEO */ diff --git a/flight/targets/boards/oplinkmini/firmware/pios_board.c b/flight/targets/boards/oplinkmini/firmware/pios_board.c index dbcf00416..ec865d3a6 100644 --- a/flight/targets/boards/oplinkmini/firmware/pios_board.c +++ b/flight/targets/boards/oplinkmini/firmware/pios_board.c @@ -75,6 +75,12 @@ uint8_t *pios_uart_tx_buffer; uintptr_t pios_uavo_settings_fs_id; +// Forward definitions +static void PIOS_InitUartMainPort(); +static void PIOS_InitUartFlexiPort(); +static void PIOS_InitPPMMainPort(bool input); +static void PIOS_InitPPMFlexiPort(bool input); + /** * PIOS_Board_Init() * initializes all the core subsystems on this specific hardware @@ -216,12 +222,14 @@ void PIOS_Board_Init(void) /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) { + // Configure the RFM22B device const struct pios_board_info *bdinfo = &pios_board_info_blob; const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) { PIOS_Assert(0); } + // Configure the radio com interface 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); @@ -231,10 +239,40 @@ void PIOS_Board_Init(void) tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { PIOS_Assert(0); } + uint32_t comBaud = 9600; + switch (oplinkSettings.ComSpeed) { + case OPLINKSETTINGS_COMSPEED_2400: + comBaud = 2400; + break; + case OPLINKSETTINGS_COMSPEED_4800: + comBaud = 4800; + break; + case OPLINKSETTINGS_COMSPEED_9600: + comBaud = 9600; + break; + case OPLINKSETTINGS_COMSPEED_19200: + comBaud = 19200; + break; + case OPLINKSETTINGS_COMSPEED_38400: + comBaud = 38400; + break; + case OPLINKSETTINGS_COMSPEED_57600: + comBaud = 57600; + break; + case OPLINKSETTINGS_COMSPEED_115200: + comBaud = 115200; + break; + } + PIOS_COM_ChangeBaud(pios_com_rfm22b_id, comBaud); - /* Set the RFM22B bindings. */ - PIOS_RFM22B_SetBindings(pios_rfm22b_id, oplinkSettings.Bindings, oplinkSettings.RemoteMainPort, - oplinkSettings.RemoteFlexiPort, oplinkSettings.RemoteVCPPort, oplinkSettings.ComSpeed); + // Set the radio configuration parameters. + PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, oplinkSettings.NumChannels, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, + oplinkSettings.PacketTime, (oplinkSettings.OneWayLink == OPLINKSETTINGS_ONEWAYLINK_TRUE)); + PIOS_RFM22B_SetCoordinator(pios_rfm22b_id, (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE)); + PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID); + + // Reinitilize the modem to affect te changes. + PIOS_RFM22B_Reinit(pios_rfm22b_id); } #endif /* PIOS_INCLUDE_RFM22B */ @@ -242,6 +280,75 @@ void PIOS_Board_Init(void) pios_uart_rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RX_BUF_LEN); pios_uart_tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_TX_BUF_LEN); + // Configure the main port + bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id); + switch (oplinkSettings.MainPort) { + case OPLINKSETTINGS_MAINPORT_TELEMETRY: + case OPLINKSETTINGS_MAINPORT_SERIAL: + /* Configure the main port for uart serial */ + PIOS_InitUartMainPort(); + PIOS_COM_TELEMETRY = PIOS_COM_TELEM_UART_MAIN; + break; + case OPLINKSETTINGS_MAINPORT_PPM: + PIOS_InitPPMMainPort(is_coordinator); + break; + case OPLINKSETTINGS_MAINPORT_DISABLED: + break; + } + + // Configure the flexi port + switch (oplinkSettings.FlexiPort) { + case OPLINKSETTINGS_FLEXIPORT_TELEMETRY: + case OPLINKSETTINGS_FLEXIPORT_SERIAL: + /* Configure the flexi port as uart serial */ + PIOS_InitUartFlexiPort(); + PIOS_COM_TELEMETRY = PIOS_COM_TELEM_UART_FLEXI; + break; + case OPLINKSETTINGS_FLEXIPORT_PPM: + PIOS_InitPPMFlexiPort(is_coordinator); + break; + case OPLINKSETTINGS_FLEXIPORT_DISABLED: + break; + } + + // Configure the USB VCP port + switch (oplinkSettings.VCPPort) { + case OPLINKSETTINGS_VCPPORT_SERIAL: + PIOS_COM_TELEMETRY = PIOS_COM_TELEM_USB_VCP; + break; + case OPLINKSETTINGS_VCPPORT_DISABLED: + break; + } + + // Update the com baud rate. + uint32_t comBaud = 9600; + switch (oplinkSettings.ComSpeed) { + case OPLINKSETTINGS_COMSPEED_2400: + comBaud = 2400; + break; + case OPLINKSETTINGS_COMSPEED_4800: + comBaud = 4800; + break; + case OPLINKSETTINGS_COMSPEED_9600: + comBaud = 9600; + break; + case OPLINKSETTINGS_COMSPEED_19200: + comBaud = 19200; + break; + case OPLINKSETTINGS_COMSPEED_38400: + comBaud = 38400; + break; + case OPLINKSETTINGS_COMSPEED_57600: + comBaud = 57600; + break; + case OPLINKSETTINGS_COMSPEED_115200: + comBaud = 115200; + break; + } + if (PIOS_COM_TELEMETRY) { + PIOS_COM_ChangeBaud(PIOS_COM_TELEMETRY, comBaud); + } + /* Remap AFIO pin */ GPIO_PinRemapConfig(GPIO_Remap_SWJ_NoJTRST, ENABLE); @@ -251,7 +358,7 @@ void PIOS_Board_Init(void) PIOS_GPIO_Init(); } -void PIOS_InitUartMainPort() +static void PIOS_InitUartMainPort() { #ifndef PIOS_RFM22B_DEBUG_ON_TELEM uint32_t pios_usart1_id; @@ -268,7 +375,7 @@ void PIOS_InitUartMainPort() #endif } -void PIOS_InitUartFlexiPort() +static void PIOS_InitUartFlexiPort() { uint32_t pios_usart3_id; @@ -284,7 +391,7 @@ void PIOS_InitUartFlexiPort() } } -void PIOS_InitPPMMainPort(bool input) +static void PIOS_InitPPMMainPort(bool input) { #if defined(PIOS_INCLUDE_PPM) /* PPM input is configured on the coordinator modem and output on the remote modem. */ @@ -300,7 +407,7 @@ void PIOS_InitPPMMainPort(bool input) #endif /* PIOS_INCLUDE_PPM */ } -void PIOS_InitPPMFlexiPort(bool input) +static void PIOS_InitPPMFlexiPort(bool input) { #if defined(PIOS_INCLUDE_PPM) /* PPM input is configured on the coordinator modem and output on the remote modem. */ diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index 57c9f180e..a8f47f8ba 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -87,7 +87,10 @@ UAVOBJSRCFILENAMES += hwsettings UAVOBJSRCFILENAMES += receiveractivity UAVOBJSRCFILENAMES += cameradesired UAVOBJSRCFILENAMES += camerastabsettings -UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += altitudeholdsettings +UAVOBJSRCFILENAMES += oplinksettings +UAVOBJSRCFILENAMES += oplinkstatus + UAVOBJSRCFILENAMES += altitudeholddesired UAVOBJSRCFILENAMES += waypoint UAVOBJSRCFILENAMES += waypointactive diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index ab74c31a0..7ef8dc475 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -118,7 +118,7 @@ #define PIOS_INCLUDE_RFM22B_COM #define PIOS_INCLUDE_RFM22B_RCVR /* #define PIOS_INCLUDE_PPM_OUT */ -/* #define PIOS_RFM22B_DEBUG_ON_TELEM */ +#define PIOS_RFM22B_DEBUG_ON_TELEM /* PIOS misc peripherals */ /* #define PIOS_INCLUDE_VIDEO */ diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 229acb3b5..855d9a849 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -30,6 +30,8 @@ #include #include #include +#include +#include #include /* @@ -391,8 +393,9 @@ void PIOS_Board_Init(void) /* Initialize UAVObject libraries */ EventDispatcherInitialize(); UAVObjInitialize(); - HwSettingsInitialize(); + OPLinkStatusInitialize(); + /* Initialize the alarms library */ AlarmsInitialize(); @@ -704,25 +707,54 @@ void PIOS_Board_Init(void) /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) + /* Is the radio turned on? */ uint8_t hwsettings_radioport; HwSettingsRadioPortGet(&hwsettings_radioport); - uint8_t hwsettings_maxrfpower; - HwSettingsMaxRFPowerGet(&hwsettings_maxrfpower); - uint32_t hwsettings_deffreq; - HwSettingsDefaultFrequencyGet(&hwsettings_deffreq); switch (hwsettings_radioport) { case HWSETTINGS_RADIOPORT_DISABLED: break; case HWSETTINGS_RADIOPORT_TELEMETRY: { + // Initialize out status object. + OPLinkStatusData oplinkStatus; + OPLinkStatusGet(&oplinkStatus); + oplinkStatus.BoardType = bdinfo->board_type; + PIOS_BL_HELPER_FLASH_Read_Description(oplinkStatus.Description, OPLINKSTATUS_DESCRIPTION_NUMELEM); + PIOS_SYS_SerialNumberGetBinary(oplinkStatus.CPUSerial); + oplinkStatus.BoardRevision = bdinfo->board_rev; + OPLinkStatusSet(&oplinkStatus); + + /* Initialize the OPLinkSettings object. */ + OPLinkSettingsInitialize(); + + /* Fetch the OPinkSettings object. */ + OPLinkSettingsData oplinkSettings; + OPLinkSettingsGet(&oplinkSettings); + + /* Configure the RFM22B device. */ const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) { PIOS_Assert(0); } - // Set the modem parameters and reinitilize the modem. - PIOS_RFM22B_SetInitialFrequency(pios_rfm22b_id, hwsettings_deffreq); - switch (hwsettings_maxrfpower) { + /* Configure the radio com interface */ + 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_telem_rf_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); + } + + /* Set the radio configuration parameters. */ + PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, oplinkSettings.NumChannels, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, + oplinkSettings.PacketTime, (oplinkSettings.OneWayLink == OPLINKSETTINGS_ONEWAYLINK_TRUE)); + PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID); + + /* Set the modem Tx poer level */ + switch (oplinkSettings.MaxRFPower) { case OPLINKSETTINGS_MAXRFPOWER_125: PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); break; @@ -751,19 +783,10 @@ void PIOS_Board_Init(void) // do nothing break; } + + /* Reinitialize the modem. */ PIOS_RFM22B_Reinit(pios_rfm22b_id); -#ifdef PIOS_INCLUDE_RFM22B_COM - 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_telem_rf_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 #ifdef PIOS_INCLUDE_RFM22B_RCVR if (PIOS_RFM22B_RCVR_Init(pios_rfm22b_id) != 0) { PIOS_Assert(0); diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index 8bdebb671..369d0e26b 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -229,6 +229,7 @@ extern uint32_t pios_packet_handler; #define PIOS_RCVR_MAX_CHANNELS 12 #define PIOS_GCSRCVR_TIMEOUT_MS 100 #define PIOS_RFM22B_RCVR_TIMEOUT_MS 200 +#define PIOS_OPLINK_RCVR_TIMEOUT_MS 100 // ------------------------- // Receiver PPM input diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index 27f0603ac..c74357f22 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -63,11 +63,16 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget addUAVObjectToWidgetRelation("OPLinkSettings", "MainPort", m_oplink->MainPort); addUAVObjectToWidgetRelation("OPLinkSettings", "FlexiPort", m_oplink->FlexiPort); addUAVObjectToWidgetRelation("OPLinkSettings", "VCPPort", m_oplink->VCPPort); + addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed); addUAVObjectToWidgetRelation("OPLinkSettings", "MaxRFPower", m_oplink->MaxRFTxPower); - addUAVObjectToWidgetRelation("OPLinkSettings", "MinFrequency", m_oplink->MinimumFrequency); - addUAVObjectToWidgetRelation("OPLinkSettings", "MaxFrequency", m_oplink->MaximumFrequency); - addUAVObjectToWidgetRelation("OPLinkSettings", "InitFrequency", m_oplink->InitFrequency); - addUAVObjectToWidgetRelation("OPLinkSettings", "ChannelSpacing", m_oplink->StepSize); + addUAVObjectToWidgetRelation("OPLinkSettings", "NumChannels", m_oplink->NumChannels); + addUAVObjectToWidgetRelation("OPLinkSettings", "MinChannel", m_oplink->MinimumChannel); + addUAVObjectToWidgetRelation("OPLinkSettings", "MaxChannel", m_oplink->MaximumChannel); + addUAVObjectToWidgetRelation("OPLinkSettings", "ChannelSet", m_oplink->ChannelSet); + addUAVObjectToWidgetRelation("OPLinkSettings", "PacketTime", m_oplink->PacketLength); + addUAVObjectToWidgetRelation("OPLinkSettings", "CoordID", m_oplink->CoordID); + addUAVObjectToWidgetRelation("OPLinkSettings", "Coordinator", m_oplink->Coordinator); + addUAVObjectToWidgetRelation("OPLinkSettings", "OneWayLink", m_oplink->OneWayLink); addUAVObjectToWidgetRelation("OPLinkStatus", "DeviceID", m_oplink->DeviceID); addUAVObjectToWidgetRelation("OPLinkStatus", "RxGood", m_oplink->Good); @@ -89,82 +94,11 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget addUAVObjectToWidgetRelation("OPLinkStatus", "RXRate", m_oplink->RXRate); addUAVObjectToWidgetRelation("OPLinkStatus", "TXRate", m_oplink->TXRate); - signalMapperAddBinding = new QSignalMapper(this); - signalMapperRemBinding = new QSignalMapper(this); - connect(m_oplink->BindingAdd_1, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_1, (QWidget *)(m_oplink->BindingID_1)); - connect(m_oplink->BindingRemove_1, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_1, (QWidget *)(m_oplink->BindingID_1)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_1, "0"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_1, "0"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_1, "0"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_1, "0"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_1, "0"); - connect(m_oplink->BindingAdd_2, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_2, (QWidget *)(m_oplink->BindingID_2)); - connect(m_oplink->BindingRemove_2, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_2, (QWidget *)(m_oplink->BindingID_2)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_2, "1"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_2, "1"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_2, "1"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_2, "1"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_2, "1"); - connect(m_oplink->BindingAdd_3, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_3, (QWidget *)(m_oplink->BindingID_3)); - connect(m_oplink->BindingRemove_3, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_3, (QWidget *)(m_oplink->BindingID_3)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_3, "2"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_3, "2"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_3, "2"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_3, "2"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_3, "2"); - connect(m_oplink->BindingAdd_4, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_4, (QWidget *)(m_oplink->BindingID_4)); - connect(m_oplink->BindingRemove_4, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_4, (QWidget *)(m_oplink->BindingID_4)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_4, "3"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_4, "3"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_4, "3"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_4, "3"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_4, "3"); - connect(m_oplink->BindingAdd_5, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_5, (QWidget *)(m_oplink->BindingID_5)); - connect(m_oplink->BindingRemove_5, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_5, (QWidget *)(m_oplink->BindingID_5)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_5, "4"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_5, "4"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_5, "4"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_5, "4"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_5, "4"); - connect(m_oplink->BindingAdd_6, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_6, (QWidget *)(m_oplink->BindingID_6)); - connect(m_oplink->BindingRemove_6, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_6, (QWidget *)(m_oplink->BindingID_6)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_6, "5"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_6, "5"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_6, "5"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_6, "5"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_6, "5"); - connect(m_oplink->BindingAdd_7, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_7, (QWidget *)(m_oplink->BindingID_7)); - connect(m_oplink->BindingRemove_7, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_7, (QWidget *)(m_oplink->BindingID_7)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_7, "6"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_7, "6"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_7, "6"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_7, "6"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_7, "6"); - connect(m_oplink->BindingAdd_8, SIGNAL(clicked()), signalMapperAddBinding, SLOT(map())); - signalMapperAddBinding->setMapping(m_oplink->BindingAdd_8, (QWidget *)(m_oplink->BindingID_8)); - connect(m_oplink->BindingRemove_8, SIGNAL(clicked()), signalMapperRemBinding, SLOT(map())); - signalMapperRemBinding->setMapping(m_oplink->BindingRemove_8, (QWidget *)(m_oplink->BindingID_8)); - addUAVObjectToWidgetRelation("OPLinkSettings", "Bindings", m_oplink->BindingID_8, "7"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteMainPort", m_oplink->RemoteMainPort_8, "7"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteFlexiPort", m_oplink->RemoteFlexiPort_8, "7"); - addUAVObjectToWidgetRelation("OPLinkSettings", "RemoteVCPPort", m_oplink->RemoteVCPPort_8, "7"); - addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed_8, "7"); - connect(signalMapperAddBinding, SIGNAL(mapped(QWidget *)), this, SLOT(addBinding(QWidget *))); - connect(signalMapperRemBinding, SIGNAL(mapped(QWidget *)), this, SLOT(removeBinding(QWidget *))); + // Connect the bind buttons + connect(m_oplink->Bind1, SIGNAL(clicked()), this, SLOT(bind1())); + connect(m_oplink->Bind2, SIGNAL(clicked()), this, SLOT(bind2())); + connect(m_oplink->Bind3, SIGNAL(clicked()), this, SLOT(bind3())); + connect(m_oplink->Bind4, SIGNAL(clicked()), this, SLOT(bind3())); // Add scroll bar when necessary QScrollArea *scroll = new QScrollArea; @@ -179,10 +113,7 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget } ConfigPipXtremeWidget::~ConfigPipXtremeWidget() -{ - delete signalMapperAddBinding; - delete signalMapperRemBinding; -} +{} /*! \brief Called by updates to @OPLinkStatus @@ -200,19 +131,19 @@ void ConfigPipXtremeWidget::updateStatus(UAVObject *object) quint32 pairid1 = pairIdField->getValue(0).toUInt(); m_oplink->PairID1->setText(QString::number(pairid1, 16).toUpper()); m_oplink->PairID1->setEnabled(false); - m_oplink->PairSelect1->setEnabled(pairid1); + m_oplink->Bind1->setEnabled(pairid1); quint32 pairid2 = pairIdField->getValue(1).toUInt(); m_oplink->PairID2->setText(QString::number(pairIdField->getValue(1).toUInt(), 16).toUpper()); m_oplink->PairID2->setEnabled(false); - m_oplink->PairSelect2->setEnabled(pairid2); + m_oplink->Bind2->setEnabled(pairid2); quint32 pairid3 = pairIdField->getValue(2).toUInt(); m_oplink->PairID3->setText(QString::number(pairIdField->getValue(2).toUInt(), 16).toUpper()); m_oplink->PairID3->setEnabled(false); - m_oplink->PairSelect3->setEnabled(pairid3); + m_oplink->Bind3->setEnabled(pairid3); quint32 pairid4 = pairIdField->getValue(3).toUInt(); m_oplink->PairID4->setText(QString::number(pairIdField->getValue(3).toUInt(), 16).toUpper()); m_oplink->PairID4->setEnabled(false); - m_oplink->PairSelect4->setEnabled(pairid4); + m_oplink->Bind4->setEnabled(pairid4); } else { qDebug() << "PipXtremeGadgetWidget: Count not read PairID field."; } @@ -307,32 +238,36 @@ void ConfigPipXtremeWidget::disconnected() } } -void ConfigPipXtremeWidget::addBinding(QWidget *w) +void ConfigPipXtremeWidget::SetPairID(QLineEdit *pairIdWidget) { - if (QLineEdit * le = qobject_cast(w)) { - // Get the pair ID out of the selection widget - quint32 pairID = 0; - bool okay; - if (m_oplink->PairSelect1->isChecked()) { - pairID = m_oplink->PairID1->text().toUInt(&okay, 16); - } else if (m_oplink->PairSelect2->isChecked()) { - pairID = m_oplink->PairID2->text().toUInt(&okay, 16); - } else if (m_oplink->PairSelect3->isChecked()) { - pairID = m_oplink->PairID3->text().toUInt(&okay, 16); - } else if (m_oplink->PairSelect4->isChecked()) { - pairID = m_oplink->PairID4->text().toUInt(&okay, 16); - } + // Get the pair ID out of the selection widget + quint32 pairID = 0; + bool okay; - // Store the ID in the first open slot (or the last slot if all are full). - le->setText(QString::number(pairID, 16).toUpper()); - } + pairID = pairIdWidget->text().toUInt(&okay, 16); + + // Store the ID in the coord ID field. + m_oplink->CoordID->setText(QString::number(pairID, 16).toUpper()); } -void ConfigPipXtremeWidget::removeBinding(QWidget *w) +void ConfigPipXtremeWidget::bind1() { - if (QLineEdit * le = qobject_cast(w)) { - le->setText(QString::number(0, 16).toUpper()); - } + SetPairID(m_oplink->PairID1); +} + +void ConfigPipXtremeWidget::bind2() +{ + SetPairID(m_oplink->PairID1); +} + +void ConfigPipXtremeWidget::bind3() +{ + SetPairID(m_oplink->PairID1); +} + +void ConfigPipXtremeWidget::bind4() +{ + SetPairID(m_oplink->PairID1); } /** diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h index 304ebc478..6966691ce 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h @@ -55,14 +55,14 @@ private: // Are the settings current? bool settingsUpdated; - // Signal mappers to add arguments to signals. - QSignalMapper *signalMapperAddBinding; - QSignalMapper *signalMapperRemBinding; + void SetPairID(QLineEdit *pairIdWidget); private slots: void disconnected(); - void addBinding(QWidget *w); - void removeBinding(QWidget *w); + void bind1(); + void bind2(); + void bind3(); + void bind4(); }; #endif // CONFIGTXPIDWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp index 22da2b6b8..5842ba2ce 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp @@ -66,8 +66,7 @@ ConfigRevoHWWidget::ConfigRevoHWWidget(QWidget *parent) : ConfigTaskWidget(paren addUAVObjectToWidgetRelation("HwSettings", "ComUsbBridgeSpeed", m_ui->cbMainComSpeed); addUAVObjectToWidgetRelation("HwSettings", "RadioPort", m_ui->cbModem); - addUAVObjectToWidgetRelation("HwSettings", "MaxRFPower", m_ui->cbTxPower); - addUAVObjectToWidgetRelation("HwSettings", "DefaultFrequency", m_ui->leInitFreq); + addUAVObjectToWidgetRelation("HwSettings", "TelemetrySpeed", m_ui->cbRadioSpeed); connect(m_ui->cchwHelp, SIGNAL(clicked()), this, SLOT(openHelp())); @@ -291,18 +290,12 @@ void ConfigRevoHWWidget::modemPortChanged(int index) if (m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_TELEMETRY) { m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); } - m_ui->lblTxPower->setVisible(true); - m_ui->cbTxPower->setVisible(true); - m_ui->lblInitFreq->setVisible(true); - m_ui->leInitFreq->setVisible(true); + m_ui->cbRadioSpeed->setVisible(true); if (!m_refreshing) { QMessageBox::warning(this, tr("Warning"), tr("Activating the Radio requires an antenna be attached or modem damage will occur.")); } } else { - m_ui->lblTxPower->setVisible(false); - m_ui->cbTxPower->setVisible(false); - m_ui->lblInitFreq->setVisible(false); - m_ui->leInitFreq->setVisible(false); + m_ui->cbRadioSpeed->setVisible(false); } } diff --git a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.ui b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.ui index a8b3f65dd..43b605d35 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.ui +++ b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.ui @@ -113,8 +113,8 @@ 0 0 - 810 - 665 + 818 + 673 @@ -122,77 +122,22 @@ 12 - - - - - - 0 - 0 - + + + + + Qt::Horizontal - - USB VCP Function + + QSizePolicy::Minimum - - Qt::AlignBottom|Qt::AlignHCenter + + + 120 + 10 + - - - - - - Speed - - - Qt::AlignBottom|Qt::AlignHCenter - - - - - - - false - - - - - - - - 0 - 0 - - - - Speed - - - Qt::AlignBottom|Qt::AlignHCenter - - - - - - - - - - - 0 - 0 - - - - USB HID Function - - - Qt::AlignBottom|Qt::AlignHCenter - - - - - + @@ -243,6 +188,29 @@ + + + + false + + + + + + + + 0 + 0 + + + + Speed + + + Qt::AlignBottom|Qt::AlignHCenter + + + @@ -293,9 +261,51 @@ + + + + Qt::Vertical + + + QSizePolicy::Fixed + + + + 20 + 120 + + + + + + + + + 0 + 0 + + + + USB VCP Function + + + Qt::AlignBottom|Qt::AlignHCenter + + + + + + + Speed + + + Qt::AlignBottom|Qt::AlignHCenter + + + @@ -342,6 +352,28 @@ + + + + + + + + 0 + 0 + + + + USB HID Function + + + Qt::AlignBottom|Qt::AlignHCenter + + + + + + @@ -379,9 +411,9 @@ - + - Max Tx Power (mW) + Speed Qt::AlignBottom|Qt::AlignHCenter @@ -389,24 +421,7 @@ - - - - - - Initial Frequency (Hz) - - - Qt::AlignBottom|Qt::AlignHCenter - - - - - - - - - + @@ -466,38 +481,6 @@ - - - - Qt::Vertical - - - QSizePolicy::Fixed - - - - 20 - 120 - - - - - - - - Qt::Horizontal - - - QSizePolicy::Minimum - - - - 120 - 10 - - - - @@ -594,6 +577,19 @@ + + + + Qt::Horizontal + + + + 120 + 10 + + + + diff --git a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui index 8df5ea3e9..2a8daaa7f 100644 --- a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui +++ b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui @@ -23,10 +23,7 @@ QFrame::Raised - - - - + @@ -113,7 +110,24 @@ - + + + + + + Qt::Vertical + + + + 20 + 40 + + + + + + + QFrame::StyledPanel @@ -136,132 +150,16 @@ true + + Qt::LeftToRight + Status + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + - - - - Firmware Ver. - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - false - - - true - - - - - - - Serial Number - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 0 - 0 - - - - - 0 - 0 - - - - - 16777215 - 16777215 - - - - - 75 - false - true - - - - false - - - The modems serial number - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - true - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - - - - - Device ID - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - @@ -366,6 +264,163 @@ + + + + Firmware Ver. + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 75 + true + + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); + /* background: transparent; */ + /* selection-background-color: darkgray;*/ + } + + + false + + + true + + + + + + + + 0 + 0 + + + + + 101 + 16777215 + + + + + 75 + true + + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); + /* background: transparent; */ + /* selection-background-color: darkgray;*/ + } + + + true + + + + + + + Serial Number + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 0 + 0 + + + + + 0 + 0 + + + + + 16777215 + 16777215 + + + + + 75 + false + true + + + + false + + + The modems serial number + + + QLineEdit { + border: none; + border-radius: 1px; + padding: 0 4px; + background: rgba(0, 0, 0, 16); + /* background: transparent; */ + /* selection-background-color: darkgray;*/ + } + + + true + + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + + + true + + + + + + + Device ID + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + @@ -418,13 +473,13 @@ - - - - - 0 - 0 - + + + + + 0 + 0 + @@ -438,6 +493,9 @@ true + + The number of packets that were unable to be transmitted + QLineEdit { border: none; @@ -448,6 +506,9 @@ /* selection-background-color: darkgray;*/ } + + Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter + true @@ -922,47 +983,6 @@ - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - The number of packets that were unable to be transmitted - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - @@ -1239,1186 +1259,6 @@ - - - - - 75 - true - - - - Remote Modems - - - - - - - - - - - - - - - - -100dB - - - - - - - -127 - - - 0 - - - 0 - - - false - - - %v dBm - - - - - - - -100dB - - - - - - - - 100 - 16777215 - - - - - - - - -127 - - - 0 - - - 0 - - - false - - - %v dBm - - - - - - - -100dB - - - - - - - -127 - - - 0 - - - -127 - - - false - - - %v dBm - - - - - - - - 100 - 16777215 - - - - - - - - -100dB - - - - - - - - 100 - 16777215 - - - - 12345678 - - - - - - - - - - - - - - -127 - - - 0 - - - 0 - - - false - - - %v dBm - - - - - - - - - - - - - - - 100 - 16777215 - - - - - - - - - - - - - - - - - - - - - - 75 - true - - - - Bindings - - - - - - Add - - - Qt::AlignCenter - - - - - - - Clear - - - Qt::AlignCenter - - - - - - - Device ID - - - Qt::AlignCenter - - - - - - - Main Port - - - Qt::AlignCenter - - - - - - - Flexi Port - - - Qt::AlignCenter - - - - - - - VCP Port - - - Qt::AlignCenter - - - - - - - COM Speed - - - Qt::AlignCenter - - - - - - - true - - - - - 0 - 0 - 757 - 244 - - - - - - - Add >> - - - - - - - Clear - - - - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - false - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - 12345678 - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote main port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote flexi port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote USB virtual com port - - - - - - - - 16777215 - 16777215 - - - - Set the telemetry port speed - - - - - - - Add >> - - - - - - - Clear - - - - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - false - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - 12345678 - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote main port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote flexi port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote USB virtual com port - - - - - - - - 16777215 - 16777215 - - - - Set the telemetry port speed - - - - - - - Add >> - - - - - - - Clear - - - - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - false - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - 12345678 - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote main port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote flexi port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote USB virtual com port - - - - - - - - 16777215 - 16777215 - - - - Set the telemetry port speed - - - - - - - Add >> - - - - - - - Clear - - - - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - false - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - 12345678 - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote main port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote flexi port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote USB virtual com port - - - - - - - - 16777215 - 16777215 - - - - Set the telemetry port speed - - - - - - - Add >> - - - - - - - Clear - - - - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - false - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - 12345678 - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote main port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote flexi port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote USB virtual com port - - - - - - - - 16777215 - 16777215 - - - - Set the telemetry port speed - - - - - - - Add >> - - - - - - - Clear - - - - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - false - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - 12345678 - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote main port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote flexi port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote USB virtual com port - - - - - - - - 16777215 - 16777215 - - - - Set the telemetry port speed - - - - - - - Add >> - - - - - - - Clear - - - - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - false - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - 12345678 - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote main port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote flexi port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote USB virtual com port - - - - - - - - 16777215 - 16777215 - - - - Set the telemetry port speed - - - - - - - Add >> - - - - - - - Clear - - - - - - - - 0 - 0 - - - - - 101 - 16777215 - - - - - 75 - true - - - - QLineEdit { - border: none; - border-radius: 1px; - padding: 0 4px; - background: rgba(0, 0, 0, 16); - /* background: transparent; */ - /* selection-background-color: darkgray;*/ - } - - - false - - - Qt::AlignLeading|Qt::AlignLeft|Qt::AlignVCenter - - - true - - - 12345678 - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote main port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote flexi port - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the remote USB virtual com port - - - - - - - - 16777215 - 16777215 - - - - Set the telemetry port speed - - - - - - - - - - @@ -2437,7 +1277,37 @@ Configuration - + + + + Max Chan + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Packet Length + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Num Chan + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + Main Port @@ -2447,87 +1317,7 @@ - - - - - 16777215 - 16777215 - - - - Choose the function for the main port - - - - - - - Min Freq - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - - - - - - - Flexi Port - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the flexi port - - - - - - - Max Freq - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - - - - - - - VCP Port - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - + @@ -2540,24 +1330,114 @@ - - + + + + 250 + + + + + - Initial Freq + Flexi Port Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - + + + + + + + VCP Port + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - + + + + + 16777215 + 16777215 + + + + Choose the function for the main port + + + + + + + Com Speed + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + Min Chan + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 249 + + + + + + + Channel Set + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 249 + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the flexi port + + + + + + + 249 + + + + Max Power @@ -2567,7 +1447,7 @@ - + @@ -2586,38 +1466,260 @@ - - - - Step Size + + + + - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + 10 + + + 50 + + + 15 - - - - + + + + Coordinator + + + + + + + One-Way Link - - - - Qt::Vertical + + + + + 75 + true + - - - 20 - 40 - + + Remote Modems - + + + + + + + + -100dB + + + + + + + -127 + + + 0 + + + 0 + + + false + + + %v dBm + + + + + + + -100dB + + + + + + + + 100 + 16777215 + + + + + + + + -127 + + + 0 + + + 0 + + + false + + + %v dBm + + + + + + + + 100 + 16777215 + + + + + + + + -127 + + + 0 + + + -127 + + + false + + + %v dBm + + + + + + + -100dB + + + + + + + -127 + + + 0 + + + 0 + + + false + + + %v dBm + + + + + + + -100dB + + + + + + + + 100 + 16777215 + + + + + + + + + 100 + 16777215 + + + + 12345678 + + + + + + + Bind + + + + + + + Bind + + + + + + + Bind + + + + + + + Bind + + + + + + + Qt::LeftToRight + + + Coord ID + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 100 + 16777215 + + + + 8 + + + + + + + + @@ -2625,17 +1727,12 @@ - PairSelect1 PairID1 - PairSelect2 PairID2 - PairSelect3 PairID3 - PairSelect4 PairID4 FirmwareVersion SerialNumber - MaxRFTxPower Apply Save diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 43a772af5..c71d6ef34 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -101,6 +101,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/poilocation.h \ $$UAVOBJECT_SYNTHETICS/oplinksettings.h \ $$UAVOBJECT_SYNTHETICS/oplinkstatus.h \ + $$UAVOBJECT_SYNTHETICS/oplinkreceiver.h \ $$UAVOBJECT_SYNTHETICS/osdsettings.h \ $$UAVOBJECT_SYNTHETICS/waypoint.h \ $$UAVOBJECT_SYNTHETICS/waypointactive.h \ @@ -185,6 +186,7 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/poilocation.cpp \ $$UAVOBJECT_SYNTHETICS/oplinksettings.cpp \ $$UAVOBJECT_SYNTHETICS/oplinkstatus.cpp \ + $$UAVOBJECT_SYNTHETICS/oplinkreceiver.cpp \ $$UAVOBJECT_SYNTHETICS/osdsettings.cpp \ $$UAVOBJECT_SYNTHETICS/waypoint.cpp \ $$UAVOBJECT_SYNTHETICS/waypointactive.cpp \ diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 5d0a29fec..61c24d4f1 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -17,8 +17,6 @@ - - diff --git a/shared/uavobjectdefinition/oplinksettings.xml b/shared/uavobjectdefinition/oplinksettings.xml index b78b8d4c9..e0969e9dd 100644 --- a/shared/uavobjectdefinition/oplinksettings.xml +++ b/shared/uavobjectdefinition/oplinksettings.xml @@ -1,19 +1,19 @@ OPLink configurations options. - - - - - + + + + - - - - + + + + + diff --git a/shared/uavobjectdefinition/oplinkstatus.xml b/shared/uavobjectdefinition/oplinkstatus.xml index 1e79630fe..81172bcf1 100644 --- a/shared/uavobjectdefinition/oplinkstatus.xml +++ b/shared/uavobjectdefinition/oplinkstatus.xml @@ -30,7 +30,7 @@ - + From 774a74884a0a0b909107d4d02d6c4614d890bd02 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 16 Jun 2013 08:48:18 -0700 Subject: [PATCH 08/28] OP-932 Replaces the RFM22B PPM/receiver code with an OPLinkReceiver UAVObject. This object is generated on a coordinator OPLink, and is used by both the OPLink to generate PPM output, and the OPLinkReceiver on the Revo. --- .../modules/RadioComBridge/RadioComBridge.c | 97 +++++++++++-- flight/pios/common/pios_rfm22b.c | 16 --- flight/pios/common/pios_rfm22b_rcvr.c | 130 ------------------ flight/pios/inc/pios_rfm22b.h | 1 - flight/pios/inc/pios_rfm22b_priv.h | 8 -- flight/pios/inc/pios_rfm22b_rcvr.h | 40 ------ flight/pios/pios.h | 4 - .../coptercontrol/firmware/inc/pios_config.h | 1 - .../boards/oplinkmini/firmware/Makefile | 2 +- .../oplinkmini/firmware/inc/pios_config.h | 3 +- .../boards/oplinkmini/firmware/pios_board.c | 35 ++++- flight/targets/boards/oplinkmini/pios_board.h | 3 +- .../boards/osd/firmware/inc/pios_config.h | 1 - .../boards/revolution/firmware/UAVObjects.inc | 1 + .../revolution/firmware/inc/pios_config.h | 1 - .../boards/revolution/firmware/pios_board.c | 25 ++-- .../revoproto/firmware/inc/pios_config.h | 1 - make/apps-defs.mk | 1 - shared/uavobjectdefinition/oplinksettings.xml | 2 +- 19 files changed, 139 insertions(+), 233 deletions(-) delete mode 100644 flight/pios/common/pios_rfm22b_rcvr.c delete mode 100644 flight/pios/inc/pios_rfm22b_rcvr.h diff --git a/flight/modules/RadioComBridge/RadioComBridge.c b/flight/modules/RadioComBridge/RadioComBridge.c index 9724f6e3f..eafb4e30b 100644 --- a/flight/modules/RadioComBridge/RadioComBridge.c +++ b/flight/modules/RadioComBridge/RadioComBridge.c @@ -35,7 +35,9 @@ #include #include #include +#include #include +#include #include #include #if defined(PIOS_INCLUDE_FLASH_EEPROM) @@ -65,6 +67,7 @@ typedef struct { xTaskHandle telemetryRxTaskHandle; xTaskHandle radioTxTaskHandle; xTaskHandle radioRxTaskHandle; + xTaskHandle PPMInputTaskHandle; // The UAVTalk connection on the com side. UAVTalkConnection telemUAVTalkCon; @@ -94,11 +97,13 @@ static void telemetryTxTask(void *parameters); static void telemetryRxTask(void *parameters); static void radioTxTask(void *parameters); static void radioRxTask(void *parameters); +static void PPMInputTask(void *parameters); static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length); static int32_t RadioSendHandler(uint8_t *buf, int32_t length); static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte); static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte); static void objectPersistenceUpdatedCb(UAVObjEvent *objEv); +static void oplinkReceiverUpdatedCb(UAVObjEvent *objEv); // **************** // Private variables @@ -116,6 +121,7 @@ static int32_t RadioComBridgeStart(void) // Get the settings. OPLinkSettingsData oplinkSettings; OPLinkSettingsGet(&oplinkSettings); + bool is_coordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); // We will not parse/send UAVTalk if any ports are configured as Serial (except for over the USB HID port). data->parseUAVTalk = ((oplinkSettings.MainPort != OPLINKSETTINGS_MAINPORT_SERIAL) && @@ -156,12 +162,29 @@ static int32_t RadioComBridgeStart(void) // Configure our UAVObjects for updates. UAVObjConnectQueue(UAVObjGetByID(OPLINKSTATUS_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL); + if (is_coordinator) { + UAVObjConnectQueue(UAVObjGetByID(OPLINKRECEIVER_OBJID), data->radioEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); + } else { + UAVObjConnectQueue(UAVObjGetByID(OPLINKRECEIVER_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); + } + + // Configure the UAVObject callbacks + ObjectPersistenceConnectCallback(&objectPersistenceUpdatedCb); + if (!is_coordinator) { + OPLinkReceiverConnectCallback(&oplinkReceiverUpdatedCb); + } // Start the primary tasks for receiving/sending UAVTalk packets from the GCS. xTaskCreate(telemetryTxTask, (signed char *)"telemetryTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->telemetryTxTaskHandle)); xTaskCreate(telemetryRxTask, (signed char *)"telemetryRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->telemetryRxTaskHandle)); xTaskCreate(radioTxTask, (signed char *)"radioTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioTxTaskHandle)); xTaskCreate(radioRxTask, (signed char *)"radioRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioRxTaskHandle)); + if (PIOS_PPM_RECEIVER != 0) { + xTaskCreate(PPMInputTask, (signed char *)"PPMInputTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->PPMInputTaskHandle)); +#ifdef PIOS_INCLUDE_WDG + PIOS_WDG_RegisterFlag(PIOS_WDG_PPMINPUT); +#endif + } // Register the watchdog timers. #ifdef PIOS_INCLUDE_WDG @@ -192,9 +215,7 @@ static int32_t RadioComBridgeInitialize(void) // Initialize the UAVObjects that we use OPLinkStatusInitialize(); ObjectPersistenceInitialize(); - - // Configure the UAVObject callbacks - ObjectPersistenceConnectCallback(&objectPersistenceUpdatedCb); + OPLinkReceiverInitialize(); // Initialise UAVTalk data->telemUAVTalkCon = UAVTalkInitialize(&UAVTalkSendHandler); @@ -204,13 +225,6 @@ static int32_t RadioComBridgeInitialize(void) data->uavtalkEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); data->radioEventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(UAVObjEvent)); - // Configure our UAVObjects for updates. - UAVObjConnectQueue(UAVObjGetByID(OPLINKSTATUS_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); - UAVObjConnectQueue(UAVObjGetByID(OBJECTPERSISTENCE_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL); -#if defined(PIOS_INCLUDE_RFM22B_GCSRECEIVER) - UAVObjConnectQueue(UAVObjGetByID(GCSRECEIVER_OBJID), data->uavtalkEventQueue, EV_UPDATED | EV_UPDATED_MANUAL | EV_UPDATE_REQ); -#endif - // Initialize the statistics. data->comTxErrors = 0; data->comTxRetries = 0; @@ -389,6 +403,42 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) } } + +/** + * @brief Reads the PPM input device and sends out OPLinkReceiver objects. + * + * @param[in] parameters The task parameters (unused) + */ +static void PPMInputTask(__attribute__((unused)) void *parameters) +{ + xSemaphoreHandle sem = PIOS_RCVR_GetSemaphore(PIOS_PPM_RECEIVER, 1); + OPLinkReceiverData opl_rcvr; + + // Task loop + while (1) { +#ifdef PIOS_INCLUDE_WDG + PIOS_WDG_UpdateFlag(PIOS_WDG_PPMINPUT); +#endif + + // Wait for the receiver semaphore. + bool valid_input_detected = false; + if (xSemaphoreTake(sem, MAX_PORT_DELAY) == pdTRUE) { + // Read the receiver inputs. + for (uint8_t i = 0; i < OPLINKRECEIVER_CHANNEL_NUMELEM; ++i) { + opl_rcvr.Channel[i] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i + 1); + if ((opl_rcvr.Channel[i] != PIOS_RCVR_INVALID) && (opl_rcvr.Channel[i] != PIOS_RCVR_TIMEOUT)) { + valid_input_detected = true; + } + } + } + + // Set the receiver UAVO if we detected valid input. + if (valid_input_detected) { + OPLinkReceiverSet(&opl_rcvr); + } + } +} + /** * @brief Transmit data buffer to the com port. * @@ -471,8 +521,15 @@ static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConn } else if (state == UAVTALK_STATE_COMPLETE) { // We only want to unpack certain objects from the remote modem. uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle); - if (objId != OPLINKSTATUS_OBJID) { + switch (objId) { + case OPLINKSTATUS_OBJID: + break; + case OPLINKRECEIVER_OBJID: + UAVTalkReceiveObject(inConnectionHandle); + break; + default: UAVTalkRelayPacket(inConnectionHandle, outConnectionHandle); + break; } } } @@ -491,7 +548,7 @@ static void objectPersistenceUpdatedCb(__attribute__((unused)) UAVObjEvent *objE // Is this concerning or setting object? if (obj_per.ObjectID == OPLINKSETTINGS_OBJID) { // Is this a save, load, or delete? - bool success = true; + bool success = false; switch (obj_per.Operation) { case OBJECTPERSISTENCE_OPERATION_LOAD: { @@ -542,3 +599,19 @@ static void objectPersistenceUpdatedCb(__attribute__((unused)) UAVObjEvent *objE } } } + +/** + * @brief Callback that is called when the OPLinkReceiver UAVObject is changed. + * @param[in] objEv The event that precipitated the callback. + */ +static void oplinkReceiverUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) +{ + // Get the OPLinkReceiver object. + OPLinkReceiverData opl_rcvr; + + OPLinkReceiverGet(&opl_rcvr); + + for (uint8_t i = 0; i < OPLINKRECEIVER_CHANNEL_NUMELEM; ++i) { + PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, opl_rcvr.Channel[i]); + } +} diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index 94538607d..4266f40fc 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -482,22 +482,6 @@ static bool rfm22_isConnected(struct pios_rfm22b_dev *rfm22b_dev) return (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) || (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTING); } -/** - * Returns true if the modem is configured as a coordinator. - * - * @param[in] rfm22b_id The RFM22B device index. - * @return True if the modem is configured as a coordinator. - */ -bool PIOS_RFM22B_IsCoordinator(uint32_t rfm22b_id) -{ - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - - if (PIOS_RFM22B_Validate(rfm22b_dev)) { - return rfm22_isCoordinator(rfm22b_dev); - } - return false; -} - /** * Returns true if the modem is not actively sending or receiving a packet. * diff --git a/flight/pios/common/pios_rfm22b_rcvr.c b/flight/pios/common/pios_rfm22b_rcvr.c deleted file mode 100644 index f0dbc3bc4..000000000 --- a/flight/pios/common/pios_rfm22b_rcvr.c +++ /dev/null @@ -1,130 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS_RFM22B_RCVR RFM22B Receiver Input Functions - * @brief Code to output the PPM signal from the RFM22B - * @{ - * - * @file pios_rfm22b_rcvr.c - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @brief Implements a receiver interface to the RFM22B device - * @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 "pios.h" - -#ifdef PIOS_INCLUDE_RFM22B_RCVR - -#include "pios_rfm22b_priv.h" - -#include - -/* Provide a RCVR driver */ -static int32_t PIOS_RFM22B_RCVR_Get(uint32_t rcvr_id, uint8_t channel); -static void PIOS_RFM22B_RCVR_Supervisor(uint32_t rcvr_id); - -const struct pios_rcvr_driver pios_rfm22b_rcvr_driver = { - .read = PIOS_RFM22B_RCVR_Get, -}; - -/** - * Initialize the receiver. - * - * @param[in] rfm22b_dev The receiver ID. - * @return < 0 on failure. - */ -int32_t PIOS_RFM22B_RCVR_Init(uint32_t rcvr_id) -{ - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rcvr_id; - - if (!PIOS_RFM22B_Validate(rfm22b_dev)) { - return -1; - } - - // Initialize - for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) { - rfm22b_dev->ppm_channel[i] = PIOS_RCVR_TIMEOUT; - } - rfm22b_dev->ppm_supv_timer = 0; - - // Register the failsafe timer callback. - if (!PIOS_RTC_RegisterTickCallback(PIOS_RFM22B_RCVR_Supervisor, rcvr_id)) { - PIOS_DEBUG_Assert(0); - } - - return 0; -} - -/** - * Get a channel from the receiver. - * - * @param[in] rcvr_id The receiver ID. - * @return The channel value, or -1 on failure. - */ -static int32_t PIOS_RFM22B_RCVR_Get(uint32_t rcvr_id, uint8_t channel) -{ - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rcvr_id; - - if (!PIOS_RFM22B_Validate(rfm22b_dev)) { - return -1; - } - - if (channel >= GCSRECEIVER_CHANNEL_NUMELEM) { - /* channel is out of range */ - return -1; - } - - return rfm22b_dev->ppm_channel[channel]; -} - -/** - * The supervisor function that ensures that the data is current. - * - * @param[in] rcvr_id The receiver ID. - */ -static void PIOS_RFM22B_RCVR_Supervisor(uint32_t rcvr_id) -{ - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rcvr_id; - - if (!PIOS_RFM22B_Validate(rfm22b_dev)) { - return; - } - - // RTC runs at 625Hz. - if (++(rfm22b_dev->ppm_supv_timer) < (PIOS_RFM22B_RCVR_TIMEOUT_MS * 625 / 1000)) { - return; - } - rfm22b_dev->ppm_supv_timer = 0; - - // Have we received fresh values since the last update? - if (!rfm22b_dev->ppm_fresh) { - for (uint8_t i = 0; i < PIOS_RFM22B_RCVR_MAX_CHANNELS; ++i) { - rfm22b_dev->ppm_channel[i] = PIOS_RCVR_TIMEOUT; - } - } - rfm22b_dev->ppm_fresh = false; -} - -#endif /* PIOS_INCLUDE_RFM22B_RCVR */ - -/** - * @} - * @} - */ diff --git a/flight/pios/inc/pios_rfm22b.h b/flight/pios/inc/pios_rfm22b.h index b28f8ff6b..1bb23395e 100644 --- a/flight/pios/inc/pios_rfm22b.h +++ b/flight/pios/inc/pios_rfm22b.h @@ -112,7 +112,6 @@ extern void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, uint8_t num_chan, u extern void PIOS_RFM22B_SetCoordinator(uint32_t rfm22b_id, bool coordinator); extern void PIOS_RFM22B_SetCoordinatorID(uint32_t rfm22b_id, uint32_t coord_id); extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id); -extern bool PIOS_RFM22B_IsCoordinator(uint32_t rfb22b_id); extern void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats); extern uint8_t PIOS_RFM2B_GetPairStats(uint32_t rfm22b_id, uint32_t *device_ids, int8_t *RSSIs, uint8_t max_pairs); extern bool PIOS_RFM22B_InRxWait(uint32_t rfb22b_id); diff --git a/flight/pios/inc/pios_rfm22b_priv.h b/flight/pios/inc/pios_rfm22b_priv.h index ad21e68a9..3823fbcc4 100644 --- a/flight/pios/inc/pios_rfm22b_priv.h +++ b/flight/pios/inc/pios_rfm22b_priv.h @@ -36,7 +36,6 @@ #include #include #include "pios_rfm22b.h" -#include "pios_rfm22b_rcvr.h" // ************************************ @@ -770,13 +769,6 @@ struct pios_rfm22b_dev { portTickType tx_complete_ticks; portTickType time_delta; bool on_sync_channel; - -#ifdef PIOS_INCLUDE_RFM22B_RCVR - // The PPM channel values - uint16_t ppm_channel[PIOS_RFM22B_RCVR_MAX_CHANNELS]; - uint32_t ppm_supv_timer; - bool ppm_fresh; -#endif }; diff --git a/flight/pios/inc/pios_rfm22b_rcvr.h b/flight/pios/inc/pios_rfm22b_rcvr.h deleted file mode 100644 index 74d176c4e..000000000 --- a/flight/pios/inc/pios_rfm22b_rcvr.h +++ /dev/null @@ -1,40 +0,0 @@ -/** - ****************************************************************************** - * @addtogroup PIOS PIOS Core hardware abstraction layer - * @{ - * @addtogroup PIOS RFM22B Receiver Input Functions - * @{ - * - * @file pios_rfm22b_rcvr.h - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. - * @brief RFM22B Receiver Input functions header. - * @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 PIOS_RFM22B_RCVR_H - -#define PIOS_RFM22B_RCVR_MAX_CHANNELS 12 - -extern const struct pios_rcvr_driver pios_rfm22b_rcvr_driver; - -extern int32_t PIOS_RFM22B_RCVR_Init(uint32_t rcvr_id); - -#define PIOS_RFM22B_RCVR_H - -#endif /* PIOS_RFM22B_RCVR_H */ diff --git a/flight/pios/pios.h b/flight/pios/pios.h index 6ac38703b..59875f3bc 100644 --- a/flight/pios/pios.h +++ b/flight/pios/pios.h @@ -241,10 +241,6 @@ #include #endif -#ifdef PIOS_INCLUDE_GCSRCVR -/* only priv header */ -#endif - /* PIOS abstract receiver interface */ #ifdef PIOS_INCLUDE_RCVR #include diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index 26527d716..09f9b0b51 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -116,7 +116,6 @@ /* PIOS radio modules */ /* #define PIOS_INCLUDE_RFM22B */ /* #define PIOS_INCLUDE_RFM22B_COM */ -/* #define PIOS_INCLUDE_RFM22B_RCVR */ /* #define PIOS_INCLUDE_PPM_OUT */ /* #define PIOS_RFM22B_DEBUG_ON_TELEM */ diff --git a/flight/targets/boards/oplinkmini/firmware/Makefile b/flight/targets/boards/oplinkmini/firmware/Makefile index c36788a60..12a491df5 100644 --- a/flight/targets/boards/oplinkmini/firmware/Makefile +++ b/flight/targets/boards/oplinkmini/firmware/Makefile @@ -49,10 +49,10 @@ ifndef TESTAPP SRC += $(OPUAVOBJ)/callbackscheduler.c ## UAVObjects - SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c SRC += $(OPUAVSYNTHDIR)/oplinkstatus.c SRC += $(OPUAVSYNTHDIR)/oplinksettings.c SRC += $(OPUAVSYNTHDIR)/objectpersistence.c + SRC += $(OPUAVSYNTHDIR)/oplinkreceiver.c else ## Test Code SRC += $(OPTESTS)/test_common.c diff --git a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h index da5b7d039..76b3860e7 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h @@ -116,9 +116,8 @@ /* PIOS radio modules */ #define PIOS_INCLUDE_RFM22B #define PIOS_INCLUDE_RFM22B_COM -/* #define PIOS_INCLUDE_RFM22B_RCVR */ #define PIOS_INCLUDE_PPM_OUT -#define PIOS_RFM22B_DEBUG_ON_TELEM +/* #define PIOS_RFM22B_DEBUG_ON_TELEM */ /* PIOS misc peripherals */ /* #define PIOS_INCLUDE_VIDEO */ diff --git a/flight/targets/boards/oplinkmini/firmware/pios_board.c b/flight/targets/boards/oplinkmini/firmware/pios_board.c index ec865d3a6..01d141ab0 100644 --- a/flight/targets/boards/oplinkmini/firmware/pios_board.c +++ b/flight/targets/boards/oplinkmini/firmware/pios_board.c @@ -221,6 +221,7 @@ void PIOS_Board_Init(void) /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) + bool is_coordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); { // Configure the RFM22B device const struct pios_board_info *bdinfo = &pios_board_info_blob; @@ -265,10 +266,41 @@ void PIOS_Board_Init(void) } PIOS_COM_ChangeBaud(pios_com_rfm22b_id, comBaud); + /* Set the modem Tx poer level */ + switch (oplinkSettings.MaxRFPower) { + case OPLINKSETTINGS_MAXRFPOWER_125: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_0); + break; + case OPLINKSETTINGS_MAXRFPOWER_16: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_1); + break; + case OPLINKSETTINGS_MAXRFPOWER_316: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_2); + break; + case OPLINKSETTINGS_MAXRFPOWER_63: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_3); + break; + case OPLINKSETTINGS_MAXRFPOWER_126: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_4); + break; + case OPLINKSETTINGS_MAXRFPOWER_25: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_5); + break; + case OPLINKSETTINGS_MAXRFPOWER_50: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_6); + break; + case OPLINKSETTINGS_MAXRFPOWER_100: + PIOS_RFM22B_SetTxPower(pios_rfm22b_id, RFM22_tx_pwr_txpow_7); + break; + default: + // do nothing + break; + } + // Set the radio configuration parameters. PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, oplinkSettings.NumChannels, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, oplinkSettings.PacketTime, (oplinkSettings.OneWayLink == OPLINKSETTINGS_ONEWAYLINK_TRUE)); - PIOS_RFM22B_SetCoordinator(pios_rfm22b_id, (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE)); + PIOS_RFM22B_SetCoordinator(pios_rfm22b_id, is_coordinator); PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID); // Reinitilize the modem to affect te changes. @@ -281,7 +313,6 @@ void PIOS_Board_Init(void) pios_uart_tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_TX_BUF_LEN); // Configure the main port - bool is_coordinator = PIOS_RFM22B_IsCoordinator(pios_rfm22b_id); switch (oplinkSettings.MainPort) { case OPLINKSETTINGS_MAINPORT_TELEMETRY: case OPLINKSETTINGS_MAINPORT_SERIAL: diff --git a/flight/targets/boards/oplinkmini/pios_board.h b/flight/targets/boards/oplinkmini/pios_board.h index 9d2ae2b6e..bfb69dab9 100644 --- a/flight/targets/boards/oplinkmini/pios_board.h +++ b/flight/targets/boards/oplinkmini/pios_board.h @@ -75,7 +75,8 @@ #define PIOS_WDG_TELEMETRYRX 0x0002 #define PIOS_WDG_RADIOTX 0x0004 #define PIOS_WDG_RADIORX 0x0008 -#define PIOS_WDG_RFM22B 0x0016 +#define PIOS_WDG_RFM22B 0x000f +#define PIOS_WDG_PPMINPUT 0x0010 // ------------------------ // TELEMETRY diff --git a/flight/targets/boards/osd/firmware/inc/pios_config.h b/flight/targets/boards/osd/firmware/inc/pios_config.h index f4fbdf4be..a6400957f 100644 --- a/flight/targets/boards/osd/firmware/inc/pios_config.h +++ b/flight/targets/boards/osd/firmware/inc/pios_config.h @@ -117,7 +117,6 @@ /* PIOS radio modules */ /* #define PIOS_INCLUDE_RFM22B */ /* #define PIOS_INCLUDE_RFM22B_COM */ -/* #define PIOS_INCLUDE_RFM22B_RCVR */ /* #define PIOS_INCLUDE_PPM_OUT */ /* #define PIOS_RFM22B_DEBUG_ON_TELEM */ diff --git a/flight/targets/boards/revolution/firmware/UAVObjects.inc b/flight/targets/boards/revolution/firmware/UAVObjects.inc index a8f47f8ba..3c8fcc238 100644 --- a/flight/targets/boards/revolution/firmware/UAVObjects.inc +++ b/flight/targets/boards/revolution/firmware/UAVObjects.inc @@ -59,6 +59,7 @@ UAVOBJSRCFILENAMES += mixersettings UAVOBJSRCFILENAMES += mixerstatus UAVOBJSRCFILENAMES += nedaccel UAVOBJSRCFILENAMES += objectpersistence +UAVOBJSRCFILENAMES += oplinkreceiver UAVOBJSRCFILENAMES += overosyncstats UAVOBJSRCFILENAMES += overosyncsettings UAVOBJSRCFILENAMES += pathaction diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index 7ef8dc475..528ae6278 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -116,7 +116,6 @@ /* PIOS radio modules */ #define PIOS_INCLUDE_RFM22B #define PIOS_INCLUDE_RFM22B_COM -#define PIOS_INCLUDE_RFM22B_RCVR /* #define PIOS_INCLUDE_PPM_OUT */ #define PIOS_RFM22B_DEBUG_ON_TELEM diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 855d9a849..89d675e24 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -32,6 +32,8 @@ #include #include #include +#include +#include #include /* @@ -787,16 +789,6 @@ void PIOS_Board_Init(void) /* Reinitialize the modem. */ PIOS_RFM22B_Reinit(pios_rfm22b_id); -#ifdef PIOS_INCLUDE_RFM22B_RCVR - if (PIOS_RFM22B_RCVR_Init(pios_rfm22b_id) != 0) { - PIOS_Assert(0); - } - uint32_t pios_rfm22b_rcvr_id; - if (PIOS_RCVR_Init(&pios_rfm22b_rcvr_id, &pios_rfm22b_rcvr_driver, pios_rfm22b_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK] = pios_rfm22b_rcvr_id; -#endif break; } } @@ -858,6 +850,19 @@ void PIOS_Board_Init(void) pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; #endif /* PIOS_INCLUDE_GCSRCVR */ +#if defined(PIOS_INCLUDE_OPLINKRCVR) + { + OPLinkReceiverInitialize(); + uint32_t pios_oplinkrcvr_id; + PIOS_OPLinkRCVR_Init(&pios_oplinkrcvr_id); + uint32_t pios_oplinkrcvr_rcvr_id; + if (PIOS_RCVR_Init(&pios_oplinkrcvr_rcvr_id, &pios_oplinkrcvr_rcvr_driver, pios_oplinkrcvr_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_OPLINK] = pios_oplinkrcvr_rcvr_id; + } +#endif /* PIOS_INCLUDE_OPLINKRCVR */ + #ifndef PIOS_ENABLE_DEBUG_PINS // pios_servo_cfg points to the correct configuration based on input port settings PIOS_Servo_Init(pios_servo_cfg); diff --git a/flight/targets/boards/revoproto/firmware/inc/pios_config.h b/flight/targets/boards/revoproto/firmware/inc/pios_config.h index 8c5268885..a4c2042df 100644 --- a/flight/targets/boards/revoproto/firmware/inc/pios_config.h +++ b/flight/targets/boards/revoproto/firmware/inc/pios_config.h @@ -116,7 +116,6 @@ /* PIOS radio modules */ /* #define PIOS_INCLUDE_RFM22B */ /* #define PIOS_INCLUDE_RFM22B_COM */ -/* #define PIOS_INCLUDE_RFM22B_RCVR */ /* #define PIOS_INCLUDE_PPM_OUT */ /* #define PIOS_RFM22B_DEBUG_ON_TELEM */ diff --git a/make/apps-defs.mk b/make/apps-defs.mk index dd7c3dc77..bfbb66c2a 100644 --- a/make/apps-defs.mk +++ b/make/apps-defs.mk @@ -84,7 +84,6 @@ SRC += $(PIOSCOMMON)/pios_flash_jedec.c SRC += $(PIOSCOMMON)/pios_rcvr.c SRC += $(PIOSCOMMON)/pios_rfm22b.c SRC += $(PIOSCOMMON)/pios_rfm22b_com.c -SRC += $(PIOSCOMMON)/pios_rfm22b_rcvr.c SRC += $(PIOSCOMMON)/pios_sbus.c SRC += $(PIOSCOMMON)/pios_sdcard.c diff --git a/shared/uavobjectdefinition/oplinksettings.xml b/shared/uavobjectdefinition/oplinksettings.xml index e0969e9dd..08c116ba1 100644 --- a/shared/uavobjectdefinition/oplinksettings.xml +++ b/shared/uavobjectdefinition/oplinksettings.xml @@ -13,7 +13,7 @@ - + From fc0328a3cdbcd5e926bcd81cfc658345b00ea6df Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 16 Jun 2013 10:07:56 -0700 Subject: [PATCH 09/28] OP-932 Adds working one-way mode to the OPLink radio connection. In this mode, packets will only flow from the ground side (coordinator modem) to the flight side modem. --- flight/pios/common/pios_rfm22b.c | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index 4266f40fc..6d7f1e281 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -2013,8 +2013,18 @@ static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, po static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev) { portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); + bool is_coordinator = rfm22_isCoordinator(rfm22b_dev); - if (!rfm22_isCoordinator(rfm22b_dev)) { + // If this is a one-way link, only the coordinator can send. + if (rfm22b_dev->one_way_link) { + if (is_coordinator) { + return ((time -1) % (rfm22b_dev->packet_period)) == 0; + } else { + return false; + } + } + + if (!is_coordinator) { time += rfm22b_dev->packet_period - 1; } else { time -= 1; From cdd51537f1357b8a06c791ae7df04030a6274660 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Wed, 5 Jun 2013 20:34:38 -0700 Subject: [PATCH 10/28] Fixes the 9600 bps air datarate for the RFM33B modem. --- flight/pios/common/pios_rfm22b.c | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index 6d7f1e281..5f9eb2665 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -297,30 +297,30 @@ static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_ST static const uint32_t data_rate[] = { 500, 1000, 2000, 4000, 8000, 9600, 16000, 19200, 24000, 32000, 57600, 64000, 128000, 192000, 256000 }; static const uint8_t modulation_index[] = { 16, 8, 4, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }; -static const uint8_t reg_1C[] = { 0x37, 0x37, 0x37, 0x37, 0x3A, 0x3B, 0x26, 0x28, 0x2E, 0x16, 0x06, 0x07, 0x83, 0x8A, 0x8C }; // rfm22_if_filter_bandwidth +static const uint8_t reg_1C[] = { 0x37, 0x37, 0x37, 0x37, 0x3A, 0x01, 0x26, 0x28, 0x2E, 0x16, 0x06, 0x07, 0x83, 0x8A, 0x8C }; // rfm22_if_filter_bandwidth -static const uint8_t reg_1D[] = { 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x44, 0x40, 0x44, 0x44, 0x44, 0x44 }; // rfm22_afc_loop_gearshift_override +static const uint8_t reg_1D[] = { 0x44, 0x44, 0x44, 0x44, 0x44, 0x40, 0x44, 0x44, 0x44, 0x44, 0x40, 0x44, 0x44, 0x44, 0x44 }; // rfm22_afc_loop_gearshift_override static const uint8_t reg_1E[] = { 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x02 }; // rfm22_afc_timing_control static const uint8_t reg_1F[] = { 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03 }; // rfm22_clk_recovery_gearshift_override -static const uint8_t reg_20[] = { 0xE8, 0xF4, 0xFA, 0x70, 0x3F, 0x34, 0x3F, 0x34, 0x2A, 0x3F, 0x45, 0x3F, 0x5E, 0x3F, 0x2F }; // rfm22_clk_recovery_oversampling_ratio -static const uint8_t reg_21[] = { 0x60, 0x20, 0x00, 0x01, 0x02, 0x02, 0x02, 0x02, 0x03, 0x02, 0x01, 0x02, 0x01, 0x02, 0x02 }; // rfm22_clk_recovery_offset2 -static const uint8_t reg_22[] = { 0x20, 0x41, 0x83, 0x06, 0x0C, 0x75, 0x0C, 0x75, 0x12, 0x0C, 0xD7, 0x0c, 0x5D, 0x0C, 0xBB }; // rfm22_clk_recovery_offset1 -static const uint8_t reg_23[] = { 0xC5, 0x89, 0x12, 0x25, 0x4A, 0x25, 0x4A, 0x25, 0x6F, 0x4A, 0xDC, 0x4A, 0x86, 0x4A, 0x0D }; // rfm22_clk_recovery_offset0 -static const uint8_t reg_24[] = { 0x00, 0x00, 0x00, 0x02, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x05, 0x07, 0x07 }; // rfm22_clk_recovery_timing_loop_gain1 -static const uint8_t reg_25[] = { 0x0A, 0x23, 0x85, 0x0E, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0xFF, 0x6E, 0xFF, 0x74, 0xFF, 0xFF }; // rfm22_clk_recovery_timing_loop_gain0 +static const uint8_t reg_20[] = { 0xE8, 0xF4, 0xFA, 0x70, 0x3F, 0xA1, 0x3F, 0x34, 0x2A, 0x3F, 0x45, 0x3F, 0x5E, 0x3F, 0x2F }; // rfm22_clk_recovery_oversampling_ratio +static const uint8_t reg_21[] = { 0x60, 0x20, 0x00, 0x01, 0x02, 0x20, 0x02, 0x02, 0x03, 0x02, 0x01, 0x02, 0x01, 0x02, 0x02 }; // rfm22_clk_recovery_offset2 +static const uint8_t reg_22[] = { 0x20, 0x41, 0x83, 0x06, 0x0C, 0x4E, 0x0C, 0x75, 0x12, 0x0C, 0xD7, 0x0c, 0x5D, 0x0C, 0xBB }; // rfm22_clk_recovery_offset1 +static const uint8_t reg_23[] = { 0xC5, 0x89, 0x12, 0x25, 0x4A, 0xA5, 0x4A, 0x25, 0x6F, 0x4A, 0xDC, 0x4A, 0x86, 0x4A, 0x0D }; // rfm22_clk_recovery_offset0 +static const uint8_t reg_24[] = { 0x00, 0x00, 0x00, 0x02, 0x07, 0x00, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x05, 0x07, 0x07 }; // rfm22_clk_recovery_timing_loop_gain1 +static const uint8_t reg_25[] = { 0x0A, 0x23, 0x85, 0x0E, 0xFF, 0x34, 0xFF, 0xFF, 0xFF, 0xFF, 0x6E, 0xFF, 0x74, 0xFF, 0xFF }; // rfm22_clk_recovery_timing_loop_gain0 -static const uint8_t reg_2A[] = { 0x0E, 0x0E, 0x0E, 0x0E, 0x0E, 0x0D, 0x0D, 0x0E, 0x12, 0x17, 0x2D, 0x31, 0x50, 0x50, 0x50 }; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz +static const uint8_t reg_2A[] = { 0x0E, 0x0E, 0x0E, 0x0E, 0x0E, 0x1E, 0x0D, 0x0E, 0x12, 0x17, 0x2D, 0x31, 0x50, 0x50, 0x50 }; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz static const uint8_t reg_58[] = { 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80 }; // rfm22_cpcuu -static const uint8_t reg_69[] = { 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x20, 0x60, 0x20, 0x20, 0x20, 0x20 }; // rfm22_agc_override1 +static const uint8_t reg_69[] = { 0x20, 0x20, 0x20, 0x20, 0x20, 0x60, 0x20, 0x20, 0x20, 0x20, 0x60, 0x20, 0x20, 0x20, 0x20 }; // rfm22_agc_override1 static const uint8_t reg_6E[] = { 0x04, 0x08, 0x10, 0x20, 0x41, 0x4E, 0x83, 0x9D, 0xC4, 0x08, 0x0E, 0x10, 0x20, 0x31, 0x41 }; // rfm22_tx_data_rate1 static const uint8_t reg_6F[] = { 0x19, 0x31, 0x62, 0xC5, 0x89, 0xA5, 0x12, 0x49, 0x9C, 0x31, 0xBF, 0x62, 0xC5, 0x27, 0x89 }; // rfm22_tx_data_rate0 -static const uint8_t reg_70[] = { 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x0D, 0x0C, 0x0C, 0x0D, 0x0D, 0x0D }; // rfm22_modulation_mode_control1 +static const uint8_t reg_70[] = { 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2C, 0x2D, 0x2D, 0x2D, 0x0D, 0x0C, 0x0C, 0x0D, 0x0D, 0x0D }; // rfm22_modulation_mode_control1 static const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23 }; // rfm22_modulation_mode_control2 -static const uint8_t reg_72[] = { 0x06, 0x06, 0x06, 0x06, 0x06, 0x08, 0x0D, 0x0F, 0x13, 0x1A, 0x2E, 0x33, 0x66, 0x9A, 0xCD }; // rfm22_frequency_deviation +static const uint8_t reg_72[] = { 0x06, 0x06, 0x06, 0x06, 0x06, 0x30, 0x0D, 0x0F, 0x13, 0x1A, 0x2E, 0x33, 0x66, 0x9A, 0xCD }; // rfm22_frequency_deviation static struct pios_rfm22b_dev *g_rfm22b_dev = NULL; From f8d60e6deee0e0f5e97643bfe497d7764e2ab7d3 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Mon, 17 Jun 2013 07:06:47 -0700 Subject: [PATCH 11/28] OP-932 Adds additional thread and UAVTalk connection to the telemetry module that reads from the uart/radio link on the Revo. This allows the Revo to read OAVObjects over both the uart/radio link and the USB link at the same time. Unfortunately, there's no way to know which link to send UAVObjects out on, so all UAVObjects go to the USB port if USB is connected. This allows UAVObjects to be received from the OPLink radio, even when USB is connected. --- flight/modules/Telemetry/telemetry.c | 64 ++++++++++++++++++++++--- flight/uavtalk/inc/uavtalk.h | 1 + flight/uavtalk/uavtalk.c | 28 +++++++++++ shared/uavobjectdefinition/taskinfo.xml | 3 ++ 4 files changed, 90 insertions(+), 6 deletions(-) diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 1eccd33d5..69ed7fa20 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -44,6 +44,7 @@ #define STACK_SIZE_BYTES PIOS_TELEM_STACK_SIZE #define TASK_PRIORITY_RX (tskIDLE_PRIORITY + 2) #define TASK_PRIORITY_TX (tskIDLE_PRIORITY + 2) +#define TASK_PRIORITY_RADRX (tskIDLE_PRIORITY + 2) #define TASK_PRIORITY_TXPRI (tskIDLE_PRIORITY + 2) #define REQ_TIMEOUT_MS 250 #define MAX_RETRIES 2 @@ -66,14 +67,23 @@ static void telemetryTxPriTask(void *parameters); static xTaskHandle telemetryTxTaskHandle; static xTaskHandle telemetryRxTaskHandle; +#ifdef PIOS_INCLUDE_RFM22B +static xTaskHandle radioRxTaskHandle; +#endif static uint32_t txErrors; static uint32_t txRetries; static uint32_t timeOfLastObjectUpdate; static UAVTalkConnection uavTalkCon; +#ifdef PIOS_INCLUDE_RFM22B +static UAVTalkConnection radioUavTalkCon; +#endif // Private functions static void telemetryTxTask(void *parameters); static void telemetryRxTask(void *parameters); +#ifdef PIOS_INCLUDE_RFM22B +static void radioRxTask(void *parameters); +#endif static int32_t transmitData(uint8_t *data, int32_t length); static void registerObject(UAVObjHandle obj); static void updateObject(UAVObjHandle obj, int32_t eventType); @@ -82,7 +92,7 @@ static void processObjEvent(UAVObjEvent *ev); static void updateTelemetryStats(); static void gcsTelemetryStatsUpdated(); static void updateSettings(); -static uint32_t getComPort(); +static uint32_t getComPort(bool input); /** * Initialise the telemetry module @@ -103,6 +113,11 @@ int32_t TelemetryStart(void) xTaskCreate(telemetryRxTask, (signed char *)"TelRx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_RX, &telemetryRxTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYRX, telemetryRxTaskHandle); +#ifdef PIOS_INCLUDE_RFM22B + xTaskCreate(radioRxTask, (signed char *)"RadioRx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_RADRX, &radioRxTaskHandle); + PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_RADIORX, radioRxTaskHandle); +#endif + #if defined(PIOS_TELEM_PRIORITY_QUEUE) xTaskCreate(telemetryTxPriTask, (signed char *)"TelPriTx", STACK_SIZE_BYTES / 4, NULL, TASK_PRIORITY_TXPRI, &telemetryTxPriTaskHandle); PIOS_TASK_MONITOR_RegisterTask(TASKINFO_RUNNING_TELEMETRYTXPRI, telemetryTxPriTaskHandle); @@ -137,6 +152,9 @@ int32_t TelemetryInitialize(void) // Initialise UAVTalk uavTalkCon = UAVTalkInitialize(&transmitData); +#ifdef PIOS_INCLUDE_RFM22B + radioUavTalkCon = UAVTalkInitialize(&transmitData); +#endif // Create periodic event that will be used to update the telemetry stats txErrors = 0; @@ -342,13 +360,13 @@ static void telemetryTxPriTask(__attribute__((unused)) void *parameters) #endif /** - * Telemetry transmit task. Processes queue events and periodic updates. + * Telemetry receive task. Processes queue events and periodic updates. */ static void telemetryRxTask(__attribute__((unused)) void *parameters) { // Task loop while (1) { - uint32_t inputPort = getComPort(); + uint32_t inputPort = getComPort(true); if (inputPort) { // Block until data are available @@ -367,6 +385,32 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) } } +#ifdef PIOS_INCLUDE_RFM22B +/** + * Radio telemetry receive task. Processes queue events and periodic updates. + */ +static void radioRxTask(__attribute__((unused)) void *parameters) +{ + // Task loop + while (1) { + if (telemetryPort) { + // Block until data are available + uint8_t serial_data[1]; + uint16_t bytes_to_process; + + bytes_to_process = PIOS_COM_ReceiveBuffer(telemetryPort, serial_data, sizeof(serial_data), 500); + if (bytes_to_process > 0) { + for (uint8_t i = 0; i < bytes_to_process; i++) { + UAVTalkProcessInputStream(radioUavTalkCon, serial_data[i]); + } + } + } else { + vTaskDelay(5); + } + } +} +#endif /* PIOS_INCLUDE_RFM22B */ + /** * Transmit data buffer to the modem or USB port. * \param[in] data Data buffer to send @@ -376,7 +420,7 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) */ static int32_t transmitData(uint8_t *data, int32_t length) { - uint32_t outputPort = getComPort(); + uint32_t outputPort = getComPort(false); if (outputPort) { return PIOS_COM_SendBuffer(outputPort, data, length); @@ -434,6 +478,10 @@ static void updateTelemetryStats() // Get stats UAVTalkGetStats(uavTalkCon, &utalkStats); +#ifdef PIOS_INCLUDE_RFM22B + UAVTalkAddStats(radioUavTalkCon, &utalkStats); + UAVTalkResetStats(radioUavTalkCon); +#endif UAVTalkResetStats(uavTalkCon); // Get object data @@ -553,15 +601,19 @@ static void updateSettings() /** * Determine input/output com port as highest priority available + * @param[in] input Returns the approproate input com port if true, else the appropriate output com port */ -static uint32_t getComPort() +static uint32_t getComPort(bool input) { +#ifndef PIOS_INCLUDE_RFM22B + input = false; +#endif #if defined(PIOS_INCLUDE_USB) if (PIOS_COM_Available(PIOS_COM_TELEM_USB)) { return PIOS_COM_TELEM_USB; } else #endif /* PIOS_INCLUDE_USB */ - if (PIOS_COM_Available(telemetryPort)) { + if (!input && PIOS_COM_Available(telemetryPort)) { return telemetryPort; } else { return 0; diff --git a/flight/uavtalk/inc/uavtalk.h b/flight/uavtalk/inc/uavtalk.h index 7b35606ff..3067b502c 100644 --- a/flight/uavtalk/inc/uavtalk.h +++ b/flight/uavtalk/inc/uavtalk.h @@ -62,6 +62,7 @@ UAVTalkRxState UAVTalkProcessInputStreamQuiet(UAVTalkConnection connection, uint UAVTalkRxState UAVTalkRelayPacket(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle); int32_t UAVTalkReceiveObject(UAVTalkConnection connectionHandle); void UAVTalkGetStats(UAVTalkConnection connection, UAVTalkStats *stats); +void UAVTalkAddStats(UAVTalkConnection connection, UAVTalkStats *stats); void UAVTalkResetStats(UAVTalkConnection connection); void UAVTalkGetLastTimestamp(UAVTalkConnection connection, uint16_t *timestamp); uint32_t UAVTalkGetPacketObjId(UAVTalkConnection connection); diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index ae6031feb..a6b70d25f 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -136,6 +136,34 @@ void UAVTalkGetStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut) xSemaphoreGiveRecursive(connection->lock); } +/** + * Get communication statistics counters + * \param[in] connection UAVTalkConnection to be used + * @param[out] statsOut Statistics counters + */ +void UAVTalkAddStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut) +{ + UAVTalkConnectionData *connection; + + CHECKCONHANDLE(connectionHandle, connection, return ); + + // Lock + xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); + + // Copy stats + statsOut->txBytes += connection->stats.txBytes; + statsOut->rxBytes += connection->stats.rxBytes; + statsOut->txObjectBytes += connection->stats.txObjectBytes; + statsOut->rxObjectBytes += connection->stats.rxObjectBytes; + statsOut->txObjects += connection->stats.txObjects; + statsOut->rxObjects += connection->stats.rxObjects; + statsOut->txErrors += connection->stats.txErrors; + statsOut->txErrors += connection->stats.txErrors; + + // Release lock + xSemaphoreGiveRecursive(connection->lock); +} + /** * Reset the statistics counters. * \param[in] connection UAVTalkConnection to be used diff --git a/shared/uavobjectdefinition/taskinfo.xml b/shared/uavobjectdefinition/taskinfo.xml index 869e3b84b..a8c993f56 100644 --- a/shared/uavobjectdefinition/taskinfo.xml +++ b/shared/uavobjectdefinition/taskinfo.xml @@ -10,6 +10,7 @@ TelemetryTx TelemetryTxPri TelemetryRx + RadioRx GPS ManualControl Altitude @@ -44,6 +45,7 @@ TelemetryTx TelemetryTxPri TelemetryRx + RadioRx GPS ManualControl Altitude @@ -82,6 +84,7 @@ TelemetryTx TelemetryTxPri TelemetryRx + RadioRx GPS ManualControl Altitude From 61c453f6ff2a47eeaa6665bb4d98bea0d8c71dc3 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Mon, 17 Jun 2013 19:19:35 -0700 Subject: [PATCH 12/28] Removes several of the air datarate settings, and fine tunes the calculation of the max packet length from the datarate and packet time. 9600, 19200, 36400, and 57600 modes tested. Tested 19200 in one-way mode for PPM only. --- flight/pios/common/pios_rfm22b.c | 42 ++++++++++++++-------------- flight/pios/common/pios_rfm22b_com.c | 10 ++----- flight/pios/inc/pios_rfm22b.h | 23 ++++++--------- 3 files changed, 31 insertions(+), 44 deletions(-) diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index 5f9eb2665..e5bc88b6a 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -294,33 +294,33 @@ static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_ST }; // xtal 10 ppm, 434MHz -static const uint32_t data_rate[] = { 500, 1000, 2000, 4000, 8000, 9600, 16000, 19200, 24000, 32000, 57600, 64000, 128000, 192000, 256000 }; -static const uint8_t modulation_index[] = { 16, 8, 4, 2, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1 }; +static const uint32_t data_rate[] = { 9600, 19200, 32000, 57600, 64000, 128000, 192000, 256000 }; +static const uint8_t modulation_index[] = { 1, 1, 1, 1, 1, 1, 1, 1 }; -static const uint8_t reg_1C[] = { 0x37, 0x37, 0x37, 0x37, 0x3A, 0x01, 0x26, 0x28, 0x2E, 0x16, 0x06, 0x07, 0x83, 0x8A, 0x8C }; // rfm22_if_filter_bandwidth +static const uint8_t reg_1C[] = { 0x01, 0x28, 0x16, 0x06, 0x07, 0x83, 0x8A, 0x8C }; // rfm22_if_filter_bandwidth -static const uint8_t reg_1D[] = { 0x44, 0x44, 0x44, 0x44, 0x44, 0x40, 0x44, 0x44, 0x44, 0x44, 0x40, 0x44, 0x44, 0x44, 0x44 }; // rfm22_afc_loop_gearshift_override -static const uint8_t reg_1E[] = { 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x02 }; // rfm22_afc_timing_control +static const uint8_t reg_1D[] = { 0x40, 0x44, 0x44, 0x40, 0x44, 0x44, 0x44, 0x44 }; // rfm22_afc_loop_gearshift_override +static const uint8_t reg_1E[] = { 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x02 }; // rfm22_afc_timing_control -static const uint8_t reg_1F[] = { 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03 }; // rfm22_clk_recovery_gearshift_override -static const uint8_t reg_20[] = { 0xE8, 0xF4, 0xFA, 0x70, 0x3F, 0xA1, 0x3F, 0x34, 0x2A, 0x3F, 0x45, 0x3F, 0x5E, 0x3F, 0x2F }; // rfm22_clk_recovery_oversampling_ratio -static const uint8_t reg_21[] = { 0x60, 0x20, 0x00, 0x01, 0x02, 0x20, 0x02, 0x02, 0x03, 0x02, 0x01, 0x02, 0x01, 0x02, 0x02 }; // rfm22_clk_recovery_offset2 -static const uint8_t reg_22[] = { 0x20, 0x41, 0x83, 0x06, 0x0C, 0x4E, 0x0C, 0x75, 0x12, 0x0C, 0xD7, 0x0c, 0x5D, 0x0C, 0xBB }; // rfm22_clk_recovery_offset1 -static const uint8_t reg_23[] = { 0xC5, 0x89, 0x12, 0x25, 0x4A, 0xA5, 0x4A, 0x25, 0x6F, 0x4A, 0xDC, 0x4A, 0x86, 0x4A, 0x0D }; // rfm22_clk_recovery_offset0 -static const uint8_t reg_24[] = { 0x00, 0x00, 0x00, 0x02, 0x07, 0x00, 0x07, 0x07, 0x07, 0x07, 0x07, 0x07, 0x05, 0x07, 0x07 }; // rfm22_clk_recovery_timing_loop_gain1 -static const uint8_t reg_25[] = { 0x0A, 0x23, 0x85, 0x0E, 0xFF, 0x34, 0xFF, 0xFF, 0xFF, 0xFF, 0x6E, 0xFF, 0x74, 0xFF, 0xFF }; // rfm22_clk_recovery_timing_loop_gain0 +static const uint8_t reg_1F[] = { 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03 }; // rfm22_clk_recovery_gearshift_override +static const uint8_t reg_20[] = { 0xA1, 0x34, 0x3F, 0x45, 0x3F, 0x5E, 0x3F, 0x2F }; // rfm22_clk_recovery_oversampling_ratio +static const uint8_t reg_21[] = { 0x20, 0x02, 0x02, 0x01, 0x02, 0x01, 0x02, 0x02 }; // rfm22_clk_recovery_offset2 +static const uint8_t reg_22[] = { 0x4E, 0x75, 0x0C, 0xD7, 0x0c, 0x5D, 0x0C, 0xBB }; // rfm22_clk_recovery_offset1 +static const uint8_t reg_23[] = { 0xA5, 0x25, 0x4A, 0xDC, 0x4A, 0x86, 0x4A, 0x0D }; // rfm22_clk_recovery_offset0 +static const uint8_t reg_24[] = { 0x00, 0x07, 0x07, 0x07, 0x07, 0x05, 0x07, 0x07 }; // rfm22_clk_recovery_timing_loop_gain1 +static const uint8_t reg_25[] = { 0x34, 0xFF, 0xFF, 0x6E, 0xFF, 0x74, 0xFF, 0xFF }; // rfm22_clk_recovery_timing_loop_gain0 -static const uint8_t reg_2A[] = { 0x0E, 0x0E, 0x0E, 0x0E, 0x0E, 0x1E, 0x0D, 0x0E, 0x12, 0x17, 0x2D, 0x31, 0x50, 0x50, 0x50 }; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz +static const uint8_t reg_2A[] = { 0x1E, 0x0E, 0x17, 0x2D, 0x31, 0x50, 0x50, 0x50 }; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz -static const uint8_t reg_58[] = { 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80 }; // rfm22_cpcuu -static const uint8_t reg_69[] = { 0x20, 0x20, 0x20, 0x20, 0x20, 0x60, 0x20, 0x20, 0x20, 0x20, 0x60, 0x20, 0x20, 0x20, 0x20 }; // rfm22_agc_override1 -static const uint8_t reg_6E[] = { 0x04, 0x08, 0x10, 0x20, 0x41, 0x4E, 0x83, 0x9D, 0xC4, 0x08, 0x0E, 0x10, 0x20, 0x31, 0x41 }; // rfm22_tx_data_rate1 -static const uint8_t reg_6F[] = { 0x19, 0x31, 0x62, 0xC5, 0x89, 0xA5, 0x12, 0x49, 0x9C, 0x31, 0xBF, 0x62, 0xC5, 0x27, 0x89 }; // rfm22_tx_data_rate0 +static const uint8_t reg_58[] = { 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80 }; // rfm22_cpcuu +static const uint8_t reg_69[] = { 0x60, 0x20, 0x20, 0x60, 0x20, 0x20, 0x20, 0x20 }; // rfm22_agc_override1 +static const uint8_t reg_6E[] = { 0x4E, 0x9D, 0x08, 0x0E, 0x10, 0x20, 0x31, 0x41 }; // rfm22_tx_data_rate1 +static const uint8_t reg_6F[] = { 0xA5, 0x49, 0x31, 0xBF, 0x62, 0xC5, 0x27, 0x89 }; // rfm22_tx_data_rate0 -static const uint8_t reg_70[] = { 0x2D, 0x2D, 0x2D, 0x2D, 0x2D, 0x2C, 0x2D, 0x2D, 0x2D, 0x0D, 0x0C, 0x0C, 0x0D, 0x0D, 0x0D }; // rfm22_modulation_mode_control1 -static const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23 }; // rfm22_modulation_mode_control2 +static const uint8_t reg_70[] = { 0x2C, 0x2D, 0x0D, 0x0C, 0x0C, 0x0D, 0x0D, 0x0D }; // rfm22_modulation_mode_control1 +static const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23 }; // rfm22_modulation_mode_control2 -static const uint8_t reg_72[] = { 0x06, 0x06, 0x06, 0x06, 0x06, 0x30, 0x0D, 0x0F, 0x13, 0x1A, 0x2E, 0x33, 0x66, 0x9A, 0xCD }; // rfm22_frequency_deviation +static const uint8_t reg_72[] = { 0x30, 0x0F, 0x1A, 0x2E, 0x33, 0x66, 0x9A, 0xCD }; // rfm22_frequency_deviation static struct pios_rfm22b_dev *g_rfm22b_dev = NULL; @@ -1446,7 +1446,7 @@ static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev) uint32_t datarate_bps = data_rate[datarate]; // Calculate the maximum packet length from the datarate. - float bytes_per_period = (float)datarate_bps * (float)(rfm22b_dev->packet_period) / 8000; + float bytes_per_period = (float)datarate_bps * (float)(rfm22b_dev->packet_period) / 9000; rfm22b_dev->max_packet_len = bytes_per_period - TX_PREAMBLE_NIBBLES / 2 - SYNC_BYTES - HEADER_BYTES - 5; if (rfm22b_dev->max_packet_len > RFM22B_MAX_PACKET_LEN) { diff --git a/flight/pios/common/pios_rfm22b_com.c b/flight/pios/common/pios_rfm22b_com.c index 9275d48b6..9812ff366 100644 --- a/flight/pios/common/pios_rfm22b_com.c +++ b/flight/pios/common/pios_rfm22b_com.c @@ -67,14 +67,8 @@ static void PIOS_RFM22B_COM_ChangeBaud(uint32_t rfm22b_id, uint32_t baud) } // Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex. enum rfm22b_datarate datarate = RFM22_datarate_64000; - if (baud <= 1024) { - datarate = RFM22_datarate_500; - } else if (baud <= 2048) { - datarate = RFM22_datarate_1000; - } else if (baud <= 4096) { - datarate = RFM22_datarate_8000; - } else if (baud <= 9600) { - datarate = RFM22_datarate_16000; + if (baud <= 9600) { + datarate = RFM22_datarate_19200; } else if (baud <= 19200) { datarate = RFM22_datarate_32000; } else if (baud <= 38400) { diff --git a/flight/pios/inc/pios_rfm22b.h b/flight/pios/inc/pios_rfm22b.h index 1bb23395e..c5663ea6d 100644 --- a/flight/pios/inc/pios_rfm22b.h +++ b/flight/pios/inc/pios_rfm22b.h @@ -58,21 +58,14 @@ enum rfm22b_tx_power { }; enum rfm22b_datarate { - RFM22_datarate_500 = 0, - RFM22_datarate_1000 = 1, - RFM22_datarate_2000 = 2, - RFM22_datarate_4000 = 3, - RFM22_datarate_8000 = 4, - RFM22_datarate_9600 = 5, - RFM22_datarate_16000 = 6, - RFM22_datarate_19200 = 7, - RFM22_datarate_24000 = 8, - RFM22_datarate_32000 = 9, - RFM22_datarate_57600 = 10, - RFM22_datarate_64000 = 11, - RFM22_datarate_128000 = 12, - RFM22_datarate_192000 = 13, - RFM22_datarate_256000 = 14, + RFM22_datarate_9600 = 0, + RFM22_datarate_19200 = 1, + RFM22_datarate_32000 = 2, + RFM22_datarate_57600 = 3, + RFM22_datarate_64000 = 4, + RFM22_datarate_128000 = 5, + RFM22_datarate_192000 = 6, + RFM22_datarate_256000 = 7, }; typedef enum { From 56a60f2983b8b2c115cc9509c802306c606031b6 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Mon, 17 Jun 2013 20:55:15 -0700 Subject: [PATCH 13/28] OP-932 Updates the 19.2 kbps datarate using the spreadsheet. --- flight/pios/common/pios_rfm22b.c | 24 ++++++++++++------------ 1 file changed, 12 insertions(+), 12 deletions(-) diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index e5bc88b6a..a8e80d547 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -297,30 +297,30 @@ static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_ST static const uint32_t data_rate[] = { 9600, 19200, 32000, 57600, 64000, 128000, 192000, 256000 }; static const uint8_t modulation_index[] = { 1, 1, 1, 1, 1, 1, 1, 1 }; -static const uint8_t reg_1C[] = { 0x01, 0x28, 0x16, 0x06, 0x07, 0x83, 0x8A, 0x8C }; // rfm22_if_filter_bandwidth +static const uint8_t reg_1C[] = { 0x01, 0x05, 0x16, 0x06, 0x07, 0x83, 0x8A, 0x8C }; // rfm22_if_filter_bandwidth -static const uint8_t reg_1D[] = { 0x40, 0x44, 0x44, 0x40, 0x44, 0x44, 0x44, 0x44 }; // rfm22_afc_loop_gearshift_override +static const uint8_t reg_1D[] = { 0x40, 0x40, 0x44, 0x40, 0x44, 0x44, 0x44, 0x44 }; // rfm22_afc_loop_gearshift_override static const uint8_t reg_1E[] = { 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x02 }; // rfm22_afc_timing_control static const uint8_t reg_1F[] = { 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03 }; // rfm22_clk_recovery_gearshift_override -static const uint8_t reg_20[] = { 0xA1, 0x34, 0x3F, 0x45, 0x3F, 0x5E, 0x3F, 0x2F }; // rfm22_clk_recovery_oversampling_ratio -static const uint8_t reg_21[] = { 0x20, 0x02, 0x02, 0x01, 0x02, 0x01, 0x02, 0x02 }; // rfm22_clk_recovery_offset2 -static const uint8_t reg_22[] = { 0x4E, 0x75, 0x0C, 0xD7, 0x0c, 0x5D, 0x0C, 0xBB }; // rfm22_clk_recovery_offset1 -static const uint8_t reg_23[] = { 0xA5, 0x25, 0x4A, 0xDC, 0x4A, 0x86, 0x4A, 0x0D }; // rfm22_clk_recovery_offset0 -static const uint8_t reg_24[] = { 0x00, 0x07, 0x07, 0x07, 0x07, 0x05, 0x07, 0x07 }; // rfm22_clk_recovery_timing_loop_gain1 -static const uint8_t reg_25[] = { 0x34, 0xFF, 0xFF, 0x6E, 0xFF, 0x74, 0xFF, 0xFF }; // rfm22_clk_recovery_timing_loop_gain0 +static const uint8_t reg_20[] = { 0xA1, 0xD0, 0x3F, 0x45, 0x3F, 0x5E, 0x3F, 0x2F }; // rfm22_clk_recovery_oversampling_ratio +static const uint8_t reg_21[] = { 0x20, 0x00, 0x02, 0x01, 0x02, 0x01, 0x02, 0x02 }; // rfm22_clk_recovery_offset2 +static const uint8_t reg_22[] = { 0x4E, 0x9D, 0x0C, 0xD7, 0x0c, 0x5D, 0x0C, 0xBB }; // rfm22_clk_recovery_offset1 +static const uint8_t reg_23[] = { 0xA5, 0x49, 0x4A, 0xDC, 0x4A, 0x86, 0x4A, 0x0D }; // rfm22_clk_recovery_offset0 +static const uint8_t reg_24[] = { 0x00, 0x00, 0x07, 0x07, 0x07, 0x05, 0x07, 0x07 }; // rfm22_clk_recovery_timing_loop_gain1 +static const uint8_t reg_25[] = { 0x34, 0x8, 0xFF, 0x6E, 0xFF, 0x74, 0xFF, 0xFF }; // rfm22_clk_recovery_timing_loop_gain0 -static const uint8_t reg_2A[] = { 0x1E, 0x0E, 0x17, 0x2D, 0x31, 0x50, 0x50, 0x50 }; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz +static const uint8_t reg_2A[] = { 0x1E, 0x24, 0x17, 0x2D, 0x31, 0x50, 0x50, 0x50 }; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz static const uint8_t reg_58[] = { 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80 }; // rfm22_cpcuu -static const uint8_t reg_69[] = { 0x60, 0x20, 0x20, 0x60, 0x20, 0x20, 0x20, 0x20 }; // rfm22_agc_override1 +static const uint8_t reg_69[] = { 0x60, 0x60, 0x20, 0x60, 0x20, 0x20, 0x20, 0x20 }; // rfm22_agc_override1 static const uint8_t reg_6E[] = { 0x4E, 0x9D, 0x08, 0x0E, 0x10, 0x20, 0x31, 0x41 }; // rfm22_tx_data_rate1 static const uint8_t reg_6F[] = { 0xA5, 0x49, 0x31, 0xBF, 0x62, 0xC5, 0x27, 0x89 }; // rfm22_tx_data_rate0 -static const uint8_t reg_70[] = { 0x2C, 0x2D, 0x0D, 0x0C, 0x0C, 0x0D, 0x0D, 0x0D }; // rfm22_modulation_mode_control1 +static const uint8_t reg_70[] = { 0x2C, 0x2C, 0x0D, 0x0C, 0x0C, 0x0D, 0x0D, 0x0D }; // rfm22_modulation_mode_control1 static const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23 }; // rfm22_modulation_mode_control2 -static const uint8_t reg_72[] = { 0x30, 0x0F, 0x1A, 0x2E, 0x33, 0x66, 0x9A, 0xCD }; // rfm22_frequency_deviation +static const uint8_t reg_72[] = { 0x30, 0x48, 0x1A, 0x2E, 0x33, 0x66, 0x9A, 0xCD }; // rfm22_frequency_deviation static struct pios_rfm22b_dev *g_rfm22b_dev = NULL; From 44a0266e41af53fb62c819840f162111fc92b627 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Tue, 18 Jun 2013 06:54:04 -0700 Subject: [PATCH 14/28] OP-932 Adds claiming of the SPI bus in the OPLink channel change function. --- flight/pios/common/pios_rfm22b.c | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index a8e80d547..466589b03 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -1584,7 +1584,7 @@ static bool rfm22_setFreqHopChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t D3_LED_TOGGLE; #endif // PIOS_RFM22B_DEBUG_ON_TELEM rfm22b_dev->channel = channel; - rfm22_write(rfm22b_dev, RFM22_frequency_hopping_channel_select, channel); + rfm22_write_claim(rfm22b_dev, RFM22_frequency_hopping_channel_select, channel); return true; } From b25ba75fd9cb90bbbe826a08119242f48a709150 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Tue, 18 Jun 2013 07:43:12 -0700 Subject: [PATCH 15/28] OP-932 Removed obsolete include of pios_rfm22b_rcvr from pios.h. --- flight/pios/pios.h | 3 --- 1 file changed, 3 deletions(-) diff --git a/flight/pios/pios.h b/flight/pios/pios.h index 59875f3bc..97a3fdaee 100644 --- a/flight/pios/pios.h +++ b/flight/pios/pios.h @@ -296,9 +296,6 @@ #ifdef PIOS_INCLUDE_RFM22B_COM #include #endif -#ifdef PIOS_INCLUDE_RFM22B_RCVR -#include -#endif #endif /* PIOS_INCLUDE_RFM22B */ /* PIOS misc peripherals */ From 2aea2342ab10df1698a3c04c641b83ecaea15713 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Tue, 25 Jun 2013 20:54:35 -0700 Subject: [PATCH 16/28] OP-932 Adds a direct PPM channel on each Tx packet when in PPM mode, and adds a PPM only mode that uses a 9600 bps air datarate. Also updates the OPLink configuration tab and moves all OPLink configuration to the OPLink configuration tab on the Revo, and customizes the OPLink configuration tab for the Revo. --- flight/modules/OPLink/oplinkmod.c | 16 - .../modules/RadioComBridge/RadioComBridge.c | 175 +++---- flight/modules/Telemetry/telemetry.c | 4 +- flight/pios/common/pios_rfm22b.c | 362 ++++++++++---- flight/pios/common/pios_rfm22b_com.c | 37 +- flight/pios/inc/pios_rfm22b.h | 8 +- flight/pios/inc/pios_rfm22b_priv.h | 38 +- .../boards/oplinkmini/firmware/pios_board.c | 162 +++--- flight/targets/boards/oplinkmini/pios_board.h | 1 + .../revolution/firmware/inc/pios_config.h | 2 +- .../boards/revolution/firmware/pios_board.c | 93 ++-- flight/uavtalk/uavtalk.c | 12 +- .../plugins/config/configpipxtremewidget.cpp | 96 +++- .../plugins/config/configpipxtremewidget.h | 4 +- .../src/plugins/config/configrevohwwidget.cpp | 31 -- .../src/plugins/config/configrevohwwidget.h | 1 - .../src/plugins/config/configrevohwwidget.ui | 468 +++++++++--------- .../src/plugins/config/pipxtreme.ui | 468 +++++++++--------- shared/uavobjectdefinition/hwsettings.xml | 2 - shared/uavobjectdefinition/oplinksettings.xml | 10 +- shared/uavobjectdefinition/oplinkstatus.xml | 2 +- 21 files changed, 1100 insertions(+), 892 deletions(-) diff --git a/flight/modules/OPLink/oplinkmod.c b/flight/modules/OPLink/oplinkmod.c index 2c976ac16..8b4c7617d 100644 --- a/flight/modules/OPLink/oplinkmod.c +++ b/flight/modules/OPLink/oplinkmod.c @@ -97,22 +97,6 @@ int32_t OPLinkModInitialize(void) { // Must registers objects here for system thread because ObjectManager started in OpenPilotInit - // Initialize out status object. - OPLinkStatusInitialize(); - OPLinkStatusData oplinkStatus; - OPLinkStatusGet(&oplinkStatus); - - // Get our hardware information. - const struct pios_board_info *bdinfo = &pios_board_info_blob; - - oplinkStatus.BoardType = bdinfo->board_type; - PIOS_BL_HELPER_FLASH_Read_Description(oplinkStatus.Description, OPLINKSTATUS_DESCRIPTION_NUMELEM); - PIOS_SYS_SerialNumberGetBinary(oplinkStatus.CPUSerial); - oplinkStatus.BoardRevision = bdinfo->board_rev; - - // Update the object - OPLinkStatusSet(&oplinkStatus); - // Call the module start function. OPLinkModStart(); diff --git a/flight/modules/RadioComBridge/RadioComBridge.c b/flight/modules/RadioComBridge/RadioComBridge.c index eafb4e30b..a5054859e 100644 --- a/flight/modules/RadioComBridge/RadioComBridge.c +++ b/flight/modules/RadioComBridge/RadioComBridge.c @@ -37,7 +37,6 @@ #include #include #include -#include #include #include #if defined(PIOS_INCLUDE_FLASH_EEPROM) @@ -49,14 +48,14 @@ // **************** // Private constants -#define STACK_SIZE_BYTES 150 -#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) -#define MAX_RETRIES 2 -#define RETRY_TIMEOUT_MS 20 -#define EVENT_QUEUE_SIZE 10 -#define MAX_PORT_DELAY 200 -#define EV_SEND_ACK 0x20 -#define EV_SEND_NACK 0x30 +#define STACK_SIZE_BYTES 150 +#define TASK_PRIORITY (tskIDLE_PRIORITY + 1) +#define MAX_RETRIES 2 +#define RETRY_TIMEOUT_MS 20 +#define EVENT_QUEUE_SIZE 10 +#define MAX_PORT_DELAY 200 +#define SERIAL_RX_BUF_LEN 100 +#define PPM_INPUT_TIMEOUT 100 // **************** // Private types @@ -68,6 +67,7 @@ typedef struct { xTaskHandle radioTxTaskHandle; xTaskHandle radioRxTaskHandle; xTaskHandle PPMInputTaskHandle; + xTaskHandle serialRxTaskHandle; // The UAVTalk connection on the com side. UAVTalkConnection telemUAVTalkCon; @@ -77,14 +77,17 @@ typedef struct { xQueueHandle uavtalkEventQueue; xQueueHandle radioEventQueue; + // The raw serial Rx buffer + uint8_t serialRxBuf[SERIAL_RX_BUF_LEN]; + // Error statistics. - uint32_t comTxErrors; - uint32_t comTxRetries; - uint32_t UAVTalkErrors; - uint32_t droppedPackets; + uint32_t comTxErrors; + uint32_t comTxRetries; + uint32_t UAVTalkErrors; + uint32_t droppedPackets; // Should we parse UAVTalk? - bool parseUAVTalk; + bool parseUAVTalk; // The current configured uart speed OPLinkSettingsComSpeedOptions comSpeed; @@ -95,6 +98,7 @@ typedef struct { static void telemetryTxTask(void *parameters); static void telemetryRxTask(void *parameters); +static void serialRxTask(void *parameters); static void radioTxTask(void *parameters); static void radioRxTask(void *parameters); static void PPMInputTask(void *parameters); @@ -103,7 +107,6 @@ static int32_t RadioSendHandler(uint8_t *buf, int32_t length); static void ProcessTelemetryStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte); static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConnection outConnectionHandle, uint8_t rxbyte); static void objectPersistenceUpdatedCb(UAVObjEvent *objEv); -static void oplinkReceiverUpdatedCb(UAVObjEvent *objEv); // **************** // Private variables @@ -170,21 +173,25 @@ static int32_t RadioComBridgeStart(void) // Configure the UAVObject callbacks ObjectPersistenceConnectCallback(&objectPersistenceUpdatedCb); - if (!is_coordinator) { - OPLinkReceiverConnectCallback(&oplinkReceiverUpdatedCb); - } // Start the primary tasks for receiving/sending UAVTalk packets from the GCS. xTaskCreate(telemetryTxTask, (signed char *)"telemetryTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->telemetryTxTaskHandle)); xTaskCreate(telemetryRxTask, (signed char *)"telemetryRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->telemetryRxTaskHandle)); - xTaskCreate(radioTxTask, (signed char *)"radioTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioTxTaskHandle)); - xTaskCreate(radioRxTask, (signed char *)"radioRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioRxTaskHandle)); if (PIOS_PPM_RECEIVER != 0) { xTaskCreate(PPMInputTask, (signed char *)"PPMInputTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->PPMInputTaskHandle)); #ifdef PIOS_INCLUDE_WDG PIOS_WDG_RegisterFlag(PIOS_WDG_PPMINPUT); #endif } + if (!data->parseUAVTalk) { + // If the user wants raw serial communication, we need to spawn another thread to handle it. + xTaskCreate(serialRxTask, (signed char *)"serialRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->serialRxTaskHandle)); +#ifdef PIOS_INCLUDE_WDG + PIOS_WDG_RegisterFlag(PIOS_WDG_SERIALRX); +#endif + } + xTaskCreate(radioTxTask, (signed char *)"radioTxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioTxTaskHandle)); + xTaskCreate(radioRxTask, (signed char *)"radioRxTask", STACK_SIZE_BYTES, NULL, TASK_PRIORITY, &(data->radioRxTaskHandle)); // Register the watchdog timers. #ifdef PIOS_INCLUDE_WDG @@ -264,28 +271,6 @@ static void telemetryTxTask(__attribute__((unused)) void *parameters) } } data->comTxRetries += retries; - } else if (ev.event == EV_SEND_ACK) { - // Send the ACK - uint32_t retries = 0; - int32_t success = -1; - while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendAck(data->telemUAVTalkCon, ev.obj, ev.instId) == 0; - if (!success) { - ++retries; - } - } - data->comTxRetries += retries; - } else if (ev.event == EV_SEND_NACK) { - // Send the NACK - uint32_t retries = 0; - int32_t success = -1; - while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendNack(data->telemUAVTalkCon, UAVObjGetID(ev.obj)) == 0; - if (!success) { - ++retries; - } - } - data->comTxRetries += retries; } } } @@ -320,28 +305,6 @@ static void radioTxTask(__attribute__((unused)) void *parameters) } } data->comTxRetries += retries; - } else if (ev.event == EV_SEND_ACK) { - // Send the ACK - uint32_t retries = 0; - int32_t success = -1; - while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendAck(data->radioUAVTalkCon, ev.obj, ev.instId) == 0; - if (!success) { - ++retries; - } - } - data->comTxRetries += retries; - } else if (ev.event == EV_SEND_NACK) { - // Send the NACK - uint32_t retries = 0; - int32_t success = -1; - while (retries < MAX_RETRIES && success == -1) { - success = UAVTalkSendNack(data->radioUAVTalkCon, UAVObjGetID(ev.obj)) == 0; - if (!success) { - ++retries; - } - } - data->comTxRetries += retries; } } } @@ -359,13 +322,17 @@ static void radioRxTask(__attribute__((unused)) void *parameters) #ifdef PIOS_INCLUDE_WDG PIOS_WDG_UpdateFlag(PIOS_WDG_RADIORX); #endif - uint8_t serial_data[1]; - uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, serial_data, sizeof(serial_data), MAX_PORT_DELAY); - if (bytes_to_process > 0) { - // Pass the data through the UAVTalk parser. - for (uint8_t i = 0; i < bytes_to_process; i++) { - ProcessRadioStream(data->radioUAVTalkCon, data->telemUAVTalkCon, serial_data[i]); + if (PIOS_COM_RADIO) { + uint8_t serial_data[1]; + uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, serial_data, sizeof(serial_data), MAX_PORT_DELAY); + if (bytes_to_process > 0) { + // Pass the data through the UAVTalk parser. + for (uint8_t i = 0; i < bytes_to_process; i++) { + ProcessRadioStream(data->radioUAVTalkCon, data->telemUAVTalkCon, serial_data[i]); + } } + } else { + vTaskDelay(5); } } } @@ -403,7 +370,6 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) } } - /** * @brief Reads the PPM input device and sends out OPLinkReceiver objects. * @@ -412,29 +378,54 @@ static void telemetryRxTask(__attribute__((unused)) void *parameters) static void PPMInputTask(__attribute__((unused)) void *parameters) { xSemaphoreHandle sem = PIOS_RCVR_GetSemaphore(PIOS_PPM_RECEIVER, 1); - OPLinkReceiverData opl_rcvr; + int16_t channels[RFM22B_PPM_NUM_CHANNELS]; - // Task loop while (1) { -#ifdef PIOS_INCLUDE_WDG + #ifdef PIOS_INCLUDE_WDG PIOS_WDG_UpdateFlag(PIOS_WDG_PPMINPUT); -#endif + #endif // Wait for the receiver semaphore. - bool valid_input_detected = false; - if (xSemaphoreTake(sem, MAX_PORT_DELAY) == pdTRUE) { + if (xSemaphoreTake(sem, PPM_INPUT_TIMEOUT) == pdTRUE) { // Read the receiver inputs. for (uint8_t i = 0; i < OPLINKRECEIVER_CHANNEL_NUMELEM; ++i) { - opl_rcvr.Channel[i] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i + 1); - if ((opl_rcvr.Channel[i] != PIOS_RCVR_INVALID) && (opl_rcvr.Channel[i] != PIOS_RCVR_TIMEOUT)) { - valid_input_detected = true; - } + channels[i] = PIOS_RCVR_Read(PIOS_PPM_RECEIVER, i + 1); + } + } else { + // Failsafe + for (uint8_t i = 0; i < OPLINKRECEIVER_CHANNEL_NUMELEM; ++i) { + channels[i] = PIOS_RCVR_INVALID; } } - // Set the receiver UAVO if we detected valid input. - if (valid_input_detected) { - OPLinkReceiverSet(&opl_rcvr); + // Pass the channel values to the radio device. + PIOS_RFM22B_PPMSet(pios_rfm22b_id, channels); + } +} + +/** + * @brief Receive raw serial data from the USB/COM port. + * + * @param[in] parameters The task parameters + */ +static void serialRxTask(__attribute__((unused)) void *parameters) +{ + // Task loop + while (1) { + uint32_t inputPort = PIOS_COM_TELEMETRY; +#ifdef PIOS_INCLUDE_WDG + PIOS_WDG_UpdateFlag(PIOS_WDG_SERIALRX); +#endif + if (inputPort && PIOS_COM_RADIO) { + // Receive some data. + uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(inputPort, data->serialRxBuf, sizeof(data->serialRxBuf), MAX_PORT_DELAY); + + // Send the data over the radio link. + if (bytes_to_process > 0) { + PIOS_COM_SendBufferNonBlocking(PIOS_COM_RADIO, data->serialRxBuf, bytes_to_process); + } + } else { + vTaskDelay(5); } } } @@ -599,19 +590,3 @@ static void objectPersistenceUpdatedCb(__attribute__((unused)) UAVObjEvent *objE } } } - -/** - * @brief Callback that is called when the OPLinkReceiver UAVObject is changed. - * @param[in] objEv The event that precipitated the callback. - */ -static void oplinkReceiverUpdatedCb(__attribute__((unused)) UAVObjEvent *objEv) -{ - // Get the OPLinkReceiver object. - OPLinkReceiverData opl_rcvr; - - OPLinkReceiverGet(&opl_rcvr); - - for (uint8_t i = 0; i < OPLINKRECEIVER_CHANNEL_NUMELEM; ++i) { - PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, opl_rcvr.Channel[i]); - } -} diff --git a/flight/modules/Telemetry/telemetry.c b/flight/modules/Telemetry/telemetry.c index 69ed7fa20..2ddb3f5aa 100644 --- a/flight/modules/Telemetry/telemetry.c +++ b/flight/modules/Telemetry/telemetry.c @@ -157,8 +157,8 @@ int32_t TelemetryInitialize(void) #endif // Create periodic event that will be used to update the telemetry stats - txErrors = 0; - txRetries = 0; + txErrors = 0; + txRetries = 0; UAVObjEvent ev; memset(&ev, 0, sizeof(UAVObjEvent)); EventPeriodicQueueCreate(&ev, priorityQueue, STATS_UPDATE_PERIOD_MS); diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index 466589b03..7ac514c84 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -34,15 +34,22 @@ // This module uses the RFM22B's internal packet handling hardware to // encapsulate our own packet data. // -// The RFM22B internal hardware packet handler configuration is as follows .. +// The RFM22B internal hardware packet handler configuration is as follows: // -// 4-byte (32-bit) preamble .. alternating 0's & 1's +// 6-byte (32-bit) preamble .. alternating 0's & 1's // 4-byte (32-bit) sync // 1-byte packet length (number of data bytes to follow) // 0 to 255 user data bytes +// 4 byte ECC // -// Our own packet data will also contain it's own header and 32-bit CRC -// as a single 16-bit CRC is not sufficient for wireless comms. +// OR in PPM only mode: +// +// 6-byte (32-bit) preamble .. alternating 0's & 1's +// 4-byte (32-bit) sync +// 1-byte packet length (number of data bytes to follow) +// 1 byte valid bitmask +// 8 PPM values (0-255) +// 1 byte CRC // // ***************************************************************** @@ -64,11 +71,10 @@ #define RFM22B_DEFAULT_TX_POWER RFM22_tx_pwr_txpow_0 #define RFM22B_NOMINAL_CARRIER_FREQUENCY 430000000 #define RFM22B_LINK_QUALITY_THRESHOLD 20 -#define RFM22B_DEFAULT_NUM_CHANNELS 1 #define RFM22B_DEFAULT_MIN_CHANNEL 0 #define RFM22B_DEFAULT_MAX_CHANNEL 250 #define RFM22B_DEFAULT_CHANNEL_SET 24 -#define RFM22B_DEFAULT_PACKET_PERIOD 15 +#define RFM22B_PPM_ONLY_DATARATE RFM22_datarate_9600 // The maximum amount of time without activity before initiating a reset. #define PIOS_RFM22B_SUPERVISOR_TIMEOUT 100 // ms @@ -80,6 +86,7 @@ #define RX_PREAMBLE_NIBBLES 6 // 5 to 31 (number of nibbles) #define SYNC_BYTES 4 #define HEADER_BYTES 4 +#define LENGTH_BYTES 1 // the size of the rf modules internal FIFO buffers #define FIFO_SIZE 64 @@ -217,12 +224,13 @@ static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_ST [RADIO_STATE_RX_MODE] = { .entry_fn = radio_setRxMode, .next_state = { - [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, [RADIO_EVENT_INT_RECEIVED] = RADIO_STATE_RX_DATA, - [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, - [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, - [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, - [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, + [RADIO_EVENT_TX_START] = RADIO_STATE_TX_START, + [RADIO_EVENT_RX_MODE] = RADIO_STATE_RX_MODE, + [RADIO_EVENT_TIMEOUT] = RADIO_STATE_TIMEOUT, + [RADIO_EVENT_ERROR] = RADIO_STATE_ERROR, + [RADIO_EVENT_INITIALIZE] = RADIO_STATE_INITIALIZING, + [RADIO_EVENT_FATAL_ERROR] = RADIO_STATE_FATAL_ERROR, }, }, [RADIO_STATE_RX_DATA] = { @@ -294,33 +302,37 @@ static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_ST }; // xtal 10 ppm, 434MHz -static const uint32_t data_rate[] = { 9600, 19200, 32000, 57600, 64000, 128000, 192000, 256000 }; -static const uint8_t modulation_index[] = { 1, 1, 1, 1, 1, 1, 1, 1 }; +static const uint32_t data_rate[] = { 9600, 19200, 32000, 57600, 128000, 192000, 256000 }; +static const uint8_t modulation_index[] = { 1, 1, 1, 1, 1, 1, 1 }; -static const uint8_t reg_1C[] = { 0x01, 0x05, 0x16, 0x06, 0x07, 0x83, 0x8A, 0x8C }; // rfm22_if_filter_bandwidth +static const uint8_t reg_1C[] = { 0x01, 0x05, 0x16, 0x06, 0x83, 0x8A, 0x8C }; // rfm22_if_filter_bandwidth -static const uint8_t reg_1D[] = { 0x40, 0x40, 0x44, 0x40, 0x44, 0x44, 0x44, 0x44 }; // rfm22_afc_loop_gearshift_override -static const uint8_t reg_1E[] = { 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x02 }; // rfm22_afc_timing_control +static const uint8_t reg_1D[] = { 0x40, 0x40, 0x44, 0x40, 0x44, 0x44, 0x44 }; // rfm22_afc_loop_gearshift_override +static const uint8_t reg_1E[] = { 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x02 }; // rfm22_afc_timing_control -static const uint8_t reg_1F[] = { 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03 }; // rfm22_clk_recovery_gearshift_override -static const uint8_t reg_20[] = { 0xA1, 0xD0, 0x3F, 0x45, 0x3F, 0x5E, 0x3F, 0x2F }; // rfm22_clk_recovery_oversampling_ratio -static const uint8_t reg_21[] = { 0x20, 0x00, 0x02, 0x01, 0x02, 0x01, 0x02, 0x02 }; // rfm22_clk_recovery_offset2 +static const uint8_t reg_1F[] = { 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03 }; // rfm22_clk_recovery_gearshift_override +static const uint8_t reg_20[] = { 0xA1, 0xD0, 0x3F, 0x45, 0x5E, 0x3F, 0x2F }; // rfm22_clk_recovery_oversampling_ratio +static const uint8_t reg_21[] = { 0x20, 0x00, 0x02, 0x01, 0x01, 0x02, 0x02 }; // rfm22_clk_recovery_offset2 static const uint8_t reg_22[] = { 0x4E, 0x9D, 0x0C, 0xD7, 0x0c, 0x5D, 0x0C, 0xBB }; // rfm22_clk_recovery_offset1 -static const uint8_t reg_23[] = { 0xA5, 0x49, 0x4A, 0xDC, 0x4A, 0x86, 0x4A, 0x0D }; // rfm22_clk_recovery_offset0 +static const uint8_t reg_23[] = { 0xA5, 0x49, 0x4A, 0xDC, 0x86, 0x4A, 0x0D }; // rfm22_clk_recovery_offset0 static const uint8_t reg_24[] = { 0x00, 0x00, 0x07, 0x07, 0x07, 0x05, 0x07, 0x07 }; // rfm22_clk_recovery_timing_loop_gain1 -static const uint8_t reg_25[] = { 0x34, 0x8, 0xFF, 0x6E, 0xFF, 0x74, 0xFF, 0xFF }; // rfm22_clk_recovery_timing_loop_gain0 +static const uint8_t reg_25[] = { 0x34, 0x8, 0xFF, 0x6E, 0x74, 0xFF, 0xFF }; // rfm22_clk_recovery_timing_loop_gain0 -static const uint8_t reg_2A[] = { 0x1E, 0x24, 0x17, 0x2D, 0x31, 0x50, 0x50, 0x50 }; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz +static const uint8_t reg_2A[] = { 0x1E, 0x24, 0x17, 0x2D, 0x50, 0x50, 0x50 }; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz -static const uint8_t reg_58[] = { 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80 }; // rfm22_cpcuu -static const uint8_t reg_69[] = { 0x60, 0x60, 0x20, 0x60, 0x20, 0x20, 0x20, 0x20 }; // rfm22_agc_override1 -static const uint8_t reg_6E[] = { 0x4E, 0x9D, 0x08, 0x0E, 0x10, 0x20, 0x31, 0x41 }; // rfm22_tx_data_rate1 -static const uint8_t reg_6F[] = { 0xA5, 0x49, 0x31, 0xBF, 0x62, 0xC5, 0x27, 0x89 }; // rfm22_tx_data_rate0 +static const uint8_t reg_58[] = { 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80 }; // rfm22_cpcuu +static const uint8_t reg_69[] = { 0x60, 0x60, 0x20, 0x60, 0x20, 0x20, 0x20 }; // rfm22_agc_override1 +static const uint8_t reg_6E[] = { 0x4E, 0x9D, 0x08, 0x0E, 0x20, 0x31, 0x41 }; // rfm22_tx_data_rate1 +static const uint8_t reg_6F[] = { 0xA5, 0x49, 0x31, 0xBF, 0xC5, 0x27, 0x89 }; // rfm22_tx_data_rate0 -static const uint8_t reg_70[] = { 0x2C, 0x2C, 0x0D, 0x0C, 0x0C, 0x0D, 0x0D, 0x0D }; // rfm22_modulation_mode_control1 -static const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23 }; // rfm22_modulation_mode_control2 +static const uint8_t reg_70[] = { 0x2C, 0x2C, 0x0D, 0x0C, 0x0D, 0x0D, 0x0D }; // rfm22_modulation_mode_control1 +static const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23 }; // rfm22_modulation_mode_control2 -static const uint8_t reg_72[] = { 0x30, 0x48, 0x1A, 0x2E, 0x33, 0x66, 0x9A, 0xCD }; // rfm22_frequency_deviation +static const uint8_t reg_72[] = { 0x30, 0x48, 0x1A, 0x2E, 0x66, 0x9A, 0xCD }; // rfm22_frequency_deviation + +static const uint8_t packet_time[] = { 80, 40, 25, 15, 8, 6, 5 }; +static const uint8_t packet_time_ppm[] = { 26, 25, 25, 15, 8, 6, 5 }; +static const uint8_t num_channels[] = { 3, 3, 4, 6, 9, 12, 15 }; static struct pios_rfm22b_dev *g_rfm22b_dev = NULL; @@ -364,6 +376,9 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->rx_in_cb = NULL; rfm22b_dev->tx_out_cb = NULL; + // Initialzie the PPM callback. + rfm22b_dev->ppm_callback = NULL; + // Initialize the stats. rfm22b_dev->stats.packets_per_sec = 0; rfm22b_dev->stats.rx_good = 0; @@ -381,7 +396,8 @@ int32_t PIOS_RFM22B_Init(uint32_t *rfm22b_id, uint32_t spi_id, uint32_t slave_nu rfm22b_dev->stats.tx_failure = 0; // Initialize the channels. - PIOS_RFM22B_SetChannelConfig(*rfm22b_id, RFM22B_DEFAULT_NUM_CHANNELS, RFM22B_DEFAULT_MIN_CHANNEL, RFM22B_DEFAULT_MAX_CHANNEL, RFM22B_DEFAULT_CHANNEL_SET, RFM22B_DEFAULT_PACKET_PERIOD, false); + PIOS_RFM22B_SetChannelConfig(*rfm22b_id, RFM22B_DEFAULT_RX_DATARATE, RFM22B_DEFAULT_MIN_CHANNEL, + RFM22B_DEFAULT_MAX_CHANNEL, RFM22B_DEFAULT_CHANNEL_SET, false, false, false, false); // Create the event queue rfm22b_dev->eventQueue = xQueueCreate(EVENT_QUEUE_SIZE, sizeof(enum pios_radio_event)); @@ -521,33 +537,54 @@ void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr) * The channel spacing is 10MHz / 250 = 40kHz * * @param[in] rfm22b_id The RFM22B device index. - * @param[in] num_channels The number of channels to use for frequency hopping. + * @param[in] datarate The desired datarate. * @param[in] min_chan The minimum channel. * @param[in] max_chan The maximum channel. * @param[in] chan_set The "seed" for selecting a channel sequence. - * @param[in] packet_period The fixed time alloted to sending a single packet + * @param[in] coordinator Is this modem an coordinator. + * @param[in] ppm_mode Should this modem send/receive ppm packets? * @param[in] oneway Only the coordinator can send packets if true. */ -void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, uint8_t num_chan, uint8_t min_chan, uint8_t max_chan, uint8_t chan_set, uint8_t packet_period, bool oneway) +void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datarate, uint8_t min_chan, uint8_t max_chan, uint8_t chan_set, bool coordinator, bool oneway, bool ppm_mode, bool ppm_only) { struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return; } - rfm22b_dev->packet_period = packet_period; - rfm22b_dev->num_channels = num_chan; + ppm_mode = ppm_mode || ppm_only; + rfm22b_dev->coordinator = coordinator; rfm22b_dev->one_way_link = oneway; + rfm22b_dev->ppm_send_mode = ppm_mode && coordinator; + rfm22b_dev->ppm_recv_mode = ppm_mode && !coordinator; + if (ppm_mode && (datarate <= RFM22B_PPM_ONLY_DATARATE)) { + ppm_only = true; + } + rfm22b_dev->ppm_only_mode = ppm_only; + if (ppm_only) { + rfm22b_dev->datarate = RFM22B_PPM_ONLY_DATARATE; + } else { + rfm22b_dev->datarate = datarate; + } + rfm22b_dev->packet_time = (ppm_mode ? packet_time_ppm[datarate] : packet_time[datarate]); // Find the first N channels that meet the min/max criteria out of the random channel list. uint8_t num_found = 0; - for (uint16_t i = 0; (i < RFM22B_NUM_CHANNELS) && (num_found < num_chan); ++i) { + for (uint16_t i = 0; (i < RFM22B_NUM_CHANNELS) && (num_found < num_channels[datarate]); ++i) { uint8_t idx = (i + chan_set) % RFM22B_NUM_CHANNELS; uint8_t chan = channel_list[idx]; if ((chan >= min_chan) && (chan <= max_chan)) { rfm22b_dev->channels[num_found++] = chan; } } + + // Calculate the maximum packet length from the datarate. + float bytes_per_period = (float)data_rate[datarate] * (float)(rfm22b_dev->packet_time - 2) / 9000; + + rfm22b_dev->max_packet_len = bytes_per_period - TX_PREAMBLE_NIBBLES / 2 - SYNC_BYTES - HEADER_BYTES - LENGTH_BYTES; + if (rfm22b_dev->max_packet_len > RFM22B_MAX_PACKET_LEN) { + rfm22b_dev->max_packet_len = RFM22B_MAX_PACKET_LEN; + } } /** @@ -656,11 +693,6 @@ bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, uint8_t *p) if (!PIOS_RFM22B_Validate(rfm22b_dev)) { return false; } - - // Are we already in Rx mode? - if ((rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_MODE) || (rfm22b_dev->rfm22b_state == RFM22B_STATE_RX_WAIT)) { - return false; - } rfm22b_dev->rx_packet_handle = p; // Claim the SPI bus. @@ -1005,6 +1037,61 @@ pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id) return ret; } +/** + * Set the PPM packet received callback. + * + * @param[in] rfm22b_dev The RFM22B device ID. + * @param[in] cb The callback function pointer. + */ +void PIOS_RFM22B_SetPPMCallback(uint32_t rfm22b_id, PPMReceivedCallback cb) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { + return; + } + + rfm22b_dev->ppm_callback = cb; +} + +/** + * Set the PPM values to be transmitted. + * + * @param[in] rfm22b_dev The RFM22B device ID. + * @param[in] channels The PPM channel values. + */ +extern void PIOS_RFM22B_PPMSet(uint32_t rfm22b_id, int16_t *channels) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { + return; + } + + for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) { + rfm22b_dev->ppm[i] = channels[i]; + } +} + +/** + * Fetch the PPM values that have been received. + * + * @param[in] rfm22b_dev The RFM22B device structure pointer. + * @param[out] channels The PPM channel values. + */ +extern void PIOS_RFM22B_PPMGet(uint32_t rfm22b_id, int16_t *channels) +{ + struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; + + if (!PIOS_RFM22B_Validate(rfm22b_dev)) { + return; + } + + for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) { + channels[i] = rfm22b_dev->ppm[i]; + } +} + /** * Validate that the device structure is valid. * @@ -1057,42 +1144,44 @@ static void pios_rfm22_task(void *parameters) // Has it been too long since the last event? portTickType curTicks = xTaskGetTickCount(); if (pios_rfm22_time_difference_ms(lastEventTicks, curTicks) > PIOS_RFM22B_SUPERVISOR_TIMEOUT) { - // Transsition through an error event. - rfm22_process_event(rfm22b_dev, RADIO_EVENT_ERROR); - // Clear the event queue. enum pios_radio_event event; while (xQueueReceive(rfm22b_dev->eventQueue, &event, 0) == pdTRUE) { // Do nothing; } lastEventTicks = xTaskGetTickCount(); + + // Transsition through an error event. + rfm22_process_event(rfm22b_dev, RADIO_EVENT_ERROR); } } // Change channels if necessary. - rfm22_changeChannel(rfm22b_dev); + if (rfm22_changeChannel(rfm22b_dev)) { + rfm22_process_event(rfm22b_dev, RADIO_EVENT_RX_MODE); + } portTickType curTicks = xTaskGetTickCount(); // Have we been sending / receiving this packet too long? + if ((rfm22b_dev->packet_start_ticks > 0) && - (pios_rfm22_time_difference_ms(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->packet_period * 3))) { + (pios_rfm22_time_difference_ms(rfm22b_dev->packet_start_ticks, curTicks) > (rfm22b_dev->packet_time * 3))) { rfm22_process_event(rfm22b_dev, RADIO_EVENT_TIMEOUT); } - // Send a packet if it's our time slice + // Start transmitting a packet if it's time. bool time_to_send = rfm22_timeToSend(rfm22b_dev); -#ifdef PIOS_RFM22B_DEBUG_ON_TELEM + #ifdef PIOS_RFM22B_DEBUG_ON_TELEM if (time_to_send) { D4_LED_ON; } else { D4_LED_OFF; } -#endif + #endif - // Start transmitting a packet if it's time. if (time_to_send && PIOS_RFM22B_InRxWait((uint32_t)rfm22b_dev)) { // If the on_sync_channel flag is set, it means that we were on the sync channel, but no packet was received, so transition to a non-connected state. - if (rfm22b_dev->on_sync_channel) { + if (!rfm22_isCoordinator(rfm22b_dev) && rfm22b_dev->on_sync_channel) { rfm22b_dev->on_sync_channel = false; if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) { rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTING; @@ -1234,7 +1323,7 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) } // Initialize the state - rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; + rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_ENABLED; // Initialize the packets. rfm22b_dev->rx_packet_len = 0; @@ -1442,16 +1531,7 @@ static enum pios_radio_event rfm22_init(struct pios_rfm22b_dev *rfm22b_dev) static void pios_rfm22_setDatarate(struct pios_rfm22b_dev *rfm22b_dev) { enum rfm22b_datarate datarate = rfm22b_dev->datarate; - bool data_whitening = true; - uint32_t datarate_bps = data_rate[datarate]; - - // Calculate the maximum packet length from the datarate. - float bytes_per_period = (float)datarate_bps * (float)(rfm22b_dev->packet_period) / 9000; - - rfm22b_dev->max_packet_len = bytes_per_period - TX_PREAMBLE_NIBBLES / 2 - SYNC_BYTES - HEADER_BYTES - 5; - if (rfm22b_dev->max_packet_len > RFM22B_MAX_PACKET_LEN) { - rfm22b_dev->max_packet_len = RFM22B_MAX_PACKET_LEN; - } + bool data_whitening = true; // Claim the SPI bus. rfm22_claimBus(rfm22b_dev); @@ -1649,9 +1729,9 @@ static void rfm22_rxFailure(struct pios_rfm22b_dev *rfm22b_dev) */ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev) { - uint8_t *p = radio_dev->rx_packet; + uint8_t *p = radio_dev->tx_packet; uint8_t len = 0; - uint8_t max_data_len = radio_dev->max_packet_len - RS_ECC_NPARITY; + uint8_t max_data_len = radio_dev->max_packet_len - (radio_dev->ppm_only_mode ? 0 : RS_ECC_NPARITY); // Don't send if it's not our turn, or if we're receiving a packet. if (!rfm22_timeToSend(radio_dev) || !PIOS_RFM22B_InRxWait((uint32_t)radio_dev)) { @@ -1663,10 +1743,44 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev) return RADIO_EVENT_RX_MODE; } - // Try to get some data to send - if (radio_dev->tx_out_cb) { + // Should we append PPM data to the packet? + if (radio_dev->ppm_send_mode) { + len = RFM22B_PPM_NUM_CHANNELS + (radio_dev->ppm_only_mode ? 2 : 1); + + // Ensure we can fit the PPM data in the packet. + if (max_data_len < len) { + return RADIO_EVENT_RX_MODE; + } + + // The first byte is a bitmask of valid channels. + p[0] = 0; + + // Read the PPM input. + for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) { + int32_t val = radio_dev->ppm[i]; + if ((val == PIOS_RCVR_INVALID) || (val == PIOS_RCVR_TIMEOUT)) { + p[i + 1] = 0; + } else { + p[0] |= 1 << i; + p[i + 1] = (val < 1000) ? 0 : ((val >= 1900) ? 255 : (uint8_t)(256 * (val - 1000) / 900)); + } + } + + // The last byte is a CRC. + if (radio_dev->ppm_only_mode) { + uint8_t crc = 0; + for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS + 1; ++i) { + crc = PIOS_CRC_updateByte(crc, p[i]); + } + p[RFM22B_PPM_NUM_CHANNELS + 1] = crc; + } + } + + // Append data from the com interface if applicable. + if (!radio_dev->ppm_only_mode && radio_dev->tx_out_cb) { + // Try to get some data to send bool need_yield = false; - len = (radio_dev->tx_out_cb)(radio_dev->tx_out_context, p, max_data_len, NULL, &need_yield); + len += (radio_dev->tx_out_cb)(radio_dev->tx_out_context, p + len, max_data_len - len, NULL, &need_yield); } // Always send a packet on the sync channel if this modem is a coordinator. @@ -1677,14 +1791,13 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev) // Increment the packet sequence number. radio_dev->stats.tx_seq++; - // Change the channel. - rfm22_changeChannel(radio_dev); - // Add the error correcting code. - if (len != 0) { - encode_data((unsigned char *)p, len, (unsigned char *)p); + if (!radio_dev->ppm_only_mode) { + if (len != 0) { + encode_data((unsigned char *)p, len, (unsigned char *)p); + } + len += RS_ECC_NPARITY; } - len += RS_ECC_NPARITY; // Transmit the packet. PIOS_RFM22B_TransmitPacket((uint32_t)radio_dev, p, len); @@ -1693,7 +1806,7 @@ static enum pios_radio_event radio_txStart(struct pios_rfm22b_dev *radio_dev) } /** - * Receive packet data. + * Transmit packet data. * * @param[in] rfm22b_dev The device structure * @return enum pios_radio_event The next event to inject @@ -1749,19 +1862,66 @@ static enum pios_radio_event radio_setRxMode(struct pios_rfm22b_dev *rfm22b_dev) */ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_dev, uint8_t *p, uint16_t rx_len) { - uint8_t data_len = rx_len - RS_ECC_NPARITY; - // Attempt to correct any errors in the packet. bool good_packet = true; bool corrected_packet = false; + uint8_t data_len = rx_len; - if (data_len > 0) { - decode_data((unsigned char *)p, rx_len); + // We don't rsencode ppm only packets. + if (!radio_dev->ppm_only_mode) { + data_len -= RS_ECC_NPARITY; - good_packet = check_syndrome() == 0; - // We have an error. Try to correct it. - if (!good_packet && (correct_errors_erasures((unsigned char *)p, rx_len, 0, 0) != 0)) { - // We corrected it - corrected_packet = true; + // Attempt to correct any errors in the packet. + if (data_len > 0) { + decode_data((unsigned char *)p, rx_len); + good_packet = check_syndrome() == 0; + + // We have an error. Try to correct it. + if (!good_packet && (correct_errors_erasures((unsigned char *)p, rx_len, 0, 0) != 0)) { + // We corrected it + corrected_packet = true; + } + } + } + + // Should we pull PPM data off of the head of the packet? + if ((good_packet || corrected_packet) && radio_dev->ppm_recv_mode) { + uint8_t ppm_len = RFM22B_PPM_NUM_CHANNELS + (radio_dev->ppm_only_mode ? 2 : 1); + + // Ensure the packet it long enough + if (data_len < ppm_len) { + good_packet = false; + } + + // Verify the CRC if this is a PPM only packet. + if ((good_packet || corrected_packet) && radio_dev->ppm_only_mode) { + uint8_t crc = 0; + for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS + 1; ++i) { + crc = PIOS_CRC_updateByte(crc, p[i]); + } + if (p[RFM22B_PPM_NUM_CHANNELS + 1] != crc) { + good_packet = false; + corrected_packet = false; + } + } + + if (good_packet || corrected_packet) { + for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) { + // Is this a valid channel? + if (p[0] & (1 << i)) { + uint32_t val = p[i + 1]; + radio_dev->ppm[i] = (uint16_t)(1000 + val * 900 / 256); + } else { + radio_dev->ppm[i] = PIOS_RCVR_INVALID; + } + } + + p += RFM22B_PPM_NUM_CHANNELS + 1; + data_len -= RFM22B_PPM_NUM_CHANNELS + 1; + + // Call the PPM received callback if it's available. + if (radio_dev->ppm_callback) { + radio_dev->ppm_callback(radio_dev->ppm); + } } } @@ -1780,7 +1940,7 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d if (good_packet || corrected_packet) { // Send the data to the com port bool rx_need_yield; - if (radio_dev->rx_in_cb) { + if (radio_dev->rx_in_cb && (data_len > 0) && !radio_dev->ppm_only_mode) { (radio_dev->rx_in_cb)(radio_dev->rx_in_context, p, data_len, NULL, &rx_need_yield); } @@ -1985,10 +2145,14 @@ static void rfm22_synchronizeClock(struct pios_rfm22b_dev *rfm22b_dev) portTickType start_time = rfm22b_dev->packet_start_ticks; // This packet was transmitted on channel 0, calculate the time delta that will force us to transmit on channel 0 at the time this packet started. - uint16_t frequency_hop_cycle_time = rfm22b_dev->packet_period * rfm22b_dev->num_channels; + uint8_t num_chan = num_channels[rfm22b_dev->datarate]; + uint16_t frequency_hop_cycle_time = rfm22b_dev->packet_time * num_chan; uint16_t time_delta = start_time % frequency_hop_cycle_time; - rfm22b_dev->time_delta = frequency_hop_cycle_time - time_delta + 1; + // Calculate the adjustment for the preamble + uint8_t offset = (uint8_t)ceil(35000.0F / data_rate[rfm22b_dev->datarate]); + + rfm22b_dev->time_delta = frequency_hop_cycle_time - time_delta + offset; } /** @@ -2012,24 +2176,26 @@ static portTickType rfm22_coordinatorTime(struct pios_rfm22b_dev *rfm22b_dev, po */ static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev) { - portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); - bool is_coordinator = rfm22_isCoordinator(rfm22b_dev); + portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); + bool is_coordinator = rfm22_isCoordinator(rfm22b_dev); // If this is a one-way link, only the coordinator can send. + uint8_t packet_period = rfm22b_dev->packet_time; + if (rfm22b_dev->one_way_link) { if (is_coordinator) { - return ((time -1) % (rfm22b_dev->packet_period)) == 0; + return ((time - 1) % (packet_period)) == 0; } else { return false; } } if (!is_coordinator) { - time += rfm22b_dev->packet_period - 1; + time += packet_period - 1; } else { time -= 1; } - return (time % (rfm22b_dev->packet_period * 2)) == 0; + return (time % (packet_period * 2)) == 0; } /** @@ -2041,11 +2207,14 @@ static bool rfm22_timeToSend(struct pios_rfm22b_dev *rfm22b_dev) static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t index) { // Make sure we don't index outside of the range. - uint8_t idx = index % rfm22b_dev->num_channels; + uint8_t num_chan = num_channels[rfm22b_dev->datarate]; + uint8_t idx = index % num_chan; - rfm22b_dev->channel_index = idx; - if (idx == 0) { - rfm22b_dev->on_sync_channel = true; + if (idx != rfm22b_dev->channel_index) { + rfm22b_dev->channel_index = idx; + if (idx == 0) { + rfm22b_dev->on_sync_channel = true; + } } return rfm22b_dev->channels[idx]; } @@ -2060,7 +2229,8 @@ static uint8_t rfm22_calcChannelFromClock(struct pios_rfm22b_dev *rfm22b_dev) portTickType time = rfm22_coordinatorTime(rfm22b_dev, xTaskGetTickCount()); // Divide time into 8ms blocks. Coordinator sends in first 2 ms, and remote send in 5th and 6th ms. // Channel changes occur in the last 2 ms. - uint8_t n = (time / rfm22b_dev->packet_period) % rfm22b_dev->num_channels; + uint8_t num_chan = num_channels[rfm22b_dev->datarate]; + uint8_t n = (time / rfm22b_dev->packet_time) % num_chan; return rfm22_calcChannel(rfm22b_dev, n); } diff --git a/flight/pios/common/pios_rfm22b_com.c b/flight/pios/common/pios_rfm22b_com.c index 9812ff366..964cee585 100644 --- a/flight/pios/common/pios_rfm22b_com.c +++ b/flight/pios/common/pios_rfm22b_com.c @@ -55,31 +55,12 @@ const struct pios_com_driver pios_rfm22b_com_driver = { /** * Changes the baud rate of the RFM22B peripheral without re-initialising. * - * @param[in] rfm22b_id RFM22B name (GPS, TELEM, AUX) + * @param[in] rfm22b_id The defice ID * @param[in] baud Requested baud rate */ -static void PIOS_RFM22B_COM_ChangeBaud(uint32_t rfm22b_id, uint32_t baud) -{ - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; - - if (!PIOS_RFM22B_Validate(rfm22b_dev)) { - return; - } - // Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex. - enum rfm22b_datarate datarate = RFM22_datarate_64000; - if (baud <= 9600) { - datarate = RFM22_datarate_19200; - } else if (baud <= 19200) { - datarate = RFM22_datarate_32000; - } else if (baud <= 38400) { - datarate = RFM22_datarate_57600; - } else if (baud <= 57600) { - datarate = RFM22_datarate_128000; - } else if (baud <= 115200) { - datarate = RFM22_datarate_192000; - } - rfm22b_dev->datarate = datarate; -} +static void PIOS_RFM22B_COM_ChangeBaud(__attribute__((unused)) uint32_t rfm22b_id, + __attribute__((unused)) uint32_t baud) +{} /** * Start a receive from the COM device @@ -97,14 +78,10 @@ static void PIOS_RFM22B_COM_RxStart(__attribute__((unused)) uint32_t rfm22b_id, * @param[in] rfm22b_dev The device ID. * @param[in] tx_bytes_available The number of bytes available to transmit */ -static void PIOS_RFM22B_COM_TxStart(uint32_t rfm22b_id, __attribute__((unused)) uint16_t tx_bytes_avail) -{ - struct pios_rfm22b_dev *rfm22b_dev = (struct pios_rfm22b_dev *)rfm22b_id; +static void PIOS_RFM22B_COM_TxStart(__attribute__((unused)) uint32_t rfm22b_id, + __attribute__((unused)) uint16_t tx_bytes_avail) +{} - if (!PIOS_RFM22B_Validate(rfm22b_dev)) { - return; - } -} /** * Register the callback to pass received data to diff --git a/flight/pios/inc/pios_rfm22b.h b/flight/pios/inc/pios_rfm22b.h index c5663ea6d..856c8096b 100644 --- a/flight/pios/inc/pios_rfm22b.h +++ b/flight/pios/inc/pios_rfm22b.h @@ -36,6 +36,7 @@ /* Constant definitions */ enum gpio_direction { GPIO0_TX_GPIO1_RX, GPIO0_RX_GPIO1_TX }; +#define RFM22B_PPM_NUM_CHANNELS 8 /* Global Types */ struct pios_rfm22b_cfg { @@ -45,6 +46,7 @@ struct pios_rfm22b_cfg { uint8_t slave_num; enum gpio_direction gpio_direction; }; +typedef void (*PPMReceivedCallback)(const int16_t *channels); enum rfm22b_tx_power { RFM22_tx_pwr_txpow_0 = 0x00, // +1dBm .. 1.25mW @@ -101,8 +103,7 @@ struct rfm22b_stats { extern int32_t PIOS_RFM22B_Init(uint32_t *rfb22b_id, uint32_t spi_id, uint32_t slave_num, const struct pios_rfm22b_cfg *cfg); extern void PIOS_RFM22B_Reinit(uint32_t rfb22b_id); extern void PIOS_RFM22B_SetTxPower(uint32_t rfm22b_id, enum rfm22b_tx_power tx_pwr); -extern void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, uint8_t num_chan, uint8_t min_chan, uint8_t max_chan, uint8_t chan_set, uint8_t packet_period, bool oneway); -extern void PIOS_RFM22B_SetCoordinator(uint32_t rfm22b_id, bool coordinator); +extern void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datarate, uint8_t min_chan, uint8_t max_chan, uint8_t chan_set, bool coordinator, bool oneway, bool ppm_mode, bool ppm_only); extern void PIOS_RFM22B_SetCoordinatorID(uint32_t rfm22b_id, uint32_t coord_id); extern uint32_t PIOS_RFM22B_DeviceID(uint32_t rfb22b_id); extern void PIOS_RFM22B_GetStats(uint32_t rfm22b_id, struct rfm22b_stats *stats); @@ -113,6 +114,9 @@ extern bool PIOS_RFM22B_ReceivePacket(uint32_t rfm22b_id, uint8_t *p); extern bool PIOS_RFM22B_TransmitPacket(uint32_t rfm22b_id, uint8_t *p, uint8_t len); extern pios_rfm22b_int_result PIOS_RFM22B_ProcessTx(uint32_t rfm22b_id); extern pios_rfm22b_int_result PIOS_RFM22B_ProcessRx(uint32_t rfm22b_id); +extern void PIOS_RFM22B_SetPPMCallback(uint32_t rfm22b_id, PPMReceivedCallback cb); +extern void PIOS_RFM22B_PPMSet(uint32_t rfm22b_id, int16_t *channels); +extern void PIOS_RFM22B_PPMGet(uint32_t rfm22b_id, int16_t *channels); /* Global Variables */ extern const struct pios_com_driver pios_rfm22b_com_driver; diff --git a/flight/pios/inc/pios_rfm22b_priv.h b/flight/pios/inc/pios_rfm22b_priv.h index 3823fbcc4..bacc2dcda 100644 --- a/flight/pios/inc/pios_rfm22b_priv.h +++ b/flight/pios/inc/pios_rfm22b_priv.h @@ -718,38 +718,50 @@ struct pios_rfm22b_dev { struct rfm22b_stats stats; // Stats - uint16_t errors; + uint16_t errors; // RSSI in dBm - int8_t rssi_dBm; + int8_t rssi_dBm; // The tx data packet - uint8_t tx_packet[RFM22B_MAX_PACKET_LEN]; + uint8_t tx_packet[RFM22B_MAX_PACKET_LEN]; // The current tx packet - uint8_t *tx_packet_handle; + uint8_t *tx_packet_handle; // The tx data read index - uint16_t tx_data_rd; + uint16_t tx_data_rd; // The tx data write index - uint16_t tx_data_wr; + uint16_t tx_data_wr; // The tx packet sequence number - uint16_t tx_seq; + uint16_t tx_seq; // The rx data packet - uint8_t rx_packet[RFM22B_MAX_PACKET_LEN]; + uint8_t rx_packet[RFM22B_MAX_PACKET_LEN]; // The rx data packet - uint8_t *rx_packet_handle; + uint8_t *rx_packet_handle; // The receive buffer write index - uint16_t rx_buffer_wr; + uint16_t rx_buffer_wr; // The receive buffer write index - uint16_t rx_packet_len; + uint16_t rx_packet_len; + + // The PPM buffer + int16_t ppm[RFM22B_PPM_NUM_CHANNELS]; + // The PPM packet received callback. + PPMReceivedCallback ppm_callback; + // The id that the packet was received from uint32_t rx_destination_id; // The maximum packet length (including header, etc.) uint8_t max_packet_len; - // The time alloted for transmission of a packet. - uint8_t packet_period; + // The packet transmit time in ms. + uint8_t packet_time; // Do all packets originate from the coordinator modem? bool one_way_link; + // Should this modem send PPM data? + bool ppm_send_mode; + // Should this modem receive PPM data? + bool ppm_recv_mode; + // Are we sending / receiving only PPM data? + bool ppm_only_mode; // The channel list uint8_t channels[RFM22B_NUM_CHANNELS]; diff --git a/flight/targets/boards/oplinkmini/firmware/pios_board.c b/flight/targets/boards/oplinkmini/firmware/pios_board.c index 01d141ab0..60178f9fa 100644 --- a/flight/targets/boards/oplinkmini/firmware/pios_board.c +++ b/flight/targets/boards/oplinkmini/firmware/pios_board.c @@ -29,6 +29,7 @@ #include "inc/openpilot.h" #include +#include #include #include @@ -80,6 +81,7 @@ static void PIOS_InitUartMainPort(); static void PIOS_InitUartFlexiPort(); static void PIOS_InitPPMMainPort(bool input); static void PIOS_InitPPMFlexiPort(bool input); +static void PIOS_Board_PPM_callback(const int16_t *channels); /** * PIOS_Board_Init() @@ -121,7 +123,10 @@ void PIOS_Board_Init(void) PIOS_RTC_Init(&pios_rtc_main_cfg); #endif /* PIOS_INCLUDE_RTC */ +#if defined(PIOS_INCLUDE_RFM22B) OPLinkSettingsInitialize(); + OPLinkStatusInitialize(); +#endif /* PIOS_INCLUDE_RFM22B */ #if defined(PIOS_INCLUDE_LED) PIOS_LED_Init(&pios_led_cfg); @@ -218,13 +223,72 @@ void PIOS_Board_Init(void) } #endif + /* Allocate the uart buffers. */ + pios_uart_rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RX_BUF_LEN); + pios_uart_tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_TX_BUF_LEN); + + // Configure the main port + bool is_coordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); + bool is_oneway = (oplinkSettings.OneWay == OPLINKSETTINGS_ONEWAY_TRUE); + bool ppm_only = (oplinkSettings.PPMOnly == OPLINKSETTINGS_PPMONLY_TRUE); + bool ppm_mode = false; + switch (oplinkSettings.MainPort) { + case OPLINKSETTINGS_MAINPORT_TELEMETRY: + case OPLINKSETTINGS_MAINPORT_SERIAL: + /* Configure the main port for uart serial */ + PIOS_InitUartMainPort(); + PIOS_COM_TELEMETRY = PIOS_COM_TELEM_UART_MAIN; + break; + case OPLINKSETTINGS_MAINPORT_PPM: + PIOS_InitPPMMainPort(is_coordinator); + ppm_mode = true; + break; + case OPLINKSETTINGS_MAINPORT_DISABLED: + break; + } + + // Configure the flexi port + switch (oplinkSettings.FlexiPort) { + case OPLINKSETTINGS_FLEXIPORT_TELEMETRY: + case OPLINKSETTINGS_FLEXIPORT_SERIAL: + /* Configure the flexi port as uart serial */ + PIOS_InitUartFlexiPort(); + PIOS_COM_TELEMETRY = PIOS_COM_TELEM_UART_FLEXI; + break; + case OPLINKSETTINGS_FLEXIPORT_PPM: + PIOS_InitPPMFlexiPort(is_coordinator); + ppm_mode = true; + break; + case OPLINKSETTINGS_FLEXIPORT_DISABLED: + break; + } + + // Configure the USB VCP port + switch (oplinkSettings.VCPPort) { + case OPLINKSETTINGS_VCPPORT_SERIAL: + PIOS_COM_TELEMETRY = PIOS_COM_TELEM_USB_VCP; + break; + case OPLINKSETTINGS_VCPPORT_DISABLED: + break; + } + + // Initialize out status object. + OPLinkStatusData oplinkStatus; + OPLinkStatusGet(&oplinkStatus); + + // Get our hardware information. + const struct pios_board_info *bdinfo = &pios_board_info_blob; + + oplinkStatus.BoardType = bdinfo->board_type; + PIOS_BL_HELPER_FLASH_Read_Description(oplinkStatus.Description, OPLINKSTATUS_DESCRIPTION_NUMELEM); + PIOS_SYS_SerialNumberGetBinary(oplinkStatus.CPUSerial); + oplinkStatus.BoardRevision = bdinfo->board_rev; /* Initalize the RFM22B radio COM device. */ -#if defined(PIOS_INCLUDE_RFM22B) - bool is_coordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); - { + if (oplinkSettings.MaxRFPower != OPLINKSETTINGS_MAXRFPOWER_0) { + oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_ENABLED; + // Configure the RFM22B device - const struct pios_board_info *bdinfo = &pios_board_info_blob; const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) { PIOS_Assert(0); @@ -240,31 +304,29 @@ void PIOS_Board_Init(void) tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { PIOS_Assert(0); } - uint32_t comBaud = 9600; + + // Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex. + enum rfm22b_datarate datarate = RFM22_datarate_64000; switch (oplinkSettings.ComSpeed) { - case OPLINKSETTINGS_COMSPEED_2400: - comBaud = 2400; - break; case OPLINKSETTINGS_COMSPEED_4800: - comBaud = 4800; + datarate = RFM22_datarate_9600; break; case OPLINKSETTINGS_COMSPEED_9600: - comBaud = 9600; + datarate = RFM22_datarate_19200; break; case OPLINKSETTINGS_COMSPEED_19200: - comBaud = 19200; + datarate = RFM22_datarate_32000; break; case OPLINKSETTINGS_COMSPEED_38400: - comBaud = 38400; + datarate = RFM22_datarate_57600; break; case OPLINKSETTINGS_COMSPEED_57600: - comBaud = 57600; + datarate = RFM22_datarate_128000; break; case OPLINKSETTINGS_COMSPEED_115200: - comBaud = 115200; + datarate = RFM22_datarate_192000; break; } - PIOS_COM_ChangeBaud(pios_com_rfm22b_id, comBaud); /* Set the modem Tx poer level */ switch (oplinkSettings.MaxRFPower) { @@ -298,65 +360,26 @@ void PIOS_Board_Init(void) } // Set the radio configuration parameters. - PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, oplinkSettings.NumChannels, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, - oplinkSettings.PacketTime, (oplinkSettings.OneWayLink == OPLINKSETTINGS_ONEWAYLINK_TRUE)); - PIOS_RFM22B_SetCoordinator(pios_rfm22b_id, is_coordinator); + PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, is_coordinator, is_oneway, ppm_mode, ppm_only); PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID); + /* Set the PPM callback if we should be receiving PPM. */ + if (ppm_mode) { + PIOS_RFM22B_SetPPMCallback(pios_rfm22b_id, PIOS_Board_PPM_callback); + } + // Reinitilize the modem to affect te changes. PIOS_RFM22B_Reinit(pios_rfm22b_id); - } -#endif /* PIOS_INCLUDE_RFM22B */ - - /* Allocate the uart buffers. */ - pios_uart_rx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_RX_BUF_LEN); - pios_uart_tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_TX_BUF_LEN); - - // Configure the main port - switch (oplinkSettings.MainPort) { - case OPLINKSETTINGS_MAINPORT_TELEMETRY: - case OPLINKSETTINGS_MAINPORT_SERIAL: - /* Configure the main port for uart serial */ - PIOS_InitUartMainPort(); - PIOS_COM_TELEMETRY = PIOS_COM_TELEM_UART_MAIN; - break; - case OPLINKSETTINGS_MAINPORT_PPM: - PIOS_InitPPMMainPort(is_coordinator); - break; - case OPLINKSETTINGS_MAINPORT_DISABLED: - break; + } else { + oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED; } - // Configure the flexi port - switch (oplinkSettings.FlexiPort) { - case OPLINKSETTINGS_FLEXIPORT_TELEMETRY: - case OPLINKSETTINGS_FLEXIPORT_SERIAL: - /* Configure the flexi port as uart serial */ - PIOS_InitUartFlexiPort(); - PIOS_COM_TELEMETRY = PIOS_COM_TELEM_UART_FLEXI; - break; - case OPLINKSETTINGS_FLEXIPORT_PPM: - PIOS_InitPPMFlexiPort(is_coordinator); - break; - case OPLINKSETTINGS_FLEXIPORT_DISABLED: - break; - } - - // Configure the USB VCP port - switch (oplinkSettings.VCPPort) { - case OPLINKSETTINGS_VCPPORT_SERIAL: - PIOS_COM_TELEMETRY = PIOS_COM_TELEM_USB_VCP; - break; - case OPLINKSETTINGS_VCPPORT_DISABLED: - break; - } + // Update the object + OPLinkStatusSet(&oplinkStatus); // Update the com baud rate. uint32_t comBaud = 9600; switch (oplinkSettings.ComSpeed) { - case OPLINKSETTINGS_COMSPEED_2400: - comBaud = 2400; - break; case OPLINKSETTINGS_COMSPEED_4800: comBaud = 4800; break; @@ -458,6 +481,17 @@ static void PIOS_InitPPMFlexiPort(bool input) #endif /* PIOS_INCLUDE_PPM */ } +static void PIOS_Board_PPM_callback(const int16_t *channels) +{ +#if defined(PIOS_INCLUDE_PPM) && defined(PIOS_INCLUDE_PPM_OUT) + if (pios_ppm_out_id) { + for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) { + PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, channels[i]); + } + } +#endif /* PIOS_INCLUDE_PPM && PIOS_INCLUDE_PPM_OUT */ +} + /** * @} */ diff --git a/flight/targets/boards/oplinkmini/pios_board.h b/flight/targets/boards/oplinkmini/pios_board.h index bfb69dab9..8615b0c6e 100644 --- a/flight/targets/boards/oplinkmini/pios_board.h +++ b/flight/targets/boards/oplinkmini/pios_board.h @@ -77,6 +77,7 @@ #define PIOS_WDG_RADIORX 0x0008 #define PIOS_WDG_RFM22B 0x000f #define PIOS_WDG_PPMINPUT 0x0010 +#define PIOS_WDG_SERIALRX 0x0020 // ------------------------ // TELEMETRY diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index 528ae6278..5d229db01 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -117,7 +117,7 @@ #define PIOS_INCLUDE_RFM22B #define PIOS_INCLUDE_RFM22B_COM /* #define PIOS_INCLUDE_PPM_OUT */ -#define PIOS_RFM22B_DEBUG_ON_TELEM +/* #define PIOS_RFM22B_DEBUG_ON_TELEM */ /* PIOS misc peripherals */ /* #define PIOS_INCLUDE_VIDEO */ diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 89d675e24..4f714b891 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -319,6 +319,17 @@ static void PIOS_Board_configure_ppm(const struct pios_ppm_cfg *ppm_cfg) pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; } +static void PIOS_Board_PPM_callback(const int16_t *channels) +{ + uint8_t max_chan = (RFM22B_PPM_NUM_CHANNELS < OPLINKRECEIVER_CHANNEL_NUMELEM) ? RFM22B_PPM_NUM_CHANNELS : OPLINKRECEIVER_CHANNEL_NUMELEM; + OPLinkReceiverData opl_rcvr; + + for (uint8_t i = 0; i < max_chan; ++i) { + opl_rcvr.Channel[i] = channels[i]; + } + OPLinkReceiverSet(&opl_rcvr); +} + /** * PIOS_Board_Init() * initializes all the core subsystems on this specific hardware @@ -396,7 +407,10 @@ void PIOS_Board_Init(void) EventDispatcherInitialize(); UAVObjInitialize(); HwSettingsInitialize(); +#if defined(PIOS_INCLUDE_RFM22B) + OPLinkSettingsInitialize(); OPLinkStatusInitialize(); +#endif /* PIOS_INCLUDE_RFM22B */ /* Initialize the alarms library */ AlarmsInitialize(); @@ -709,30 +723,24 @@ void PIOS_Board_Init(void) /* Initalize the RFM22B radio COM device. */ #if defined(PIOS_INCLUDE_RFM22B) + + /* Fetch the OPinkSettings object. */ + OPLinkSettingsData oplinkSettings; + OPLinkSettingsGet(&oplinkSettings); + + // Initialize out status object. + OPLinkStatusData oplinkStatus; + OPLinkStatusGet(&oplinkStatus); + oplinkStatus.BoardType = bdinfo->board_type; + PIOS_BL_HELPER_FLASH_Read_Description(oplinkStatus.Description, OPLINKSTATUS_DESCRIPTION_NUMELEM); + PIOS_SYS_SerialNumberGetBinary(oplinkStatus.CPUSerial); + oplinkStatus.BoardRevision = bdinfo->board_rev; + /* Is the radio turned on? */ - uint8_t hwsettings_radioport; - HwSettingsRadioPortGet(&hwsettings_radioport); - switch (hwsettings_radioport) { - case HWSETTINGS_RADIOPORT_DISABLED: - break; - case HWSETTINGS_RADIOPORT_TELEMETRY: - { - // Initialize out status object. - OPLinkStatusData oplinkStatus; - OPLinkStatusGet(&oplinkStatus); - oplinkStatus.BoardType = bdinfo->board_type; - PIOS_BL_HELPER_FLASH_Read_Description(oplinkStatus.Description, OPLINKSTATUS_DESCRIPTION_NUMELEM); - PIOS_SYS_SerialNumberGetBinary(oplinkStatus.CPUSerial); - oplinkStatus.BoardRevision = bdinfo->board_rev; - OPLinkStatusSet(&oplinkStatus); - - /* Initialize the OPLinkSettings object. */ - OPLinkSettingsInitialize(); - - /* Fetch the OPinkSettings object. */ - OPLinkSettingsData oplinkSettings; - OPLinkSettingsGet(&oplinkSettings); - + bool is_oneway = (oplinkSettings.OneWay == OPLINKSETTINGS_ONEWAY_TRUE); + bool ppm_mode = (oplinkSettings.PPM == OPLINKSETTINGS_PPM_TRUE); + bool ppm_only = (oplinkSettings.PPMOnly == OPLINKSETTINGS_PPMONLY_TRUE); + if (oplinkSettings.MaxRFPower != OPLINKSETTINGS_MAXRFPOWER_0) { /* Configure the RFM22B device. */ const struct pios_rfm22b_cfg *rfm22b_cfg = PIOS_BOARD_HW_DEFS_GetRfm22Cfg(bdinfo->board_rev); if (PIOS_RFM22B_Init(&pios_rfm22b_id, PIOS_RFM22_SPI_PORT, rfm22b_cfg->slave_num, rfm22b_cfg)) { @@ -749,12 +757,40 @@ void PIOS_Board_Init(void) tx_buffer, PIOS_COM_RFM22B_RF_TX_BUF_LEN)) { PIOS_Assert(0); } + oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_ENABLED; + + // Set the RF data rate on the modem to ~2X the selected buad rate because the modem is half duplex. + enum rfm22b_datarate datarate = RFM22_datarate_64000; + switch (oplinkSettings.ComSpeed) { + case OPLINKSETTINGS_COMSPEED_4800: + datarate = RFM22_datarate_9600; + break; + case OPLINKSETTINGS_COMSPEED_9600: + datarate = RFM22_datarate_19200; + break; + case OPLINKSETTINGS_COMSPEED_19200: + datarate = RFM22_datarate_32000; + break; + case OPLINKSETTINGS_COMSPEED_38400: + datarate = RFM22_datarate_57600; + break; + case OPLINKSETTINGS_COMSPEED_57600: + datarate = RFM22_datarate_128000; + break; + case OPLINKSETTINGS_COMSPEED_115200: + datarate = RFM22_datarate_192000; + break; + } /* Set the radio configuration parameters. */ - PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, oplinkSettings.NumChannels, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, - oplinkSettings.PacketTime, (oplinkSettings.OneWayLink == OPLINKSETTINGS_ONEWAYLINK_TRUE)); + PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, false, is_oneway, ppm_mode, ppm_only); PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID); + /* Set the PPM callback if we should be receiving PPM. */ + if (ppm_mode) { + PIOS_RFM22B_SetPPMCallback(pios_rfm22b_id, PIOS_Board_PPM_callback); + } + /* Set the modem Tx poer level */ switch (oplinkSettings.MaxRFPower) { case OPLINKSETTINGS_MAXRFPOWER_125: @@ -788,10 +824,11 @@ void PIOS_Board_Init(void) /* Reinitialize the modem. */ PIOS_RFM22B_Reinit(pios_rfm22b_id); + } else { + oplinkStatus.LinkState = OPLINKSTATUS_LINKSTATE_DISABLED; + } - break; - } - } + OPLinkStatusSet(&oplinkStatus); #endif /* PIOS_INCLUDE_RFM22B */ #if defined(PIOS_INCLUDE_PWM) || defined(PIOS_INCLUDE_PWM) diff --git a/flight/uavtalk/uavtalk.c b/flight/uavtalk/uavtalk.c index a6b70d25f..c20bc9690 100644 --- a/flight/uavtalk/uavtalk.c +++ b/flight/uavtalk/uavtalk.c @@ -151,14 +151,14 @@ void UAVTalkAddStats(UAVTalkConnection connectionHandle, UAVTalkStats *statsOut) xSemaphoreTakeRecursive(connection->lock, portMAX_DELAY); // Copy stats - statsOut->txBytes += connection->stats.txBytes; - statsOut->rxBytes += connection->stats.rxBytes; + statsOut->txBytes += connection->stats.txBytes; + statsOut->rxBytes += connection->stats.rxBytes; statsOut->txObjectBytes += connection->stats.txObjectBytes; statsOut->rxObjectBytes += connection->stats.rxObjectBytes; - statsOut->txObjects += connection->stats.txObjects; - statsOut->rxObjects += connection->stats.rxObjects; - statsOut->txErrors += connection->stats.txErrors; - statsOut->txErrors += connection->stats.txErrors; + statsOut->txObjects += connection->stats.txObjects; + statsOut->rxObjects += connection->stats.rxObjects; + statsOut->txErrors += connection->stats.txErrors; + statsOut->txErrors += connection->stats.txErrors; // Release lock xSemaphoreGiveRecursive(connection->lock); diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index c74357f22..3cd9669ae 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -33,7 +33,7 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget(parent) { - m_oplink = new Ui_PipXtremeWidget(); + m_oplink = new Ui_OPLinkWidget(); m_oplink->setupUi(this); // Connect to the OPLinkStatus object updates @@ -65,14 +65,14 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget addUAVObjectToWidgetRelation("OPLinkSettings", "VCPPort", m_oplink->VCPPort); addUAVObjectToWidgetRelation("OPLinkSettings", "ComSpeed", m_oplink->ComSpeed); addUAVObjectToWidgetRelation("OPLinkSettings", "MaxRFPower", m_oplink->MaxRFTxPower); - addUAVObjectToWidgetRelation("OPLinkSettings", "NumChannels", m_oplink->NumChannels); addUAVObjectToWidgetRelation("OPLinkSettings", "MinChannel", m_oplink->MinimumChannel); addUAVObjectToWidgetRelation("OPLinkSettings", "MaxChannel", m_oplink->MaximumChannel); addUAVObjectToWidgetRelation("OPLinkSettings", "ChannelSet", m_oplink->ChannelSet); - addUAVObjectToWidgetRelation("OPLinkSettings", "PacketTime", m_oplink->PacketLength); addUAVObjectToWidgetRelation("OPLinkSettings", "CoordID", m_oplink->CoordID); addUAVObjectToWidgetRelation("OPLinkSettings", "Coordinator", m_oplink->Coordinator); - addUAVObjectToWidgetRelation("OPLinkSettings", "OneWayLink", m_oplink->OneWayLink); + addUAVObjectToWidgetRelation("OPLinkSettings", "OneWay", m_oplink->OneWayLink); + addUAVObjectToWidgetRelation("OPLinkSettings", "PPMOnly", m_oplink->PPMOnly); + addUAVObjectToWidgetRelation("OPLinkSettings", "PPM", m_oplink->PPM); addUAVObjectToWidgetRelation("OPLinkStatus", "DeviceID", m_oplink->DeviceID); addUAVObjectToWidgetRelation("OPLinkStatus", "RxGood", m_oplink->Good); @@ -100,6 +100,12 @@ ConfigPipXtremeWidget::ConfigPipXtremeWidget(QWidget *parent) : ConfigTaskWidget connect(m_oplink->Bind3, SIGNAL(clicked()), this, SLOT(bind3())); connect(m_oplink->Bind4, SIGNAL(clicked()), this, SLOT(bind3())); + // Connect the selection changed signals. + connect(m_oplink->PPMOnly, SIGNAL(toggled(bool)), this, SLOT(ppmOnlyToggled(bool))); + connect(m_oplink->ComSpeed, SIGNAL(currentIndexChanged(int)), this, SLOT(comSpeedChanged(int))); + + ppmOnlyToggled(m_oplink->PPMOnly->isChecked()); + // Add scroll bar when necessary QScrollArea *scroll = new QScrollArea; scroll->setWidget(m_oplink->frame_3); @@ -226,6 +232,56 @@ void ConfigPipXtremeWidget::updateSettings(UAVObject *object) if (!settingsUpdated) { settingsUpdated = true; + + // Enable components based on the board type connected. + UAVObjectField *board_type_field = oplinkStatusObj->getField("BoardType"); + if (board_type_field) { + switch (board_type_field->getValue().toInt()) { + case 0x09: // Revolution + m_oplink->MainPort->setVisible(false); + m_oplink->MainPortLabel->setVisible(false); + m_oplink->FlexiPort->setVisible(false); + m_oplink->FlexiPortLabel->setVisible(false); + m_oplink->VCPPort->setVisible(false); + m_oplink->VCPPortLabel->setVisible(false); + m_oplink->FlexiIOPort->setVisible(false); + m_oplink->FlexiIOPortLabel->setVisible(false); + m_oplink->Coordinator->setVisible(false); + m_oplink->PPM->setVisible(true); + break; + case 0x03: // OPLinkMini + m_oplink->MainPort->setVisible(true); + m_oplink->MainPortLabel->setVisible(true); + m_oplink->FlexiPort->setVisible(true); + m_oplink->FlexiPortLabel->setVisible(true); + m_oplink->VCPPort->setVisible(true); + m_oplink->VCPPortLabel->setVisible(true); + m_oplink->FlexiIOPort->setVisible(false); + m_oplink->FlexiIOPortLabel->setVisible(false); + m_oplink->Coordinator->setVisible(true); + m_oplink->PPM->setVisible(false); + break; + case 0x0A: + m_oplink->MainPort->setVisible(true); + m_oplink->MainPortLabel->setVisible(true); + m_oplink->FlexiPort->setVisible(true); + m_oplink->FlexiPortLabel->setVisible(true); + m_oplink->VCPPort->setVisible(true); + m_oplink->VCPPortLabel->setVisible(true); + m_oplink->FlexiIOPort->setVisible(true); + m_oplink->FlexiIOPortLabel->setVisible(true); + m_oplink->Coordinator->setVisible(true); + m_oplink->PPM->setVisible(false); + break; + default: + // This shouldn't happen. + break; + } + } else { + qDebug() << "BoardType not found."; + } + + // Enable the push buttons. enableControls(true); } } @@ -234,6 +290,8 @@ void ConfigPipXtremeWidget::disconnected() { if (settingsUpdated) { settingsUpdated = false; + + // Enable the push buttons. enableControls(false); } } @@ -270,6 +328,36 @@ void ConfigPipXtremeWidget::bind4() SetPairID(m_oplink->PairID1); } +void ConfigPipXtremeWidget::ppmOnlyToggled(bool on) +{ + if (on) { + m_oplink->PPM->setEnabled(false); + m_oplink->OneWayLink->setEnabled(false); + m_oplink->ComSpeed->setEnabled(false); + } else { + m_oplink->PPM->setEnabled(true); + m_oplink->OneWayLink->setEnabled(true); + m_oplink->ComSpeed->setEnabled(true); + // Change the comspeed from 4800 of PPM only is turned off. + if (m_oplink->ComSpeed->currentIndex() == OPLinkSettings::COMSPEED_4800) { + m_oplink->ComSpeed->setCurrentIndex(OPLinkSettings::COMSPEED_9600); + } + } +} + +void ConfigPipXtremeWidget::comSpeedChanged(int index) +{ + qDebug() << "comSpeedChanged: " << index; + switch (index) { + case OPLinkSettings::COMSPEED_4800: + m_oplink->PPMOnly->setChecked(true); + break; + default: + m_oplink->PPMOnly->setChecked(false); + break; + } +} + /** @} @} diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h index 6966691ce..4b5493103 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.h @@ -44,7 +44,7 @@ public slots: void updateSettings(UAVObject *object1); private: - Ui_PipXtremeWidget *m_oplink; + Ui_OPLinkWidget *m_oplink; // The OPLink status UAVObject UAVDataObject *oplinkStatusObj; @@ -63,6 +63,8 @@ private slots: void bind2(); void bind3(); void bind4(); + void ppmOnlyToggled(bool toggled); + void comSpeedChanged(int index); }; #endif // CONFIGTXPIDWIDGET_H diff --git a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp index 5842ba2ce..70f0c2d81 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.cpp @@ -65,9 +65,6 @@ ConfigRevoHWWidget::ConfigRevoHWWidget(QWidget *parent) : ConfigTaskWidget(paren addUAVObjectToWidgetRelation("HwSettings", "GPSSpeed", m_ui->cbMainGPSSpeed); addUAVObjectToWidgetRelation("HwSettings", "ComUsbBridgeSpeed", m_ui->cbMainComSpeed); - addUAVObjectToWidgetRelation("HwSettings", "RadioPort", m_ui->cbModem); - addUAVObjectToWidgetRelation("HwSettings", "TelemetrySpeed", m_ui->cbRadioSpeed); - connect(m_ui->cchwHelp, SIGNAL(clicked()), this, SLOT(openHelp())); setupCustomCombos(); @@ -94,7 +91,6 @@ void ConfigRevoHWWidget::setupCustomCombos() connect(m_ui->cbFlexi, SIGNAL(currentIndexChanged(int)), this, SLOT(flexiPortChanged(int))); connect(m_ui->cbMain, SIGNAL(currentIndexChanged(int)), this, SLOT(mainPortChanged(int))); - connect(m_ui->cbModem, SIGNAL(currentIndexChanged(int)), this, SLOT(modemPortChanged(int))); } void ConfigRevoHWWidget::refreshWidgetsValues(UAVObject *obj) @@ -105,7 +101,6 @@ void ConfigRevoHWWidget::refreshWidgetsValues(UAVObject *obj) usbVCPPortChanged(0); mainPortChanged(0); flexiPortChanged(0); - modemPortChanged(0); m_refreshing = false; } @@ -202,9 +197,6 @@ void ConfigRevoHWWidget::flexiPortChanged(int index) if (m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_TELEMETRY) { m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); } - if (m_ui->cbModem->currentIndex() == HwSettings::RADIOPORT_TELEMETRY) { - m_ui->cbModem->setCurrentIndex(HwSettings::RADIOPORT_DISABLED); - } break; case HwSettings::RM_FLEXIPORT_GPS: m_ui->cbFlexiGPSSpeed->setVisible(true); @@ -248,9 +240,6 @@ void ConfigRevoHWWidget::mainPortChanged(int index) if (m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_TELEMETRY) { m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); } - if (m_ui->cbModem->currentIndex() == HwSettings::RADIOPORT_TELEMETRY) { - m_ui->cbModem->setCurrentIndex(HwSettings::RADIOPORT_DISABLED); - } break; case HwSettings::RM_MAINPORT_GPS: m_ui->cbMainGPSSpeed->setVisible(true); @@ -279,26 +268,6 @@ void ConfigRevoHWWidget::mainPortChanged(int index) } } -void ConfigRevoHWWidget::modemPortChanged(int index) -{ - Q_UNUSED(index); - - if (m_ui->cbModem->currentIndex() == HwSettings::RADIOPORT_TELEMETRY) { - if (m_ui->cbMain->currentIndex() == HwSettings::RM_MAINPORT_TELEMETRY) { - m_ui->cbMain->setCurrentIndex(HwSettings::RM_MAINPORT_DISABLED); - } - if (m_ui->cbFlexi->currentIndex() == HwSettings::RM_FLEXIPORT_TELEMETRY) { - m_ui->cbFlexi->setCurrentIndex(HwSettings::RM_FLEXIPORT_DISABLED); - } - m_ui->cbRadioSpeed->setVisible(true); - if (!m_refreshing) { - QMessageBox::warning(this, tr("Warning"), tr("Activating the Radio requires an antenna be attached or modem damage will occur.")); - } - } else { - m_ui->cbRadioSpeed->setVisible(false); - } -} - void ConfigRevoHWWidget::openHelp() { QDesktopServices::openUrl(QUrl("http://wiki.openpilot.org/x/GgDBAQ", QUrl::StrictMode)); diff --git a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.h b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.h index d7eb30b66..fd54232ab 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.h +++ b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.h @@ -57,7 +57,6 @@ private slots: void usbHIDPortChanged(int index); void flexiPortChanged(int index); void mainPortChanged(int index); - void modemPortChanged(int index); void openHelp(); }; diff --git a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.ui b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.ui index 43b605d35..6bd908f1e 100644 --- a/ground/openpilotgcs/src/plugins/config/configrevohwwidget.ui +++ b/ground/openpilotgcs/src/plugins/config/configrevohwwidget.ui @@ -123,33 +123,16 @@ - - - - Qt::Horizontal + + + + + 0 + 0 + - - QSizePolicy::Minimum - - - - 120 - 10 - - - - - - - - true - - - - - - Main Port + USB HID Function Qt::AlignBottom|Qt::AlignHCenter @@ -172,8 +155,28 @@ - - + + + + 0 + + + + + true + + + + + + + + + + + + + Qt::Horizontal @@ -188,21 +191,8 @@ - - - - false - - - - - - - - 0 - 0 - - + + Speed @@ -242,9 +232,116 @@ + + + + + + + Qt::Horizontal + + + QSizePolicy::Minimum + + + + 70 + 10 + + + + + + + + Flexi Port + + + Qt::AlignBottom|Qt::AlignHCenter + + + + + + + false + + + + + + + + + + Qt::Vertical + + + QSizePolicy::Fixed + + + + 20 + 20 + + + + + + + + Main Port + + + Qt::AlignBottom|Qt::AlignHCenter + + + + + + + true + + + + + + + Qt::Horizontal + + + QSizePolicy::Minimum + + + + 120 + 10 + + + + + + + + + + + + 0 + 0 + + + + Speed + + + Qt::AlignBottom|Qt::AlignHCenter + + + @@ -261,194 +358,6 @@ - - - - Qt::Vertical - - - QSizePolicy::Fixed - - - - 20 - 120 - - - - - - - - - - - - 0 - 0 - - - - USB VCP Function - - - Qt::AlignBottom|Qt::AlignHCenter - - - - - - - Speed - - - Qt::AlignBottom|Qt::AlignHCenter - - - - - - - 0 - - - - - true - - - - - - - - - - - - - - - Qt::Horizontal - - - QSizePolicy::Minimum - - - - 120 - 10 - - - - - - - - Flexi Port - - - Qt::AlignBottom|Qt::AlignHCenter - - - - - - - - - - - 0 - 0 - - - - USB HID Function - - - Qt::AlignBottom|Qt::AlignHCenter - - - - - - - - - - 0 - - - - - true - - - - - - - - - - - - - - - - - - Radio - - - Qt::AlignBottom|Qt::AlignHCenter - - - - - - - - - - Speed - - - Qt::AlignBottom|Qt::AlignHCenter - - - - - - - - - - Speed - - - Qt::AlignBottom|Qt::AlignHCenter - - - - - - - Qt::Horizontal - - - QSizePolicy::Minimum - - - - 120 - 10 - - - - @@ -481,6 +390,87 @@ + + + + Qt::Horizontal + + + QSizePolicy::Minimum + + + + 120 + 10 + + + + + + + + + 0 + 0 + + + + USB VCP Function + + + Qt::AlignBottom|Qt::AlignHCenter + + + + + + + Qt::Vertical + + + QSizePolicy::Fixed + + + + 20 + 120 + + + + + + + + Speed + + + Qt::AlignBottom|Qt::AlignHCenter + + + + + + + 0 + + + + + true + + + + + + + + + + + + + + @@ -545,22 +535,6 @@ - - - - Qt::Vertical - - - QSizePolicy::Fixed - - - - 20 - 20 - - - - @@ -584,7 +558,7 @@ - 120 + 80 10 diff --git a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui index 2a8daaa7f..3cfd47ca1 100644 --- a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui +++ b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui @@ -1,7 +1,7 @@ - PipXtremeWidget - + OPLinkWidget + 0 @@ -1259,246 +1259,6 @@ - - - - - 0 - 0 - - - - - 75 - true - - - - Configuration - - - - - - Max Chan - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - Packet Length - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - Num Chan - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - Main Port - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the USB virtual com port - - - - - - - 250 - - - - - - - Flexi Port - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - - - VCP Port - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the main port - - - - - - - Com Speed - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - Min Chan - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - 249 - - - - - - - Channel Set - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - 249 - - - - - - - - 16777215 - 16777215 - - - - Choose the function for the flexi port - - - - - - - 249 - - - - - - - Max Power - - - Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter - - - - - - - - 16777215 - 16777215 - - - - Set the maximum TX output power the modem will use (mW) - - - Qt::LeftToRight - - - 0 - - - - - - - - - - 10 - - - 50 - - - 15 - - - - - - - Coordinator - - - - - - - One-Way Link - - - - - - @@ -1715,12 +1475,236 @@ + + + + Coordinator + + + + + + + + 0 + 0 + + + + + 75 + true + + + + Configuration + + + + + + + + + Com Speed + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + VCP Port + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the flexi port + + + + + + + Main Port + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the main port + + + + + + + + 16777215 + 16777215 + + + + Set the maximum TX output power the modem will use (mW) + + + Qt::LeftToRight + + + 0 + + + + + + + Max Power + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + 16777215 + 16777215 + + + + Choose the function for the USB virtual com port + + + + + + + FlexiIO Port + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + + + + Flexi Port + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + One-Way Link + + + + + + + PPM Only + + + + + + + Max Chan + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 249 + + + + + + + Min Chan + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 249 + + + + + + + Channel Set + + + Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter + + + + + + + 249 + + + + + + + PPM + + + + + + diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 61c24d4f1..20b65c136 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -15,8 +15,6 @@ - - diff --git a/shared/uavobjectdefinition/oplinksettings.xml b/shared/uavobjectdefinition/oplinksettings.xml index 08c116ba1..69aafed5d 100644 --- a/shared/uavobjectdefinition/oplinksettings.xml +++ b/shared/uavobjectdefinition/oplinksettings.xml @@ -2,18 +2,18 @@ OPLink configurations options. - + + + - - + + - - diff --git a/shared/uavobjectdefinition/oplinkstatus.xml b/shared/uavobjectdefinition/oplinkstatus.xml index 81172bcf1..178c3afa8 100644 --- a/shared/uavobjectdefinition/oplinkstatus.xml +++ b/shared/uavobjectdefinition/oplinkstatus.xml @@ -24,7 +24,7 @@ - + From caa8d3e70e3022792cded4728e46c5be70a456d6 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Wed, 26 Jun 2013 17:47:42 -0700 Subject: [PATCH 17/28] OP-932 Forces one-way to be set when PPM only is configured. --- flight/pios/common/pios_rfm22b.c | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index 7ac514c84..cecf59e24 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -554,7 +554,6 @@ void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datar } ppm_mode = ppm_mode || ppm_only; rfm22b_dev->coordinator = coordinator; - rfm22b_dev->one_way_link = oneway; rfm22b_dev->ppm_send_mode = ppm_mode && coordinator; rfm22b_dev->ppm_recv_mode = ppm_mode && !coordinator; if (ppm_mode && (datarate <= RFM22B_PPM_ONLY_DATARATE)) { @@ -562,9 +561,11 @@ void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datar } rfm22b_dev->ppm_only_mode = ppm_only; if (ppm_only) { - rfm22b_dev->datarate = RFM22B_PPM_ONLY_DATARATE; + rfm22b_dev->one_way_link = true; + rfm22b_dev->datarate = RFM22B_PPM_ONLY_DATARATE; } else { - rfm22b_dev->datarate = datarate; + rfm22b_dev->one_way_link = oneway; + rfm22b_dev->datarate = datarate; } rfm22b_dev->packet_time = (ppm_mode ? packet_time_ppm[datarate] : packet_time[datarate]); From d5f16faaecadaecd50da07bfc1ed75e9ca22cfc4 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Wed, 26 Jun 2013 20:11:39 -0700 Subject: [PATCH 18/28] OP-932 Fixes the datarate used on PPM only mode, and fixes failsafe on PPM output on OPLM. --- flight/pios/common/pios_rfm22b.c | 7 +++++-- 1 file changed, 5 insertions(+), 2 deletions(-) diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index cecf59e24..4446a5579 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -562,6 +562,7 @@ void PIOS_RFM22B_SetChannelConfig(uint32_t rfm22b_id, enum rfm22b_datarate datar rfm22b_dev->ppm_only_mode = ppm_only; if (ppm_only) { rfm22b_dev->one_way_link = true; + datarate = RFM22B_PPM_ONLY_DATARATE; rfm22b_dev->datarate = RFM22B_PPM_ONLY_DATARATE; } else { rfm22b_dev->one_way_link = oneway; @@ -1906,11 +1907,13 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d } if (good_packet || corrected_packet) { + bool valid_input_detected = false; for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) { // Is this a valid channel? if (p[0] & (1 << i)) { uint32_t val = p[i + 1]; - radio_dev->ppm[i] = (uint16_t)(1000 + val * 900 / 256); + radio_dev->ppm[i] = (uint16_t)(1000 + val * 900 / 256); + valid_input_detected = true; } else { radio_dev->ppm[i] = PIOS_RCVR_INVALID; } @@ -1920,7 +1923,7 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d data_len -= RFM22B_PPM_NUM_CHANNELS + 1; // Call the PPM received callback if it's available. - if (radio_dev->ppm_callback) { + if (valid_input_detected && radio_dev->ppm_callback) { radio_dev->ppm_callback(radio_dev->ppm); } } From 03a2076050069df16c9e8b1e7998b594d47b6837 Mon Sep 17 00:00:00 2001 From: Alessio Morale Date: Thu, 27 Jun 2013 11:44:20 +0200 Subject: [PATCH 19/28] updated WHATSNEW.txt --- WHATSNEW.txt | 23 ++++++++++++++++------- 1 file changed, 16 insertions(+), 7 deletions(-) diff --git a/WHATSNEW.txt b/WHATSNEW.txt index e56bcf58f..0c83c542e 100644 --- a/WHATSNEW.txt +++ b/WHATSNEW.txt @@ -1,9 +1,18 @@ -2013-06-24 -Temporary disabled AltitudeHold and AltitudeVario flight modes. They were not -officially supported. But since people expected well-known production quality -behavior, it is better to make them final, then reenable. +--- RELEASE-13.06.01 --- Italian Stallion Release --- +It applies the following changes to previously not released to public RELEASE-13.06 ---- RELEASE-13.06 --- Italian Stallion Release --- +- Temporary disabled AltitudeHold and AltitudeVario flight modes. They were not +officially supported. But since people expected well-known production quality +behavior, it is better to make them final, then reenable +- Fix windows hid connection failure if board was already connected and gcs started; +- Fixed a bug that lead to disabled controls with some settings combination in CC/CC3D hardware page +- Fixed a bug that prevent to correct saving stabilization settings for CC/CC3D +- Fixes Uploader GUI and automatically close AutoUpdate panel after 7s + +JIRA issues addressed in this release: +OP-1028 OP-1020 OP-1024 + +--- RELEASE-13.06 --- This is the first official OpenPilot Revolution software release. This version also supports the CopterControl, CC3D, OPLinkMini and the upcoming OP OSD. @@ -138,14 +147,14 @@ KNOWN ISSUES: enabled, but you should properly calibrate them first. That's the reason why they are disabled by default. -- AltitudeHold mode is enabled, but it is not officially supported. Do +- AltitudeHold/Vario modes are enabled but not officially supported. Do not expect it to work perfectly and be considered production quality. You may play with it and report your issues and suggestions at your own risk. If you are not using a case for your Revo, we strongly recommend covering the barometer sensor with some foam to shield the sensor from wind and light. -- Note that throttle stick in AltitudeHold mode is used to control vertical +- Note that throttle stick in AltitudeVario mode is used to control vertical velocity, sometimes called vario altitude in other platforms, centre stick means hold altitude and there is a dead band around centre stick. From 6386b12026294a3b8fa637ca364e24d5847f6cc9 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Thu, 27 Jun 2013 19:16:01 -0700 Subject: [PATCH 20/28] OP-932 Fixes raw serial on UART and USB VCP. --- flight/modules/RadioComBridge/RadioComBridge.c | 14 +++++++++++--- 1 file changed, 11 insertions(+), 3 deletions(-) diff --git a/flight/modules/RadioComBridge/RadioComBridge.c b/flight/modules/RadioComBridge/RadioComBridge.c index a5054859e..c2f69266c 100644 --- a/flight/modules/RadioComBridge/RadioComBridge.c +++ b/flight/modules/RadioComBridge/RadioComBridge.c @@ -326,9 +326,14 @@ static void radioRxTask(__attribute__((unused)) void *parameters) uint8_t serial_data[1]; uint16_t bytes_to_process = PIOS_COM_ReceiveBuffer(PIOS_COM_RADIO, serial_data, sizeof(serial_data), MAX_PORT_DELAY); if (bytes_to_process > 0) { - // Pass the data through the UAVTalk parser. - for (uint8_t i = 0; i < bytes_to_process; i++) { - ProcessRadioStream(data->radioUAVTalkCon, data->telemUAVTalkCon, serial_data[i]); + if (data->parseUAVTalk) { + // Pass the data through the UAVTalk parser. + for (uint8_t i = 0; i < bytes_to_process; i++) { + ProcessRadioStream(data->radioUAVTalkCon, data->telemUAVTalkCon, serial_data[i]); + } + } else if (PIOS_COM_TELEMETRY) { + // Send the data straight to the telemetry port. + PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEMETRY, serial_data, bytes_to_process); } } } else { @@ -465,6 +470,9 @@ static int32_t UAVTalkSendHandler(uint8_t *buf, int32_t length) */ static int32_t RadioSendHandler(uint8_t *buf, int32_t length) { + if (!data->parseUAVTalk) { + return length; + } uint32_t outputPort = PIOS_COM_RADIO; // Don't send any data unless the radio port is available. From bab69401b99f091cf39b270fcf038b634bcb41b9 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Thu, 27 Jun 2013 19:55:01 -0700 Subject: [PATCH 21/28] OP-932 Allows configuring the Revo as coordinator. --- flight/targets/boards/revolution/firmware/pios_board.c | 3 ++- .../openpilotgcs/src/plugins/config/configpipxtremewidget.cpp | 3 --- 2 files changed, 2 insertions(+), 4 deletions(-) diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 4f714b891..e63f2d70c 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -737,6 +737,7 @@ void PIOS_Board_Init(void) oplinkStatus.BoardRevision = bdinfo->board_rev; /* Is the radio turned on? */ + bool is_coordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); bool is_oneway = (oplinkSettings.OneWay == OPLINKSETTINGS_ONEWAY_TRUE); bool ppm_mode = (oplinkSettings.PPM == OPLINKSETTINGS_PPM_TRUE); bool ppm_only = (oplinkSettings.PPMOnly == OPLINKSETTINGS_PPMONLY_TRUE); @@ -783,7 +784,7 @@ void PIOS_Board_Init(void) } /* Set the radio configuration parameters. */ - PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, false, is_oneway, ppm_mode, ppm_only); + PIOS_RFM22B_SetChannelConfig(pios_rfm22b_id, datarate, oplinkSettings.MinChannel, oplinkSettings.MaxChannel, oplinkSettings.ChannelSet, is_coordinator, is_oneway, ppm_mode, ppm_only); PIOS_RFM22B_SetCoordinatorID(pios_rfm22b_id, oplinkSettings.CoordID); /* Set the PPM callback if we should be receiving PPM. */ diff --git a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp index 3cd9669ae..6fe506357 100644 --- a/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp +++ b/ground/openpilotgcs/src/plugins/config/configpipxtremewidget.cpp @@ -246,7 +246,6 @@ void ConfigPipXtremeWidget::updateSettings(UAVObject *object) m_oplink->VCPPortLabel->setVisible(false); m_oplink->FlexiIOPort->setVisible(false); m_oplink->FlexiIOPortLabel->setVisible(false); - m_oplink->Coordinator->setVisible(false); m_oplink->PPM->setVisible(true); break; case 0x03: // OPLinkMini @@ -258,7 +257,6 @@ void ConfigPipXtremeWidget::updateSettings(UAVObject *object) m_oplink->VCPPortLabel->setVisible(true); m_oplink->FlexiIOPort->setVisible(false); m_oplink->FlexiIOPortLabel->setVisible(false); - m_oplink->Coordinator->setVisible(true); m_oplink->PPM->setVisible(false); break; case 0x0A: @@ -270,7 +268,6 @@ void ConfigPipXtremeWidget::updateSettings(UAVObject *object) m_oplink->VCPPortLabel->setVisible(true); m_oplink->FlexiIOPort->setVisible(true); m_oplink->FlexiIOPortLabel->setVisible(true); - m_oplink->Coordinator->setVisible(true); m_oplink->PPM->setVisible(false); break; default: From 491e5fe68508841ec20235dfeaaab3334989fdf4 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sat, 29 Jun 2013 15:00:38 -0700 Subject: [PATCH 22/28] Adds a failsafe on PPM output on OPLink that kicks in when the link goes down. Also updates all air datarate settings and adds new 64 and 100 kbps modes. --- flight/pios/common/pios_rfm22b.c | 61 +++++++++++-------- flight/pios/inc/pios_rfm22b.h | 7 ++- .../boards/oplinkmini/firmware/pios_board.c | 4 +- .../boards/revolution/firmware/pios_board.c | 4 +- 4 files changed, 44 insertions(+), 32 deletions(-) diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index 4446a5579..9d0267053 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -302,37 +302,46 @@ static const struct pios_rfm22b_transition rfm22b_transitions[RADIO_STATE_NUM_ST }; // xtal 10 ppm, 434MHz -static const uint32_t data_rate[] = { 9600, 19200, 32000, 57600, 128000, 192000, 256000 }; -static const uint8_t modulation_index[] = { 1, 1, 1, 1, 1, 1, 1 }; +static const uint32_t data_rate[] = { + 9600, // 96 kbps, 433 HMz, 30 khz freq dev + 19200, // 19.2 kbps, 433 MHz, 45 khz freq dev + 32000, // 32 kbps, 433 MHz, 45 khz freq dev + 57600, // 57.6 kbps, 433 MHz, 45 khz freq dev + 64000, // 64 kbps, 433 MHz, 45 khz freq dev + 100000, // 100 kbps, 433 MHz, 60 khz freq dev + 128000, // 128 kbps, 433 MHz, 90 khz freq dev + 192000, // 192 kbps, 433 MHz, 128 khz freq dev + 256000, // 256 kbps, 433 MHz, 150 khz freq dev +}; -static const uint8_t reg_1C[] = { 0x01, 0x05, 0x16, 0x06, 0x83, 0x8A, 0x8C }; // rfm22_if_filter_bandwidth +static const uint8_t reg_1C[] = { 0x01, 0x05, 0x06, 0x95, 0x95, 0x81, 0x88, 0x8B, 0x8D }; // rfm22_if_filter_bandwidth -static const uint8_t reg_1D[] = { 0x40, 0x40, 0x44, 0x40, 0x44, 0x44, 0x44 }; // rfm22_afc_loop_gearshift_override -static const uint8_t reg_1E[] = { 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x02 }; // rfm22_afc_timing_control +static const uint8_t reg_1D[] = { 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40, 0x40 }; // rfm22_afc_loop_gearshift_override +static const uint8_t reg_1E[] = { 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x0A, 0x02 }; // rfm22_afc_timing_control -static const uint8_t reg_1F[] = { 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03 }; // rfm22_clk_recovery_gearshift_override -static const uint8_t reg_20[] = { 0xA1, 0xD0, 0x3F, 0x45, 0x5E, 0x3F, 0x2F }; // rfm22_clk_recovery_oversampling_ratio -static const uint8_t reg_21[] = { 0x20, 0x00, 0x02, 0x01, 0x01, 0x02, 0x02 }; // rfm22_clk_recovery_offset2 -static const uint8_t reg_22[] = { 0x4E, 0x9D, 0x0C, 0xD7, 0x0c, 0x5D, 0x0C, 0xBB }; // rfm22_clk_recovery_offset1 -static const uint8_t reg_23[] = { 0xA5, 0x49, 0x4A, 0xDC, 0x86, 0x4A, 0x0D }; // rfm22_clk_recovery_offset0 -static const uint8_t reg_24[] = { 0x00, 0x00, 0x07, 0x07, 0x07, 0x05, 0x07, 0x07 }; // rfm22_clk_recovery_timing_loop_gain1 -static const uint8_t reg_25[] = { 0x34, 0x8, 0xFF, 0x6E, 0x74, 0xFF, 0xFF }; // rfm22_clk_recovery_timing_loop_gain0 +static const uint8_t reg_1F[] = { 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03, 0x03 }; // rfm22_clk_recovery_gearshift_override +static const uint8_t reg_20[] = { 0xA1, 0xD0, 0x7D, 0x68, 0x5E, 0x78, 0x5E, 0x3F, 0x2F }; // rfm22_clk_recovery_oversampling_ratio +static const uint8_t reg_21[] = { 0x20, 0x00, 0x01, 0x01, 0x01, 0x01, 0x01, 0x02, 0x02 }; // rfm22_clk_recovery_offset2 +static const uint8_t reg_22[] = { 0x4E, 0x9D, 0x06, 0x3A, 0x5D, 0x11, 0x5D, 0x0C, 0xBB }; // rfm22_clk_recovery_offset1 +static const uint8_t reg_23[] = { 0xA5, 0x49, 0x25, 0x93, 0x86, 0x11, 0x86, 0x4A, 0x0D }; // rfm22_clk_recovery_offset0 +static const uint8_t reg_24[] = { 0x00, 0x00, 0x01, 0x03, 0x03, 0x03, 0x03, 0x06, 0x07 }; // rfm22_clk_recovery_timing_loop_gain1 +static const uint8_t reg_25[] = { 0x34, 0x88, 0x77, 0x29, 0xE2, 0x90, 0xE2, 0x1A, 0xFF }; // rfm22_clk_recovery_timing_loop_gain0 -static const uint8_t reg_2A[] = { 0x1E, 0x24, 0x17, 0x2D, 0x50, 0x50, 0x50 }; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz +static const uint8_t reg_2A[] = { 0x1E, 0x24, 0x28, 0x3C, 0x3C, 0x50, 0x50, 0x50, 0x50 }; // rfm22_afc_limiter .. AFC_pull_in_range = �AFCLimiter[7:0] x (hbsel+1) x 625 Hz -static const uint8_t reg_58[] = { 0x80, 0x80, 0x80, 0x80, 0x80, 0x80, 0x80 }; // rfm22_cpcuu -static const uint8_t reg_69[] = { 0x60, 0x60, 0x20, 0x60, 0x20, 0x20, 0x20 }; // rfm22_agc_override1 -static const uint8_t reg_6E[] = { 0x4E, 0x9D, 0x08, 0x0E, 0x20, 0x31, 0x41 }; // rfm22_tx_data_rate1 -static const uint8_t reg_6F[] = { 0xA5, 0x49, 0x31, 0xBF, 0xC5, 0x27, 0x89 }; // rfm22_tx_data_rate0 +static const uint8_t reg_58[] = { 0x80, 0x80, 0x80, 0x80, 0x80, 0xC0, 0xC0, 0xC0, 0xED }; // rfm22_cpcuu +static const uint8_t reg_69[] = { 0x60, 0x60, 0x60, 0x60, 0x60, 0x60, 0x60, 0x60, 0x60 }; // rfm22_agc_override1 +static const uint8_t reg_6E[] = { 0x4E, 0x9D, 0x08, 0x0E, 0x10, 0x19, 0x20, 0x31, 0x41 }; // rfm22_tx_data_rate1 +static const uint8_t reg_6F[] = { 0xA5, 0x49, 0x31, 0xBF, 0x62, 0x9A, 0xC5, 0x27, 0x89 }; // rfm22_tx_data_rate0 -static const uint8_t reg_70[] = { 0x2C, 0x2C, 0x0D, 0x0C, 0x0D, 0x0D, 0x0D }; // rfm22_modulation_mode_control1 -static const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23 }; // rfm22_modulation_mode_control2 +static const uint8_t reg_70[] = { 0x2C, 0x2C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C, 0x0C }; // rfm22_modulation_mode_control1 +static const uint8_t reg_71[] = { 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23, 0x23 }; // rfm22_modulation_mode_control2 -static const uint8_t reg_72[] = { 0x30, 0x48, 0x1A, 0x2E, 0x66, 0x9A, 0xCD }; // rfm22_frequency_deviation +static const uint8_t reg_72[] = { 0x30, 0x48, 0x48, 0x48, 0x48, 0x60, 0x90, 0xCD, 0x0F }; // rfm22_frequency_deviation -static const uint8_t packet_time[] = { 80, 40, 25, 15, 8, 6, 5 }; -static const uint8_t packet_time_ppm[] = { 26, 25, 25, 15, 8, 6, 5 }; -static const uint8_t num_channels[] = { 3, 3, 4, 6, 9, 12, 15 }; +static const uint8_t packet_time[] = { 80, 40, 25, 15, 13, 10, 8, 6, 5 }; +static const uint8_t packet_time_ppm[] = { 26, 25, 25, 15, 13, 10, 8, 6, 5 }; +static const uint8_t num_channels[] = { 4, 4, 4, 6, 8, 8, 10, 12, 16 }; static struct pios_rfm22b_dev *g_rfm22b_dev = NULL; @@ -1186,9 +1195,11 @@ static void pios_rfm22_task(void *parameters) if (!rfm22_isCoordinator(rfm22b_dev) && rfm22b_dev->on_sync_channel) { rfm22b_dev->on_sync_channel = false; if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) { - rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_CONNECTING; - } else { rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; + // Set the PPM outputs to INVALID + for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) { + rfm22b_dev->ppm[i] = PIOS_RCVR_INVALID; + } } } diff --git a/flight/pios/inc/pios_rfm22b.h b/flight/pios/inc/pios_rfm22b.h index 856c8096b..b6001ba38 100644 --- a/flight/pios/inc/pios_rfm22b.h +++ b/flight/pios/inc/pios_rfm22b.h @@ -65,9 +65,10 @@ enum rfm22b_datarate { RFM22_datarate_32000 = 2, RFM22_datarate_57600 = 3, RFM22_datarate_64000 = 4, - RFM22_datarate_128000 = 5, - RFM22_datarate_192000 = 6, - RFM22_datarate_256000 = 7, + RFM22_datarate_100000 = 5, + RFM22_datarate_128000 = 6, + RFM22_datarate_192000 = 7, + RFM22_datarate_256000 = 8, }; typedef enum { diff --git a/flight/targets/boards/oplinkmini/firmware/pios_board.c b/flight/targets/boards/oplinkmini/firmware/pios_board.c index 60178f9fa..3b44e27b7 100644 --- a/flight/targets/boards/oplinkmini/firmware/pios_board.c +++ b/flight/targets/boards/oplinkmini/firmware/pios_board.c @@ -318,10 +318,10 @@ void PIOS_Board_Init(void) datarate = RFM22_datarate_32000; break; case OPLINKSETTINGS_COMSPEED_38400: - datarate = RFM22_datarate_57600; + datarate = RFM22_datarate_64000; break; case OPLINKSETTINGS_COMSPEED_57600: - datarate = RFM22_datarate_128000; + datarate = RFM22_datarate_100000; break; case OPLINKSETTINGS_COMSPEED_115200: datarate = RFM22_datarate_192000; diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index e63f2d70c..391f5940a 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -773,10 +773,10 @@ void PIOS_Board_Init(void) datarate = RFM22_datarate_32000; break; case OPLINKSETTINGS_COMSPEED_38400: - datarate = RFM22_datarate_57600; + datarate = RFM22_datarate_64000; break; case OPLINKSETTINGS_COMSPEED_57600: - datarate = RFM22_datarate_128000; + datarate = RFM22_datarate_100000; break; case OPLINKSETTINGS_COMSPEED_115200: datarate = RFM22_datarate_192000; From c76ee7de9d155d6f946ffaf76ce16bed96d93699 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Sun, 30 Jun 2013 16:08:50 -0700 Subject: [PATCH 23/28] OP-932 Fixes the failsafe that detects when the Rx modem loses sync with the Tx modem. This was not working on one-way mode. --- flight/pios/common/pios_rfm22b.c | 38 +++++++++++++++++++------------- 1 file changed, 23 insertions(+), 15 deletions(-) diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index 9d0267053..90f51f9ca 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -1189,20 +1189,7 @@ static void pios_rfm22_task(void *parameters) D4_LED_OFF; } #endif - if (time_to_send && PIOS_RFM22B_InRxWait((uint32_t)rfm22b_dev)) { - // If the on_sync_channel flag is set, it means that we were on the sync channel, but no packet was received, so transition to a non-connected state. - if (!rfm22_isCoordinator(rfm22b_dev) && rfm22b_dev->on_sync_channel) { - rfm22b_dev->on_sync_channel = false; - if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) { - rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; - // Set the PPM outputs to INVALID - for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) { - rfm22b_dev->ppm[i] = PIOS_RCVR_INVALID; - } - } - } - rfm22_process_event(rfm22b_dev, RADIO_EVENT_TX_START); } } @@ -2225,12 +2212,33 @@ static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t ind uint8_t num_chan = num_channels[rfm22b_dev->datarate]; uint8_t idx = index % num_chan; + // Are we switching to a new channel? if (idx != rfm22b_dev->channel_index) { - rfm22b_dev->channel_index = idx; - if (idx == 0) { + + // If the on_sync_channel flag is set, it means that we were on the sync channel, but no packet was received, so transition to a non-connected state. + if (!rfm22_isCoordinator(rfm22b_dev) && (rfm22b_dev->channel_index == 0) && rfm22b_dev->on_sync_channel) { + rfm22b_dev->on_sync_channel = false; + + // Set the link state to disconnected. + if (rfm22b_dev->stats.link_state == OPLINKSTATUS_LINKSTATE_CONNECTED) { + rfm22b_dev->stats.link_state = OPLINKSTATUS_LINKSTATE_DISCONNECTED; + // Set the PPM outputs to INVALID + for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) { + rfm22b_dev->ppm[i] = PIOS_RCVR_INVALID; + } + } + + // Stay on the sync channel. + idx = 0; + + } else if (idx == 0) { + // If we're switching to the sync channel, set a flag that can be used to detect if a packet was received. rfm22b_dev->on_sync_channel = true; } + + rfm22b_dev->channel_index = idx; } + return rfm22b_dev->channels[idx]; } From d0d8a0aadd49f80875e6a7ee75c02bf8ab7b1a76 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Wed, 3 Jul 2013 18:58:34 -0700 Subject: [PATCH 24/28] OP-932 Adds PWM outputs to OPLinkMini. Both gpio pins on the main and/or flexi port can be configured to output PWM for a total of up to 4 PWM outputs. --- flight/pios/common/pios_rfm22b.c | 8 +- .../targets/boards/oplinkmini/board_hw_defs.c | 276 ++++++++++++------ .../oplinkmini/firmware/inc/pios_config.h | 4 +- .../boards/oplinkmini/firmware/pios_board.c | 180 +++++++----- flight/targets/boards/oplinkmini/pios_board.h | 5 + shared/uavobjectdefinition/oplinksettings.xml | 4 +- 6 files changed, 305 insertions(+), 172 deletions(-) diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index 90f51f9ca..09b8907fe 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -77,7 +77,7 @@ #define RFM22B_PPM_ONLY_DATARATE RFM22_datarate_9600 // The maximum amount of time without activity before initiating a reset. -#define PIOS_RFM22B_SUPERVISOR_TIMEOUT 100 // ms +#define PIOS_RFM22B_SUPERVISOR_TIMEOUT 150 // ms // this is too adjust the RF module so that it is on frequency #define OSC_LOAD_CAP 0x7F // cap = 12.5pf .. default @@ -1905,13 +1905,11 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d } if (good_packet || corrected_packet) { - bool valid_input_detected = false; for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) { // Is this a valid channel? if (p[0] & (1 << i)) { uint32_t val = p[i + 1]; radio_dev->ppm[i] = (uint16_t)(1000 + val * 900 / 256); - valid_input_detected = true; } else { radio_dev->ppm[i] = PIOS_RCVR_INVALID; } @@ -1921,7 +1919,7 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d data_len -= RFM22B_PPM_NUM_CHANNELS + 1; // Call the PPM received callback if it's available. - if (valid_input_detected && radio_dev->ppm_callback) { + if (radio_dev->ppm_callback) { radio_dev->ppm_callback(radio_dev->ppm); } } @@ -2214,7 +2212,6 @@ static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t ind // Are we switching to a new channel? if (idx != rfm22b_dev->channel_index) { - // If the on_sync_channel flag is set, it means that we were on the sync channel, but no packet was received, so transition to a non-connected state. if (!rfm22_isCoordinator(rfm22b_dev) && (rfm22b_dev->channel_index == 0) && rfm22b_dev->on_sync_channel) { rfm22b_dev->on_sync_channel = false; @@ -2230,7 +2227,6 @@ static uint8_t rfm22_calcChannel(struct pios_rfm22b_dev *rfm22b_dev, uint8_t ind // Stay on the sync channel. idx = 0; - } else if (idx == 0) { // If we're switching to the sync channel, set a flag that can be used to detect if a packet was received. rfm22b_dev->on_sync_channel = true; diff --git a/flight/targets/boards/oplinkmini/board_hw_defs.c b/flight/targets/boards/oplinkmini/board_hw_defs.c index cec0d3d51..6bb023d18 100644 --- a/flight/targets/boards/oplinkmini/board_hw_defs.c +++ b/flight/targets/boards/oplinkmini/board_hw_defs.c @@ -432,9 +432,79 @@ static const struct pios_tim_clock_cfg tim_4_cfg = { }, }; -static const struct pios_tim_channel pios_tim_ppm_flexi_port = { +static const struct pios_tim_channel pios_tim_all_port_pins[] = { + /* Main Tx */ + { + .timer = TIM1, + .timer_chan = TIM_Channel_2, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + /* Main Rx */ + { + .timer = TIM1, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + }, + /* Flexi Tx */ + { + .timer = TIM2, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap2_TIM2, + }, + /* Flexi Rx */ + { + .timer = TIM2, + .timer_chan = TIM_Channel_4, + .pin = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Mode = GPIO_Mode_AF_PP, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, + .remap = GPIO_PartialRemap2_TIM2, + }, +}; + +static const struct pios_tim_channel pios_tim_main_port_ppm = { + .timer = TIM1, + .timer_chan = TIM_Channel_3, + .pin = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Mode = GPIO_Mode_IPD, + .GPIO_Speed = GPIO_Speed_2MHz, + }, + }, +}; + +static const struct pios_tim_channel pios_tim_flexi_port_ppm = { .timer = TIM2, - .timer_chan = TIM_Channel_4, + .timer_chan = TIM_Channel_4, .pin = { .gpio = GPIOB, .init = { @@ -443,24 +513,130 @@ static const struct pios_tim_channel pios_tim_ppm_flexi_port = { .GPIO_Speed = GPIO_Speed_2MHz, }, }, - .remap = GPIO_PartialRemap2_TIM2, -}; - -static const struct pios_tim_channel pios_tim_ppm_main_port = { - .timer = TIM1, - .timer_chan = TIM_Channel_2, - .pin = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Mode = GPIO_Mode_IPD, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, + .remap = GPIO_PartialRemap2_TIM2, }; #endif /* PIOS_INCLUDE_TIM */ +#if defined(PIOS_INCLUDE_SERVO) && defined(PIOS_INCLUDE_TIM) +/* + * Servo outputs + */ +#include + +const struct pios_servo_cfg pios_servo_main_cfg = { + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_all_port_pins, + .num_channels = 2 +}; + +const struct pios_servo_cfg pios_servo_flexi_cfg = { + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = &(pios_tim_all_port_pins[2]), + .num_channels = 2 +}; + +const struct pios_servo_cfg pios_servo_main_flexi_cfg = { + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_High, + .TIM_OCNPolarity = TIM_OCPolarity_High, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channels = pios_tim_all_port_pins, + .num_channels = 4 +}; + +#endif /* PIOS_INCLUDE_SERVO && PIOS_INCLUDE_TIM */ + +/* + * PPM Inputs + */ +#if defined(PIOS_INCLUDE_PPM) +#include + +const struct pios_ppm_cfg pios_ppm_main_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = &pios_tim_main_port_ppm, + .num_channels = 1, +}; + +const struct pios_ppm_cfg pios_ppm_flexi_cfg = { + .tim_ic_init = { + .TIM_ICPolarity = TIM_ICPolarity_Rising, + .TIM_ICSelection = TIM_ICSelection_DirectTI, + .TIM_ICPrescaler = TIM_ICPSC_DIV1, + .TIM_ICFilter = 0x0, + }, + .channels = &pios_tim_flexi_port_ppm, + .num_channels = 1, +}; + +#endif /* PIOS_INCLUDE_PPM */ + +/* + * PPM Output + */ +#if defined(PIOS_INCLUDE_PPM_OUT) +#include + +const struct pios_ppm_out_cfg pios_main_ppm_out_cfg = { + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_Low, + .TIM_OCNPolarity = TIM_OCPolarity_Low, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channel = &(pios_tim_all_port_pins[0]), +}; + +const struct pios_ppm_out_cfg pios_flexi_ppm_out_cfg = { + .tim_oc_init = { + .TIM_OCMode = TIM_OCMode_PWM1, + .TIM_OutputState = TIM_OutputState_Enable, + .TIM_OutputNState = TIM_OutputNState_Disable, + .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, + .TIM_OCPolarity = TIM_OCPolarity_Low, + .TIM_OCNPolarity = TIM_OCPolarity_Low, + .TIM_OCIdleState = TIM_OCIdleState_Reset, + .TIM_OCNIdleState = TIM_OCNIdleState_Reset, + }, + .channel = &(pios_tim_all_port_pins[2]), +}; + +#endif /* PIOS_INCLUDE_PPM_OUT */ + #if defined(PIOS_INCLUDE_USART) #include @@ -576,74 +752,6 @@ void PIOS_RTC_IRQ_Handler(void) #endif /* if defined(PIOS_INCLUDE_RTC) */ -/* - * PPM Inputs - */ -#if defined(PIOS_INCLUDE_PPM) -#include - -const struct pios_ppm_cfg pios_ppm_flexi_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = &pios_tim_ppm_flexi_port, - .num_channels = 1, -}; - -const struct pios_ppm_cfg pios_ppm_main_cfg = { - .tim_ic_init = { - .TIM_ICPolarity = TIM_ICPolarity_Rising, - .TIM_ICSelection = TIM_ICSelection_DirectTI, - .TIM_ICPrescaler = TIM_ICPSC_DIV1, - .TIM_ICFilter = 0x0, - }, - .channels = &pios_tim_ppm_main_port, - .num_channels = 1, -}; - -#endif /* PIOS_INCLUDE_PPM */ - -/* - * PPM Output - */ -#if defined(PIOS_INCLUDE_PPM_OUT) -#include - -static const struct pios_tim_channel pios_tim_ppmout[] = { - { - .timer = TIM2, - .timer_chan = TIM_Channel_3, - .pin = { - .gpio = GPIOB, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Mode = GPIO_Mode_AF_PP, - .GPIO_Speed = GPIO_Speed_2MHz, - }, - }, - .remap = GPIO_FullRemap_TIM2, - } -}; - -const struct pios_ppm_out_cfg pios_ppm_out_cfg = { - .tim_oc_init = { - .TIM_OCMode = TIM_OCMode_PWM1, - .TIM_OutputState = TIM_OutputState_Enable, - .TIM_OutputNState = TIM_OutputNState_Disable, - .TIM_Pulse = PIOS_SERVOS_INITIAL_POSITION, - .TIM_OCPolarity = TIM_OCPolarity_Low, - .TIM_OCNPolarity = TIM_OCPolarity_Low, - .TIM_OCIdleState = TIM_OCIdleState_Reset, - .TIM_OCNIdleState = TIM_OCNIdleState_Reset, - }, - .channel = pios_tim_ppmout, -}; - -#endif /* PIOS_INCLUDE_PPM_OUT */ - #if defined(PIOS_INCLUDE_RCVR) #include "pios_rcvr_priv.h" diff --git a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h index 76b3860e7..97f307cb5 100644 --- a/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h +++ b/flight/targets/boards/oplinkmini/firmware/inc/pios_config.h @@ -87,7 +87,7 @@ /* #define PIOS_INCLUDE_HCSR04 */ /* PIOS receiver drivers */ -/* #define PIOS_INCLUDE_PWM */ +#define PIOS_INCLUDE_PWM #define PIOS_INCLUDE_PPM /* #define PIOS_INCLUDE_PPM_FLEXI */ /* #define PIOS_INCLUDE_DSM */ @@ -101,7 +101,7 @@ /* PIOS common peripherals */ #define PIOS_INCLUDE_LED #define PIOS_INCLUDE_IAP -/* #define PIOS_INCLUDE_SERVO */ +#define PIOS_INCLUDE_SERVO /* #define PIOS_INCLUDE_I2C_ESC */ /* #define PIOS_INCLUDE_OVERO */ /* #define PIOS_OVERO_SPI */ diff --git a/flight/targets/boards/oplinkmini/firmware/pios_board.c b/flight/targets/boards/oplinkmini/firmware/pios_board.c index 3b44e27b7..9f904026c 100644 --- a/flight/targets/boards/oplinkmini/firmware/pios_board.c +++ b/flight/targets/boards/oplinkmini/firmware/pios_board.c @@ -32,6 +32,9 @@ #include #include #include +#ifdef PIOS_INCLUDE_SERVO +#include +#endif /* * Pull in the board-specific static HW definitions. @@ -76,11 +79,9 @@ uint8_t *pios_uart_tx_buffer; uintptr_t pios_uavo_settings_fs_id; +uint8_t servo_count = 0; + // Forward definitions -static void PIOS_InitUartMainPort(); -static void PIOS_InitUartFlexiPort(); -static void PIOS_InitPPMMainPort(bool input); -static void PIOS_InitPPMFlexiPort(bool input); static void PIOS_Board_PPM_callback(const int16_t *channels); /** @@ -232,16 +233,53 @@ void PIOS_Board_Init(void) bool is_oneway = (oplinkSettings.OneWay == OPLINKSETTINGS_ONEWAY_TRUE); bool ppm_only = (oplinkSettings.PPMOnly == OPLINKSETTINGS_PPMONLY_TRUE); bool ppm_mode = false; + bool servo_main = false; + bool servo_flexi = false; switch (oplinkSettings.MainPort) { case OPLINKSETTINGS_MAINPORT_TELEMETRY: case OPLINKSETTINGS_MAINPORT_SERIAL: + { /* Configure the main port for uart serial */ - PIOS_InitUartMainPort(); +#ifndef PIOS_RFM22B_DEBUG_ON_TELEM + uint32_t pios_usart1_id; + if (PIOS_USART_Init(&pios_usart1_id, &pios_usart_serial_cfg)) { + PIOS_Assert(0); + } + PIOS_Assert(pios_uart_rx_buffer); + PIOS_Assert(pios_uart_tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_uart_main_id, &pios_usart_com_driver, pios_usart1_id, + pios_uart_rx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, + pios_uart_tx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { + PIOS_Assert(0); + } PIOS_COM_TELEMETRY = PIOS_COM_TELEM_UART_MAIN; +#endif break; + } case OPLINKSETTINGS_MAINPORT_PPM: - PIOS_InitPPMMainPort(is_coordinator); + { +#if defined(PIOS_INCLUDE_PPM) + /* PPM input is configured on the coordinator modem and output on the remote modem. */ + if (is_coordinator) { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_main_cfg); + + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + } + // For some reason, PPM output on the main port doesn't work. +#if defined(PIOS_INCLUDE_PPM_OUT) + else { + PIOS_PPM_Out_Init(&pios_ppm_out_id, &pios_main_ppm_out_cfg); + } +#endif /* PIOS_INCLUDE_PPM_OUT */ ppm_mode = true; +#endif /* PIOS_INCLUDE_PPM */ + break; + } + case OPLINKSETTINGS_MAINPORT_PWM: + servo_main = true; break; case OPLINKSETTINGS_MAINPORT_DISABLED: break; @@ -251,14 +289,45 @@ void PIOS_Board_Init(void) switch (oplinkSettings.FlexiPort) { case OPLINKSETTINGS_FLEXIPORT_TELEMETRY: case OPLINKSETTINGS_FLEXIPORT_SERIAL: + { /* Configure the flexi port as uart serial */ - PIOS_InitUartFlexiPort(); + uint32_t pios_usart3_id; + + if (PIOS_USART_Init(&pios_usart3_id, &pios_usart_telem_flexi_cfg)) { + PIOS_Assert(0); + } + PIOS_Assert(pios_uart_rx_buffer); + PIOS_Assert(pios_uart_tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_uart_flexi_id, &pios_usart_com_driver, pios_usart3_id, + pios_uart_rx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, + pios_uart_tx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { + PIOS_Assert(0); + } PIOS_COM_TELEMETRY = PIOS_COM_TELEM_UART_FLEXI; break; + } case OPLINKSETTINGS_FLEXIPORT_PPM: - PIOS_InitPPMFlexiPort(is_coordinator); + { +#if defined(PIOS_INCLUDE_PPM) + /* PPM input is configured on the coordinator modem and output on the remote modem. */ + if (is_coordinator) { + uint32_t pios_ppm_id; + PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_flexi_cfg); + + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { + PIOS_Assert(0); + } + } + else { + PIOS_PPM_Out_Init(&pios_ppm_out_id, &pios_flexi_ppm_out_cfg); + } +#endif /* PIOS_INCLUDE_PPM */ ppm_mode = true; break; + } + case OPLINKSETTINGS_FLEXIPORT_PWM: + servo_flexi = true; + break; case OPLINKSETTINGS_FLEXIPORT_DISABLED: break; } @@ -272,6 +341,22 @@ void PIOS_Board_Init(void) break; } +#if defined(PIOS_INCLUDE_SERVO) + if (servo_main) { + if (servo_flexi) { + servo_count = 4; + PIOS_Servo_Init(&pios_servo_main_flexi_cfg); + } else { + servo_count = 2; + PIOS_Servo_Init(&pios_servo_main_cfg); + } + } else if (servo_flexi) { + servo_count = 2; + PIOS_Servo_Init(&pios_servo_flexi_cfg); + } + ppm_mode = ppm_mode || (servo_count > 0); +#endif + // Initialize out status object. OPLinkStatusData oplinkStatus; OPLinkStatusGet(&oplinkStatus); @@ -412,83 +497,22 @@ void PIOS_Board_Init(void) PIOS_GPIO_Init(); } -static void PIOS_InitUartMainPort() -{ -#ifndef PIOS_RFM22B_DEBUG_ON_TELEM - uint32_t pios_usart1_id; - if (PIOS_USART_Init(&pios_usart1_id, &pios_usart_serial_cfg)) { - PIOS_Assert(0); - } - PIOS_Assert(pios_uart_rx_buffer); - PIOS_Assert(pios_uart_tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_uart_main_id, &pios_usart_com_driver, pios_usart1_id, - pios_uart_rx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, - pios_uart_tx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { - PIOS_Assert(0); - } -#endif -} - -static void PIOS_InitUartFlexiPort() -{ - uint32_t pios_usart3_id; - - if (PIOS_USART_Init(&pios_usart3_id, &pios_usart_telem_flexi_cfg)) { - PIOS_Assert(0); - } - PIOS_Assert(pios_uart_rx_buffer); - PIOS_Assert(pios_uart_tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_uart_flexi_id, &pios_usart_com_driver, pios_usart3_id, - pios_uart_rx_buffer, PIOS_COM_TELEM_RX_BUF_LEN, - pios_uart_tx_buffer, PIOS_COM_TELEM_TX_BUF_LEN)) { - PIOS_Assert(0); - } -} - -static void PIOS_InitPPMMainPort(bool input) -{ -#if defined(PIOS_INCLUDE_PPM) - /* PPM input is configured on the coordinator modem and output on the remote modem. */ - if (input) { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_main_cfg); - - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - } - // For some reason, PPM output on the main port doesn't work. -#endif /* PIOS_INCLUDE_PPM */ -} - -static void PIOS_InitPPMFlexiPort(bool input) -{ -#if defined(PIOS_INCLUDE_PPM) - /* PPM input is configured on the coordinator modem and output on the remote modem. */ - if (input) { - uint32_t pios_ppm_id; - PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_flexi_cfg); - - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { - PIOS_Assert(0); - } - } -#if defined(PIOS_INCLUDE_PPM_OUT) - else { - PIOS_PPM_Out_Init(&pios_ppm_out_id, &pios_ppm_out_cfg); - } -#endif /* PIOS_INCLUDE_PPM_OUT */ -#endif /* PIOS_INCLUDE_PPM */ -} - static void PIOS_Board_PPM_callback(const int16_t *channels) { #if defined(PIOS_INCLUDE_PPM) && defined(PIOS_INCLUDE_PPM_OUT) if (pios_ppm_out_id) { for (uint8_t i = 0; i < RFM22B_PPM_NUM_CHANNELS; ++i) { - PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, channels[i]); + if (channels[i] != PIOS_RCVR_INVALID) { + PIOS_PPM_OUT_Set(PIOS_PPM_OUTPUT, i, channels[i]); + } } } +#if defined(PIOS_INCLUDE_SERVO) + for (uint8_t i = 0; i < servo_count; ++i) { + uint16_t val = (channels[i] == PIOS_RCVR_INVALID) ? 0 : channels[i]; + PIOS_Servo_Set(i, val); + } +#endif /* PIOS_INCLUDE_SERVO */ #endif /* PIOS_INCLUDE_PPM && PIOS_INCLUDE_PPM_OUT */ } diff --git a/flight/targets/boards/oplinkmini/pios_board.h b/flight/targets/boards/oplinkmini/pios_board.h index 8615b0c6e..185f90992 100644 --- a/flight/targets/boards/oplinkmini/pios_board.h +++ b/flight/targets/boards/oplinkmini/pios_board.h @@ -249,6 +249,11 @@ extern uint32_t pios_ppm_out_id; #define PIOS_PPM_MAX_DEVS 1 #define PIOS_PPM_NUM_INPUTS 8 +// ------------------------- +// Receiver PWM inputs +// ------------------------- +#define PIOS_PWM_NUM_INPUTS 4 + // ------------------------- // Servo outputs // ------------------------- diff --git a/shared/uavobjectdefinition/oplinksettings.xml b/shared/uavobjectdefinition/oplinksettings.xml index 69aafed5d..5c8f56807 100644 --- a/shared/uavobjectdefinition/oplinksettings.xml +++ b/shared/uavobjectdefinition/oplinksettings.xml @@ -6,8 +6,8 @@ - - + + From fd8d5352e29aa7ca969f8501a6987d862e30cafc Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Wed, 3 Jul 2013 20:20:31 -0700 Subject: [PATCH 25/28] OP-932 Adds tooltips to the channel widgets. --- .../src/plugins/config/pipxtreme.ui | 27 ++++++++++++++++++- 1 file changed, 26 insertions(+), 1 deletion(-) diff --git a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui index 3cfd47ca1..5730bb139 100644 --- a/ground/openpilotgcs/src/plugins/config/pipxtreme.ui +++ b/ground/openpilotgcs/src/plugins/config/pipxtreme.ui @@ -1477,6 +1477,9 @@ + + This modem will be a coordinator and other modems will bind to it. + Coordinator @@ -1507,7 +1510,11 @@ - + + + Com speed in bps. + + @@ -1632,6 +1639,9 @@ + + If selected, data will only be transmitted from the coordinator to the Rx modem. + One-Way Link @@ -1639,6 +1649,9 @@ + + Only PPM packets will be transmitted. + PPM Only @@ -1656,6 +1669,9 @@ + + Channel 0 is 430 MHz, channel 249 is 440 MHz, and the channel spacing is 40 KHz. + 249 @@ -1673,6 +1689,9 @@ + + Channel 0 is 430 MHz, channel 249 is 440 MHz, and the channel spacing is 40 KHz. + 249 @@ -1690,6 +1709,9 @@ + + Sets the random sequence of channels to use for frequency hopping. + 249 @@ -1697,6 +1719,9 @@ + + PPM packets will be received by this modem. Must be selected if Coordinator modem is configured for PPM. + PPM From 6dbecca7a51efed4b046fc1a3af2e1f93e32e6f7 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Fri, 5 Jul 2013 18:25:52 -0700 Subject: [PATCH 26/28] OP-932 Fixes erasing settings on OPLink and blocks OPLinkSettings objects from being transmitted over the radio. --- .../modules/RadioComBridge/RadioComBridge.c | 1 + .../boards/oplinkmini/firmware/pios_board.c | 36 ++++++++----------- 2 files changed, 15 insertions(+), 22 deletions(-) diff --git a/flight/modules/RadioComBridge/RadioComBridge.c b/flight/modules/RadioComBridge/RadioComBridge.c index c2f69266c..88b2bb9d9 100644 --- a/flight/modules/RadioComBridge/RadioComBridge.c +++ b/flight/modules/RadioComBridge/RadioComBridge.c @@ -522,6 +522,7 @@ static void ProcessRadioStream(UAVTalkConnection inConnectionHandle, UAVTalkConn uint32_t objId = UAVTalkGetPacketObjId(inConnectionHandle); switch (objId) { case OPLINKSTATUS_OBJID: + case OPLINKSETTINGS_OBJID: break; case OPLINKRECEIVER_OBJID: UAVTalkReceiveObject(inConnectionHandle); diff --git a/flight/targets/boards/oplinkmini/firmware/pios_board.c b/flight/targets/boards/oplinkmini/firmware/pios_board.c index 9f904026c..f5921a37f 100644 --- a/flight/targets/boards/oplinkmini/firmware/pios_board.c +++ b/flight/targets/boards/oplinkmini/firmware/pios_board.c @@ -124,6 +124,18 @@ void PIOS_Board_Init(void) PIOS_RTC_Init(&pios_rtc_main_cfg); #endif /* PIOS_INCLUDE_RTC */ + /* IAP System Setup */ + PIOS_IAP_Init(); + // check for safe mode commands from gcs + if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && + PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && + PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { + PIOS_FLASHFS_Format(pios_uavo_settings_fs_id); + PIOS_IAP_WriteBootCmd(0, 0); + PIOS_IAP_WriteBootCmd(1, 0); + PIOS_IAP_WriteBootCmd(2, 0); + } + #if defined(PIOS_INCLUDE_RFM22B) OPLinkSettingsInitialize(); OPLinkStatusInitialize(); @@ -133,28 +145,6 @@ void PIOS_Board_Init(void) PIOS_LED_Init(&pios_led_cfg); #endif /* PIOS_INCLUDE_LED */ - OPLinkSettingsData oplinkSettings; - - /* IAP System Setup */ - PIOS_IAP_Init(); - // check for safe mode commands from gcs - if (PIOS_IAP_ReadBootCmd(0) == PIOS_IAP_CLEAR_FLASH_CMD_0 && - PIOS_IAP_ReadBootCmd(1) == PIOS_IAP_CLEAR_FLASH_CMD_1 && - PIOS_IAP_ReadBootCmd(2) == PIOS_IAP_CLEAR_FLASH_CMD_2) { - OPLinkSettingsGet(&oplinkSettings); - OPLinkSettingsSetDefaults(&oplinkSettings, 0); - OPLinkSettingsSet(&oplinkSettings); - // PIOS_EEPROM_Save((uint8_t*)&oplinkSettings, sizeof(OPLinkSettingsData)); - for (uint32_t i = 0; i < 10; i++) { - PIOS_DELAY_WaitmS(100); - PIOS_LED_Toggle(PIOS_LED_HEARTBEAT); - } - PIOS_IAP_WriteBootCmd(0, 0); - PIOS_IAP_WriteBootCmd(1, 0); - PIOS_IAP_WriteBootCmd(2, 0); - } - OPLinkSettingsGet(&oplinkSettings); - /* Initialize the delayed callback library */ CallbackSchedulerInitialize(); @@ -229,6 +219,8 @@ void PIOS_Board_Init(void) pios_uart_tx_buffer = (uint8_t *)pvPortMalloc(PIOS_COM_TELEM_TX_BUF_LEN); // Configure the main port + OPLinkSettingsData oplinkSettings; + OPLinkSettingsGet(&oplinkSettings); bool is_coordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); bool is_oneway = (oplinkSettings.OneWay == OPLINKSETTINGS_ONEWAY_TRUE); bool ppm_only = (oplinkSettings.PPMOnly == OPLINKSETTINGS_PPMONLY_TRUE); From 64fd0a7a6fd0016f1648cf200c957a6f8b2adf9d Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Fri, 5 Jul 2013 18:46:24 -0700 Subject: [PATCH 27/28] Updates WHATSNEW.txt for 13.06.02 release. --- WHATSNEW.txt | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/WHATSNEW.txt b/WHATSNEW.txt index e56bcf58f..360f5dc30 100644 --- a/WHATSNEW.txt +++ b/WHATSNEW.txt @@ -1,3 +1,9 @@ +2013-07-05 +Refactoring of OPLink radio driver. Auto-configuration was removed, and a +one-way link was added, including a ppm-only mode that is intended to be used +when only a PPM link is desired. PPM-only mode configures the modem as a +one-way link running at 9600 bps (air datarate) and only sends PPM packets. + 2013-06-24 Temporary disabled AltitudeHold and AltitudeVario flight modes. They were not officially supported. But since people expected well-known production quality From 0d02df4d02354de4098f086cc00aa2ad3551c017 Mon Sep 17 00:00:00 2001 From: Brian Webb Date: Fri, 5 Jul 2013 18:49:26 -0700 Subject: [PATCH 28/28] Uncrustified OPLink code. --- flight/pios/common/pios_rfm22b.c | 2 +- flight/targets/boards/oplinkmini/board_hw_defs.c | 6 +++--- .../targets/boards/oplinkmini/firmware/pios_board.c | 13 ++++++------- 3 files changed, 10 insertions(+), 11 deletions(-) diff --git a/flight/pios/common/pios_rfm22b.c b/flight/pios/common/pios_rfm22b.c index 09b8907fe..c663da70d 100644 --- a/flight/pios/common/pios_rfm22b.c +++ b/flight/pios/common/pios_rfm22b.c @@ -1909,7 +1909,7 @@ static enum pios_radio_event radio_receivePacket(struct pios_rfm22b_dev *radio_d // Is this a valid channel? if (p[0] & (1 << i)) { uint32_t val = p[i + 1]; - radio_dev->ppm[i] = (uint16_t)(1000 + val * 900 / 256); + radio_dev->ppm[i] = (uint16_t)(1000 + val * 900 / 256); } else { radio_dev->ppm[i] = PIOS_RCVR_INVALID; } diff --git a/flight/targets/boards/oplinkmini/board_hw_defs.c b/flight/targets/boards/oplinkmini/board_hw_defs.c index 6bb023d18..cd106914a 100644 --- a/flight/targets/boards/oplinkmini/board_hw_defs.c +++ b/flight/targets/boards/oplinkmini/board_hw_defs.c @@ -491,7 +491,7 @@ static const struct pios_tim_channel pios_tim_all_port_pins[] = { static const struct pios_tim_channel pios_tim_main_port_ppm = { .timer = TIM1, - .timer_chan = TIM_Channel_3, + .timer_chan = TIM_Channel_3, .pin = { .gpio = GPIOA, .init = { @@ -504,7 +504,7 @@ static const struct pios_tim_channel pios_tim_main_port_ppm = { static const struct pios_tim_channel pios_tim_flexi_port_ppm = { .timer = TIM2, - .timer_chan = TIM_Channel_4, + .timer_chan = TIM_Channel_4, .pin = { .gpio = GPIOB, .init = { @@ -513,7 +513,7 @@ static const struct pios_tim_channel pios_tim_flexi_port_ppm = { .GPIO_Speed = GPIO_Speed_2MHz, }, }, - .remap = GPIO_PartialRemap2_TIM2, + .remap = GPIO_PartialRemap2_TIM2, }; #endif /* PIOS_INCLUDE_TIM */ diff --git a/flight/targets/boards/oplinkmini/firmware/pios_board.c b/flight/targets/boards/oplinkmini/firmware/pios_board.c index f5921a37f..5c429d57e 100644 --- a/flight/targets/boards/oplinkmini/firmware/pios_board.c +++ b/flight/targets/boards/oplinkmini/firmware/pios_board.c @@ -222,10 +222,10 @@ void PIOS_Board_Init(void) OPLinkSettingsData oplinkSettings; OPLinkSettingsGet(&oplinkSettings); bool is_coordinator = (oplinkSettings.Coordinator == OPLINKSETTINGS_COORDINATOR_TRUE); - bool is_oneway = (oplinkSettings.OneWay == OPLINKSETTINGS_ONEWAY_TRUE); - bool ppm_only = (oplinkSettings.PPMOnly == OPLINKSETTINGS_PPMONLY_TRUE); - bool ppm_mode = false; - bool servo_main = false; + bool is_oneway = (oplinkSettings.OneWay == OPLINKSETTINGS_ONEWAY_TRUE); + bool ppm_only = (oplinkSettings.PPMOnly == OPLINKSETTINGS_PPMONLY_TRUE); + bool ppm_mode = false; + bool servo_main = false; bool servo_flexi = false; switch (oplinkSettings.MainPort) { case OPLINKSETTINGS_MAINPORT_TELEMETRY: @@ -255,7 +255,7 @@ void PIOS_Board_Init(void) if (is_coordinator) { uint32_t pios_ppm_id; PIOS_PPM_Init(&pios_ppm_id, &pios_ppm_main_cfg); - + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { PIOS_Assert(0); } @@ -309,8 +309,7 @@ void PIOS_Board_Init(void) if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, pios_ppm_id)) { PIOS_Assert(0); } - } - else { + } else { PIOS_PPM_Out_Init(&pios_ppm_out_id, &pios_flexi_ppm_out_cfg); } #endif /* PIOS_INCLUDE_PPM */