From 9a882811a275ecc55e507bfc04ac46dfbc805cb8 Mon Sep 17 00:00:00 2001 From: Oleg Semyonov Date: Mon, 1 Aug 2011 12:40:45 +0300 Subject: [PATCH 1/5] packaging: put only CC firmware into the distributable package All firmware still will be built, but only CC firmware is packaged to prevent abuse of bootloader updaters etc. --- package/osx/package | 3 ++- package/winx86/openpilotgcs.nsi | 6 ++++-- 2 files changed, 6 insertions(+), 3 deletions(-) diff --git a/package/osx/package b/package/osx/package index a12e7e9fe..9a8108015 100755 --- a/package/osx/package +++ b/package/osx/package @@ -20,7 +20,8 @@ device=$(hdiutil attach "${TEMP_FILE}" | \ # packaging goes here cp -r "${APP_PATH}" "/Volumes/${VOL_NAME}" -cp -r "${FW_DIR}" "/Volumes/${VOL_NAME}/firmware" +#cp -r "${FW_DIR}" "/Volumes/${VOL_NAME}/firmware" +cp "${FW_DIR}/fw_coptercontrol-${PACKAGE_LBL}.opfw" "/Volumes/${VOL_NAME}/firmware" "${ROOT_DIR}/package/osx/libraries" \ "/Volumes/${VOL_NAME}/OpenPilot GCS.app" || exit 1 diff --git a/package/winx86/openpilotgcs.nsi b/package/winx86/openpilotgcs.nsi index 9a61e4f96..e5f9eddb6 100644 --- a/package/winx86/openpilotgcs.nsi +++ b/package/winx86/openpilotgcs.nsi @@ -185,8 +185,10 @@ Section "Localization" InSecLocalization SectionEnd Section "Firmware" InSecFirmware - SetOutPath "$INSTDIR\firmware\${FIRMWARE_DIR}" - File /r "${PACKAGE_DIR}\${FIRMWARE_DIR}\*" +; SetOutPath "$INSTDIR\firmware\${FIRMWARE_DIR}" +; File /r "${PACKAGE_DIR}\${FIRMWARE_DIR}\*" + SetOutPath "$INSTDIR\firmware" + File /r "${PACKAGE_DIR}\${FIRMWARE_DIR}\fw_coptercontrol-${PACKAGE_LBL}.opfw" SectionEnd Section "Shortcuts" InSecShortcuts From 829b8b83f60cbc98d2d349fc40afe479e134854f Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Thu, 28 Jul 2011 08:22:47 -0400 Subject: [PATCH 2/5] rcvr: Add GCS receiver driver for rcvr via telemetry This allows the GCS to emulate a receiver device via the telemetry link. Select "GCS" as your input type in the manualcontrol config screen and calibrate it as normal. Note: The expected values for the channels are in microseconds just like a PWM or PPM input device. The channel values are validated against minimum/maximum pulse lengths just like normal receivers. --- flight/CopterControl/Makefile | 2 + flight/CopterControl/System/inc/pios_config.h | 1 + flight/CopterControl/System/pios_board.c | 20 +++++ flight/PiOS/Common/pios_gcsrcvr.c | 75 +++++++++++++++++++ flight/PiOS/inc/pios_gcsrcvr_priv.h | 47 ++++++++++++ flight/PiOS/inc/pios_spektrum_priv.h | 2 +- .../src/plugins/uavobjects/uavobjects.pro | 2 + shared/uavobjectdefinition/gcsreceiver.xml | 10 +++ .../manualcontrolsettings.xml | 2 +- 9 files changed, 159 insertions(+), 2 deletions(-) create mode 100644 flight/PiOS/Common/pios_gcsrcvr.c create mode 100644 flight/PiOS/inc/pios_gcsrcvr_priv.h create mode 100644 shared/uavobjectdefinition/gcsreceiver.xml diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 78e62bb89..3ac59e59b 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -164,6 +164,7 @@ SRC += $(OPUAVSYNTHDIR)/mixerstatus.c SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c SRC += $(OPUAVSYNTHDIR)/attitudesettings.c SRC += $(OPUAVSYNTHDIR)/hwsettings.c +SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c #${wildcard ${OBJ}/$(shell echo $(VAR) | tr A-Z a-z)/*.c} #SRC += ${foreach OBJ, ${UAVOBJECTS}, $(UAVOBJECTS)/$(OBJ).c} # Cant use until i can automatically generate list of UAVObjects @@ -207,6 +208,7 @@ SRC += $(PIOSCOMMON)/pios_i2c_esc.c SRC += $(PIOSCOMMON)/pios_iap.c SRC += $(PIOSCOMMON)/pios_bl_helper.c SRC += $(PIOSCOMMON)/pios_rcvr.c +SRC += $(PIOSCOMMON)/pios_gcsrcvr.c SRC += $(PIOSCOMMON)/printf-stdarg.c ## Libraries for flight calculations SRC += $(FLIGHTLIB)/fifo_buffer.c diff --git a/flight/CopterControl/System/inc/pios_config.h b/flight/CopterControl/System/inc/pios_config.h index 944fe38c8..c2d535709 100644 --- a/flight/CopterControl/System/inc/pios_config.h +++ b/flight/CopterControl/System/inc/pios_config.h @@ -50,6 +50,7 @@ #define PIOS_INCLUDE_SBUS //#define PIOS_INCLUDE_PPM #define PIOS_INCLUDE_PWM +#define PIOS_INCLUDE_GCSRCVR /* Supported USART-based PIOS modules */ #define PIOS_INCLUDE_TELEMETRY_RF diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index ee3deaf5c..50677de27 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -856,6 +856,10 @@ void PIOS_I2C_main_adapter_er_irq_handler(void) #endif /* PIOS_INCLUDE_I2C */ +#if defined(PIOS_INCLUDE_GCSRCVR) +#include "pios_gcsrcvr_priv.h" +#endif /* PIOS_INCLUDE_GCSRCVR */ + #if defined(PIOS_INCLUDE_RCVR) #include "pios_rcvr_priv.h" @@ -1142,6 +1146,22 @@ void PIOS_Board_Init(void) { } #endif /* PIOS_INCLUDE_SBUS */ break; + case MANUALCONTROLSETTINGS_INPUTMODE_GCS: +#if defined(PIOS_INCLUDE_GCSRCVR) + PIOS_GCSRCVR_Init(); + uint32_t pios_gcsrcvr_rcvr_id; + if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, 0)) { + PIOS_Assert(0); + } + for (uint8_t i = 0; + i < GCSRECEIVER_CHANNEL_NUMELEM && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); + i++) { + pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_gcsrcvr_rcvr_id; + pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; + pios_rcvr_max_channel++; + } +#endif /* PIOS_INCLUDE_GCSRCVR */ + break; } /* Remap AFIO pin */ diff --git a/flight/PiOS/Common/pios_gcsrcvr.c b/flight/PiOS/Common/pios_gcsrcvr.c new file mode 100644 index 000000000..18e6644a6 --- /dev/null +++ b/flight/PiOS/Common/pios_gcsrcvr.c @@ -0,0 +1,75 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_GCSRCVR GCS Receiver Input Functions + * @brief Code to read the channels within the GCS Receiver UAVObject + * @{ + * + * @file pios_gcsrcvr.c + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @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 + */ + +/* Project Includes */ +#include "pios.h" + +#if defined(PIOS_INCLUDE_GCSRCVR) + +#include "pios_gcsrcvr_priv.h" + +static GCSReceiverData gcsreceiverdata; + +/* Provide a RCVR driver */ +static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel); + +const struct pios_rcvr_driver pios_gcsrcvr_rcvr_driver = { + .read = PIOS_GCSRCVR_Get, +}; + +static void gcsreceiver_updated(UAVObjEvent * ev) +{ + if (ev->obj == GCSReceiverHandle()) { + GCSReceiverGet(&gcsreceiverdata); + } +} + +void PIOS_GCSRCVR_Init(void) +{ + /* Register uavobj callback */ + GCSReceiverConnectCallback (gcsreceiver_updated); +} + +static int32_t PIOS_GCSRCVR_Get(uint32_t rcvr_id, uint8_t channel) +{ + if (channel >= GCSRECEIVER_CHANNEL_NUMELEM) { + /* channel is out of range */ + return -1; + } + + return (gcsreceiverdata.Channel[channel]); +} + +#endif /* PIOS_INCLUDE_GCSRCVR */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS/inc/pios_gcsrcvr_priv.h b/flight/PiOS/inc/pios_gcsrcvr_priv.h new file mode 100644 index 000000000..9a828cb95 --- /dev/null +++ b/flight/PiOS/inc/pios_gcsrcvr_priv.h @@ -0,0 +1,47 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_GCSRCVR GCS Receiver Functions + * @brief PIOS interface to read from GCS receiver port + * @{ + * + * @file pios_gcsrcvr_priv.h + * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010. + * @brief GCS 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_GCSRCVR_PRIV_H +#define PIOS_GCSRCVR_PRIV_H + +#include + +#include "gcsreceiver.h" + +extern const struct pios_rcvr_driver pios_gcsrcvr_rcvr_driver; + +extern void PIOS_GCSRCVR_Init(void); + +#endif /* PIOS_GCSRCVR_PRIV_H */ + +/** + * @} + * @} + */ diff --git a/flight/PiOS/inc/pios_spektrum_priv.h b/flight/PiOS/inc/pios_spektrum_priv.h index 2d31a8027..eb82c4f44 100644 --- a/flight/PiOS/inc/pios_spektrum_priv.h +++ b/flight/PiOS/inc/pios_spektrum_priv.h @@ -44,7 +44,7 @@ extern const struct pios_rcvr_driver pios_spektrum_rcvr_driver; extern int32_t PIOS_SPEKTRUM_Init(uint32_t * spektrum_id, const struct pios_spektrum_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id, bool bind); -#endif /* PIOS_PWM_PRIV_H */ +#endif /* PIOS_SPEKTRUM_PRIV_H */ /** * @} diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 9c6188fad..3831a4ec3 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -69,6 +69,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/sonaraltitude.h \ $$UAVOBJECT_SYNTHETICS/flightstatus.h \ $$UAVOBJECT_SYNTHETICS/hwsettings.h \ + $$UAVOBJECT_SYNTHETICS/gcsreceiver.h \ $$UAVOBJECT_SYNTHETICS/attitudesettings.h SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ @@ -118,4 +119,5 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/uavobjectsinit.cpp \ $$UAVOBJECT_SYNTHETICS/flightstatus.cpp \ $$UAVOBJECT_SYNTHETICS/hwsettings.cpp \ + $$UAVOBJECT_SYNTHETICS/gcsreceiver.cpp \ $$UAVOBJECT_SYNTHETICS/attitudesettings.cpp diff --git a/shared/uavobjectdefinition/gcsreceiver.xml b/shared/uavobjectdefinition/gcsreceiver.xml new file mode 100644 index 000000000..d21598e48 --- /dev/null +++ b/shared/uavobjectdefinition/gcsreceiver.xml @@ -0,0 +1,10 @@ + + + A receiver channel group carried over the telemetry link. + + + + + + + diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index bfd692f91..3b76d70f8 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -1,7 +1,7 @@ Settings to indicate how to decode receiver input by @ref ManualControlModule. - + From 06cdeb7b61dc83f9cf245e9851686013655bbc58 Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Sat, 30 Jul 2011 15:04:24 -0400 Subject: [PATCH 3/5] rcvr: support multiple simultaneous receivers Now also supports multiple instances of the Spektrum driver. These are configured as Spektrum1 and Spektrum2. --- flight/CopterControl/System/pios_board.c | 124 +++---- flight/Modules/ManualControl/manualcontrol.c | 63 ++-- flight/OpenPilot/System/pios_board.c | 219 ++++++------- flight/OpenPilot/UAVObjects.inc | 1 + flight/PiOS/Boards/STM32103CB_CC_Rev1.h | 3 +- flight/PiOS/Boards/STM3210E_OP.h | 1 + flight/PiOS/STM32F10x/pios_spektrum.c | 303 +++++++++++------- flight/PiOS/inc/pios_rcvr.h | 7 - shared/uavobjectdefinition/hwsettings.xml | 10 +- .../manualcontrolsettings.xml | 24 +- 10 files changed, 413 insertions(+), 342 deletions(-) diff --git a/flight/CopterControl/System/pios_board.c b/flight/CopterControl/System/pios_board.c index 50677de27..59b66ea3a 100644 --- a/flight/CopterControl/System/pios_board.c +++ b/flight/CopterControl/System/pios_board.c @@ -863,8 +863,12 @@ void PIOS_I2C_main_adapter_er_irq_handler(void) #if defined(PIOS_INCLUDE_RCVR) #include "pios_rcvr_priv.h" -struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[PIOS_RCVR_MAX_CHANNELS]; -uint32_t pios_rcvr_max_channel; +/* One slot per selectable receiver group. + * eg. PWM, PPM, GCS, SPEKTRUM1, SPEKTRUM2, SBUS + * NOTE: No slot in this map for NONE. + */ +uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; + #endif /* PIOS_INCLUDE_RCVR */ #if defined(PIOS_INCLUDE_USB_HID) @@ -984,7 +988,8 @@ void PIOS_Board_Init(void) { } #endif /* PIOS_INCLUDE_GPS */ break; - case HWSETTINGS_CC_MAINPORT_SPEKTRUM: + case HWSETTINGS_CC_MAINPORT_SPEKTRUM1: + case HWSETTINGS_CC_MAINPORT_SPEKTRUM2: #if defined(PIOS_INCLUDE_SPEKTRUM) { uint32_t pios_usart_spektrum_id; @@ -996,6 +1001,16 @@ void PIOS_Board_Init(void) { if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_main_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) { PIOS_Assert(0); } + + uint32_t pios_spektrum_rcvr_id; + if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, pios_spektrum_id)) { + PIOS_Assert(0); + } + if (hwsettings_cc_mainport == HWSETTINGS_CC_MAINPORT_SPEKTRUM1) { + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM1] = pios_spektrum_rcvr_id; + } else { + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM2] = pios_spektrum_rcvr_id; + } } #endif /* PIOS_INCLUDE_SPEKTRUM */ break; @@ -1046,7 +1061,8 @@ void PIOS_Board_Init(void) { } #endif /* PIOS_INCLUDE_GPS */ break; - case HWSETTINGS_CC_FLEXIPORT_SPEKTRUM: + case HWSETTINGS_CC_FLEXIPORT_SPEKTRUM1: + case HWSETTINGS_CC_FLEXIPORT_SPEKTRUM2: #if defined(PIOS_INCLUDE_SPEKTRUM) { uint32_t pios_usart_spektrum_id; @@ -1058,6 +1074,16 @@ void PIOS_Board_Init(void) { if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_flexi_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) { PIOS_Assert(0); } + + uint32_t pios_spektrum_rcvr_id; + if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, pios_spektrum_id)) { + PIOS_Assert(0); + } + if (hwsettings_cc_flexiport == HWSETTINGS_CC_FLEXIPORT_SPEKTRUM1) { + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM1] = pios_spektrum_rcvr_id; + } else { + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM2] = pios_spektrum_rcvr_id; + } } #endif /* PIOS_INCLUDE_SPEKTRUM */ break; @@ -1074,96 +1100,44 @@ void PIOS_Board_Init(void) { break; } - /* Configure the selected receiver */ - uint8_t manualcontrolsettings_inputmode; - ManualControlSettingsInputModeGet(&manualcontrolsettings_inputmode); + /* Configure the rcvr port */ + uint8_t hwsettings_rcvrport; + HwSettingsRcvrPortGet(&hwsettings_rcvrport); - switch (manualcontrolsettings_inputmode) { - case MANUALCONTROLSETTINGS_INPUTMODE_PWM: + switch (hwsettings_rcvrport) { + case HWSETTINGS_RCVRPORT_DISABLED: + break; + case HWSETTINGS_RCVRPORT_PWM: #if defined(PIOS_INCLUDE_PWM) PIOS_PWM_Init(); uint32_t pios_pwm_rcvr_id; if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, 0)) { PIOS_Assert(0); } - for (uint8_t i = 0; - i < PIOS_PWM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); - i++) { - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_pwm_rcvr_id; - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; - pios_rcvr_max_channel++; - } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; #endif /* PIOS_INCLUDE_PWM */ break; - case MANUALCONTROLSETTINGS_INPUTMODE_PPM: + case HWSETTINGS_RCVRPORT_PPM: #if defined(PIOS_INCLUDE_PPM) PIOS_PPM_Init(); uint32_t pios_ppm_rcvr_id; if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, 0)) { PIOS_Assert(0); } - for (uint8_t i = 0; - i < PIOS_PPM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); - i++) { - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_ppm_rcvr_id; - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; - pios_rcvr_max_channel++; - } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; #endif /* PIOS_INCLUDE_PPM */ break; - case MANUALCONTROLSETTINGS_INPUTMODE_SPEKTRUM: -#if defined(PIOS_INCLUDE_SPEKTRUM) - if (hwsettings_cc_mainport == HWSETTINGS_CC_MAINPORT_SPEKTRUM || - hwsettings_cc_flexiport == HWSETTINGS_CC_FLEXIPORT_SPEKTRUM) { - uint32_t pios_spektrum_rcvr_id; - if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, 0)) { - PIOS_Assert(0); - } - for (uint8_t i = 0; - i < PIOS_SPEKTRUM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); - i++) { - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_spektrum_rcvr_id; - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; - pios_rcvr_max_channel++; - } - } -#endif /* PIOS_INCLUDE_SPEKTRUM */ - break; - case MANUALCONTROLSETTINGS_INPUTMODE_SBUS: -#if defined(PIOS_INCLUDE_SBUS) - if (hwsettings_cc_mainport == HWSETTINGS_CC_MAINPORT_SBUS) { - uint32_t pios_sbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_sbus_rcvr_id, &pios_sbus_rcvr_driver, 0)) { - PIOS_Assert(0); - } - for (uint8_t i = 0; - i < SBUS_NUMBER_OF_CHANNELS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); - i++) { - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_sbus_rcvr_id; - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; - pios_rcvr_max_channel++; - } - } -#endif /* PIOS_INCLUDE_SBUS */ - break; - case MANUALCONTROLSETTINGS_INPUTMODE_GCS: -#if defined(PIOS_INCLUDE_GCSRCVR) - PIOS_GCSRCVR_Init(); - uint32_t pios_gcsrcvr_rcvr_id; - if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, 0)) { - PIOS_Assert(0); - } - for (uint8_t i = 0; - i < GCSRECEIVER_CHANNEL_NUMELEM && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); - i++) { - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_gcsrcvr_rcvr_id; - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; - pios_rcvr_max_channel++; - } -#endif /* PIOS_INCLUDE_GCSRCVR */ - break; } +#if defined(PIOS_INCLUDE_GCSRCVR) + PIOS_GCSRCVR_Init(); + uint32_t pios_gcsrcvr_rcvr_id; + if (PIOS_RCVR_Init(&pios_gcsrcvr_rcvr_id, &pios_gcsrcvr_rcvr_driver, 0)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS] = pios_gcsrcvr_rcvr_id; +#endif /* PIOS_INCLUDE_GCSRCVR */ + /* Remap AFIO pin */ GPIO_PinRemapConfig( GPIO_Remap_SWJ_NoJTRST, ENABLE); PIOS_Servo_Init(); diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 19b6bc293..7c16ac172 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -141,7 +141,7 @@ static void manualControlTask(void *parameters) // Main task loop lastSysTime = xTaskGetTickCount(); while (1) { - float scaledChannel[MANUALCONTROLCOMMAND_CHANNEL_NUMELEM]; + float scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM]; // Wait until next update vTaskDelayUntil(&lastSysTime, UPDATE_PERIOD_MS / portTICK_RATE_MS); @@ -165,22 +165,28 @@ static void manualControlTask(void *parameters) if (!ManualControlCommandReadOnly(&cmd)) { // Read channel values in us - for (int n = 0; n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; ++n) { - if (pios_rcvr_channel_to_id_map[n].id) { - cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_channel_to_id_map[n].id, - pios_rcvr_channel_to_id_map[n].channel); - } else { + for (uint8_t n = 0; + n < MANUALCONTROLSETTINGS_CHANNELGROUPS_NUMELEM && n < MANUALCONTROLCOMMAND_CHANNEL_NUMELEM; + ++n) { + extern uint32_t pios_rcvr_group_map[]; + + if (settings.ChannelGroups[n] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { cmd.Channel[n] = -1; + } else if (!pios_rcvr_group_map[settings.ChannelGroups[n]]) { + cmd.Channel[n] = -2; + } else { + cmd.Channel[n] = PIOS_RCVR_Read(pios_rcvr_group_map[settings.ChannelGroups[n]], + settings.ChannelNumber[n]); } scaledChannel[n] = scaleChannel(cmd.Channel[n], settings.ChannelMax[n], settings.ChannelMin[n], settings.ChannelNeutral[n]); } // Check settings, if error raise alarm - if (settings.Roll >= MANUALCONTROLSETTINGS_ROLL_NONE || - settings.Pitch >= MANUALCONTROLSETTINGS_PITCH_NONE || - settings.Yaw >= MANUALCONTROLSETTINGS_YAW_NONE || - settings.Throttle >= MANUALCONTROLSETTINGS_THROTTLE_NONE || - settings.FlightMode >= MANUALCONTROLSETTINGS_FLIGHTMODE_NONE) { + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || + settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || + settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || + settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE || + settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE] >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_CRITICAL); cmd.Connected = MANUALCONTROLCOMMAND_CONNECTED_FALSE; ManualControlCommandSet(&cmd); @@ -188,10 +194,10 @@ static void manualControlTask(void *parameters) } // decide if we have valid manual input or not - bool valid_input_detected = validInputRange(settings.ChannelMin[settings.Throttle], settings.ChannelMax[settings.Throttle], cmd.Channel[settings.Throttle]) && - validInputRange(settings.ChannelMin[settings.Roll], settings.ChannelMax[settings.Roll], cmd.Channel[settings.Roll]) && - validInputRange(settings.ChannelMin[settings.Yaw], settings.ChannelMax[settings.Yaw], cmd.Channel[settings.Yaw]) && - validInputRange(settings.ChannelMin[settings.Pitch], settings.ChannelMax[settings.Pitch], cmd.Channel[settings.Pitch]); + bool valid_input_detected = validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]) && + validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]) && + validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]) && + validInputRange(settings.ChannelMin[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], settings.ChannelMax[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH], cmd.Channel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]); // Implement hysteresis loop on connection status if (valid_input_detected && (++connected_count > 10)) { @@ -218,28 +224,31 @@ static void manualControlTask(void *parameters) AlarmsClear(SYSTEMALARMS_ALARM_MANUALCONTROL); // Scale channels to -1 -> +1 range - cmd.Roll = scaledChannel[settings.Roll]; - cmd.Pitch = scaledChannel[settings.Pitch]; - cmd.Yaw = scaledChannel[settings.Yaw]; - cmd.Throttle = scaledChannel[settings.Throttle]; - flightMode = scaledChannel[settings.FlightMode]; + cmd.Roll = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ROLL]; + cmd.Pitch = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_PITCH]; + cmd.Yaw = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_YAW]; + cmd.Throttle = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_THROTTLE]; + flightMode = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_FLIGHTMODE]; AccessoryDesiredData accessory; // Set Accessory 0 - if(settings.Accessory0 != MANUALCONTROLSETTINGS_ACCESSORY0_NONE) { - accessory.AccessoryVal = scaledChannel[settings.Accessory0]; + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0] != + MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY0]; if(AccessoryDesiredInstSet(0, &accessory) != 0) AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } // Set Accessory 1 - if(settings.Accessory1 != MANUALCONTROLSETTINGS_ACCESSORY1_NONE) { - accessory.AccessoryVal = scaledChannel[settings.Accessory1]; + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1] != + MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY1]; if(AccessoryDesiredInstSet(1, &accessory) != 0) AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } - // Set Accsesory 2 - if(settings.Accessory2 != MANUALCONTROLSETTINGS_ACCESSORY2_NONE) { - accessory.AccessoryVal = scaledChannel[settings.Accessory2]; + // Set Accessory 2 + if (settings.ChannelGroups[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2] != + MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + accessory.AccessoryVal = scaledChannel[MANUALCONTROLSETTINGS_CHANNELGROUPS_ACCESSORY2]; if(AccessoryDesiredInstSet(2, &accessory) != 0) AlarmsSet(SYSTEMALARMS_ALARM_MANUALCONTROL, SYSTEMALARMS_ALARM_WARNING); } diff --git a/flight/OpenPilot/System/pios_board.c b/flight/OpenPilot/System/pios_board.c index 822b7a8ae..9a9397acb 100644 --- a/flight/OpenPilot/System/pios_board.c +++ b/flight/OpenPilot/System/pios_board.c @@ -30,6 +30,7 @@ #include #include #include +#include #include "manualcontrolsettings.h" //#define I2C_DEBUG_PIN 0 @@ -496,7 +497,6 @@ static const struct pios_usart_cfg pios_usart_spektrum_cfg = { }, }; -#include static const struct pios_spektrum_cfg pios_spektrum_cfg = { .bind = { .gpio = GPIOA, @@ -964,8 +964,12 @@ static const struct stm32_gpio pios_debug_pins[] = { #if defined(PIOS_INCLUDE_RCVR) #include "pios_rcvr_priv.h" -struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[PIOS_RCVR_MAX_CHANNELS]; -uint32_t pios_rcvr_max_channel; +/* One slot per selectable receiver group. + * eg. PWM, PPM, GCS, SPEKTRUM1, SPEKTRUM2, SBUS + * NOTE: No slot in this map for NONE. + */ +uint32_t pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE]; + #endif /* PIOS_INCLUDE_RCVR */ #if defined(PIOS_INCLUDE_USB_HID) @@ -1047,126 +1051,125 @@ void PIOS_Board_Init(void) { /* Bind the AHRS comms layer to the AHRS SPI link */ AhrsConnect(pios_spi_ahrs_id); - /* Initialize the PiOS library */ -#if defined(PIOS_INCLUDE_COM) + /* Configure the main IO port */ + uint8_t hwsettings_op_mainport; + HwSettingsOP_MainPortGet(&hwsettings_op_mainport); + + switch (hwsettings_op_mainport) { + case HWSETTINGS_OP_MAINPORT_DISABLED: + break; + case HWSETTINGS_OP_MAINPORT_TELEMETRY: #if defined(PIOS_INCLUDE_TELEMETRY_RF) - { - uint32_t pios_usart_telem_rf_id; - if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) { - PIOS_Assert(0); - } + { + uint32_t pios_usart_telem_rf_id; + if (PIOS_USART_Init(&pios_usart_telem_rf_id, &pios_usart_telem_cfg)) { + PIOS_Assert(0); + } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); - uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); - PIOS_Assert(rx_buffer); - PIOS_Assert(tx_buffer); - if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id, - rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, - tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { - PIOS_Assert(0); + uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_RX_BUF_LEN); + uint8_t * tx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_TELEM_RF_TX_BUF_LEN); + PIOS_Assert(rx_buffer); + PIOS_Assert(tx_buffer); + if (PIOS_COM_Init(&pios_com_telem_rf_id, &pios_usart_com_driver, pios_usart_telem_rf_id, + rx_buffer, PIOS_COM_TELEM_RF_RX_BUF_LEN, + tx_buffer, PIOS_COM_TELEM_RF_TX_BUF_LEN)) { + PIOS_Assert(0); + } } - } #endif /* PIOS_INCLUDE_TELEMETRY_RF */ - -#if defined(PIOS_INCLUDE_GPS) - { - uint32_t pios_usart_gps_id; - if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { - PIOS_Assert(0); - } - uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); - PIOS_Assert(rx_buffer); - if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, - rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, - NULL, 0)) { - PIOS_Assert(0); - } + break; } + + /* Configure the flexi port */ + uint8_t hwsettings_op_flexiport; + HwSettingsOP_FlexiPortGet(&hwsettings_op_flexiport); + + switch (hwsettings_op_flexiport) { + case HWSETTINGS_OP_FLEXIPORT_DISABLED: + break; + case HWSETTINGS_OP_FLEXIPORT_GPS: +#if defined(PIOS_INCLUDE_GPS) + { + uint32_t pios_usart_gps_id; + if (PIOS_USART_Init(&pios_usart_gps_id, &pios_usart_gps_cfg)) { + PIOS_Assert(0); + } + uint8_t * rx_buffer = (uint8_t *) pvPortMalloc(PIOS_COM_GPS_RX_BUF_LEN); + PIOS_Assert(rx_buffer); + if (PIOS_COM_Init(&pios_com_gps_id, &pios_usart_com_driver, pios_usart_gps_id, + rx_buffer, PIOS_COM_GPS_RX_BUF_LEN, + NULL, 0)) { + PIOS_Assert(0); + } + } #endif /* PIOS_INCLUDE_GPS */ -#endif + break; + } PIOS_Servo_Init(); PIOS_ADC_Init(); PIOS_GPIO_Init(); - /* Configure the selected receiver */ - uint8_t manualcontrolsettings_inputmode; - ManualControlSettingsInputModeGet(&manualcontrolsettings_inputmode); + /* Configure the aux port */ + uint8_t hwsettings_op_auxport; + HwSettingsOP_AuxPortGet(&hwsettings_op_auxport); - switch (manualcontrolsettings_inputmode) { - case MANUALCONTROLSETTINGS_INPUTMODE_PWM: -#if defined(PIOS_INCLUDE_PWM) -#if (PIOS_PWM_NUM_INPUTS > PIOS_RCVR_MAX_CHANNELS) -#error More receiver inputs than available devices -#endif - PIOS_PWM_Init(); - uint32_t pios_pwm_rcvr_id; - if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, 0)) { - PIOS_Assert(0); - } - for (uint8_t i = 0; - i < PIOS_PWM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); - i++) { - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_pwm_rcvr_id; - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; - pios_rcvr_max_channel++; - } -#endif /* PIOS_INCLUDE_PWM */ - break; - case MANUALCONTROLSETTINGS_INPUTMODE_PPM: -#if defined(PIOS_INCLUDE_PPM) -#if (PIOS_PPM_NUM_INPUTS > PIOS_RCVR_MAX_CHANNELS) -#error More receiver inputs than available devices -#endif - PIOS_PPM_Init(); - uint32_t pios_ppm_rcvr_id; - if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, 0)) { - PIOS_Assert(0); - } - for (uint8_t i = 0; - i < PIOS_PPM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); - i++) { - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_ppm_rcvr_id; - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; - pios_rcvr_max_channel++; - } -#endif /* PIOS_INCLUDE_PPM */ - break; - case MANUALCONTROLSETTINGS_INPUTMODE_SPEKTRUM: + switch (hwsettings_op_auxport) { + case HWSETTINGS_OP_AUXPORT_DISABLED: + break; + case HWSETTINGS_OP_AUXPORT_DEBUG: + /* Not supported yet */ + break; + case HWSETTINGS_OP_AUXPORT_SPEKTRUM1: #if defined(PIOS_INCLUDE_SPEKTRUM) -#if (PIOS_SPEKTRUM_NUM_INPUTS > PIOS_RCVR_MAX_CHANNELS) -#error More receiver inputs than available devices -#endif - { - uint32_t pios_usart_spektrum_id; - if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_spektrum_id; - if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) { - PIOS_Assert(0); - } - - uint32_t pios_spektrum_rcvr_id; - if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, 0)) { - PIOS_Assert(0); - } - for (uint8_t i = 0; - i < PIOS_SPEKTRUM_NUM_INPUTS && pios_rcvr_max_channel < NELEMENTS(pios_rcvr_channel_to_id_map); - i++) { - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].id = pios_spektrum_rcvr_id; - pios_rcvr_channel_to_id_map[pios_rcvr_max_channel].channel = i; - pios_rcvr_max_channel++; - } + { + uint32_t pios_usart_spektrum_id; + if (PIOS_USART_Init(&pios_usart_spektrum_id, &pios_usart_spektrum_cfg)) { + PIOS_Assert(0); } + + uint32_t pios_spektrum_id; + if (PIOS_SPEKTRUM_Init(&pios_spektrum_id, &pios_spektrum_cfg, &pios_usart_com_driver, pios_usart_spektrum_id, false)) { + PIOS_Assert(0); + } + + uint32_t pios_spektrum_rcvr_id; + if (PIOS_RCVR_Init(&pios_spektrum_rcvr_id, &pios_spektrum_rcvr_driver, pios_spektrum_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM1] = pios_spektrum_rcvr_id; + } #endif - break; - case MANUALCONTROLSETTINGS_INPUTMODE_SBUS: -#if defined(PIOS_INCLUDE_SBUS) -#error SBUS NOT ON OP YET -#endif /* PIOS_INCLUDE_SBUS */ - break; + break; + } + + /* Configure the rcvr port */ + uint8_t hwsettings_rcvrport; + HwSettingsRcvrPortGet(&hwsettings_rcvrport); + + switch (hwsettings_rcvrport) { + case HWSETTINGS_RCVRPORT_DISABLED: + break; + case HWSETTINGS_RCVRPORT_PWM: +#if defined(PIOS_INCLUDE_PWM) + PIOS_PWM_Init(); + uint32_t pios_pwm_rcvr_id; + if (PIOS_RCVR_Init(&pios_pwm_rcvr_id, &pios_pwm_rcvr_driver, 0)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM] = pios_pwm_rcvr_id; +#endif /* PIOS_INCLUDE_PWM */ + break; + case HWSETTINGS_RCVRPORT_PPM: +#if defined(PIOS_INCLUDE_PPM) + PIOS_PPM_Init(); + uint32_t pios_ppm_rcvr_id; + if (PIOS_RCVR_Init(&pios_ppm_rcvr_id, &pios_ppm_rcvr_driver, 0)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM] = pios_ppm_rcvr_id; +#endif /* PIOS_INCLUDE_PPM */ + break; } #if defined(PIOS_INCLUDE_USB_HID) diff --git a/flight/OpenPilot/UAVObjects.inc b/flight/OpenPilot/UAVObjects.inc index 3eb2fa571..a30dcf540 100644 --- a/flight/OpenPilot/UAVObjects.inc +++ b/flight/OpenPilot/UAVObjects.inc @@ -69,6 +69,7 @@ UAVOBJSRCFILENAMES += velocityactual UAVOBJSRCFILENAMES += velocitydesired UAVOBJSRCFILENAMES += watchdogstatus UAVOBJSRCFILENAMES += flightstatus +UAVOBJSRCFILENAMES += hwsettings UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h index 18e0da624..8c1467b0d 100644 --- a/flight/PiOS/Boards/STM32103CB_CC_Rev1.h +++ b/flight/PiOS/Boards/STM32103CB_CC_Rev1.h @@ -206,7 +206,7 @@ extern uint32_t pios_com_telem_usb_id; // PIOS_RCVR // See also pios_board.c //------------------------ -#define PIOS_RCVR_MAX_DEVS 1 +#define PIOS_RCVR_MAX_DEVS 3 #define PIOS_RCVR_MAX_CHANNELS 12 //------------------------- @@ -222,6 +222,7 @@ extern uint32_t pios_com_telem_usb_id; //------------------------- // Receiver SPEKTRUM input //------------------------- +#define PIOS_SPEKTRUM_MAX_DEVS 2 #define PIOS_SPEKTRUM_NUM_INPUTS 12 //------------------------- diff --git a/flight/PiOS/Boards/STM3210E_OP.h b/flight/PiOS/Boards/STM3210E_OP.h index 3098ee259..159ebbd88 100644 --- a/flight/PiOS/Boards/STM3210E_OP.h +++ b/flight/PiOS/Boards/STM3210E_OP.h @@ -195,6 +195,7 @@ extern uint32_t pios_com_aux_id; //------------------------- // Receiver SPEKTRUM input //------------------------- +#define PIOS_SPEKTRUM_MAX_DEVS 1 #define PIOS_SPEKTRUM_NUM_INPUTS 12 //------------------------- diff --git a/flight/PiOS/STM32F10x/pios_spektrum.c b/flight/PiOS/STM32F10x/pios_spektrum.c index 8b0f38c6b..e062e6314 100644 --- a/flight/PiOS/STM32F10x/pios_spektrum.c +++ b/flight/PiOS/STM32F10x/pios_spektrum.c @@ -31,10 +31,11 @@ /* Project Includes */ #include "pios.h" -#include "pios_spektrum_priv.h" #if defined(PIOS_INCLUDE_SPEKTRUM) +#include "pios_spektrum_priv.h" + /** * @Note Framesyncing: * The code resets the watchdog timer whenever a single byte is received, so what watchdog code @@ -52,22 +53,80 @@ const struct pios_rcvr_driver pios_spektrum_rcvr_driver = { .read = PIOS_SPEKTRUM_Get, }; -/* Local Variables */ -static uint16_t CaptureValue[PIOS_SPEKTRUM_NUM_INPUTS],CaptureValueTemp[PIOS_SPEKTRUM_NUM_INPUTS]; -static uint8_t prev_byte = 0xFF, sync = 0, bytecount = 0, datalength=0, frame_error=0, byte_array[20] = { 0 }; -uint8_t sync_of = 0; -uint16_t supv_timer=0; +enum pios_spektrum_dev_magic { + PIOS_SPEKTRUM_DEV_MAGIC = 0xa9b9c9d9, +}; + +struct pios_spektrum_fsm { + uint16_t channel; + uint16_t CaptureValue[PIOS_SPEKTRUM_NUM_INPUTS]; + uint16_t CaptureValueTemp[PIOS_SPEKTRUM_NUM_INPUTS]; + uint8_t prev_byte; + uint8_t sync; + uint8_t bytecount; + uint8_t datalength; + uint8_t frame_error; + uint8_t sync_of; +}; + +struct pios_spektrum_dev { + enum pios_spektrum_dev_magic magic; + const struct pios_spektrum_cfg * cfg; + + struct pios_spektrum_fsm fsm; + + uint16_t supv_timer; +}; + +static bool PIOS_SPEKTRUM_validate(struct pios_spektrum_dev * spektrum_dev) +{ + return (spektrum_dev->magic == PIOS_SPEKTRUM_DEV_MAGIC); +} + +#if defined(PIOS_INCLUDE_FREERTOS) && 0 +static struct pios_spektrum_dev * PIOS_SPEKTRUM_alloc(void) +{ + struct pios_spektrum_dev * spektrum_dev; + + spektrum_dev = (struct pios_spektrum_dev *)malloc(sizeof(*spektrum_dev)); + if (!spektrum_dev) return(NULL); + + spektrum_dev->magic = PIOS_SPEKTRUM_DEV_MAGIC; + return(spektrum_dev); +} +#else +static struct pios_spektrum_dev pios_spektrum_devs[PIOS_SPEKTRUM_MAX_DEVS]; +static uint8_t pios_spektrum_num_devs; +static struct pios_spektrum_dev * PIOS_SPEKTRUM_alloc(void) +{ + struct pios_spektrum_dev * spektrum_dev; + + if (pios_spektrum_num_devs >= PIOS_SPEKTRUM_MAX_DEVS) { + return (NULL); + } + + spektrum_dev = &pios_spektrum_devs[pios_spektrum_num_devs++]; + spektrum_dev->magic = PIOS_SPEKTRUM_DEV_MAGIC; + + return (spektrum_dev); +} +#endif static void PIOS_SPEKTRUM_Supervisor(uint32_t spektrum_id); static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg); -static int32_t PIOS_SPEKTRUM_Decode(uint8_t b); +static int32_t PIOS_SPEKTRUM_UpdateFSM(struct pios_spektrum_fsm * fsm, uint8_t b); static uint16_t PIOS_SPEKTRUM_RxInCallback(uint32_t context, uint8_t * buf, uint16_t buf_len, uint16_t * headroom, bool * need_yield) { + struct pios_spektrum_dev * spektrum_dev = (struct pios_spektrum_dev *)context; + + bool valid = PIOS_SPEKTRUM_validate(spektrum_dev); + PIOS_Assert(valid); + /* process byte(s) and clear receive timer */ for (uint8_t i = 0; i < buf_len; i++) { - PIOS_SPEKTRUM_Decode(buf[i]); - supv_timer = 0; + PIOS_SPEKTRUM_UpdateFSM(&(spektrum_dev->fsm), buf[i]); + spektrum_dev->supv_timer = 0; } /* Always signal that we can accept another byte */ @@ -82,23 +141,118 @@ static uint16_t PIOS_SPEKTRUM_RxInCallback(uint32_t context, uint8_t * buf, uint return (buf_len); } +static void PIOS_SPEKTRUM_ResetFSM(struct pios_spektrum_fsm * fsm) +{ + fsm->channel = 0; + fsm->prev_byte = 0xFF; + fsm->sync = 0; + fsm->bytecount = 0; + fsm->datalength = 0; + fsm->frame_error = 0; + fsm->sync_of = 0; +} + +/** +* Decodes a byte +* \param[in] b byte which should be spektrum decoded +* \return 0 if no error +* \return -1 if USART not available +* \return -2 if buffer full (retry) +*/ +static int32_t PIOS_SPEKTRUM_UpdateFSM(struct pios_spektrum_fsm * fsm, uint8_t b) +{ + fsm->bytecount++; + if (fsm->sync == 0) { + /* Known sync bytes, 0x01, 0x02, 0x12 */ + if (fsm->bytecount == 2) { + if (b == 0x01) { + fsm->datalength=0; // 10bit + fsm->sync = 1; + fsm->bytecount = 2; + } + else if(b == 0x02) { + fsm->datalength=0; // 10bit + fsm->sync = 1; + fsm->bytecount = 2; + } + else if(b == 0x12) { + fsm->datalength=1; // 11bit + fsm->sync = 1; + fsm->bytecount = 2; + } + else + { + fsm->bytecount = 0; + } + } + } else { + if ((fsm->bytecount % 2) == 0) { + uint16_t data; + uint8_t channeln; + + fsm->channel = (fsm->prev_byte << 8) + b; + channeln = (fsm->channel >> (10+fsm->datalength)) & 0x0F; + data = fsm->channel & (0x03FF+(0x0400*fsm->datalength)); + if(channeln==0 && data<10) // discard frame if throttle misbehaves + { + fsm->frame_error=1; + } + if (channeln < PIOS_SPEKTRUM_NUM_INPUTS && !fsm->frame_error) + fsm->CaptureValueTemp[channeln] = data; + } + } + if (fsm->bytecount == 16) { + fsm->bytecount = 0; + fsm->sync = 0; + fsm->sync_of = 0; + if (!fsm->frame_error) + { + for(int i=0;iCaptureValue[i] = fsm->CaptureValueTemp[i]; + } + } + fsm->frame_error=0; + } + fsm->prev_byte = b; + return 0; +} + /** * Bind and Initialise Spektrum satellite receiver */ int32_t PIOS_SPEKTRUM_Init(uint32_t * spektrum_id, const struct pios_spektrum_cfg *cfg, const struct pios_com_driver * driver, uint32_t lower_id, bool bind) { - // TODO: need setting flag for bind on next powerup + PIOS_DEBUG_Assert(spektrum_id); + PIOS_DEBUG_Assert(cfg); + PIOS_DEBUG_Assert(driver); + + struct pios_spektrum_dev * spektrum_dev; + + spektrum_dev = (struct pios_spektrum_dev *) PIOS_SPEKTRUM_alloc(); + if (!spektrum_dev) goto out_fail; + + /* Bind the configuration to the device instance */ + spektrum_dev->cfg = cfg; + if (bind) { PIOS_SPEKTRUM_Bind(cfg); } - (driver->bind_rx_cb)(lower_id, PIOS_SPEKTRUM_RxInCallback, 0); + PIOS_SPEKTRUM_ResetFSM(&(spektrum_dev->fsm)); - if (!PIOS_RTC_RegisterTickCallback(PIOS_SPEKTRUM_Supervisor, 0)) { + *spektrum_id = (uint32_t)spektrum_dev; + + (driver->bind_rx_cb)(lower_id, PIOS_SPEKTRUM_RxInCallback, *spektrum_id); + + if (!PIOS_RTC_RegisterTickCallback(PIOS_SPEKTRUM_Supervisor, *spektrum_id)) { PIOS_DEBUG_Assert(0); } return (0); + +out_fail: + return(-1); } /** @@ -109,11 +263,16 @@ int32_t PIOS_SPEKTRUM_Init(uint32_t * spektrum_id, const struct pios_spektrum_cf */ static int32_t PIOS_SPEKTRUM_Get(uint32_t rcvr_id, uint8_t channel) { + struct pios_spektrum_dev * spektrum_dev = (struct pios_spektrum_dev *)rcvr_id; + + bool valid = PIOS_SPEKTRUM_validate(spektrum_dev); + PIOS_Assert(valid); + /* Return error if channel not available */ if (channel >= PIOS_SPEKTRUM_NUM_INPUTS) { return -1; } - return CaptureValue[channel]; + return spektrum_dev->fsm.CaptureValue[channel]; } /** @@ -145,114 +304,38 @@ static bool PIOS_SPEKTRUM_Bind(const struct pios_spektrum_cfg * cfg) return true; } -/** -* Decodes a byte -* \param[in] b byte which should be spektrum decoded -* \return 0 if no error -* \return -1 if USART not available -* \return -2 if buffer full (retry) -* \note Applications shouldn't call these functions directly -*/ -static int32_t PIOS_SPEKTRUM_Decode(uint8_t b) -{ - static uint16_t channel = 0; /*, sync_word = 0;*/ - uint8_t channeln = 0, frame = 0; - uint16_t data = 0; - byte_array[bytecount] = b; - bytecount++; - if (sync == 0) { - //sync_word = (prev_byte << 8) + b; -#if 0 - /* maybe create object to show this data */ - if(bytecount==1) - { - /* record losscounter into channel8 */ - CaptureValueTemp[7]=b; - /* instant write */ - CaptureValue[7]=b; - } -#endif - /* Known sync bytes, 0x01, 0x02, 0x12 */ - if (bytecount == 2) { - if (b == 0x01) { - datalength=0; // 10bit - //frames=1; - sync = 1; - bytecount = 2; - } - else if(b == 0x02) { - datalength=0; // 10bit - //frames=2; - sync = 1; - bytecount = 2; - } - else if(b == 0x12) { - datalength=1; // 11bit - //frames=2; - sync = 1; - bytecount = 2; - } - else - { - bytecount = 0; - } - } - } else { - if ((bytecount % 2) == 0) { - channel = (prev_byte << 8) + b; - frame = channel >> 15; - channeln = (channel >> (10+datalength)) & 0x0F; - data = channel & (0x03FF+(0x0400*datalength)); - if(channeln==0 && data<10) // discard frame if throttle misbehaves - { - frame_error=1; - } - if (channeln < PIOS_SPEKTRUM_NUM_INPUTS && !frame_error) - CaptureValueTemp[channeln] = data; - } - } - if (bytecount == 16) { - //PIOS_COM_SendBufferNonBlocking(PIOS_COM_TELEM_RF,byte_array,16); //00 2c 58 84 b0 dc ff - bytecount = 0; - sync = 0; - sync_of = 0; - if (!frame_error) - { - for(int i=0;i 5) { + spektrum_dev->supv_timer++; + if(spektrum_dev->supv_timer > 5) { /* sync between frames */ - sync = 0; - bytecount = 0; - prev_byte = 0xFF; - frame_error = 0; - sync_of++; + struct pios_spektrum_fsm * fsm = &(spektrum_dev->fsm); + + fsm->sync = 0; + fsm->bytecount = 0; + fsm->prev_byte = 0xFF; + fsm->frame_error = 0; + fsm->sync_of++; /* watchdog activated after 100ms silence */ - if (sync_of > 12) { + if (fsm->sync_of > 12) { /* signal lost */ - sync_of = 0; + fsm->sync_of = 0; for (int i = 0; i < PIOS_SPEKTRUM_NUM_INPUTS; i++) { - CaptureValue[i] = 0; - CaptureValueTemp[i] = 0; + fsm->CaptureValue[i] = 0; + fsm->CaptureValueTemp[i] = 0; } } - supv_timer = 0; + spektrum_dev->supv_timer = 0; } } diff --git a/flight/PiOS/inc/pios_rcvr.h b/flight/PiOS/inc/pios_rcvr.h index dfec004f5..0f4e6a973 100644 --- a/flight/PiOS/inc/pios_rcvr.h +++ b/flight/PiOS/inc/pios_rcvr.h @@ -31,13 +31,6 @@ #ifndef PIOS_RCVR_H #define PIOS_RCVR_H -struct pios_rcvr_channel_map { - uint32_t id; - uint8_t channel; -}; - -extern struct pios_rcvr_channel_map pios_rcvr_channel_to_id_map[]; - struct pios_rcvr_driver { void (*init)(uint32_t id); int32_t (*read)(uint32_t id, uint8_t channel); diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index a3494f820..7b4ed7ec4 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -1,8 +1,14 @@ Selection of optional hardware configurations. - - + + + + + + + + diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index 3b76d70f8..d6d7b90a3 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -1,15 +1,18 @@ Settings to indicate how to decode receiver input by @ref ManualControlModule. - - - - - - - - - + + + + + + @@ -20,9 +23,6 @@ - - - From 1299e110c4e462d1d11e494a4eaea56a7c3cee53 Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Sun, 31 Jul 2011 19:17:28 -0400 Subject: [PATCH 4/5] gcs: update CC hw settings to use hwsettings RcvrPort field --- .../src/plugins/config/cc_hw_settings.ui | 4 ++-- .../src/plugins/config/config_cc_hw_widget.cpp | 18 +----------------- 2 files changed, 3 insertions(+), 19 deletions(-) diff --git a/ground/openpilotgcs/src/plugins/config/cc_hw_settings.ui b/ground/openpilotgcs/src/plugins/config/cc_hw_settings.ui index 29052623c..eed5fa232 100644 --- a/ground/openpilotgcs/src/plugins/config/cc_hw_settings.ui +++ b/ground/openpilotgcs/src/plugins/config/cc_hw_settings.ui @@ -87,7 +87,7 @@ - Receiver type + RcvrPort Qt::AlignRight|Qt::AlignTrailing|Qt::AlignVCenter @@ -95,7 +95,7 @@ - + diff --git a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp index 62e2d59e2..5c7a035b4 100644 --- a/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp +++ b/ground/openpilotgcs/src/plugins/config/config_cc_hw_widget.cpp @@ -42,7 +42,7 @@ ConfigCCHWWidget::ConfigCCHWWidget(QWidget *parent) : ConfigTaskWidget(parent) addUAVObjectToWidgetRelation("TelemetrySettings","Speed",m_telemetry->telemetrySpeed); addUAVObjectToWidgetRelation("HwSettings","CC_FlexiPort",m_telemetry->cbFlexi); addUAVObjectToWidgetRelation("HwSettings","CC_MainPort",m_telemetry->cbTele); - addUAVObjectToWidgetRelation("ManualControlSettings","InputMode",m_telemetry->receiverType); + addUAVObjectToWidgetRelation("HwSettings","RcvrPort",m_telemetry->cbRcvr); enableControls(false); populateWidgets(); refreshWidgetsValues(); @@ -65,22 +65,6 @@ void ConfigCCHWWidget::widgetsContentsChanged() { m_telemetry->problems->setText("Warning: you have configured the MainPort and the FlexiPort for the same function, this is currently not suported"); } - else if((m_telemetry->cbTele->currentText()=="Spektrum" ||m_telemetry->cbFlexi->currentText()=="Spektrum") && m_telemetry->receiverType->currentText()!="Spektrum") - { - m_telemetry->problems->setText("Warning: you have a port configured as 'Spektrum' however that is not your selected receiver type"); - } - else if(m_telemetry->cbTele->currentText()=="S.Bus" && m_telemetry->receiverType->currentText()!="S.Bus") - { - m_telemetry->problems->setText("Warning: you have a port configured as 'S.Bus' however that is not your selected receiver type"); - } - else if(m_telemetry->cbTele->currentText()!="S.Bus" && m_telemetry->receiverType->currentText()=="S.Bus") - { - m_telemetry->problems->setText("Warning: you have selected 'S.Bus' as your receiver type however you have no port configured for that protocol"); - } - else if((m_telemetry->cbTele->currentText()!="Spektrum" && m_telemetry->cbFlexi->currentText()!="Spektrum") && m_telemetry->receiverType->currentText()=="Spektrum") - { - m_telemetry->problems->setText("Warning: you have selected 'Spektrum' as your receiver type however you have no port configured for that protocol"); - } else { m_telemetry->problems->setText(""); From 90b2625debed95a1ffde3b04fd30eb159f397bed Mon Sep 17 00:00:00 2001 From: Stacey Sheldon Date: Tue, 2 Aug 2011 01:27:37 -0400 Subject: [PATCH 5/5] rcvractivity: Add tracking of Rx channel activity --- flight/CopterControl/Makefile | 1 + flight/Modules/ManualControl/manualcontrol.c | 146 +++++++++++++++++- flight/OpenPilot/UAVObjects.inc | 1 + .../src/plugins/uavobjects/uavobjects.pro | 2 + .../uavobjectdefinition/receiveractivity.xml | 14 ++ 5 files changed, 163 insertions(+), 1 deletion(-) create mode 100644 shared/uavobjectdefinition/receiveractivity.xml diff --git a/flight/CopterControl/Makefile b/flight/CopterControl/Makefile index 3ac59e59b..d437d567d 100644 --- a/flight/CopterControl/Makefile +++ b/flight/CopterControl/Makefile @@ -165,6 +165,7 @@ SRC += $(OPUAVSYNTHDIR)/firmwareiapobj.c SRC += $(OPUAVSYNTHDIR)/attitudesettings.c SRC += $(OPUAVSYNTHDIR)/hwsettings.c SRC += $(OPUAVSYNTHDIR)/gcsreceiver.c +SRC += $(OPUAVSYNTHDIR)/receiveractivity.c #${wildcard ${OBJ}/$(shell echo $(VAR) | tr A-Z a-z)/*.c} #SRC += ${foreach OBJ, ${UAVOBJECTS}, $(UAVOBJECTS)/$(OBJ).c} # Cant use until i can automatically generate list of UAVObjects diff --git a/flight/Modules/ManualControl/manualcontrol.c b/flight/Modules/ManualControl/manualcontrol.c index 7c16ac172..153738dc1 100644 --- a/flight/Modules/ManualControl/manualcontrol.c +++ b/flight/Modules/ManualControl/manualcontrol.c @@ -43,6 +43,7 @@ #include "flighttelemetrystats.h" #include "flightstatus.h" #include "accessorydesired.h" +#include "receiveractivity.h" // Private constants #if defined(PIOS_MANUAL_STACK_SIZE) @@ -87,6 +88,18 @@ static uint32_t timeDifferenceMs(portTickType start_time, portTickType end_time) static bool okToArm(void); static bool validInputRange(int16_t min, int16_t max, uint16_t value); +#define RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP 12 +#define RCVR_ACTIVITY_MONITOR_MIN_RANGE 5 +struct rcvr_activity_fsm { + ManualControlSettingsChannelGroupsOptions group; + uint16_t prev[RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP]; + uint8_t sample_count; +}; +static struct rcvr_activity_fsm activity_fsm; + +static void resetRcvrActivity(struct rcvr_activity_fsm * fsm); +static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm); + #define assumptions (assumptions1 && assumptions3 && assumptions5 && assumptions7 && assumptions8 && assumptions_flightmode) /** @@ -138,6 +151,10 @@ static void manualControlTask(void *parameters) flightStatus.Armed = FLIGHTSTATUS_ARMED_DISARMED; armState = ARM_STATE_DISARMED; + /* Initialize the RcvrActivty FSM */ + portTickType lastActivityTime = xTaskGetTickCount(); + resetRcvrActivity(&activity_fsm); + // Main task loop lastSysTime = xTaskGetTickCount(); while (1) { @@ -150,6 +167,18 @@ static void manualControlTask(void *parameters) // Read settings ManualControlSettingsGet(&settings); + /* Update channel activity monitor */ + if (flightStatus.Armed == ARM_STATE_DISARMED) { + if (updateRcvrActivity(&activity_fsm)) { + /* Reset the aging timer because activity was detected */ + lastActivityTime = lastSysTime; + } + } + if (timeDifferenceMs(lastActivityTime, lastSysTime) > 5000) { + resetRcvrActivity(&activity_fsm); + lastActivityTime = lastSysTime; + } + if (ManualControlCommandReadOnly(&cmd)) { FlightTelemetryStatsData flightTelemStats; FlightTelemetryStatsGet(&flightTelemStats); @@ -266,7 +295,6 @@ static void manualControlTask(void *parameters) ManualControlCommandGet(&cmd); /* Under GCS control */ } - FlightStatusGet(&flightStatus); // Depending on the mode update the Stabilization or Actuator objects @@ -288,6 +316,122 @@ static void manualControlTask(void *parameters) } } +static void resetRcvrActivity(struct rcvr_activity_fsm * fsm) +{ + ReceiverActivityData data; + bool updated = false; + + /* Clear all channel activity flags */ + ReceiverActivityGet(&data); + if (data.ActiveGroup != RECEIVERACTIVITY_ACTIVEGROUP_NONE && + data.ActiveChannel != 255) { + data.ActiveGroup = RECEIVERACTIVITY_ACTIVEGROUP_NONE; + data.ActiveChannel = 255; + updated = true; + } + if (updated) { + ReceiverActivitySet(&data); + } + + /* Reset the FSM state */ + fsm->group = 0; + fsm->sample_count = 0; +} + +static bool updateRcvrActivity(struct rcvr_activity_fsm * fsm) +{ + bool activity_updated = false; + + if (fsm->group >= MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + /* We're out of range, reset things */ + resetRcvrActivity(fsm); + } + + extern uint32_t pios_rcvr_group_map[]; + if (!pios_rcvr_group_map[fsm->group]) { + /* Unbound group, skip it */ + goto group_completed; + } + + /* Sample the channel */ + for (uint8_t channel = 0; + channel < RCVR_ACTIVITY_MONITOR_CHANNELS_PER_GROUP; + channel++) { + if (fsm->sample_count == 0) { + fsm->prev[channel] = PIOS_RCVR_Read(pios_rcvr_group_map[fsm->group], channel); + } else { + uint16_t delta; + uint16_t prev = fsm->prev[channel]; + uint16_t curr = PIOS_RCVR_Read(pios_rcvr_group_map[fsm->group], channel); + if (curr > prev) { + delta = curr - prev; + } else { + delta = prev - curr; + } + + if (delta > RCVR_ACTIVITY_MONITOR_MIN_RANGE) { + /* Mark this channel as active */ + ReceiverActivityActiveGroupOptions group; + + /* Don't assume manualcontrolsettings and receiveractivity are in the same order. */ + switch (fsm->group) { + case MANUALCONTROLSETTINGS_CHANNELGROUPS_PWM: + group = RECEIVERACTIVITY_ACTIVEGROUP_PWM; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_PPM: + group = RECEIVERACTIVITY_ACTIVEGROUP_PPM; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM1: + group = RECEIVERACTIVITY_ACTIVEGROUP_SPEKTRUM1; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_SPEKTRUM2: + group = RECEIVERACTIVITY_ACTIVEGROUP_SPEKTRUM2; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: + group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_GCS: + group = RECEIVERACTIVITY_ACTIVEGROUP_GCS; + break; + default: + PIOS_Assert(0); + break; + } + + ReceiverActivityActiveGroupSet(&group); + ReceiverActivityActiveChannelSet(&channel); + activity_updated = true; + } + } + } + + if (++fsm->sample_count < 2) { + return (activity_updated); + } + + /* Reset the sample counter */ + fsm->sample_count = 0; + + /* + * Group Completed + */ + +group_completed: + /* Start over at channel zero */ + if (++fsm->group < MANUALCONTROLSETTINGS_CHANNELGROUPS_NONE) { + return (activity_updated); + } + + /* + * All groups completed + */ + + /* Start over at group zero */ + fsm->group = 0; + + return (activity_updated); +} + static void updateActuatorDesired(ManualControlCommandData * cmd) { ActuatorDesiredData actuator; diff --git a/flight/OpenPilot/UAVObjects.inc b/flight/OpenPilot/UAVObjects.inc index a30dcf540..3bb868a7d 100644 --- a/flight/OpenPilot/UAVObjects.inc +++ b/flight/OpenPilot/UAVObjects.inc @@ -70,6 +70,7 @@ UAVOBJSRCFILENAMES += velocitydesired UAVOBJSRCFILENAMES += watchdogstatus UAVOBJSRCFILENAMES += flightstatus UAVOBJSRCFILENAMES += hwsettings +UAVOBJSRCFILENAMES += receiveractivity UAVOBJSRC = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),$(UAVOBJSYNTHDIR)/$(UAVOBJSRCFILE).c ) UAVOBJDEFINE = $(foreach UAVOBJSRCFILE,$(UAVOBJSRCFILENAMES),-DUAVOBJ_INIT_$(UAVOBJSRCFILE) ) diff --git a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro index 3831a4ec3..f5e17d3ca 100644 --- a/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro +++ b/ground/openpilotgcs/src/plugins/uavobjects/uavobjects.pro @@ -70,6 +70,7 @@ HEADERS += $$UAVOBJECT_SYNTHETICS/accessorydesired.h \ $$UAVOBJECT_SYNTHETICS/flightstatus.h \ $$UAVOBJECT_SYNTHETICS/hwsettings.h \ $$UAVOBJECT_SYNTHETICS/gcsreceiver.h \ + $$UAVOBJECT_SYNTHETICS/receiveractivity.h \ $$UAVOBJECT_SYNTHETICS/attitudesettings.h SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ @@ -120,4 +121,5 @@ SOURCES += $$UAVOBJECT_SYNTHETICS/accessorydesired.cpp \ $$UAVOBJECT_SYNTHETICS/flightstatus.cpp \ $$UAVOBJECT_SYNTHETICS/hwsettings.cpp \ $$UAVOBJECT_SYNTHETICS/gcsreceiver.cpp \ + $$UAVOBJECT_SYNTHETICS/receiveractivity.cpp \ $$UAVOBJECT_SYNTHETICS/attitudesettings.cpp diff --git a/shared/uavobjectdefinition/receiveractivity.xml b/shared/uavobjectdefinition/receiveractivity.xml new file mode 100644 index 000000000..2e98c1362 --- /dev/null +++ b/shared/uavobjectdefinition/receiveractivity.xml @@ -0,0 +1,14 @@ + + + Monitors which receiver channels have been active within the last second. + + + + + + + +