From e6d4638201533b63522cf2fb40219ffadf3be435 Mon Sep 17 00:00:00 2001 From: Fredrik Arvidsson Date: Sun, 20 Dec 2015 11:08:15 +0100 Subject: [PATCH 01/19] LP-104 Adds support for Graupner HoTT SUMD/SUMH serial protocol used by receivers on Main and Flexi port. Still work in progress. Only supports CopterControl target and crashes on input :( --- flight/make/apps-defs.mk | 3 +- flight/pios/common/pios_hott.c | 419 ++++++++++++++++++ flight/pios/inc/pios_hott.h | 36 ++ flight/pios/inc/pios_hott_priv.h | 53 +++ flight/pios/pios.h | 5 + .../boards/coptercontrol/board_hw_defs.c | 80 ++++ .../coptercontrol/firmware/inc/pios_config.h | 1 + .../coptercontrol/firmware/pios_board.c | 48 ++ .../targets/boards/coptercontrol/pios_board.h | 6 + shared/uavobjectdefinition/hwsettings.xml | 4 +- .../manualcontrolsettings.xml | 2 +- 11 files changed, 653 insertions(+), 4 deletions(-) create mode 100644 flight/pios/common/pios_hott.c create mode 100644 flight/pios/inc/pios_hott.h create mode 100644 flight/pios/inc/pios_hott_priv.h diff --git a/flight/make/apps-defs.mk b/flight/make/apps-defs.mk index 45bdb0049..03e67ed47 100644 --- a/flight/make/apps-defs.mk +++ b/flight/make/apps-defs.mk @@ -34,7 +34,7 @@ FLIGHTLIBINC = $(FLIGHTLIB)/inc OPUAVOBJINC = $(OPUAVOBJ)/inc OPUAVTALKINC = $(OPUAVTALK)/inc -## PID +## PID PIDLIB =$(FLIGHTLIB)/pid PIDLIBINC =$(FLIGHTLIB)/pid @@ -86,6 +86,7 @@ SRC += $(PIOSCOMMON)/pios_rfm22b.c SRC += $(PIOSCOMMON)/pios_rfm22b_com.c SRC += $(PIOSCOMMON)/pios_rcvr.c SRC += $(PIOSCOMMON)/pios_sbus.c +SRC += $(PIOSCOMMON)/pios_hott.c SRC += $(PIOSCOMMON)/pios_srxl.c SRC += $(PIOSCOMMON)/pios_sdcard.c SRC += $(PIOSCOMMON)/pios_sensors.c diff --git a/flight/pios/common/pios_hott.c b/flight/pios/common/pios_hott.c new file mode 100644 index 000000000..aba62c708 --- /dev/null +++ b/flight/pios/common/pios_hott.c @@ -0,0 +1,419 @@ +/** + ****************************************************************************** + * @file pios_hott.c + * @author The LibrePilot Project, http://www.librepilot.org, Copyright (c) 2015 + * @author Tau Labs, http://taulabs.org, Copyright (C) 2013-2014 + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_HOTT Graupner HoTT receiver functions + * @{ + * @brief Graupner HoTT receiver functions for SUMD/H + *****************************************************************************/ +/* + * 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_hott_priv.h" + +#if defined(PIOS_INCLUDE_HOTT) + +#if !defined(PIOS_INCLUDE_RTC) +#error PIOS_INCLUDE_RTC must be used to use HOTT +#endif + +/** + * HOTT protocol documentation + * + * Currently known Graupner HoTT serial port settings: + * 115200bps serial stream, 8 bits, no parity, 1 stop bit + * size of each frame: 11..37 bytes + * data resolution: 14 bit + * frame period: 11ms or 22ms + * + * Currently known SUMD/SUMH frame structure: + * Section Byte_Number Byte_Name Byte_Value Remark + * Header 0 Vendor_ID 0xA8 Graupner + * Header 1 Status 0x00 valid and live SUMH data frame + * 0x01 valid and live SUMD data frame + * 0x81 valid SUMD/H data frame with + * transmitter in fail safe condition + * others invalid frame + * Header 2 N_Channels 0x02..0x20 number of transmitted channels + * Data n*2+1 Channel n MSB 0x00..0xff High Byte of channel n data + * Data n*2+2 Channel n LSB 0x00..0xff Low Byte of channel n data + * SUMD_CRC (N_Channels+1)*2+1 CRC High Byte 0x00..0xff High Byte of 16 Bit CRC + * SUMD_CRC (N_Channels+1)*2+2 CRC Low Byte 0x00..0xff Low Byte of 16 Bit CRC + * SUMH_Telemetry (N_Channels+1)*2+1 Telemetry_Req 0x00..0xff 0x00 no telemetry request + * SUMH_CRC (N_Channels+1)*2+2 CRC Byte 0x00..0xff Low Byte of all added data bytes + + * Channel Data Interpretation + * Stick Positon Channel Data Remark + * ext. low (-150%) 0x1c20 900µs + * low (-100%) 0x2260 1100µs + * neutral (0%) 0x2ee0 1500µs + * high (100%) 0x3b60 1900µs + * ext. high(150%) 0x41a0 2100µs + + * Channel Mapping (not sure) + * 1 Pitch + * 2 Aileron + * 3 Elevator + * 4 Yaw + * 5 Aux/Gyro on MX-12 + * 6 ESC + * 7 Aux/Gyr + */ + +/* HOTT frame size and contents definitions */ +#define HOTT_HEADER_LENGTH 3 +#define HOTT_CRC_LENGTH 2 +#define HOTT_MAX_CHANNELS_PER_FRAME 32 +#define HOTT_OVERHEAD_LENGTH (HOTT_HEADER_LENGTH+HOTT_CRC_LENGTH) +#define HOTT_MAX_FRAME_LENGTH (HOTT_MAX_CHANNELS_PER_FRAME*2+HOTT_OVERHEAD_LENGTH) + +#define HOTT_GRAUPNER_ID 0xA8 +#define HOTT_STATUS_LIVING_SUMH 0x00 +#define HOTT_STATUS_LIVING_SUMD 0x01 +#define HOTT_STATUS_FAILSAFE 0x81 + +/* Forward Declarations */ +static int32_t PIOS_HOTT_Get(uint32_t rcvr_id, uint8_t channel); +static uint16_t PIOS_HOTT_RxInCallback(uint32_t context, + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield); +static void PIOS_HOTT_Supervisor(uint32_t hott_id); + +/* Local Variables */ +const struct pios_rcvr_driver pios_hott_rcvr_driver = { + .read = PIOS_HOTT_Get, +}; + +enum pios_hott_dev_magic { + PIOS_HOTT_DEV_MAGIC = 0x4853554D, +}; + +struct pios_hott_state { + uint16_t channel_data[PIOS_HOTT_NUM_INPUTS]; + uint8_t received_data[HOTT_MAX_FRAME_LENGTH]; + uint8_t receive_timer; + uint8_t failsafe_timer; + uint8_t frame_found; + uint8_t tx_connected; + uint8_t byte_count; + uint8_t frame_length; +}; + +struct pios_hott_dev { + enum pios_hott_dev_magic magic; + const struct pios_hott_cfg *cfg; + enum pios_hott_proto proto; + struct pios_hott_state state; +}; + +/* Allocate HOTT device descriptor */ +#if defined(PIOS_INCLUDE_FREERTOS) +static struct pios_hott_dev *PIOS_HOTT_Alloc(void) +{ + struct pios_hott_dev *hott_dev; + + hott_dev = (struct pios_hott_dev *)pios_malloc(sizeof(*hott_dev)); + if (!hott_dev) + return NULL; + + hott_dev->magic = PIOS_HOTT_DEV_MAGIC; + return hott_dev; +} +#else +static struct pios_hott_dev pios_hott_devs[PIOS_HOTT_MAX_DEVS]; +static uint8_t pios_hott_num_devs; +static struct pios_hott_dev *PIOS_HOTT_Alloc(void) +{ + struct pios_hott_dev *hott_dev; + + if (pios_hott_num_devs >= PIOS_HOTT_MAX_DEVS) + return NULL; + + hott_dev = &pios_hott_devs[pios_hott_num_devs++]; + hott_dev->magic = PIOS_HOTT_DEV_MAGIC; + + return hott_dev; +} +#endif + +/* Validate HOTT device descriptor */ +static bool PIOS_HOTT_Validate(struct pios_hott_dev *hott_dev) +{ + return (hott_dev->magic == PIOS_HOTT_DEV_MAGIC); +} + +/* Reset channels in case of lost signal or explicit failsafe receiver flag */ +static void PIOS_HOTT_ResetChannels(struct pios_hott_state *state) +{ + for (int i = 0; i < PIOS_HOTT_NUM_INPUTS; i++) { + state->channel_data[i] = PIOS_RCVR_TIMEOUT; + } +} + +/* Reset HOTT receiver state */ +static void PIOS_HOTT_ResetState(struct pios_hott_state *state) +{ + state->receive_timer = 0; + state->failsafe_timer = 0; + state->frame_found = 0; + state->tx_connected = 0; + PIOS_HOTT_ResetChannels(state); +} + +/** + * Check and unroll complete frame data. + * \output 0 frame data accepted + * \output -1 frame error found + */ +static int PIOS_HOTT_UnrollChannels(struct pios_hott_dev *hott_dev) +{ + struct pios_hott_state *state = &(hott_dev->state); + + /* check the header and crc for a valid HoTT SUM stream */ + uint8_t vendor = state->received_data[0]; + uint8_t status = state->received_data[1]; + if (vendor != HOTT_GRAUPNER_ID) + /* Graupner ID was expected */ + goto stream_error; + + switch (status) { + case HOTT_STATUS_LIVING_SUMH: + case HOTT_STATUS_LIVING_SUMD: + case HOTT_STATUS_FAILSAFE: + /* check crc before processing */ + if (hott_dev->proto == PIOS_HOTT_PROTO_SUMD) { + /* SUMD has 16 bit CCITT CRC */ + uint16_t crc = 0; + uint8_t *s = &(state->received_data[0]); + int len = state->byte_count - 2; + for (int n = 0; n < len; n++) { + crc ^= (uint16_t)s[n] << 8; + for (int i = 0; i < 8; i++) + crc = (crc & 0x8000) ? (crc << 1) ^ 0x1021 : (crc << 1); + } + if (crc ^ (((uint16_t)s[len] << 8) | s[len + 1])) + /* wrong crc checksum found */ + goto stream_error; + } + if (hott_dev->proto == PIOS_HOTT_PROTO_SUMH) { + /* SUMH has only 8 bit added CRC */ + uint8_t crc = 0; + uint8_t *s = &(state->received_data[0]); + int len = state->byte_count - 1; + for (int n = 0; n < len; n++) + crc += s[n]; + if (crc ^ s[len]) + /* wrong crc checksum found */ + goto stream_error; + } + /* check for a living connect */ + state->tx_connected |= (status != HOTT_STATUS_FAILSAFE); + break; + default: + /* wrong header format */ + goto stream_error; + } + + /* check initial connection since reset or timeout */ + if (!(state->tx_connected)) { + /* these are failsafe data without a first connect. ignore it */ + PIOS_HOTT_ResetChannels(state); + return 0; + } + + /* unroll channels */ + uint8_t n_channels = state->received_data[2]; + uint8_t *s = &(state->received_data[3]); + uint16_t word; + + for (int i = 0; i < HOTT_MAX_CHANNELS_PER_FRAME; i++) { + if (i < n_channels) { + word = ((uint16_t)s[0] << 8) | s[1]; + s += sizeof(uint16_t); + /* save the channel value */ + if (i < PIOS_HOTT_NUM_INPUTS) { + /* floating version. channel limits from -100..+100% are mapped to 1000..2000 */ + state->channel_data[i] = (uint16_t)(word / 6.4f - 375); + } + } else + /* this channel was not received */ + state->channel_data[i] = PIOS_RCVR_INVALID; + } + + /* all channels processed */ + return 0; + +stream_error: + /* either SUMD selected with SUMH stream found, or vice-versa */ + return -1; +} + +/* Update decoder state processing input byte from the HoTT stream */ +static void PIOS_HOTT_UpdateState(struct pios_hott_dev *hott_dev, uint8_t byte) +{ + struct pios_hott_state *state = &(hott_dev->state); + if (state->frame_found) { + /* receiving the data frame */ + if (state->byte_count < HOTT_MAX_FRAME_LENGTH) { + /* store next byte */ + state->received_data[state->byte_count++] = byte; + if (state->byte_count == HOTT_HEADER_LENGTH) { + /* 3rd byte contains the number of channels. calculate frame size */ + state->frame_length = HOTT_OVERHEAD_LENGTH + 2 * byte; + } + if (state->byte_count == state->frame_length) { + /* full frame received - process and wait for new one */ + if (!PIOS_HOTT_UnrollChannels(hott_dev)) + /* data looking good */ + state->failsafe_timer = 0; + /* prepare for the next frame */ + state->frame_found = 0; + } + } + } +} + +/* Initialise HoTT receiver interface */ +int32_t PIOS_HOTT_Init(uint32_t *hott_id, + const struct pios_com_driver *driver, + uint32_t lower_id, + enum pios_hott_proto proto) +{ + PIOS_DEBUG_Assert(hott_id); + PIOS_DEBUG_Assert(driver); + + struct pios_hott_dev *hott_dev; + + hott_dev = (struct pios_hott_dev *)PIOS_HOTT_Alloc(); + if (!hott_dev) + return -1; + + /* Bind the configuration to the device instance */ + hott_dev->proto = proto; + + PIOS_HOTT_ResetState(&(hott_dev->state)); + + *hott_id = (uint32_t)hott_dev; + + /* Set comm driver callback */ + (driver->bind_rx_cb)(lower_id, PIOS_HOTT_RxInCallback, *hott_id); + + if (!PIOS_RTC_RegisterTickCallback(PIOS_HOTT_Supervisor, *hott_id)) { + PIOS_DEBUG_Assert(0); + } + + return 0; +} + +/* Comm byte received callback */ +static uint16_t PIOS_HOTT_RxInCallback(uint32_t context, + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield) +{ + struct pios_hott_dev *hott_dev = (struct pios_hott_dev *)context; + + bool valid = PIOS_HOTT_Validate(hott_dev); + PIOS_Assert(valid); + + /* process byte(s) and clear receive timer */ + for (uint8_t i = 0; i < buf_len; i++) { + PIOS_HOTT_UpdateState(hott_dev, buf[i]); + hott_dev->state.receive_timer = 0; + } + + /* Always signal that we can accept more data */ + if (headroom) + *headroom = HOTT_MAX_FRAME_LENGTH; + + /* We never need a yield */ + *need_yield = false; + + /* Always indicate that all bytes were consumed */ + return buf_len; +} + +/** + * 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_HOTT_Get(uint32_t rcvr_id, uint8_t channel) +{ + struct pios_hott_dev *hott_dev = (struct pios_hott_dev *)rcvr_id; + + if (!PIOS_HOTT_Validate(hott_dev)) + return PIOS_RCVR_INVALID; + + /* return error if channel is not available */ + if (channel >= PIOS_HOTT_NUM_INPUTS) + return PIOS_RCVR_INVALID; + + /* may also be PIOS_RCVR_TIMEOUT set by other function */ + return hott_dev->state.channel_data[channel]; +} + +/** + * Input data supervisor is called periodically and provides + * two functions: frame syncing and failsafe triggering. + * + * HOTT frames come at 11ms or 22ms rate at 115200bps. + * RTC timer is running at 625Hz (1.6ms). So with divider 5 it gives + * 8ms pause between frames which is good for both HOTT frame rates. + * + * Data receive function must clear the receive_timer to confirm new + * data reception. If no new data received in 100ms, we must call the + * failsafe function which clears all channels. + */ +static void PIOS_HOTT_Supervisor(uint32_t hott_id) +{ + struct pios_hott_dev *hott_dev = (struct pios_hott_dev *)hott_id; + + bool valid = PIOS_HOTT_Validate(hott_dev); + PIOS_Assert(valid); + + struct pios_hott_state *state = &(hott_dev->state); + + /* waiting for new frame if no bytes were received in 8ms */ + if (++state->receive_timer > 4) { + state->frame_found = 1; + state->byte_count = 0; + state->receive_timer = 0; + state->frame_length = HOTT_MAX_FRAME_LENGTH; + } + + /* activate failsafe if no frames have arrived in 102.4ms */ + if (++state->failsafe_timer > 64) { + PIOS_HOTT_ResetChannels(state); + state->failsafe_timer = 0; + state->tx_connected = 0; + } +} + +#endif /* PIOS_INCLUDE_HOTT */ + +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_hott.h b/flight/pios/inc/pios_hott.h new file mode 100644 index 000000000..bea4c6887 --- /dev/null +++ b/flight/pios/inc/pios_hott.h @@ -0,0 +1,36 @@ +/** + ****************************************************************************** + * @file pios_hott.h + * @author The LibrePilot Project, http://www.librepilot.org, Copyright (c) 2015 + * @author Tau Labs, http://taulabs.org, Copyright (C) 2013 + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_HOTT Graupner HoTT receiver functions + * @{ + * @brief Graupner HoTT receiver functions for SUMD/H + *****************************************************************************/ +/* + * 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_HOTT_H +#define PIOS_HOTT_H + +#endif /* PIOS_HOTT_H */ + +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_hott_priv.h b/flight/pios/inc/pios_hott_priv.h new file mode 100644 index 000000000..b120d82ab --- /dev/null +++ b/flight/pios/inc/pios_hott_priv.h @@ -0,0 +1,53 @@ +/** + ****************************************************************************** + * @file pios_hott_private.h + * @author The LibrePilot Project, http://www.librepilot.org, Copyright (c) 2015 + * @author Tau Labs, http://taulabs.org, Copyright (C) 2013 + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_HOTT Graupner HoTT receiver functions + * @{ + * @brief Graupner HoTT receiver functions for SUMD/H + *****************************************************************************/ +/* + * 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_HOTT_PRIV_H +#define PIOS_HOTT_PRIV_H + +#include +#include + +/* HOTT protocol variations */ +enum pios_hott_proto { + PIOS_HOTT_PROTO_SUMD, + PIOS_HOTT_PROTO_SUMH, +}; + +/* HOTT receiver instance configuration */ +extern const struct pios_rcvr_driver pios_hott_rcvr_driver; + +extern int32_t PIOS_HOTT_Init(uint32_t *hott_id, + const struct pios_com_driver *driver, + uint32_t lower_id, + enum pios_hott_proto proto); + +#endif /* PIOS_HOTT_PRIV_H */ + +/** + * @} + * @} + */ diff --git a/flight/pios/pios.h b/flight/pios/pios.h index ed3e8fd52..398b77ac4 100644 --- a/flight/pios/pios.h +++ b/flight/pios/pios.h @@ -1,6 +1,7 @@ /** ****************************************************************************** * @file pios.h + * @author The LibrePilot Project, http://www.librepilot.org, Copyright (c) 2015 * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013 * @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 * @brief Main PiOS header. @@ -228,6 +229,10 @@ extern "C" { #include #endif +#ifdef PIOS_INCLUDE_HOTT +#include +#endif + #ifdef PIOS_INCLUDE_SRXL #include #endif diff --git a/flight/targets/boards/coptercontrol/board_hw_defs.c b/flight/targets/boards/coptercontrol/board_hw_defs.c index 1e464899d..f4cde50f4 100644 --- a/flight/targets/boards/coptercontrol/board_hw_defs.c +++ b/flight/targets/boards/coptercontrol/board_hw_defs.c @@ -1021,6 +1021,86 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { #endif /* PIOS_INCLUDE_DSM */ +#if defined(PIOS_INCLUDE_HOTT) +/* + * HOTT USART + */ +#include + +static const struct pios_usart_cfg pios_usart_hott_main_cfg = { + .regs = USART1, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, +}; + +static const struct pios_usart_cfg pios_usart_hott_flexi_cfg = { + .regs = USART3, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, +}; + +#endif /* PIOS_INCLUDE_HOTT */ + #if defined(PIOS_INCLUDE_SRXL) /* * SRXL USART diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index 02fcd4979..54b1d3039 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -102,6 +102,7 @@ #define PIOS_INCLUDE_DSM #define PIOS_INCLUDE_SBUS #define PIOS_INCLUDE_SRXL +#define PIOS_INCLUDE_HOTT /* #define PIOS_INCLUDE_GCSRCVR */ /* #define PIOS_INCLUDE_OPLINKRCVR */ diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index 659980639..ef1d2b4ec 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -518,6 +518,29 @@ void PIOS_Board_Init(void) } #endif /* PIOS_INCLUDE_DSM */ break; + case HWSETTINGS_CC_MAINPORT_HOTTSUMD: + case HWSETTINGS_CC_MAINPORT_HOTTSUMH: +#if defined(PIOS_INCLUDE_HOTT) + { + uint32_t pios_usart_hott_id; + if (PIOS_USART_Init(&pios_usart_hott_id, &pios_usart_hott_main_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_hott_id; + if (PIOS_HOTT_Init(&pios_hott_id, &pios_usart_com_driver, pios_usart_hott_id, + hwsettings_cc_mainport == HWSETTINGS_CC_MAINPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH)) { + PIOS_Assert(0); + } + + uint32_t pios_hott_rcvr_id; + if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTMAINPORT] = pios_hott_rcvr_id; + } +#endif /* PIOS_INCLUDE_HOTT */ + break; case HWSETTINGS_CC_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) @@ -674,6 +697,31 @@ void PIOS_Board_Init(void) } #endif /* PIOS_INCLUDE_DSM */ break; + + case HWSETTINGS_CC_FLEXIPORT_HOTTSUMD: + case HWSETTINGS_CC_FLEXIPORT_HOTTSUMH: +#if defined(PIOS_INCLUDE_HOTT) + { + uint32_t pios_usart_hott_id; + if (PIOS_USART_Init(&pios_usart_hott_id, &pios_usart_hott_flexi_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_hott_id; + if (PIOS_HOTT_Init(&pios_hott_id, &pios_usart_com_driver, pios_usart_hott_id, + hwsettings_cc_flexiport == HWSETTINGS_CC_FLEXIPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH)) { + PIOS_Assert(0); + } + + uint32_t pios_hott_rcvr_id; + if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTFLEXIPORT] = pios_hott_rcvr_id; + } +#endif /* PIOS_INCLUDE_HOTT */ + break; + case HWSETTINGS_CC_FLEXIPORT_SRXL: #if defined(PIOS_INCLUDE_SRXL) { diff --git a/flight/targets/boards/coptercontrol/pios_board.h b/flight/targets/boards/coptercontrol/pios_board.h index 36e719378..6fad48ef9 100644 --- a/flight/targets/boards/coptercontrol/pios_board.h +++ b/flight/targets/boards/coptercontrol/pios_board.h @@ -248,6 +248,12 @@ extern uint32_t pios_com_hkosd_id; #define PIOS_SBUS_MAX_DEVS 1 #define PIOS_SBUS_NUM_INPUTS (16 + 2) +//------------------------- +// Receiver HSUM input +//------------------------- +#define PIOS_HOTT_MAX_DEVS 2 +#define PIOS_HOTT_NUM_INPUTS 32 + // ------------------------- // Receiver Multiplex SRXL input // ------------------------- diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 9d705d2cc..9b5459678 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -2,8 +2,8 @@ Selection of optional hardware configurations. - - + + diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index b95020df9..93565d66c 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -3,7 +3,7 @@ Settings to indicate how to decode receiver input by @ref ManualControlModule. + options="PWM,PPM,DSM (MainPort),DSM (FlexiPort),S.Bus,HOTT (MainPort),HOTT (FlexiPort),SRXL,GCS,OPLink,None" defaultvalue="None"/> Date: Sun, 20 Dec 2015 11:57:46 +0100 Subject: [PATCH 02/19] LP-104 Adding support to GCS for HoTT configuration --- ground/gcs/src/plugins/config/inputchannelform.cpp | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/ground/gcs/src/plugins/config/inputchannelform.cpp b/ground/gcs/src/plugins/config/inputchannelform.cpp index 962bb7006..3f5ed7956 100644 --- a/ground/gcs/src/plugins/config/inputchannelform.cpp +++ b/ground/gcs/src/plugins/config/inputchannelform.cpp @@ -161,6 +161,10 @@ void InputChannelForm::groupUpdated() case ManualControlSettings::CHANNELGROUPS_SRXL: count = 16; break; + case ManualControlSettings::CHANNELGROUPS_HOTTMAINPORT: + case ManualControlSettings::CHANNELGROUPS_HOTTFLEXIPORT: + count = 32; + break; case ManualControlSettings::CHANNELGROUPS_GCS: count = GCSReceiver::CHANNEL_NUMELEM; break; From 7a950503815c6ef7cd1715da330acd37a7849374 Mon Sep 17 00:00:00 2001 From: Fredrik Arvidsson Date: Sun, 20 Dec 2015 13:36:45 +0100 Subject: [PATCH 03/19] LP-104 Added missing code to receiver.c to detect receiver activity. This probably caused a crash on any activity of the HoTT receiver. Added 40 bytes to the receiver stack. Added options to receiver activity.xml uavo definition. --- flight/modules/Receiver/receiver.c | 6 ++++++ .../targets/boards/coptercontrol/firmware/inc/pios_config.h | 2 +- shared/uavobjectdefinition/receiveractivity.xml | 2 +- 3 files changed, 8 insertions(+), 2 deletions(-) diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c index 4dfd26770..64436fa1e 100644 --- a/flight/modules/Receiver/receiver.c +++ b/flight/modules/Receiver/receiver.c @@ -658,6 +658,12 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTMAINPORT: + group = RECEIVERACTIVITY_ACTIVEGROUP_HOTTMAINPORT; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTFLEXIPORT: + group = RECEIVERACTIVITY_ACTIVEGROUP_HOTTFLEXIPORT; + break; case MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL: group = RECEIVERACTIVITY_ACTIVEGROUP_SRXL; break; diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index 54b1d3039..3a62c81b4 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -166,7 +166,7 @@ /* Task stack sizes */ #define PIOS_ACTUATOR_STACK_SIZE 700 #define PIOS_MANUAL_STACK_SIZE 735 -#define PIOS_RECEIVER_STACK_SIZE 620 +#define PIOS_RECEIVER_STACK_SIZE 660 #define PIOS_STABILIZATION_STACK_SIZE 400 #ifdef DIAG_TASKS diff --git a/shared/uavobjectdefinition/receiveractivity.xml b/shared/uavobjectdefinition/receiveractivity.xml index 8724b3f51..33056be7a 100644 --- a/shared/uavobjectdefinition/receiveractivity.xml +++ b/shared/uavobjectdefinition/receiveractivity.xml @@ -2,7 +2,7 @@ Monitors which receiver channels have been active within the last second. From 5273df263c9673834b988becf3ff888df842df90 Mon Sep 17 00:00:00 2001 From: Fredrik Arvidsson Date: Sun, 20 Dec 2015 21:11:15 +0100 Subject: [PATCH 04/19] LP-104 Reverted stack size for receiver module after testing. --- flight/targets/boards/coptercontrol/firmware/inc/pios_config.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index 3a62c81b4..54b1d3039 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -166,7 +166,7 @@ /* Task stack sizes */ #define PIOS_ACTUATOR_STACK_SIZE 700 #define PIOS_MANUAL_STACK_SIZE 735 -#define PIOS_RECEIVER_STACK_SIZE 660 +#define PIOS_RECEIVER_STACK_SIZE 620 #define PIOS_STABILIZATION_STACK_SIZE 400 #ifdef DIAG_TASKS From 68ff383637ee9e5bd5714b74b8bd70cb1acc6730 Mon Sep 17 00:00:00 2001 From: Fredrik Arvidsson Date: Sun, 20 Dec 2015 23:45:37 +0100 Subject: [PATCH 05/19] LP-104 Adding HoTT support for Revolution and Revo Nano. --- .../targets/boards/revolution/board_hw_defs.c | 89 +++++++++++++++++++ .../revolution/firmware/inc/pios_config.h | 1 + .../boards/revolution/firmware/pios_board.c | 48 ++++++++++ flight/targets/boards/revolution/pios_board.h | 6 ++ .../targets/boards/revonano/board_hw_defs.c | 89 +++++++++++++++++++ .../revonano/firmware/inc/pios_config.h | 1 + .../boards/revonano/firmware/pios_board.c | 47 ++++++++++ flight/targets/boards/revonano/pios_board.h | 6 ++ shared/uavobjectdefinition/hwsettings.xml | 4 +- 9 files changed, 289 insertions(+), 2 deletions(-) diff --git a/flight/targets/boards/revolution/board_hw_defs.c b/flight/targets/boards/revolution/board_hw_defs.c index d77666147..b0e1904dc 100644 --- a/flight/targets/boards/revolution/board_hw_defs.c +++ b/flight/targets/boards/revolution/board_hw_defs.c @@ -1054,6 +1054,95 @@ static const struct pios_usart_cfg pios_usart_srxl_flexi_cfg = { }; #endif /* PIOS_INCLUDE_SRXL */ +#if defined(PIOS_INCLUDE_HOTT) +/* + * HOTT USART + */ +#include + +static const struct pios_usart_cfg pios_usart_hott_flexi_cfg = { + .regs = USART3, + .remap = GPIO_AF_USART3, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, +}; + +static const struct pios_usart_cfg pios_usart_hott_main_cfg = { + .regs = USART1, + .remap = GPIO_AF_USART1, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, +}; + +#endif /* PIOS_INCLUDE_HOTT */ /* * HK OSD */ diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index baa1680ba..bced97f12 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -104,6 +104,7 @@ #define PIOS_INCLUDE_DSM #define PIOS_INCLUDE_SBUS #define PIOS_INCLUDE_SRXL +#define PIOS_INCLUDE_HOTT #define PIOS_INCLUDE_GCSRCVR #define PIOS_INCLUDE_OPLINKRCVR diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 5bfc0a16c..495fe927b 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -520,6 +520,30 @@ void PIOS_Board_Init(void) } #endif break; + case HWSETTINGS_RM_FLEXIPORT_HOTTSUMD: + case HWSETTINGS_RM_FLEXIPORT_HOTTSUMH: +#if defined(PIOS_INCLUDE_HOTT) + { + uint32_t pios_usart_hott_id; + if (PIOS_USART_Init(&pios_usart_hott_id, &pios_usart_hott_flexi_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_hott_id; + if (PIOS_HOTT_Init(&pios_hott_id, &pios_usart_com_driver, pios_usart_hott_id, + hwsettings_flexiport == HWSETTINGS_RM_FLEXIPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH)) { + PIOS_Assert(0); + } + + uint32_t pios_hott_rcvr_id; + if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTFLEXIPORT] = pios_hott_rcvr_id; + } +#endif /* PIOS_INCLUDE_HOTT */ + break; + } /* hwsettings_rm_flexiport */ /* Moved this here to allow binding on flexiport */ @@ -704,6 +728,30 @@ void PIOS_Board_Init(void) PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); break; + case HWSETTINGS_RM_MAINPORT_HOTTSUMD: + case HWSETTINGS_RM_MAINPORT_HOTTSUMH: +#if defined(PIOS_INCLUDE_HOTT) + { + uint32_t pios_usart_hott_id; + if (PIOS_USART_Init(&pios_usart_hott_id, &pios_usart_hott_main_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_hott_id; + if (PIOS_HOTT_Init(&pios_hott_id, &pios_usart_com_driver, pios_usart_hott_id, + hwsettings_mainport == HWSETTINGS_RM_MAINPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH)) { + PIOS_Assert(0); + } + + uint32_t pios_hott_rcvr_id; + if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTMAINPORT] = pios_hott_rcvr_id; + } +#endif /* PIOS_INCLUDE_HOTT */ + break; + case HWSETTINGS_RM_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index ff723d419..feb778a78 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -257,6 +257,12 @@ extern uint32_t pios_packet_handler; #define PIOS_SBUS_MAX_DEVS 1 #define PIOS_SBUS_NUM_INPUTS (16 + 2) +//------------------------- +// Receiver HSUM input +//------------------------- +#define PIOS_HOTT_MAX_DEVS 2 +#define PIOS_HOTT_NUM_INPUTS 32 + // ------------------------- // Receiver Multiplex SRXL input // ------------------------- diff --git a/flight/targets/boards/revonano/board_hw_defs.c b/flight/targets/boards/revonano/board_hw_defs.c index 2c3deefe0..b0ca380e2 100644 --- a/flight/targets/boards/revonano/board_hw_defs.c +++ b/flight/targets/boards/revonano/board_hw_defs.c @@ -535,6 +535,95 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { #endif /* PIOS_INCLUDE_DSM */ +#if defined(PIOS_INCLUDE_HOTT) +/* + * HOTT USART + */ +#include +static const struct pios_usart_cfg pios_usart_hott_main_cfg = { + .regs = MAIN_USART_REGS, + .remap = MAIN_USART_REMAP, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = MAIN_USART_IRQ, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = MAIN_USART_RX_GPIO, + .init = { + .GPIO_Pin = MAIN_USART_RX_PIN, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = MAIN_USART_TX_GPIO, + .init = { + .GPIO_Pin = MAIN_USART_TX_PIN, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, +}; + +static const struct pios_usart_cfg pios_usart_hott_flexi_cfg = { + .regs = FLEXI_USART_REGS, + .remap = FLEXI_USART_REMAP, + .init = { + .USART_BaudRate = 115200, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = FLEXI_USART_IRQ, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = FLEXI_USART_RX_GPIO, + .init = { + .GPIO_Pin = FLEXI_USART_RX_PIN, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = FLEXI_USART_TX_GPIO, + .init = { + .GPIO_Pin = FLEXI_USART_TX_PIN, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, +}; + +#endif /* PIOS_INCLUDE_HOTT */ + #if defined(PIOS_INCLUDE_SRXL) /* * SRXL USART diff --git a/flight/targets/boards/revonano/firmware/inc/pios_config.h b/flight/targets/boards/revonano/firmware/inc/pios_config.h index 16803c751..f8315d626 100644 --- a/flight/targets/boards/revonano/firmware/inc/pios_config.h +++ b/flight/targets/boards/revonano/firmware/inc/pios_config.h @@ -108,6 +108,7 @@ #define PIOS_INCLUDE_DSM #define PIOS_INCLUDE_SBUS #define PIOS_INCLUDE_SRXL +#define PIOS_INCLUDE_HOTT #define PIOS_INCLUDE_GCSRCVR // #define PIOS_INCLUDE_OPLINKRCVR diff --git a/flight/targets/boards/revonano/firmware/pios_board.c b/flight/targets/boards/revonano/firmware/pios_board.c index dcf373a51..5b1a021e5 100644 --- a/flight/targets/boards/revonano/firmware/pios_board.c +++ b/flight/targets/boards/revonano/firmware/pios_board.c @@ -586,6 +586,30 @@ void PIOS_Board_Init(void) &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); } break; + case HWSETTINGS_RM_MAINPORT_HOTTSUMD: + case HWSETTINGS_RM_MAINPORT_HOTTSUMH: +#if defined(PIOS_INCLUDE_HOTT) + { + uint32_t pios_usart_hott_id; + if (PIOS_USART_Init(&pios_usart_hott_id, &pios_usart_hott_main_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_hott_id; + if (PIOS_HOTT_Init(&pios_hott_id, &pios_usart_com_driver, pios_usart_hott_id, + hwsettings_mainport == HWSETTINGS_RM_MAINPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH)) { + PIOS_Assert(0); + } + + uint32_t pios_hott_rcvr_id; + if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTMAINPORT] = pios_hott_rcvr_id; + } +#endif /* PIOS_INCLUDE_HOTT */ + break; + case HWSETTINGS_RM_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { @@ -666,6 +690,29 @@ void PIOS_Board_Init(void) } #endif break; + case HWSETTINGS_RM_FLEXIPORT_HOTTSUMD: + case HWSETTINGS_RM_FLEXIPORT_HOTTSUMH: +#if defined(PIOS_INCLUDE_HOTT) + { + uint32_t pios_usart_hott_id; + if (PIOS_USART_Init(&pios_usart_hott_id, &pios_usart_hott_flexi_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_hott_id; + if (PIOS_HOTT_Init(&pios_hott_id, &pios_usart_com_driver, pios_usart_hott_id, + hwsettings_flexiport == HWSETTINGS_RM_FLEXIPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH)) { + PIOS_Assert(0); + } + + uint32_t pios_hott_rcvr_id; + if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTFLEXIPORT] = pios_hott_rcvr_id; + } +#endif /* PIOS_INCLUDE_HOTT */ + break; } /* hwsettings_rm_flexiport */ diff --git a/flight/targets/boards/revonano/pios_board.h b/flight/targets/boards/revonano/pios_board.h index 54776fe34..0805c1a5f 100644 --- a/flight/targets/boards/revonano/pios_board.h +++ b/flight/targets/boards/revonano/pios_board.h @@ -257,6 +257,12 @@ extern uint32_t pios_packet_handler; #define PIOS_SBUS_MAX_DEVS 1 #define PIOS_SBUS_NUM_INPUTS (16 + 2) +//------------------------- +// Receiver HSUM input +//------------------------- +#define PIOS_HOTT_MAX_DEVS 2 +#define PIOS_HOTT_NUM_INPUTS 32 + // ------------------------- // Receiver Multiplex SRXL input // ------------------------- diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 9b5459678..f5fc9645f 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -15,8 +15,8 @@ - - + + From 3d56c4f3843b9fd77172f6b36c2677d62e94704e Mon Sep 17 00:00:00 2001 From: Fredrik Arvidsson Date: Tue, 22 Dec 2015 09:23:57 +0100 Subject: [PATCH 06/19] LP-104 Updated copyright headers with LibrePilot --- flight/targets/boards/revolution/board_hw_defs.c | 1 + flight/targets/boards/revolution/firmware/inc/pios_config.h | 1 + flight/targets/boards/revolution/firmware/pios_board.c | 1 + flight/targets/boards/revolution/pios_board.h | 1 + flight/targets/boards/revonano/board_hw_defs.c | 1 + flight/targets/boards/revonano/firmware/inc/pios_config.h | 1 + flight/targets/boards/revonano/firmware/pios_board.c | 1 + flight/targets/boards/revonano/pios_board.h | 1 + 8 files changed, 8 insertions(+) diff --git a/flight/targets/boards/revolution/board_hw_defs.c b/flight/targets/boards/revolution/board_hw_defs.c index b0e1904dc..76804828c 100644 --- a/flight/targets/boards/revolution/board_hw_defs.c +++ b/flight/targets/boards/revolution/board_hw_defs.c @@ -1,6 +1,7 @@ /** ****************************************************************************** * @file board_hw_defs.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 * @addtogroup OpenPilotSystem OpenPilot System diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index bced97f12..2ce3ede00 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -5,6 +5,7 @@ * @addtogroup OpenPilotCore OpenPilot Core * @{ * @file pios_config.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. * @brief PiOS configuration header, the compile time config file for the PIOS. * Defines which PiOS libraries and features are included in the firmware. diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 495fe927b..7a3d5636f 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -1,6 +1,7 @@ /** ****************************************************************************** * @file pios_board.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. * @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 * @addtogroup OpenPilotSystem OpenPilot System diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index feb778a78..028b5c891 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -5,6 +5,7 @@ * @addtogroup OpenPilotCore OpenPilot Core * @{ * @file pios_board.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. * @see The GNU Public License (GPL) Version 3 diff --git a/flight/targets/boards/revonano/board_hw_defs.c b/flight/targets/boards/revonano/board_hw_defs.c index b0ca380e2..b198ff690 100644 --- a/flight/targets/boards/revonano/board_hw_defs.c +++ b/flight/targets/boards/revonano/board_hw_defs.c @@ -1,6 +1,7 @@ /** ****************************************************************************** * @file board_hw_defs.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * @addtogroup OpenPilotSystem OpenPilot System * @{ diff --git a/flight/targets/boards/revonano/firmware/inc/pios_config.h b/flight/targets/boards/revonano/firmware/inc/pios_config.h index f8315d626..d6a8ee0db 100644 --- a/flight/targets/boards/revonano/firmware/inc/pios_config.h +++ b/flight/targets/boards/revonano/firmware/inc/pios_config.h @@ -5,6 +5,7 @@ * @addtogroup OpenPilotCore OpenPilot Core * @{ * @file pios_config.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. * @brief PiOS configuration header, the compile time config file for the PIOS. * Defines which PiOS libraries and features are included in the firmware. diff --git a/flight/targets/boards/revonano/firmware/pios_board.c b/flight/targets/boards/revonano/firmware/pios_board.c index 5b1a021e5..3c3c32a8c 100644 --- a/flight/targets/boards/revonano/firmware/pios_board.c +++ b/flight/targets/boards/revonano/firmware/pios_board.c @@ -1,6 +1,7 @@ /** ****************************************************************************** * @file pios_board.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. * @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 * @addtogroup OpenPilotSystem OpenPilot System diff --git a/flight/targets/boards/revonano/pios_board.h b/flight/targets/boards/revonano/pios_board.h index 0805c1a5f..dbb75cbad 100644 --- a/flight/targets/boards/revonano/pios_board.h +++ b/flight/targets/boards/revonano/pios_board.h @@ -5,6 +5,7 @@ * @addtogroup OpenPilotCore OpenPilot Core * @{ * @file pios_board.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. * @see The GNU Public License (GPL) Version 3 From 879c8fc9b96f2b9f67aa499332c1c610669bface Mon Sep 17 00:00:00 2001 From: Fredrik Arvidsson Date: Tue, 22 Dec 2015 18:00:49 +0100 Subject: [PATCH 07/19] LP-104 Correction of @author tag when multiple authors are present. --- flight/targets/boards/revolution/board_hw_defs.c | 4 ++-- flight/targets/boards/revolution/firmware/inc/pios_config.h | 2 +- flight/targets/boards/revolution/firmware/pios_board.c | 4 ++-- flight/targets/boards/revolution/pios_board.h | 2 +- flight/targets/boards/revonano/board_hw_defs.c | 2 +- flight/targets/boards/revonano/firmware/inc/pios_config.h | 2 +- flight/targets/boards/revonano/firmware/pios_board.c | 4 ++-- flight/targets/boards/revonano/pios_board.h | 2 +- 8 files changed, 11 insertions(+), 11 deletions(-) diff --git a/flight/targets/boards/revolution/board_hw_defs.c b/flight/targets/boards/revolution/board_hw_defs.c index 76804828c..20b99c968 100644 --- a/flight/targets/boards/revolution/board_hw_defs.c +++ b/flight/targets/boards/revolution/board_hw_defs.c @@ -2,8 +2,8 @@ ****************************************************************************** * @file board_hw_defs.c * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. - * @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 * @addtogroup OpenPilotSystem OpenPilot System * @{ * @addtogroup OpenPilotCore OpenPilot Core diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index 2ce3ede00..800827b78 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -6,7 +6,7 @@ * @{ * @file pios_config.h * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. * @brief PiOS configuration header, the compile time config file for the PIOS. * Defines which PiOS libraries and features are included in the firmware. * @see The GNU Public License (GPL) Version 3 diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 7a3d5636f..944373969 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -2,8 +2,8 @@ ****************************************************************************** * @file pios_board.c * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. - * @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. + * PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 * @addtogroup OpenPilotSystem OpenPilot System * @{ * @addtogroup OpenPilotCore OpenPilot Core diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index 028b5c891..ebfeea0ea 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -6,7 +6,7 @@ * @{ * @file pios_board.h * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. * @see The GNU Public License (GPL) Version 3 * diff --git a/flight/targets/boards/revonano/board_hw_defs.c b/flight/targets/boards/revonano/board_hw_defs.c index b198ff690..6b4ee3001 100644 --- a/flight/targets/boards/revonano/board_hw_defs.c +++ b/flight/targets/boards/revonano/board_hw_defs.c @@ -2,7 +2,7 @@ ****************************************************************************** * @file board_hw_defs.c * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2014. * @addtogroup OpenPilotSystem OpenPilot System * @{ * @addtogroup OpenPilotCore OpenPilot Core diff --git a/flight/targets/boards/revonano/firmware/inc/pios_config.h b/flight/targets/boards/revonano/firmware/inc/pios_config.h index d6a8ee0db..7738e11e2 100644 --- a/flight/targets/boards/revonano/firmware/inc/pios_config.h +++ b/flight/targets/boards/revonano/firmware/inc/pios_config.h @@ -6,7 +6,7 @@ * @{ * @file pios_config.h * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2010-2013. * @brief PiOS configuration header, the compile time config file for the PIOS. * Defines which PiOS libraries and features are included in the firmware. * @see The GNU Public License (GPL) Version 3 diff --git a/flight/targets/boards/revonano/firmware/pios_board.c b/flight/targets/boards/revonano/firmware/pios_board.c index 3c3c32a8c..818361f61 100644 --- a/flight/targets/boards/revonano/firmware/pios_board.c +++ b/flight/targets/boards/revonano/firmware/pios_board.c @@ -2,8 +2,8 @@ ****************************************************************************** * @file pios_board.c * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. - * @author PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2011. + * PhoenixPilot, http://github.com/PhoenixPilot, Copyright (C) 2012 * @addtogroup OpenPilotSystem OpenPilot System * @{ * @addtogroup OpenPilotCore OpenPilot Core diff --git a/flight/targets/boards/revonano/pios_board.h b/flight/targets/boards/revonano/pios_board.h index dbb75cbad..1a691553f 100644 --- a/flight/targets/boards/revonano/pios_board.h +++ b/flight/targets/boards/revonano/pios_board.h @@ -6,7 +6,7 @@ * @{ * @file pios_board.h * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. - * @author The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. + * The OpenPilot Team, http://www.openpilot.org Copyright (C) 2012. * @brief Defines board hardware for the OpenPilot Version 1.1 hardware. * @see The GNU Public License (GPL) Version 3 * From 3e920bc5bcf605f15b45723de5a22f739868da09 Mon Sep 17 00:00:00 2001 From: Fredrik Arvidsson Date: Sat, 26 Dec 2015 12:25:54 +0100 Subject: [PATCH 08/19] LP-196 Added driver for Jeti EX.Bus protocol. Added support for CopterControl target on Main and Flexi port. This is WIP and not tested yet. --- flight/make/apps-defs.mk | 3 +- flight/modules/Receiver/receiver.c | 6 + flight/pios/common/pios_exbus.c | 421 ++++++++++++++++++ flight/pios/inc/pios_exbus.h | 43 ++ flight/pios/inc/pios_exbus_priv.h | 47 ++ .../boards/coptercontrol/board_hw_defs.c | 78 ++++ .../coptercontrol/firmware/inc/pios_config.h | 1 + .../coptercontrol/firmware/pios_board.c | 43 ++ .../targets/boards/coptercontrol/pios_board.h | 6 + shared/uavobjectdefinition/hwsettings.xml | 8 +- .../manualcontrolsettings.xml | 2 +- .../uavobjectdefinition/receiveractivity.xml | 2 +- 12 files changed, 653 insertions(+), 7 deletions(-) create mode 100644 flight/pios/common/pios_exbus.c create mode 100644 flight/pios/inc/pios_exbus.h create mode 100644 flight/pios/inc/pios_exbus_priv.h diff --git a/flight/make/apps-defs.mk b/flight/make/apps-defs.mk index 45bdb0049..44dd6ed19 100644 --- a/flight/make/apps-defs.mk +++ b/flight/make/apps-defs.mk @@ -34,7 +34,7 @@ FLIGHTLIBINC = $(FLIGHTLIB)/inc OPUAVOBJINC = $(OPUAVOBJ)/inc OPUAVTALKINC = $(OPUAVTALK)/inc -## PID +## PID PIDLIB =$(FLIGHTLIB)/pid PIDLIBINC =$(FLIGHTLIB)/pid @@ -87,6 +87,7 @@ SRC += $(PIOSCOMMON)/pios_rfm22b_com.c SRC += $(PIOSCOMMON)/pios_rcvr.c SRC += $(PIOSCOMMON)/pios_sbus.c SRC += $(PIOSCOMMON)/pios_srxl.c +SRC += $(PIOSCOMMON)/pios_exbus.c SRC += $(PIOSCOMMON)/pios_sdcard.c SRC += $(PIOSCOMMON)/pios_sensors.c diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c index 4dfd26770..d35d1ccb5 100644 --- a/flight/modules/Receiver/receiver.c +++ b/flight/modules/Receiver/receiver.c @@ -655,6 +655,12 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT: group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT; break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_JETIEXBUSMAINPORT: + group = RECEIVERACTIVITY_ACTIVEGROUP_JETIEXBUSMAINPORT; + break; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_JETIEXBUSFLEXIPORT: + group = RECEIVERACTIVITY_ACTIVEGROUP_JETIEXBUSFLEXIPORT; + break; case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; break; diff --git a/flight/pios/common/pios_exbus.c b/flight/pios/common/pios_exbus.c new file mode 100644 index 000000000..147afcb3e --- /dev/null +++ b/flight/pios/common/pios_exbus.c @@ -0,0 +1,421 @@ +/** + ****************************************************************************** + * @file pios_exbus.c + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * Tau Labs, http://taulabs.org, Copyright (C) 2013 + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_EXBUS Jeti EX Bus receiver functions + * @{ + * @brief Jeti EX Bus receiver functions + *****************************************************************************/ +/* + * 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_exbus_priv.h" + +#if defined(PIOS_INCLUDE_EXBUS) + +#if !defined(PIOS_INCLUDE_RTC) +#error PIOS_INCLUDE_RTC must be used to use EXBUS +#endif + +/** + * Based on Jeti EX Bus protocol version 1.21 + * Available at http://www.jetimodel.com/en/Telemetry-Protocol/ + */ + +/* EXBUS frame size and contents definitions */ +#define EXBUS_HEADER_LENGTH 4 +#define EXBUS_CRC_LENGTH 2 +#define EXBUS_MAX_CHANNELS 16 +#define EXBUS_OVERHEAD_LENGTH (EXBUS_HEADER_LENGTH + EXBUS_CRC_LENGTH) +#define EXBUS_MAX_FRAME_LENGTH (EXBUS_MAX_CHANNELS*2+10 + EXBUS_OVERHEAD_LENGTH) + +#define EXBUS_SYNC_CHANNEL 0x3E +#define EXBUS_SYNC_TELEMETRY 0x3D +#define EXBUS_BYTE_REQ 0x01 +#define EXBUS_BYTE_NOREQ 0x03 + +#define EXBUS_DATA_CHANNEL 0x31 +#define EXBUS_DATA_TELEMETRY 0x3A +#define EXBUS_DATA_JETIBOX 0x3B + +/* Forward Declarations */ +static int32_t PIOS_EXBUS_Get(uint32_t rcvr_id, uint8_t channel); +static uint16_t PIOS_EXBUS_RxInCallback(uint32_t context, + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield); +static void PIOS_EXBUS_Supervisor(uint32_t exbus_id); +static uint16_t PIOS_EXBUS_CRC_Update(uint16_t crc, uint8_t data); + +/* Local Variables */ +const struct pios_rcvr_driver pios_exbus_rcvr_driver = { + .read = PIOS_EXBUS_Get, +}; + +enum pios_exbus_dev_magic { + PIOS_EXBUS_DEV_MAGIC = 0x485355FF, +}; + +enum pios_exbus_frame_state { + EXBUS_STATE_SYNC, + EXBUS_STATE_REQ, + EXBUS_STATE_LEN, + EXBUS_STATE_PACKET_ID, + EXBUS_STATE_DATA_ID, + EXBUS_STATE_SUBLEN, + EXBUS_STATE_DATA +}; + +struct pios_exbus_state { + uint16_t channel_data[PIOS_EXBUS_NUM_INPUTS]; + uint8_t received_data[EXBUS_MAX_FRAME_LENGTH]; + uint8_t receive_timer; + uint8_t failsafe_timer; + uint8_t tx_connected; + uint8_t byte_count; + uint8_t frame_length; + uint16_t crc; + bool frame_found; +}; + +struct pios_exbus_dev { + enum pios_exbus_dev_magic magic; + const struct pios_exbus_cfg *cfg; + struct pios_exbus_state state; +}; + +/* Allocate EXBUS device descriptor */ +#if defined(PIOS_INCLUDE_FREERTOS) +static struct pios_exbus_dev *PIOS_EXBUS_Alloc(void) +{ + struct pios_exbus_dev *exbus_dev; + + exbus_dev = (struct pios_exbus_dev *)pios_malloc(sizeof(*exbus_dev)); + if (!exbus_dev) + return NULL; + + exbus_dev->magic = PIOS_EXBUS_DEV_MAGIC; + return exbus_dev; +} +#else +static struct pios_exbus_dev pios_exbus_devs[PIOS_EXBUS_MAX_DEVS]; +static uint8_t pios_exbus_num_devs; +static struct pios_exbus_dev *PIOS_EXBUS_Alloc(void) +{ + struct pios_exbus_dev *exbus_dev; + + if (pios_exbus_num_devs >= PIOS_EXBUS_MAX_DEVS) + return NULL; + + exbus_dev = &pios_exbus_devs[pios_exbus_num_devs++]; + exbus_dev->magic = PIOS_EXBUS_DEV_MAGIC; + + return exbus_dev; +} +#endif + +/* Validate EXBUS device descriptor */ +static bool PIOS_EXBUS_Validate(struct pios_exbus_dev *exbus_dev) +{ + return (exbus_dev->magic == PIOS_EXBUS_DEV_MAGIC); +} + +/* Reset channels in case of lost signal or explicit failsafe receiver flag */ +static void PIOS_EXBUS_ResetChannels(struct pios_exbus_dev *exbus_dev) +{ + struct pios_exbus_state *state = &(exbus_dev->state); + for (int i = 0; i < PIOS_EXBUS_NUM_INPUTS; i++) { + state->channel_data[i] = PIOS_RCVR_TIMEOUT; + } +} + +/* Reset EXBUS receiver state */ +static void PIOS_EXBUS_ResetState(struct pios_exbus_dev *exbus_dev) +{ + struct pios_exbus_state *state = &(exbus_dev->state); + state->receive_timer = 0; + state->failsafe_timer = 0; + state->frame_found = false; + state->tx_connected = 0; + PIOS_EXBUS_ResetChannels(exbus_dev); +} + +/** + * Check and unroll complete frame data. + * \output 0 frame data accepted + * \output -1 frame error found + */ +static int PIOS_EXBUS_UnrollChannels(struct pios_exbus_dev *exbus_dev) +{ + struct pios_exbus_state *state = &(exbus_dev->state); + + if(state->crc != 0) { + /* crc failed */ + DEBUG_PRINTF(2, "Jeti CRC error!%d\r\n"); + goto stream_error; + } + + enum pios_exbus_frame_state exbus_state = EXBUS_STATE_SYNC; + uint8_t *byte = state->received_data; + uint8_t sub_length; + uint8_t n_channels = 0; + uint8_t channel = 0; + + while((byte - state->received_data) < state->frame_length) { + switch(exbus_state) { + case EXBUS_STATE_SYNC: + /* sync byte */ + if(*byte == EXBUS_SYNC_CHANNEL) { + exbus_state = EXBUS_STATE_REQ; + } + else { + goto stream_error; + } + byte += sizeof(uint8_t); + break; + + case EXBUS_STATE_REQ: + /* + * tells us whether the rx is requesting a reply or not, + * currently nothing is actually done with this information... + */ + if(*byte == EXBUS_BYTE_REQ) { + exbus_state = EXBUS_STATE_LEN; + } + else if(*byte == EXBUS_BYTE_NOREQ) { + exbus_state = EXBUS_STATE_LEN; + } + else + goto stream_error; + byte += sizeof(uint8_t); + break; + + case EXBUS_STATE_LEN: + /* total frame length */ + exbus_state = EXBUS_STATE_PACKET_ID; + byte += sizeof(uint8_t); + break; + + case EXBUS_STATE_PACKET_ID: + /* packet id */ + exbus_state = EXBUS_STATE_DATA_ID; + byte += sizeof(uint8_t); + break; + + case EXBUS_STATE_DATA_ID: + /* checks the type of data, ignore non-rc data */ + if(*byte == EXBUS_DATA_CHANNEL) { + exbus_state = EXBUS_STATE_SUBLEN; + } + else + goto stream_error; + byte += sizeof(uint8_t); + break; + case EXBUS_STATE_SUBLEN: + sub_length = *byte; + n_channels = sub_length / 2; + exbus_state = EXBUS_STATE_DATA; + byte += sizeof(uint8_t); + break; + case EXBUS_STATE_DATA: + if(channel < n_channels) { + /* 1 lsb = 1/8 us */ + state->channel_data[channel++] = (byte[1] << 8 | byte[0]) / 8; + } + byte += sizeof(uint16_t); + break; + } + } + + for(; channel < EXBUS_MAX_CHANNELS; channel++) { + /* this channel was not received */ + state->channel_data[channel] = PIOS_RCVR_INVALID; + } + +stream_error: + return -1; +} + +/* Update decoder state processing input byte from the stream */ +static void PIOS_EXBUS_UpdateState(struct pios_exbus_dev *exbus_dev, uint8_t byte) +{ + struct pios_exbus_state *state = &(exbus_dev->state); + + if(!state->frame_found) { + if(byte == EXBUS_SYNC_CHANNEL) { + state->frame_found = true; + state->byte_count = 0; + state->frame_length = EXBUS_MAX_FRAME_LENGTH; + state->crc = PIOS_EXBUS_CRC_Update(0, byte); + state->received_data[state->byte_count++] = byte; + } + } + else if(state->byte_count < EXBUS_MAX_FRAME_LENGTH) { + state->crc = PIOS_EXBUS_CRC_Update(state->crc, byte); + state->received_data[state->byte_count++] = byte; + + if(state->byte_count == 3) { + state->frame_length = byte; + } + if(state->byte_count == state->frame_length) { + if (!PIOS_EXBUS_UnrollChannels(exbus_dev)) + /* data looking good */ + state->failsafe_timer = 0; + /* get ready for the next frame */ + state->frame_found = false; + } + } + else { + state->frame_found = false; + } +} + +/* Initialise EX Bus receiver interface */ +int32_t PIOS_EXBUS_Init(uint32_t *exbus_id, + const struct pios_com_driver *driver, + uint32_t lower_id) +{ + PIOS_DEBUG_Assert(exbus_id); + PIOS_DEBUG_Assert(driver); + + struct pios_exbus_dev *exbus_dev; + + exbus_dev = (struct pios_exbus_dev *)PIOS_EXBUS_Alloc(); + if (!exbus_dev) + return -1; + + PIOS_EXBUS_ResetState(exbus_dev); + + *exbus_id = (uint32_t)exbus_dev; + + /* Set comm driver callback */ + (driver->bind_rx_cb)(lower_id, PIOS_EXBUS_RxInCallback, *exbus_id); + + if (!PIOS_RTC_RegisterTickCallback(PIOS_EXBUS_Supervisor, *exbus_id)) { + PIOS_DEBUG_Assert(0); + } + + return 0; +} + +/* Comm byte received callback */ +static uint16_t PIOS_EXBUS_RxInCallback(uint32_t context, + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield) +{ + struct pios_exbus_dev *exbus_dev = (struct pios_exbus_dev *)context; + + bool valid = PIOS_EXBUS_Validate(exbus_dev); + PIOS_Assert(valid); + + /* process byte(s) and clear receive timer */ + for (uint8_t i = 0; i < buf_len; i++) { + PIOS_EXBUS_UpdateState(exbus_dev, buf[i]); + exbus_dev->state.receive_timer = 0; + } + + /* Always signal that we can accept more data */ + if (headroom) + *headroom = EXBUS_MAX_FRAME_LENGTH; + + /* We never need a yield */ + *need_yield = false; + + /* Always indicate that all bytes were consumed */ + return buf_len; +} + +/** + * 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_EXBUS_Get(uint32_t rcvr_id, uint8_t channel) +{ + struct pios_exbus_dev *exbus_dev = (struct pios_exbus_dev *)rcvr_id; + + if (!PIOS_EXBUS_Validate(exbus_dev)) + return PIOS_RCVR_INVALID; + + /* return error if channel is not available */ + if (channel >= PIOS_EXBUS_NUM_INPUTS) + return PIOS_RCVR_INVALID; + + /* may also be PIOS_RCVR_TIMEOUT set by other function */ + return exbus_dev->state.channel_data[channel]; +} + +/** + * Input data supervisor is called periodically and provides + * two functions: frame syncing and failsafe triggering. + * + * EX.Bus frames come at 20ms or 10ms rate at 125 or 250 kbaud. + * RTC timer is running at 625Hz (1.6ms). So with divider 5 it gives + * 8ms pause between frames which is good for both EX.Bus frame rates. + * + * Data receive function must clear the receive_timer to confirm new + * data reception. If no new data received in 100ms, we must call the + * failsafe function which clears all channels. + */ +static void PIOS_EXBUS_Supervisor(uint32_t exbus_id) +{ + struct pios_exbus_dev *exbus_dev = (struct pios_exbus_dev *)exbus_id; + + bool valid = PIOS_EXBUS_Validate(exbus_dev); + PIOS_Assert(valid); + + struct pios_exbus_state *state = &(exbus_dev->state); + + /* waiting for new frame if no bytes were received in 8ms */ + if (++state->receive_timer > 4) { + state->frame_found = false; + state->byte_count = 0; + state->receive_timer = 0; + state->frame_length = EXBUS_MAX_FRAME_LENGTH; + } + + /* activate failsafe if no frames have arrived in 102.4ms */ + if (++state->failsafe_timer > 64) { + PIOS_EXBUS_ResetChannels(exbus_dev); + state->failsafe_timer = 0; + state->tx_connected = 0; + } +} + +static uint16_t PIOS_EXBUS_CRC_Update(uint16_t crc, uint8_t data) +{ + data ^= (uint8_t)crc & (uint8_t)0xFF; + data ^= data << 4; + crc = ((((uint16_t)data << 8) | ((crc & 0xFF00) >> 8)) + ^ (uint8_t)(data >> 4) ^ ((uint16_t)data << 3)); + + return crc; +} + +#endif /* PIOS_INCLUDE_EXBUS */ + +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_exbus.h b/flight/pios/inc/pios_exbus.h new file mode 100644 index 000000000..58d4a47d5 --- /dev/null +++ b/flight/pios/inc/pios_exbus.h @@ -0,0 +1,43 @@ +/** + ****************************************************************************** + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_SBus Futaba S.Bus receiver functions + * @{ + * + * @file pios_sbus.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * Tau Labs, http://taulabs.org, Copyright (C) 2013 + * @brief Futaba S.Bus 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_EXBUS_H +#define PIOS_EXBUS_H + +/* Global Types */ + +/* Public Functions */ + +#endif /* PIOS_EXBUS_H */ + +/** + * @} + * @} + */ diff --git a/flight/pios/inc/pios_exbus_priv.h b/flight/pios/inc/pios_exbus_priv.h new file mode 100644 index 000000000..e922fef57 --- /dev/null +++ b/flight/pios/inc/pios_exbus_priv.h @@ -0,0 +1,47 @@ +/** + ****************************************************************************** + * @file pios_exbus_priv.h + * @author The LibrePilot Project, http://www.librepilot.org Copyright (C) 2015. + * Tau Labs, http://taulabs.org, Copyright (C) 2013 + * @addtogroup PIOS PIOS Core hardware abstraction layer + * @{ + * @addtogroup PIOS_EXBUS Jeti EX Bus receiver functions + * @{ + * @brief Jeti EX Bus receiver functions + *****************************************************************************/ +/* + * 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_EXBUS_PRIV_H +#define PIOS_EXBUS_PRIV_H + +#include +#include + + +/* EXBUS receiver instance configuration */ +extern const struct pios_rcvr_driver pios_exbus_rcvr_driver; + +extern int32_t PIOS_EXBUS_Init(uint32_t *exbus_id, + const struct pios_com_driver *driver, + uint32_t lower_id); + +#endif /* PIOS_EXBUS_PRIV_H */ + +/** + * @} + * @} + */ diff --git a/flight/targets/boards/coptercontrol/board_hw_defs.c b/flight/targets/boards/coptercontrol/board_hw_defs.c index 1e464899d..df23500a2 100644 --- a/flight/targets/boards/coptercontrol/board_hw_defs.c +++ b/flight/targets/boards/coptercontrol/board_hw_defs.c @@ -1021,6 +1021,84 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { #endif /* PIOS_INCLUDE_DSM */ +#if defined(PIOS_INCLUDE_EXBUS) +/* + * EXBUS USART + */ +#include +static const struct pios_usart_cfg pios_usart_exbus_main_cfg = { + .regs = USART1, + .init = { + .USART_BaudRate = 250000, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, +}; + +static const struct pios_usart_cfg pios_usart_exbus_flexi_cfg = { + .regs = USART3, + .init = { + .USART_BaudRate = 250000, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IPU, + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_IN_FLOATING, + }, + }, +}; + +#endif /* PIOS_INCLUDE_EXBUS */ #if defined(PIOS_INCLUDE_SRXL) /* * SRXL USART diff --git a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h index 02fcd4979..ab54b8517 100644 --- a/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h +++ b/flight/targets/boards/coptercontrol/firmware/inc/pios_config.h @@ -101,6 +101,7 @@ #define PIOS_INCLUDE_PPM_FLEXI #define PIOS_INCLUDE_DSM #define PIOS_INCLUDE_SBUS +#define PIOS_INCLUDE_EXBUS #define PIOS_INCLUDE_SRXL /* #define PIOS_INCLUDE_GCSRCVR */ /* #define PIOS_INCLUDE_OPLINKRCVR */ diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index 659980639..db81b1992 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -518,6 +518,27 @@ void PIOS_Board_Init(void) } #endif /* PIOS_INCLUDE_DSM */ break; + case HWSETTINGS_CC_MAINPORT_JETIEXBUS: +#if defined(PIOS_INCLUDE_EXBUS) + { + uint32_t pios_usart_exbus_id; + if (PIOS_USART_Init(&pios_usart_exbus_id, &pios_usart_exbus_main_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_exbus_id; + if (PIOS_EXBUS_Init(&pios_exbus_id, &pios_usart_com_driver, pios_usart_exbus_id)) { + PIOS_Assert(0); + } + + uint32_t pios_exbus_rcvr_id; + if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_JETIEXBUSMAINPORT] = pios_exbus_rcvr_id; + } +#endif /* PIOS_INCLUDE_EXBUS */ + break; case HWSETTINGS_CC_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) @@ -674,6 +695,28 @@ void PIOS_Board_Init(void) } #endif /* PIOS_INCLUDE_DSM */ break; + case HWSETTINGS_CC_FLEXIPORT_JETIEXBUS: +#if defined(PIOS_INCLUDE_EXBUS) + { + uint32_t pios_usart_exbus_id; + if (PIOS_USART_Init(&pios_usart_exbus_id, &pios_usart_exbus_flexi_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_exbus_id; + if (PIOS_EXBUS_Init(&pios_exbus_id, &pios_usart_com_driver, pios_usart_exbus_id)) { + PIOS_Assert(0); + } + + uint32_t pios_exbus_rcvr_id; + if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_JETIEXBUSFLEXIPORT] = pios_exbus_rcvr_id; + } +#endif /* PIOS_INCLUDE_EXBUS */ + break; + case HWSETTINGS_CC_FLEXIPORT_SRXL: #if defined(PIOS_INCLUDE_SRXL) { diff --git a/flight/targets/boards/coptercontrol/pios_board.h b/flight/targets/boards/coptercontrol/pios_board.h index 36e719378..967754993 100644 --- a/flight/targets/boards/coptercontrol/pios_board.h +++ b/flight/targets/boards/coptercontrol/pios_board.h @@ -248,6 +248,12 @@ extern uint32_t pios_com_hkosd_id; #define PIOS_SBUS_MAX_DEVS 1 #define PIOS_SBUS_NUM_INPUTS (16 + 2) +// ------------------------- +// Receiver EX.Bus input +// ------------------------- +#define PIOS_EXBUS_MAX_DEVS 2 +#define PIOS_EXBUS_NUM_INPUTS 16 + // ------------------------- // Receiver Multiplex SRXL input // ------------------------- diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 9d705d2cc..95215f9c1 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -2,8 +2,8 @@ Selection of optional hardware configurations. - - + + @@ -15,8 +15,8 @@ - - + + diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index b95020df9..b5cf0bf12 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -3,7 +3,7 @@ Settings to indicate how to decode receiver input by @ref ManualControlModule. + options="PWM,PPM,DSM (MainPort),DSM (FlexiPort),Jeti EX.Bus (MainPort),Jeti EX.Bus (FlexiPort),S.Bus,SRXL,GCS,OPLink,None" defaultvalue="None"/> Monitors which receiver channels have been active within the last second. From 4b8582a3adbe1eb6fdcca909c1f41d8ff442aabe Mon Sep 17 00:00:00 2001 From: Fredrik Arvidsson Date: Sat, 26 Dec 2015 13:08:07 +0100 Subject: [PATCH 09/19] LP-196 Added support for targets Revolution and RevoNano. --- .../targets/boards/revolution/board_hw_defs.c | 89 +++++++++++++++++++ .../revolution/firmware/inc/pios_config.h | 1 + .../boards/revolution/firmware/pios_board.c | 43 +++++++++ flight/targets/boards/revolution/pios_board.h | 6 ++ .../targets/boards/revonano/board_hw_defs.c | 89 +++++++++++++++++++ .../revonano/firmware/inc/pios_config.h | 1 + .../boards/revonano/firmware/pios_board.c | 43 +++++++++ flight/targets/boards/revonano/pios_board.h | 6 ++ 8 files changed, 278 insertions(+) diff --git a/flight/targets/boards/revolution/board_hw_defs.c b/flight/targets/boards/revolution/board_hw_defs.c index d77666147..8af1f1d34 100644 --- a/flight/targets/boards/revolution/board_hw_defs.c +++ b/flight/targets/boards/revolution/board_hw_defs.c @@ -1054,6 +1054,95 @@ static const struct pios_usart_cfg pios_usart_srxl_flexi_cfg = { }; #endif /* PIOS_INCLUDE_SRXL */ +#if defined(PIOS_INCLUDE_EXBUS) +/* + * EXBUS USART + */ +#include + +static const struct pios_usart_cfg pios_usart_exbus_flexi_cfg = { + .regs = USART3, + .remap = GPIO_AF_USART3, + .init = { + .USART_BaudRate = 250000, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART3_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_11, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOB, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, +}; + +static const struct pios_usart_cfg pios_usart_exbus_main_cfg = { + .regs = USART1, + .remap = GPIO_AF_USART1, + .init = { + .USART_BaudRate = 250000, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = USART1_IRQn, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_10, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = GPIOA, + .init = { + .GPIO_Pin = GPIO_Pin_9, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, +}; +#endif /* PIOS_INCLUDE_EXBUS */ + /* * HK OSD */ diff --git a/flight/targets/boards/revolution/firmware/inc/pios_config.h b/flight/targets/boards/revolution/firmware/inc/pios_config.h index baa1680ba..5106a9d71 100644 --- a/flight/targets/boards/revolution/firmware/inc/pios_config.h +++ b/flight/targets/boards/revolution/firmware/inc/pios_config.h @@ -104,6 +104,7 @@ #define PIOS_INCLUDE_DSM #define PIOS_INCLUDE_SBUS #define PIOS_INCLUDE_SRXL +#define PIOS_INCLUDE_EXBUS #define PIOS_INCLUDE_GCSRCVR #define PIOS_INCLUDE_OPLINKRCVR diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 5bfc0a16c..0385f4dc2 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -520,6 +520,28 @@ void PIOS_Board_Init(void) } #endif break; + case HWSETTINGS_RM_FLEXIPORT_JETIEXBUS: +#if defined(PIOS_INCLUDE_EXBUS) + { + uint32_t pios_usart_exbus_id; + if (PIOS_USART_Init(&pios_usart_exbus_id, &pios_usart_exbus_flexi_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_exbus_id; + if (PIOS_EXBUS_Init(&pios_exbus_id, &pios_usart_com_driver, pios_usart_exbus_id)) { + PIOS_Assert(0); + } + + uint32_t pios_exbus_rcvr_id; + if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_JETIEXBUSFLEXIPORT] = pios_exbus_rcvr_id; + } +#endif /* PIOS_INCLUDE_EXBUS */ + break; + } /* hwsettings_rm_flexiport */ /* Moved this here to allow binding on flexiport */ @@ -696,6 +718,27 @@ void PIOS_Board_Init(void) } #endif break; + case HWSETTINGS_RM_MAINPORT_JETIEXBUS: +#if defined(PIOS_INCLUDE_EXBUS) + { + uint32_t pios_usart_exbus_id; + if (PIOS_USART_Init(&pios_usart_exbus_id, &pios_usart_exbus_main_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_exbus_id; + if (PIOS_EXBUS_Init(&pios_exbus_id, &pios_usart_com_driver, pios_usart_exbus_id)) { + PIOS_Assert(0); + } + + uint32_t pios_exbus_rcvr_id; + if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_JETIEXBUSMAINPORT] = pios_exbus_rcvr_id; + } +#endif /* PIOS_INCLUDE_EXBUS */ + break; case HWSETTINGS_RM_MAINPORT_DSM: // Force binding to zero on the main port hwsettings_DSMxBind = 0; diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index ff723d419..712906504 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -257,6 +257,12 @@ extern uint32_t pios_packet_handler; #define PIOS_SBUS_MAX_DEVS 1 #define PIOS_SBUS_NUM_INPUTS (16 + 2) +// ------------------------- +// Receiver EX.Bus input +// ------------------------- +#define PIOS_EXBUS_MAX_DEVS 2 +#define PIOS_EXBUS_NUM_INPUTS 16 + // ------------------------- // Receiver Multiplex SRXL input // ------------------------- diff --git a/flight/targets/boards/revonano/board_hw_defs.c b/flight/targets/boards/revonano/board_hw_defs.c index 2c3deefe0..d4f92002a 100644 --- a/flight/targets/boards/revonano/board_hw_defs.c +++ b/flight/targets/boards/revonano/board_hw_defs.c @@ -583,6 +583,95 @@ static const struct pios_usart_cfg pios_usart_srxl_flexi_cfg = { }; #endif /* PIOS_INCLUDE_SRXL */ + +#if defined(PIOS_INCLUDE_EXBUS) +/* + * EXBUS USART + */ +#include + +static const struct pios_usart_cfg pios_usart_exbus_flexi_cfg = { + .regs = FLEXI_USART_REGS, + .remap = FLEXI_USART_REMAP, + .init = { + .USART_BaudRate = 250000, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = FLEXI_USART_IRQ, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = FLEXI_USART_RX_GPIO, + .init = { + .GPIO_Pin = FLEXI_USART_RX_PIN, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = FLEXI_USART_TX_GPIO, + .init = { + .GPIO_Pin = FLEXI_USART_TX_PIN, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_OUT, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, +}; + +static const struct pios_usart_cfg pios_usart_exbus_main_cfg = { + .regs = MAIN_USART_REGS, + .remap = MAIN_USART_REMAP, + .init = { + .USART_BaudRate = 250000, + .USART_WordLength = USART_WordLength_8b, + .USART_Parity = USART_Parity_No, + .USART_StopBits = USART_StopBits_1, + .USART_HardwareFlowControl = USART_HardwareFlowControl_None, + .USART_Mode = USART_Mode_Rx, + }, + .irq = { + .init = { + .NVIC_IRQChannel = MAIN_USART_IRQ, + .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, + .NVIC_IRQChannelSubPriority = 0, + .NVIC_IRQChannelCmd = ENABLE, + }, + }, + .rx = { + .gpio = MAIN_USART_RX_GPIO, + .init = { + .GPIO_Pin = MAIN_USART_RX_PIN, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, + .tx = { + .gpio = MAIN_USART_TX_GPIO, + .init = { + .GPIO_Pin = MAIN_USART_TX_PIN, + .GPIO_Speed = GPIO_Speed_2MHz, + .GPIO_Mode = GPIO_Mode_AF, + .GPIO_OType = GPIO_OType_PP, + .GPIO_PuPd = GPIO_PuPd_UP + }, + }, +}; +#endif /* PIOS_INCLUDE_EXBUS */ /* * HK OSD */ diff --git a/flight/targets/boards/revonano/firmware/inc/pios_config.h b/flight/targets/boards/revonano/firmware/inc/pios_config.h index 16803c751..df62e1d7a 100644 --- a/flight/targets/boards/revonano/firmware/inc/pios_config.h +++ b/flight/targets/boards/revonano/firmware/inc/pios_config.h @@ -108,6 +108,7 @@ #define PIOS_INCLUDE_DSM #define PIOS_INCLUDE_SBUS #define PIOS_INCLUDE_SRXL +#define PIOS_INCLUDE_EXBUS #define PIOS_INCLUDE_GCSRCVR // #define PIOS_INCLUDE_OPLINKRCVR diff --git a/flight/targets/boards/revonano/firmware/pios_board.c b/flight/targets/boards/revonano/firmware/pios_board.c index dcf373a51..dfd9dd3fe 100644 --- a/flight/targets/boards/revonano/firmware/pios_board.c +++ b/flight/targets/boards/revonano/firmware/pios_board.c @@ -576,6 +576,27 @@ void PIOS_Board_Init(void) } #endif break; + case HWSETTINGS_RM_MAINPORT_JETIEXBUS: +#if defined(PIOS_INCLUDE_EXBUS) + { + uint32_t pios_usart_exbus_id; + if (PIOS_USART_Init(&pios_usart_exbus_id, &pios_usart_exbus_main_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_exbus_id; + if (PIOS_EXBUS_Init(&pios_exbus_id, &pios_usart_com_driver, pios_usart_exbus_id)) { + PIOS_Assert(0); + } + + uint32_t pios_exbus_rcvr_id; + if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_JETIEXBUSMAINPORT] = pios_exbus_rcvr_id; + } +#endif /* PIOS_INCLUDE_EXBUS */ + break; case HWSETTINGS_RM_MAINPORT_DSM: { // Force binding to zero on the main port @@ -666,6 +687,28 @@ void PIOS_Board_Init(void) } #endif break; + case HWSETTINGS_RM_FLEXIPORT_JETIEXBUS: +#if defined(PIOS_INCLUDE_EXBUS) + { + uint32_t pios_usart_exbus_id; + if (PIOS_USART_Init(&pios_usart_exbus_id, &pios_usart_exbus_flexi_cfg)) { + PIOS_Assert(0); + } + + uint32_t pios_exbus_id; + if (PIOS_EXBUS_Init(&pios_exbus_id, &pios_usart_com_driver, pios_usart_exbus_id)) { + PIOS_Assert(0); + } + + uint32_t pios_exbus_rcvr_id; + if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) { + PIOS_Assert(0); + } + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_JETIEXBUSFLEXIPORT] = pios_exbus_rcvr_id; + } +#endif /* PIOS_INCLUDE_EXBUS */ + break; + } /* hwsettings_rm_flexiport */ diff --git a/flight/targets/boards/revonano/pios_board.h b/flight/targets/boards/revonano/pios_board.h index 54776fe34..3c3e34fca 100644 --- a/flight/targets/boards/revonano/pios_board.h +++ b/flight/targets/boards/revonano/pios_board.h @@ -257,6 +257,12 @@ extern uint32_t pios_packet_handler; #define PIOS_SBUS_MAX_DEVS 1 #define PIOS_SBUS_NUM_INPUTS (16 + 2) +// ------------------------- +// Receiver EX.Bus input +// ------------------------- +#define PIOS_EXBUS_MAX_DEVS 2 +#define PIOS_EXBUS_NUM_INPUTS 16 + // ------------------------- // Receiver Multiplex SRXL input // ------------------------- From e07ded404eb6e197310eca5e13a1741545661d3c Mon Sep 17 00:00:00 2001 From: Fredrik Arvidsson Date: Thu, 31 Dec 2015 16:20:14 +0100 Subject: [PATCH 10/19] LP-196 Added Jeti receiver modes to input configuration code. --- ground/gcs/src/plugins/config/inputchannelform.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/ground/gcs/src/plugins/config/inputchannelform.cpp b/ground/gcs/src/plugins/config/inputchannelform.cpp index 962bb7006..92849a5d8 100644 --- a/ground/gcs/src/plugins/config/inputchannelform.cpp +++ b/ground/gcs/src/plugins/config/inputchannelform.cpp @@ -159,6 +159,8 @@ void InputChannelForm::groupUpdated() count = 18; break; case ManualControlSettings::CHANNELGROUPS_SRXL: + case ManualControlSettings::CHANNELGROUPS_JETIEXBUSFLEXIPORT: + case ManualControlSettings::CHANNELGROUPS_JETIEXBUSMAINPORT: count = 16; break; case ManualControlSettings::CHANNELGROUPS_GCS: From d00c169dbb67bf9e2642bc406a7e2122d124ab33 Mon Sep 17 00:00:00 2001 From: unknown Date: Sun, 3 Jan 2016 18:27:14 +0100 Subject: [PATCH 11/19] LP-196 Fixed bug in Ex.Bus decoder and changed baud rate for UART to 125 kbaud. --- flight/pios/common/pios_exbus.c | 428 +++++++++--------- .../boards/coptercontrol/board_hw_defs.c | 4 +- .../targets/boards/revolution/board_hw_defs.c | 4 +- .../targets/boards/revonano/board_hw_defs.c | 4 +- 4 files changed, 221 insertions(+), 219 deletions(-) diff --git a/flight/pios/common/pios_exbus.c b/flight/pios/common/pios_exbus.c index 147afcb3e..d5714de12 100644 --- a/flight/pios/common/pios_exbus.c +++ b/flight/pios/common/pios_exbus.c @@ -57,104 +57,104 @@ /* Forward Declarations */ static int32_t PIOS_EXBUS_Get(uint32_t rcvr_id, uint8_t channel); static uint16_t PIOS_EXBUS_RxInCallback(uint32_t context, - uint8_t *buf, - uint16_t buf_len, - uint16_t *headroom, - bool *need_yield); + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield); static void PIOS_EXBUS_Supervisor(uint32_t exbus_id); static uint16_t PIOS_EXBUS_CRC_Update(uint16_t crc, uint8_t data); /* Local Variables */ const struct pios_rcvr_driver pios_exbus_rcvr_driver = { - .read = PIOS_EXBUS_Get, + .read = PIOS_EXBUS_Get, }; enum pios_exbus_dev_magic { - PIOS_EXBUS_DEV_MAGIC = 0x485355FF, + PIOS_EXBUS_DEV_MAGIC = 0x485355FF, }; enum pios_exbus_frame_state { - EXBUS_STATE_SYNC, - EXBUS_STATE_REQ, - EXBUS_STATE_LEN, - EXBUS_STATE_PACKET_ID, - EXBUS_STATE_DATA_ID, - EXBUS_STATE_SUBLEN, - EXBUS_STATE_DATA + EXBUS_STATE_SYNC, + EXBUS_STATE_REQ, + EXBUS_STATE_LEN, + EXBUS_STATE_PACKET_ID, + EXBUS_STATE_DATA_ID, + EXBUS_STATE_SUBLEN, + EXBUS_STATE_DATA }; struct pios_exbus_state { - uint16_t channel_data[PIOS_EXBUS_NUM_INPUTS]; - uint8_t received_data[EXBUS_MAX_FRAME_LENGTH]; - uint8_t receive_timer; - uint8_t failsafe_timer; - uint8_t tx_connected; - uint8_t byte_count; - uint8_t frame_length; - uint16_t crc; - bool frame_found; + uint16_t channel_data[PIOS_EXBUS_NUM_INPUTS]; + uint8_t received_data[EXBUS_MAX_FRAME_LENGTH]; + uint8_t receive_timer; + uint8_t failsafe_timer; + uint8_t tx_connected; + uint8_t byte_count; + uint8_t frame_length; + uint16_t crc; + bool frame_found; }; struct pios_exbus_dev { - enum pios_exbus_dev_magic magic; - const struct pios_exbus_cfg *cfg; - struct pios_exbus_state state; + enum pios_exbus_dev_magic magic; + const struct pios_exbus_cfg *cfg; + struct pios_exbus_state state; }; /* Allocate EXBUS device descriptor */ #if defined(PIOS_INCLUDE_FREERTOS) static struct pios_exbus_dev *PIOS_EXBUS_Alloc(void) { - struct pios_exbus_dev *exbus_dev; + struct pios_exbus_dev *exbus_dev; - exbus_dev = (struct pios_exbus_dev *)pios_malloc(sizeof(*exbus_dev)); - if (!exbus_dev) - return NULL; + exbus_dev = (struct pios_exbus_dev *)pios_malloc(sizeof(*exbus_dev)); + if (!exbus_dev) + return NULL; - exbus_dev->magic = PIOS_EXBUS_DEV_MAGIC; - return exbus_dev; + exbus_dev->magic = PIOS_EXBUS_DEV_MAGIC; + return exbus_dev; } #else static struct pios_exbus_dev pios_exbus_devs[PIOS_EXBUS_MAX_DEVS]; static uint8_t pios_exbus_num_devs; static struct pios_exbus_dev *PIOS_EXBUS_Alloc(void) { - struct pios_exbus_dev *exbus_dev; + struct pios_exbus_dev *exbus_dev; - if (pios_exbus_num_devs >= PIOS_EXBUS_MAX_DEVS) - return NULL; + if (pios_exbus_num_devs >= PIOS_EXBUS_MAX_DEVS) + return NULL; - exbus_dev = &pios_exbus_devs[pios_exbus_num_devs++]; - exbus_dev->magic = PIOS_EXBUS_DEV_MAGIC; + exbus_dev = &pios_exbus_devs[pios_exbus_num_devs++]; + exbus_dev->magic = PIOS_EXBUS_DEV_MAGIC; - return exbus_dev; + return exbus_dev; } #endif /* Validate EXBUS device descriptor */ static bool PIOS_EXBUS_Validate(struct pios_exbus_dev *exbus_dev) { - return (exbus_dev->magic == PIOS_EXBUS_DEV_MAGIC); + return (exbus_dev->magic == PIOS_EXBUS_DEV_MAGIC); } /* Reset channels in case of lost signal or explicit failsafe receiver flag */ static void PIOS_EXBUS_ResetChannels(struct pios_exbus_dev *exbus_dev) { - struct pios_exbus_state *state = &(exbus_dev->state); - for (int i = 0; i < PIOS_EXBUS_NUM_INPUTS; i++) { - state->channel_data[i] = PIOS_RCVR_TIMEOUT; - } + struct pios_exbus_state *state = &(exbus_dev->state); + for (int i = 0; i < PIOS_EXBUS_NUM_INPUTS; i++) { + state->channel_data[i] = PIOS_RCVR_TIMEOUT; + } } /* Reset EXBUS receiver state */ static void PIOS_EXBUS_ResetState(struct pios_exbus_dev *exbus_dev) { - struct pios_exbus_state *state = &(exbus_dev->state); - state->receive_timer = 0; - state->failsafe_timer = 0; - state->frame_found = false; - state->tx_connected = 0; - PIOS_EXBUS_ResetChannels(exbus_dev); + struct pios_exbus_state *state = &(exbus_dev->state); + state->receive_timer = 0; + state->failsafe_timer = 0; + state->frame_found = false; + state->tx_connected = 0; + PIOS_EXBUS_ResetChannels(exbus_dev); } /** @@ -164,184 +164,186 @@ static void PIOS_EXBUS_ResetState(struct pios_exbus_dev *exbus_dev) */ static int PIOS_EXBUS_UnrollChannels(struct pios_exbus_dev *exbus_dev) { - struct pios_exbus_state *state = &(exbus_dev->state); + struct pios_exbus_state *state = &(exbus_dev->state); - if(state->crc != 0) { - /* crc failed */ - DEBUG_PRINTF(2, "Jeti CRC error!%d\r\n"); - goto stream_error; - } + if(state->crc != 0) { + /* crc failed */ + DEBUG_PRINTF(2, "Jeti CRC error!%d\r\n"); + goto stream_error; + } - enum pios_exbus_frame_state exbus_state = EXBUS_STATE_SYNC; - uint8_t *byte = state->received_data; - uint8_t sub_length; - uint8_t n_channels = 0; - uint8_t channel = 0; + enum pios_exbus_frame_state exbus_state = EXBUS_STATE_SYNC; + uint8_t *byte = state->received_data; + uint8_t sub_length; + uint8_t n_channels = 0; + uint8_t channel = 0; - while((byte - state->received_data) < state->frame_length) { - switch(exbus_state) { - case EXBUS_STATE_SYNC: - /* sync byte */ - if(*byte == EXBUS_SYNC_CHANNEL) { - exbus_state = EXBUS_STATE_REQ; - } - else { - goto stream_error; - } - byte += sizeof(uint8_t); - break; + while((byte - state->received_data) < state->frame_length) { + switch(exbus_state) { + case EXBUS_STATE_SYNC: + /* sync byte */ + if(*byte == EXBUS_SYNC_CHANNEL) { + exbus_state = EXBUS_STATE_REQ; + } + else { + goto stream_error; + } + byte += sizeof(uint8_t); + break; - case EXBUS_STATE_REQ: - /* - * tells us whether the rx is requesting a reply or not, - * currently nothing is actually done with this information... - */ - if(*byte == EXBUS_BYTE_REQ) { - exbus_state = EXBUS_STATE_LEN; - } - else if(*byte == EXBUS_BYTE_NOREQ) { - exbus_state = EXBUS_STATE_LEN; - } - else - goto stream_error; - byte += sizeof(uint8_t); - break; + case EXBUS_STATE_REQ: + /* + * tells us whether the rx is requesting a reply or not, + * currently nothing is actually done with this information... + */ + if(*byte == EXBUS_BYTE_REQ) { + exbus_state = EXBUS_STATE_LEN; + } + else if(*byte == EXBUS_BYTE_NOREQ) { + exbus_state = EXBUS_STATE_LEN; + } + else + goto stream_error; + byte += sizeof(uint8_t); + break; - case EXBUS_STATE_LEN: - /* total frame length */ - exbus_state = EXBUS_STATE_PACKET_ID; - byte += sizeof(uint8_t); - break; + case EXBUS_STATE_LEN: + /* total frame length */ + exbus_state = EXBUS_STATE_PACKET_ID; + byte += sizeof(uint8_t); + break; - case EXBUS_STATE_PACKET_ID: - /* packet id */ - exbus_state = EXBUS_STATE_DATA_ID; - byte += sizeof(uint8_t); - break; + case EXBUS_STATE_PACKET_ID: + /* packet id */ + exbus_state = EXBUS_STATE_DATA_ID; + byte += sizeof(uint8_t); + break; - case EXBUS_STATE_DATA_ID: - /* checks the type of data, ignore non-rc data */ - if(*byte == EXBUS_DATA_CHANNEL) { - exbus_state = EXBUS_STATE_SUBLEN; - } - else - goto stream_error; - byte += sizeof(uint8_t); - break; - case EXBUS_STATE_SUBLEN: - sub_length = *byte; - n_channels = sub_length / 2; - exbus_state = EXBUS_STATE_DATA; - byte += sizeof(uint8_t); - break; - case EXBUS_STATE_DATA: - if(channel < n_channels) { - /* 1 lsb = 1/8 us */ - state->channel_data[channel++] = (byte[1] << 8 | byte[0]) / 8; - } - byte += sizeof(uint16_t); - break; - } - } + case EXBUS_STATE_DATA_ID: + /* checks the type of data, ignore non-rc data */ + if(*byte == EXBUS_DATA_CHANNEL) { + exbus_state = EXBUS_STATE_SUBLEN; + } + else + goto stream_error; + byte += sizeof(uint8_t); + break; - for(; channel < EXBUS_MAX_CHANNELS; channel++) { - /* this channel was not received */ - state->channel_data[channel] = PIOS_RCVR_INVALID; - } + case EXBUS_STATE_SUBLEN: + sub_length = *byte; + n_channels = sub_length / 2; + exbus_state = EXBUS_STATE_DATA; + byte += sizeof(uint8_t); + break; + case EXBUS_STATE_DATA: + if(channel < n_channels) { + /* 1 lsb = 1/8 us */ + state->channel_data[channel++] = (byte[1] << 8 | byte[0]) / 8; + } + byte += sizeof(uint16_t); + break; + } + } + + for(; channel < EXBUS_MAX_CHANNELS; channel++) { + /* this channel was not received */ + state->channel_data[channel] = PIOS_RCVR_INVALID; + } + return 0; stream_error: - return -1; + return -1; } /* Update decoder state processing input byte from the stream */ static void PIOS_EXBUS_UpdateState(struct pios_exbus_dev *exbus_dev, uint8_t byte) { - struct pios_exbus_state *state = &(exbus_dev->state); + struct pios_exbus_state *state = &(exbus_dev->state); - if(!state->frame_found) { - if(byte == EXBUS_SYNC_CHANNEL) { - state->frame_found = true; - state->byte_count = 0; - state->frame_length = EXBUS_MAX_FRAME_LENGTH; - state->crc = PIOS_EXBUS_CRC_Update(0, byte); - state->received_data[state->byte_count++] = byte; - } - } - else if(state->byte_count < EXBUS_MAX_FRAME_LENGTH) { - state->crc = PIOS_EXBUS_CRC_Update(state->crc, byte); - state->received_data[state->byte_count++] = byte; + if(!state->frame_found) { + if(byte == EXBUS_SYNC_CHANNEL) { + state->frame_found = true; + state->byte_count = 0; + state->frame_length = EXBUS_MAX_FRAME_LENGTH; + state->crc = PIOS_EXBUS_CRC_Update(0, byte); + state->received_data[state->byte_count++] = byte; + } + } + else if(state->byte_count < EXBUS_MAX_FRAME_LENGTH) { + state->crc = PIOS_EXBUS_CRC_Update(state->crc, byte); + state->received_data[state->byte_count++] = byte; - if(state->byte_count == 3) { - state->frame_length = byte; - } - if(state->byte_count == state->frame_length) { - if (!PIOS_EXBUS_UnrollChannels(exbus_dev)) - /* data looking good */ - state->failsafe_timer = 0; - /* get ready for the next frame */ - state->frame_found = false; - } - } - else { - state->frame_found = false; - } + if(state->byte_count == 3) { + state->frame_length = byte; + } + if(state->byte_count == state->frame_length) { + if (!PIOS_EXBUS_UnrollChannels(exbus_dev)) + /* data looking good */ + state->failsafe_timer = 0; + /* get ready for the next frame */ + state->frame_found = false; + } + } + else { + state->frame_found = false; + } } /* Initialise EX Bus receiver interface */ int32_t PIOS_EXBUS_Init(uint32_t *exbus_id, - const struct pios_com_driver *driver, - uint32_t lower_id) + const struct pios_com_driver *driver, + uint32_t lower_id) { - PIOS_DEBUG_Assert(exbus_id); - PIOS_DEBUG_Assert(driver); + PIOS_DEBUG_Assert(exbus_id); + PIOS_DEBUG_Assert(driver); - struct pios_exbus_dev *exbus_dev; + struct pios_exbus_dev *exbus_dev; - exbus_dev = (struct pios_exbus_dev *)PIOS_EXBUS_Alloc(); - if (!exbus_dev) - return -1; + exbus_dev = (struct pios_exbus_dev *)PIOS_EXBUS_Alloc(); + if (!exbus_dev) + return -1; - PIOS_EXBUS_ResetState(exbus_dev); + PIOS_EXBUS_ResetState(exbus_dev); - *exbus_id = (uint32_t)exbus_dev; + *exbus_id = (uint32_t)exbus_dev; - /* Set comm driver callback */ - (driver->bind_rx_cb)(lower_id, PIOS_EXBUS_RxInCallback, *exbus_id); + /* Set comm driver callback */ + (driver->bind_rx_cb)(lower_id, PIOS_EXBUS_RxInCallback, *exbus_id); - if (!PIOS_RTC_RegisterTickCallback(PIOS_EXBUS_Supervisor, *exbus_id)) { - PIOS_DEBUG_Assert(0); - } + if (!PIOS_RTC_RegisterTickCallback(PIOS_EXBUS_Supervisor, *exbus_id)) { + PIOS_DEBUG_Assert(0); + } - return 0; + return 0; } /* Comm byte received callback */ static uint16_t PIOS_EXBUS_RxInCallback(uint32_t context, - uint8_t *buf, - uint16_t buf_len, - uint16_t *headroom, - bool *need_yield) + uint8_t *buf, + uint16_t buf_len, + uint16_t *headroom, + bool *need_yield) { - struct pios_exbus_dev *exbus_dev = (struct pios_exbus_dev *)context; + struct pios_exbus_dev *exbus_dev = (struct pios_exbus_dev *)context; - bool valid = PIOS_EXBUS_Validate(exbus_dev); - PIOS_Assert(valid); + bool valid = PIOS_EXBUS_Validate(exbus_dev); + PIOS_Assert(valid); - /* process byte(s) and clear receive timer */ - for (uint8_t i = 0; i < buf_len; i++) { - PIOS_EXBUS_UpdateState(exbus_dev, buf[i]); - exbus_dev->state.receive_timer = 0; - } + /* process byte(s) and clear receive timer */ + for (uint8_t i = 0; i < buf_len; i++) { + PIOS_EXBUS_UpdateState(exbus_dev, buf[i]); + exbus_dev->state.receive_timer = 0; + } - /* Always signal that we can accept more data */ - if (headroom) - *headroom = EXBUS_MAX_FRAME_LENGTH; + /* Always signal that we can accept more data */ + if (headroom) + *headroom = EXBUS_MAX_FRAME_LENGTH; - /* We never need a yield */ - *need_yield = false; + /* We never need a yield */ + *need_yield = false; - /* Always indicate that all bytes were consumed */ - return buf_len; + /* Always indicate that all bytes were consumed */ + return buf_len; } /** @@ -353,17 +355,17 @@ static uint16_t PIOS_EXBUS_RxInCallback(uint32_t context, */ static int32_t PIOS_EXBUS_Get(uint32_t rcvr_id, uint8_t channel) { - struct pios_exbus_dev *exbus_dev = (struct pios_exbus_dev *)rcvr_id; + struct pios_exbus_dev *exbus_dev = (struct pios_exbus_dev *)rcvr_id; - if (!PIOS_EXBUS_Validate(exbus_dev)) - return PIOS_RCVR_INVALID; + if (!PIOS_EXBUS_Validate(exbus_dev)) + return PIOS_RCVR_INVALID; - /* return error if channel is not available */ - if (channel >= PIOS_EXBUS_NUM_INPUTS) - return PIOS_RCVR_INVALID; + /* return error if channel is not available */ + if (channel >= PIOS_EXBUS_NUM_INPUTS) + return PIOS_RCVR_INVALID; - /* may also be PIOS_RCVR_TIMEOUT set by other function */ - return exbus_dev->state.channel_data[channel]; + /* may also be PIOS_RCVR_TIMEOUT set by other function */ + return exbus_dev->state.channel_data[channel]; } /** @@ -380,27 +382,27 @@ static int32_t PIOS_EXBUS_Get(uint32_t rcvr_id, uint8_t channel) */ static void PIOS_EXBUS_Supervisor(uint32_t exbus_id) { - struct pios_exbus_dev *exbus_dev = (struct pios_exbus_dev *)exbus_id; + struct pios_exbus_dev *exbus_dev = (struct pios_exbus_dev *)exbus_id; - bool valid = PIOS_EXBUS_Validate(exbus_dev); - PIOS_Assert(valid); + bool valid = PIOS_EXBUS_Validate(exbus_dev); + PIOS_Assert(valid); - struct pios_exbus_state *state = &(exbus_dev->state); + struct pios_exbus_state *state = &(exbus_dev->state); - /* waiting for new frame if no bytes were received in 8ms */ - if (++state->receive_timer > 4) { - state->frame_found = false; - state->byte_count = 0; - state->receive_timer = 0; - state->frame_length = EXBUS_MAX_FRAME_LENGTH; - } + /* waiting for new frame if no bytes were received in 8ms */ + if (++state->receive_timer > 4) { + state->frame_found = false; + state->byte_count = 0; + state->receive_timer = 0; + state->frame_length = EXBUS_MAX_FRAME_LENGTH; + } - /* activate failsafe if no frames have arrived in 102.4ms */ - if (++state->failsafe_timer > 64) { - PIOS_EXBUS_ResetChannels(exbus_dev); - state->failsafe_timer = 0; - state->tx_connected = 0; - } + /* activate failsafe if no frames have arrived in 102.4ms */ + if (++state->failsafe_timer > 64) { + PIOS_EXBUS_ResetChannels(exbus_dev); + state->failsafe_timer = 0; + state->tx_connected = 0; + } } static uint16_t PIOS_EXBUS_CRC_Update(uint16_t crc, uint8_t data) @@ -408,7 +410,7 @@ static uint16_t PIOS_EXBUS_CRC_Update(uint16_t crc, uint8_t data) data ^= (uint8_t)crc & (uint8_t)0xFF; data ^= data << 4; crc = ((((uint16_t)data << 8) | ((crc & 0xFF00) >> 8)) - ^ (uint8_t)(data >> 4) ^ ((uint16_t)data << 3)); + ^ (uint8_t)(data >> 4) ^ ((uint16_t)data << 3)); return crc; } diff --git a/flight/targets/boards/coptercontrol/board_hw_defs.c b/flight/targets/boards/coptercontrol/board_hw_defs.c index df23500a2..61b839a67 100644 --- a/flight/targets/boards/coptercontrol/board_hw_defs.c +++ b/flight/targets/boards/coptercontrol/board_hw_defs.c @@ -1029,7 +1029,7 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { static const struct pios_usart_cfg pios_usart_exbus_main_cfg = { .regs = USART1, .init = { - .USART_BaudRate = 250000, + .USART_BaudRate = 125000, .USART_WordLength = USART_WordLength_8b, .USART_Parity = USART_Parity_No, .USART_StopBits = USART_StopBits_1, @@ -1065,7 +1065,7 @@ static const struct pios_usart_cfg pios_usart_exbus_main_cfg = { static const struct pios_usart_cfg pios_usart_exbus_flexi_cfg = { .regs = USART3, .init = { - .USART_BaudRate = 250000, + .USART_BaudRate = 125000, .USART_WordLength = USART_WordLength_8b, .USART_Parity = USART_Parity_No, .USART_StopBits = USART_StopBits_1, diff --git a/flight/targets/boards/revolution/board_hw_defs.c b/flight/targets/boards/revolution/board_hw_defs.c index 8af1f1d34..f914cf866 100644 --- a/flight/targets/boards/revolution/board_hw_defs.c +++ b/flight/targets/boards/revolution/board_hw_defs.c @@ -1064,7 +1064,7 @@ static const struct pios_usart_cfg pios_usart_exbus_flexi_cfg = { .regs = USART3, .remap = GPIO_AF_USART3, .init = { - .USART_BaudRate = 250000, + .USART_BaudRate = 125000, .USART_WordLength = USART_WordLength_8b, .USART_Parity = USART_Parity_No, .USART_StopBits = USART_StopBits_1, @@ -1105,7 +1105,7 @@ static const struct pios_usart_cfg pios_usart_exbus_main_cfg = { .regs = USART1, .remap = GPIO_AF_USART1, .init = { - .USART_BaudRate = 250000, + .USART_BaudRate = 125000, .USART_WordLength = USART_WordLength_8b, .USART_Parity = USART_Parity_No, .USART_StopBits = USART_StopBits_1, diff --git a/flight/targets/boards/revonano/board_hw_defs.c b/flight/targets/boards/revonano/board_hw_defs.c index d4f92002a..fbf04026a 100644 --- a/flight/targets/boards/revonano/board_hw_defs.c +++ b/flight/targets/boards/revonano/board_hw_defs.c @@ -594,7 +594,7 @@ static const struct pios_usart_cfg pios_usart_exbus_flexi_cfg = { .regs = FLEXI_USART_REGS, .remap = FLEXI_USART_REMAP, .init = { - .USART_BaudRate = 250000, + .USART_BaudRate = 125000, .USART_WordLength = USART_WordLength_8b, .USART_Parity = USART_Parity_No, .USART_StopBits = USART_StopBits_1, @@ -635,7 +635,7 @@ static const struct pios_usart_cfg pios_usart_exbus_main_cfg = { .regs = MAIN_USART_REGS, .remap = MAIN_USART_REMAP, .init = { - .USART_BaudRate = 250000, + .USART_BaudRate = 125000, .USART_WordLength = USART_WordLength_8b, .USART_Parity = USART_Parity_No, .USART_StopBits = USART_StopBits_1, From 534ff1ff3cc7150296a16dbc09d61e36ba816da5 Mon Sep 17 00:00:00 2001 From: Fredrik Arvidsson Date: Tue, 5 Jan 2016 14:03:15 +0100 Subject: [PATCH 12/19] LP-196 Added Auto-Baud-Rate code. Will toggle between high (125 KBaud) and low (250 KBaud) baud rate every EXBUS_BAUD_RATE_LIMIT (set to 64 right now). Cleaned out some unused variables. --- flight/pios/common/pios_exbus.c | 32 +++++++++++++++++++++++++++----- 1 file changed, 27 insertions(+), 5 deletions(-) diff --git a/flight/pios/common/pios_exbus.c b/flight/pios/common/pios_exbus.c index d5714de12..253f327d0 100644 --- a/flight/pios/common/pios_exbus.c +++ b/flight/pios/common/pios_exbus.c @@ -54,6 +54,10 @@ #define EXBUS_DATA_TELEMETRY 0x3A #define EXBUS_DATA_JETIBOX 0x3B +#define EXBUS_LOW_BAUD_RATE 125000 +#define EXBUS_HIGH_BAUD_RATE 250000 +#define EXBUS_BAUD_RATE_LIMIT 64 + /* Forward Declarations */ static int32_t PIOS_EXBUS_Get(uint32_t rcvr_id, uint8_t channel); static uint16_t PIOS_EXBUS_RxInCallback(uint32_t context, @@ -88,7 +92,8 @@ struct pios_exbus_state { uint8_t received_data[EXBUS_MAX_FRAME_LENGTH]; uint8_t receive_timer; uint8_t failsafe_timer; - uint8_t tx_connected; + uint8_t failsafe_count; + bool high_baud_rate; uint8_t byte_count; uint8_t frame_length; uint16_t crc; @@ -97,7 +102,8 @@ struct pios_exbus_state { struct pios_exbus_dev { enum pios_exbus_dev_magic magic; - const struct pios_exbus_cfg *cfg; + uint32_t com_port_id; + const struct pios_com_driver *driver; struct pios_exbus_state state; }; @@ -152,8 +158,9 @@ static void PIOS_EXBUS_ResetState(struct pios_exbus_dev *exbus_dev) struct pios_exbus_state *state = &(exbus_dev->state); state->receive_timer = 0; state->failsafe_timer = 0; + state->failsafe_count = 0; + state->high_baud_rate = false; state->frame_found = false; - state->tx_connected = 0; PIOS_EXBUS_ResetChannels(exbus_dev); } @@ -277,9 +284,11 @@ static void PIOS_EXBUS_UpdateState(struct pios_exbus_dev *exbus_dev, uint8_t byt state->frame_length = byte; } if(state->byte_count == state->frame_length) { - if (!PIOS_EXBUS_UnrollChannels(exbus_dev)) + if (!PIOS_EXBUS_UnrollChannels(exbus_dev)) { /* data looking good */ state->failsafe_timer = 0; + state->failsafe_count = 0; + } /* get ready for the next frame */ state->frame_found = false; } @@ -314,6 +323,9 @@ int32_t PIOS_EXBUS_Init(uint32_t *exbus_id, PIOS_DEBUG_Assert(0); } + exbus_dev->driver = driver; + exbus_dev->com_port_id = lower_id; + return 0; } @@ -368,6 +380,16 @@ static int32_t PIOS_EXBUS_Get(uint32_t rcvr_id, uint8_t channel) return exbus_dev->state.channel_data[channel]; } +static void PIOS_EXBUS_Change_BaudRate(struct pios_exbus_dev *device) { + struct pios_exbus_state *state = &(device->state); + if (++state->failsafe_count >= EXBUS_BAUD_RATE_LIMIT) { + state->high_baud_rate = !state->high_baud_rate; + (device->driver->set_baud) (device->com_port_id, + state->high_baud_rate ? EXBUS_HIGH_BAUD_RATE : EXBUS_LOW_BAUD_RATE); + state->failsafe_count = 0; + } +} + /** * Input data supervisor is called periodically and provides * two functions: frame syncing and failsafe triggering. @@ -401,7 +423,7 @@ static void PIOS_EXBUS_Supervisor(uint32_t exbus_id) if (++state->failsafe_timer > 64) { PIOS_EXBUS_ResetChannels(exbus_dev); state->failsafe_timer = 0; - state->tx_connected = 0; + PIOS_EXBUS_Change_BaudRate(exbus_dev); } } From c8edd023a24b85aebb60aa8ea7aea5b2f638cd0e Mon Sep 17 00:00:00 2001 From: Fredrik Arvidsson Date: Wed, 6 Jan 2016 17:16:02 +0100 Subject: [PATCH 13/19] LP-196 Cleaned up some code. --- flight/pios/common/pios_exbus.c | 24 +++++++++--------------- 1 file changed, 9 insertions(+), 15 deletions(-) diff --git a/flight/pios/common/pios_exbus.c b/flight/pios/common/pios_exbus.c index 253f327d0..d2b25f677 100644 --- a/flight/pios/common/pios_exbus.c +++ b/flight/pios/common/pios_exbus.c @@ -93,10 +93,10 @@ struct pios_exbus_state { uint8_t receive_timer; uint8_t failsafe_timer; uint8_t failsafe_count; - bool high_baud_rate; uint8_t byte_count; uint8_t frame_length; uint16_t crc; + bool high_baud_rate; bool frame_found; }; @@ -176,7 +176,7 @@ static int PIOS_EXBUS_UnrollChannels(struct pios_exbus_dev *exbus_dev) if(state->crc != 0) { /* crc failed */ DEBUG_PRINTF(2, "Jeti CRC error!%d\r\n"); - goto stream_error; + return -1; } enum pios_exbus_frame_state exbus_state = EXBUS_STATE_SYNC; @@ -193,7 +193,7 @@ static int PIOS_EXBUS_UnrollChannels(struct pios_exbus_dev *exbus_dev) exbus_state = EXBUS_STATE_REQ; } else { - goto stream_error; + return -1; } byte += sizeof(uint8_t); break; @@ -209,8 +209,9 @@ static int PIOS_EXBUS_UnrollChannels(struct pios_exbus_dev *exbus_dev) else if(*byte == EXBUS_BYTE_NOREQ) { exbus_state = EXBUS_STATE_LEN; } - else - goto stream_error; + else { + return -1; + } byte += sizeof(uint8_t); break; @@ -231,8 +232,9 @@ static int PIOS_EXBUS_UnrollChannels(struct pios_exbus_dev *exbus_dev) if(*byte == EXBUS_DATA_CHANNEL) { exbus_state = EXBUS_STATE_SUBLEN; } - else - goto stream_error; + else { + return -1; + } byte += sizeof(uint8_t); break; @@ -251,15 +253,7 @@ static int PIOS_EXBUS_UnrollChannels(struct pios_exbus_dev *exbus_dev) break; } } - - for(; channel < EXBUS_MAX_CHANNELS; channel++) { - /* this channel was not received */ - state->channel_data[channel] = PIOS_RCVR_INVALID; - } return 0; - -stream_error: - return -1; } /* Update decoder state processing input byte from the stream */ From 08237a1ff5bc8aba577ef8562d3465a7af6cfbb5 Mon Sep 17 00:00:00 2001 From: Fredrik Arvidsson Date: Wed, 6 Jan 2016 17:17:02 +0100 Subject: [PATCH 14/19] LP-196 Removed EX.Bus on Main port for CopterControl target since it is not working. --- .../boards/coptercontrol/board_hw_defs.c | 35 ------------------- .../coptercontrol/firmware/pios_board.c | 21 ----------- .../targets/boards/coptercontrol/pios_board.h | 2 +- shared/uavobjectdefinition/hwsettings.xml | 2 +- 4 files changed, 2 insertions(+), 58 deletions(-) diff --git a/flight/targets/boards/coptercontrol/board_hw_defs.c b/flight/targets/boards/coptercontrol/board_hw_defs.c index 61b839a67..b4c705b6a 100644 --- a/flight/targets/boards/coptercontrol/board_hw_defs.c +++ b/flight/targets/boards/coptercontrol/board_hw_defs.c @@ -1026,41 +1026,6 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { * EXBUS USART */ #include -static const struct pios_usart_cfg pios_usart_exbus_main_cfg = { - .regs = USART1, - .init = { - .USART_BaudRate = 125000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, -}; static const struct pios_usart_cfg pios_usart_exbus_flexi_cfg = { .regs = USART3, diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index db81b1992..a344e181e 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -518,27 +518,6 @@ void PIOS_Board_Init(void) } #endif /* PIOS_INCLUDE_DSM */ break; - case HWSETTINGS_CC_MAINPORT_JETIEXBUS: -#if defined(PIOS_INCLUDE_EXBUS) - { - uint32_t pios_usart_exbus_id; - if (PIOS_USART_Init(&pios_usart_exbus_id, &pios_usart_exbus_main_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_exbus_id; - if (PIOS_EXBUS_Init(&pios_exbus_id, &pios_usart_com_driver, pios_usart_exbus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_exbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_JETIEXBUSMAINPORT] = pios_exbus_rcvr_id; - } -#endif /* PIOS_INCLUDE_EXBUS */ - break; case HWSETTINGS_CC_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) diff --git a/flight/targets/boards/coptercontrol/pios_board.h b/flight/targets/boards/coptercontrol/pios_board.h index 967754993..ad82a3a02 100644 --- a/flight/targets/boards/coptercontrol/pios_board.h +++ b/flight/targets/boards/coptercontrol/pios_board.h @@ -251,7 +251,7 @@ extern uint32_t pios_com_hkosd_id; // ------------------------- // Receiver EX.Bus input // ------------------------- -#define PIOS_EXBUS_MAX_DEVS 2 +#define PIOS_EXBUS_MAX_DEVS 1 #define PIOS_EXBUS_NUM_INPUTS 16 // ------------------------- diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 95215f9c1..9339f4787 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -2,7 +2,7 @@ Selection of optional hardware configurations. - + From 359266ab7af0cb8ed73e3f84fdf1768063b7d3e0 Mon Sep 17 00:00:00 2001 From: Fredrik Arvidsson Date: Thu, 7 Jan 2016 13:01:46 +0100 Subject: [PATCH 15/19] LP-104 Removed support for HoTT on Main port on coptercontrol target since it does not work. --- flight/modules/Receiver/receiver.c | 3 -- .../boards/coptercontrol/board_hw_defs.c | 36 ------------------- .../coptercontrol/firmware/pios_board.c | 23 ------------ .../targets/boards/coptercontrol/pios_board.h | 2 +- .../src/plugins/config/inputchannelform.cpp | 1 - shared/uavobjectdefinition/hwsettings.xml | 2 +- .../uavobjectdefinition/receiveractivity.xml | 2 +- 7 files changed, 3 insertions(+), 66 deletions(-) diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c index 64436fa1e..6cf858f04 100644 --- a/flight/modules/Receiver/receiver.c +++ b/flight/modules/Receiver/receiver.c @@ -658,9 +658,6 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTMAINPORT: - group = RECEIVERACTIVITY_ACTIVEGROUP_HOTTMAINPORT; - break; case MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTFLEXIPORT: group = RECEIVERACTIVITY_ACTIVEGROUP_HOTTFLEXIPORT; break; diff --git a/flight/targets/boards/coptercontrol/board_hw_defs.c b/flight/targets/boards/coptercontrol/board_hw_defs.c index f4cde50f4..7a4254056 100644 --- a/flight/targets/boards/coptercontrol/board_hw_defs.c +++ b/flight/targets/boards/coptercontrol/board_hw_defs.c @@ -1027,42 +1027,6 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { */ #include -static const struct pios_usart_cfg pios_usart_hott_main_cfg = { - .regs = USART1, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IPU, - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_IN_FLOATING, - }, - }, -}; - static const struct pios_usart_cfg pios_usart_hott_flexi_cfg = { .regs = USART3, .init = { diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index ef1d2b4ec..e74407574 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -518,29 +518,6 @@ void PIOS_Board_Init(void) } #endif /* PIOS_INCLUDE_DSM */ break; - case HWSETTINGS_CC_MAINPORT_HOTTSUMD: - case HWSETTINGS_CC_MAINPORT_HOTTSUMH: -#if defined(PIOS_INCLUDE_HOTT) - { - uint32_t pios_usart_hott_id; - if (PIOS_USART_Init(&pios_usart_hott_id, &pios_usart_hott_main_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_id; - if (PIOS_HOTT_Init(&pios_hott_id, &pios_usart_com_driver, pios_usart_hott_id, - hwsettings_cc_mainport == HWSETTINGS_CC_MAINPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_rcvr_id; - if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTMAINPORT] = pios_hott_rcvr_id; - } -#endif /* PIOS_INCLUDE_HOTT */ - break; case HWSETTINGS_CC_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_COM) #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) diff --git a/flight/targets/boards/coptercontrol/pios_board.h b/flight/targets/boards/coptercontrol/pios_board.h index 6fad48ef9..195321d99 100644 --- a/flight/targets/boards/coptercontrol/pios_board.h +++ b/flight/targets/boards/coptercontrol/pios_board.h @@ -251,7 +251,7 @@ extern uint32_t pios_com_hkosd_id; //------------------------- // Receiver HSUM input //------------------------- -#define PIOS_HOTT_MAX_DEVS 2 +#define PIOS_HOTT_MAX_DEVS 1 #define PIOS_HOTT_NUM_INPUTS 32 // ------------------------- diff --git a/ground/gcs/src/plugins/config/inputchannelform.cpp b/ground/gcs/src/plugins/config/inputchannelform.cpp index 3f5ed7956..37144fa29 100644 --- a/ground/gcs/src/plugins/config/inputchannelform.cpp +++ b/ground/gcs/src/plugins/config/inputchannelform.cpp @@ -161,7 +161,6 @@ void InputChannelForm::groupUpdated() case ManualControlSettings::CHANNELGROUPS_SRXL: count = 16; break; - case ManualControlSettings::CHANNELGROUPS_HOTTMAINPORT: case ManualControlSettings::CHANNELGROUPS_HOTTFLEXIPORT: count = 32; break; diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index f5fc9645f..743e80618 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -2,7 +2,7 @@ Selection of optional hardware configurations. - + diff --git a/shared/uavobjectdefinition/receiveractivity.xml b/shared/uavobjectdefinition/receiveractivity.xml index 33056be7a..16731158e 100644 --- a/shared/uavobjectdefinition/receiveractivity.xml +++ b/shared/uavobjectdefinition/receiveractivity.xml @@ -2,7 +2,7 @@ Monitors which receiver channels have been active within the last second. From bc80c66c2b153f63daf56802b4f91d34ab8d0172 Mon Sep 17 00:00:00 2001 From: Fredrik Arvidsson Date: Thu, 7 Jan 2016 14:22:52 +0100 Subject: [PATCH 16/19] LP-196 Removed some leftover code and definitions. --- flight/modules/Receiver/receiver.c | 3 --- ground/gcs/src/plugins/config/inputchannelform.cpp | 1 - shared/uavobjectdefinition/receiveractivity.xml | 2 +- 3 files changed, 1 insertion(+), 5 deletions(-) diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c index d35d1ccb5..78ae0f70f 100644 --- a/flight/modules/Receiver/receiver.c +++ b/flight/modules/Receiver/receiver.c @@ -655,9 +655,6 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT: group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT; break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_JETIEXBUSMAINPORT: - group = RECEIVERACTIVITY_ACTIVEGROUP_JETIEXBUSMAINPORT; - break; case MANUALCONTROLSETTINGS_CHANNELGROUPS_JETIEXBUSFLEXIPORT: group = RECEIVERACTIVITY_ACTIVEGROUP_JETIEXBUSFLEXIPORT; break; diff --git a/ground/gcs/src/plugins/config/inputchannelform.cpp b/ground/gcs/src/plugins/config/inputchannelform.cpp index 92849a5d8..483d68b0f 100644 --- a/ground/gcs/src/plugins/config/inputchannelform.cpp +++ b/ground/gcs/src/plugins/config/inputchannelform.cpp @@ -160,7 +160,6 @@ void InputChannelForm::groupUpdated() break; case ManualControlSettings::CHANNELGROUPS_SRXL: case ManualControlSettings::CHANNELGROUPS_JETIEXBUSFLEXIPORT: - case ManualControlSettings::CHANNELGROUPS_JETIEXBUSMAINPORT: count = 16; break; case ManualControlSettings::CHANNELGROUPS_GCS: diff --git a/shared/uavobjectdefinition/receiveractivity.xml b/shared/uavobjectdefinition/receiveractivity.xml index 27a5b2569..2c994a0e0 100644 --- a/shared/uavobjectdefinition/receiveractivity.xml +++ b/shared/uavobjectdefinition/receiveractivity.xml @@ -2,7 +2,7 @@ Monitors which receiver channels have been active within the last second. From 6a3a822c87421d3ebeccddb1388c272161cc4bd6 Mon Sep 17 00:00:00 2001 From: Fredrik Arvidsson Date: Sat, 9 Jan 2016 15:50:52 +0100 Subject: [PATCH 17/19] LP-104 Cleaned up configuration. Only Allow HoTT on flexi port. --- flight/modules/Receiver/receiver.c | 4 +- .../coptercontrol/firmware/pios_board.c | 2 +- .../targets/boards/revolution/board_hw_defs.c | 41 ------------------- .../boards/revolution/firmware/pios_board.c | 26 +----------- flight/targets/boards/revolution/pios_board.h | 2 +- .../targets/boards/revonano/board_hw_defs.c | 40 ------------------ .../boards/revonano/firmware/pios_board.c | 26 +----------- flight/targets/boards/revonano/pios_board.h | 2 +- shared/uavobjectdefinition/hwsettings.xml | 2 +- .../manualcontrolsettings.xml | 2 +- .../uavobjectdefinition/receiveractivity.xml | 2 +- 11 files changed, 10 insertions(+), 139 deletions(-) diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c index 6cf858f04..35db4c953 100644 --- a/flight/modules/Receiver/receiver.c +++ b/flight/modules/Receiver/receiver.c @@ -658,8 +658,8 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTFLEXIPORT: - group = RECEIVERACTIVITY_ACTIVEGROUP_HOTTFLEXIPORT; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT: + group = RECEIVERACTIVITY_ACTIVEGROUP_HOTT; break; case MANUALCONTROLSETTINGS_CHANNELGROUPS_SRXL: group = RECEIVERACTIVITY_ACTIVEGROUP_SRXL; diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index e74407574..863c9eb63 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -694,7 +694,7 @@ void PIOS_Board_Init(void) if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { PIOS_Assert(0); } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTFLEXIPORT] = pios_hott_rcvr_id; + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT] = pios_hott_rcvr_id; } #endif /* PIOS_INCLUDE_HOTT */ break; diff --git a/flight/targets/boards/revolution/board_hw_defs.c b/flight/targets/boards/revolution/board_hw_defs.c index 20b99c968..1081a8679 100644 --- a/flight/targets/boards/revolution/board_hw_defs.c +++ b/flight/targets/boards/revolution/board_hw_defs.c @@ -1102,47 +1102,6 @@ static const struct pios_usart_cfg pios_usart_hott_flexi_cfg = { }, }; -static const struct pios_usart_cfg pios_usart_hott_main_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; - #endif /* PIOS_INCLUDE_HOTT */ /* * HK OSD diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 944373969..eb3d26ab7 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -540,7 +540,7 @@ void PIOS_Board_Init(void) if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { PIOS_Assert(0); } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTFLEXIPORT] = pios_hott_rcvr_id; + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT] = pios_hott_rcvr_id; } #endif /* PIOS_INCLUDE_HOTT */ break; @@ -729,30 +729,6 @@ void PIOS_Board_Init(void) PIOS_Board_configure_dsm(&pios_usart_dsm_main_cfg, &pios_dsm_main_cfg, &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); break; - case HWSETTINGS_RM_MAINPORT_HOTTSUMD: - case HWSETTINGS_RM_MAINPORT_HOTTSUMH: -#if defined(PIOS_INCLUDE_HOTT) - { - uint32_t pios_usart_hott_id; - if (PIOS_USART_Init(&pios_usart_hott_id, &pios_usart_hott_main_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_id; - if (PIOS_HOTT_Init(&pios_hott_id, &pios_usart_com_driver, pios_usart_hott_id, - hwsettings_mainport == HWSETTINGS_RM_MAINPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_rcvr_id; - if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTMAINPORT] = pios_hott_rcvr_id; - } -#endif /* PIOS_INCLUDE_HOTT */ - break; - case HWSETTINGS_RM_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index ebfeea0ea..d525b843b 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -261,7 +261,7 @@ extern uint32_t pios_packet_handler; //------------------------- // Receiver HSUM input //------------------------- -#define PIOS_HOTT_MAX_DEVS 2 +#define PIOS_HOTT_MAX_DEVS 1 #define PIOS_HOTT_NUM_INPUTS 32 // ------------------------- diff --git a/flight/targets/boards/revonano/board_hw_defs.c b/flight/targets/boards/revonano/board_hw_defs.c index 6b4ee3001..eb24a1124 100644 --- a/flight/targets/boards/revonano/board_hw_defs.c +++ b/flight/targets/boards/revonano/board_hw_defs.c @@ -541,46 +541,6 @@ static const struct pios_dsm_cfg pios_dsm_flexi_cfg = { * HOTT USART */ #include -static const struct pios_usart_cfg pios_usart_hott_main_cfg = { - .regs = MAIN_USART_REGS, - .remap = MAIN_USART_REMAP, - .init = { - .USART_BaudRate = 115200, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = MAIN_USART_IRQ, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = MAIN_USART_RX_GPIO, - .init = { - .GPIO_Pin = MAIN_USART_RX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = MAIN_USART_TX_GPIO, - .init = { - .GPIO_Pin = MAIN_USART_TX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; static const struct pios_usart_cfg pios_usart_hott_flexi_cfg = { .regs = FLEXI_USART_REGS, diff --git a/flight/targets/boards/revonano/firmware/pios_board.c b/flight/targets/boards/revonano/firmware/pios_board.c index 818361f61..6350a3014 100644 --- a/flight/targets/boards/revonano/firmware/pios_board.c +++ b/flight/targets/boards/revonano/firmware/pios_board.c @@ -587,30 +587,6 @@ void PIOS_Board_Init(void) &pios_usart_com_driver, MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMMAINPORT, &hwsettings_DSMxBind); } break; - case HWSETTINGS_RM_MAINPORT_HOTTSUMD: - case HWSETTINGS_RM_MAINPORT_HOTTSUMH: -#if defined(PIOS_INCLUDE_HOTT) - { - uint32_t pios_usart_hott_id; - if (PIOS_USART_Init(&pios_usart_hott_id, &pios_usart_hott_main_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_id; - if (PIOS_HOTT_Init(&pios_hott_id, &pios_usart_com_driver, pios_usart_hott_id, - hwsettings_mainport == HWSETTINGS_RM_MAINPORT_HOTTSUMD ? PIOS_HOTT_PROTO_SUMD : PIOS_HOTT_PROTO_SUMH)) { - PIOS_Assert(0); - } - - uint32_t pios_hott_rcvr_id; - if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTMAINPORT] = pios_hott_rcvr_id; - } -#endif /* PIOS_INCLUDE_HOTT */ - break; - case HWSETTINGS_RM_MAINPORT_DEBUGCONSOLE: #if defined(PIOS_INCLUDE_DEBUG_CONSOLE) { @@ -710,7 +686,7 @@ void PIOS_Board_Init(void) if (PIOS_RCVR_Init(&pios_hott_rcvr_id, &pios_hott_rcvr_driver, pios_hott_id)) { PIOS_Assert(0); } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTTFLEXIPORT] = pios_hott_rcvr_id; + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_HOTT] = pios_hott_rcvr_id; } #endif /* PIOS_INCLUDE_HOTT */ break; diff --git a/flight/targets/boards/revonano/pios_board.h b/flight/targets/boards/revonano/pios_board.h index 1a691553f..64378f6ca 100644 --- a/flight/targets/boards/revonano/pios_board.h +++ b/flight/targets/boards/revonano/pios_board.h @@ -261,7 +261,7 @@ extern uint32_t pios_packet_handler; //------------------------- // Receiver HSUM input //------------------------- -#define PIOS_HOTT_MAX_DEVS 2 +#define PIOS_HOTT_MAX_DEVS 1 #define PIOS_HOTT_NUM_INPUTS 32 // ------------------------- diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 743e80618..7a1bef773 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -15,7 +15,7 @@ - + diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index 93565d66c..87f7447b1 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -3,7 +3,7 @@ Settings to indicate how to decode receiver input by @ref ManualControlModule. + options="PWM,PPM,DSM (MainPort),DSM (FlexiPort),S.Bus,HoTT,SRXL,GCS,OPLink,None" defaultvalue="None"/> Monitors which receiver channels have been active within the last second. From c48c46b8efc7da3579be2c55729e716082773994 Mon Sep 17 00:00:00 2001 From: Fredrik Arvidsson Date: Sat, 9 Jan 2016 16:07:13 +0100 Subject: [PATCH 18/19] LP-196 Cleaned up some configuration. Ex.Bus only available on Flexiport. --- flight/modules/Receiver/receiver.c | 4 +- .../coptercontrol/firmware/pios_board.c | 4 +- .../targets/boards/revolution/board_hw_defs.c | 40 ------------------ .../boards/revolution/firmware/pios_board.c | 25 +---------- flight/targets/boards/revolution/pios_board.h | 2 +- .../targets/boards/revonano/board_hw_defs.c | 41 +------------------ .../boards/revonano/firmware/pios_board.c | 25 +---------- flight/targets/boards/revonano/pios_board.h | 2 +- shared/uavobjectdefinition/hwsettings.xml | 6 +-- .../manualcontrolsettings.xml | 2 +- .../uavobjectdefinition/receiveractivity.xml | 2 +- 11 files changed, 16 insertions(+), 137 deletions(-) diff --git a/flight/modules/Receiver/receiver.c b/flight/modules/Receiver/receiver.c index 78ae0f70f..842644138 100644 --- a/flight/modules/Receiver/receiver.c +++ b/flight/modules/Receiver/receiver.c @@ -655,8 +655,8 @@ static bool updateRcvrActivityCompare(uint32_t rcvr_id, struct rcvr_activity_fsm case MANUALCONTROLSETTINGS_CHANNELGROUPS_DSMFLEXIPORT: group = RECEIVERACTIVITY_ACTIVEGROUP_DSMFLEXIPORT; break; - case MANUALCONTROLSETTINGS_CHANNELGROUPS_JETIEXBUSFLEXIPORT: - group = RECEIVERACTIVITY_ACTIVEGROUP_JETIEXBUSFLEXIPORT; + case MANUALCONTROLSETTINGS_CHANNELGROUPS_EXBUS: + group = RECEIVERACTIVITY_ACTIVEGROUP_EXBUS; break; case MANUALCONTROLSETTINGS_CHANNELGROUPS_SBUS: group = RECEIVERACTIVITY_ACTIVEGROUP_SBUS; diff --git a/flight/targets/boards/coptercontrol/firmware/pios_board.c b/flight/targets/boards/coptercontrol/firmware/pios_board.c index a344e181e..6ee7f635c 100644 --- a/flight/targets/boards/coptercontrol/firmware/pios_board.c +++ b/flight/targets/boards/coptercontrol/firmware/pios_board.c @@ -674,7 +674,7 @@ void PIOS_Board_Init(void) } #endif /* PIOS_INCLUDE_DSM */ break; - case HWSETTINGS_CC_FLEXIPORT_JETIEXBUS: + case HWSETTINGS_CC_FLEXIPORT_EXBUS: #if defined(PIOS_INCLUDE_EXBUS) { uint32_t pios_usart_exbus_id; @@ -691,7 +691,7 @@ void PIOS_Board_Init(void) if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) { PIOS_Assert(0); } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_JETIEXBUSFLEXIPORT] = pios_exbus_rcvr_id; + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_EXBUS] = pios_exbus_rcvr_id; } #endif /* PIOS_INCLUDE_EXBUS */ break; diff --git a/flight/targets/boards/revolution/board_hw_defs.c b/flight/targets/boards/revolution/board_hw_defs.c index f914cf866..d062315d5 100644 --- a/flight/targets/boards/revolution/board_hw_defs.c +++ b/flight/targets/boards/revolution/board_hw_defs.c @@ -1101,46 +1101,6 @@ static const struct pios_usart_cfg pios_usart_exbus_flexi_cfg = { }, }; -static const struct pios_usart_cfg pios_usart_exbus_main_cfg = { - .regs = USART1, - .remap = GPIO_AF_USART1, - .init = { - .USART_BaudRate = 125000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = USART1_IRQn, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_10, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = GPIOA, - .init = { - .GPIO_Pin = GPIO_Pin_9, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; #endif /* PIOS_INCLUDE_EXBUS */ /* diff --git a/flight/targets/boards/revolution/firmware/pios_board.c b/flight/targets/boards/revolution/firmware/pios_board.c index 0385f4dc2..2e1ee2723 100644 --- a/flight/targets/boards/revolution/firmware/pios_board.c +++ b/flight/targets/boards/revolution/firmware/pios_board.c @@ -520,7 +520,7 @@ void PIOS_Board_Init(void) } #endif break; - case HWSETTINGS_RM_FLEXIPORT_JETIEXBUS: + case HWSETTINGS_RM_FLEXIPORT_EXBUS: #if defined(PIOS_INCLUDE_EXBUS) { uint32_t pios_usart_exbus_id; @@ -537,7 +537,7 @@ void PIOS_Board_Init(void) if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) { PIOS_Assert(0); } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_JETIEXBUSFLEXIPORT] = pios_exbus_rcvr_id; + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_EXBUS] = pios_exbus_rcvr_id; } #endif /* PIOS_INCLUDE_EXBUS */ break; @@ -718,27 +718,6 @@ void PIOS_Board_Init(void) } #endif break; - case HWSETTINGS_RM_MAINPORT_JETIEXBUS: -#if defined(PIOS_INCLUDE_EXBUS) - { - uint32_t pios_usart_exbus_id; - if (PIOS_USART_Init(&pios_usart_exbus_id, &pios_usart_exbus_main_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_exbus_id; - if (PIOS_EXBUS_Init(&pios_exbus_id, &pios_usart_com_driver, pios_usart_exbus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_exbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_JETIEXBUSMAINPORT] = pios_exbus_rcvr_id; - } -#endif /* PIOS_INCLUDE_EXBUS */ - break; case HWSETTINGS_RM_MAINPORT_DSM: // Force binding to zero on the main port hwsettings_DSMxBind = 0; diff --git a/flight/targets/boards/revolution/pios_board.h b/flight/targets/boards/revolution/pios_board.h index 712906504..6941bc49e 100644 --- a/flight/targets/boards/revolution/pios_board.h +++ b/flight/targets/boards/revolution/pios_board.h @@ -260,7 +260,7 @@ extern uint32_t pios_packet_handler; // ------------------------- // Receiver EX.Bus input // ------------------------- -#define PIOS_EXBUS_MAX_DEVS 2 +#define PIOS_EXBUS_MAX_DEVS 1 #define PIOS_EXBUS_NUM_INPUTS 16 // ------------------------- diff --git a/flight/targets/boards/revonano/board_hw_defs.c b/flight/targets/boards/revonano/board_hw_defs.c index fbf04026a..399704ac8 100644 --- a/flight/targets/boards/revonano/board_hw_defs.c +++ b/flight/targets/boards/revonano/board_hw_defs.c @@ -631,46 +631,7 @@ static const struct pios_usart_cfg pios_usart_exbus_flexi_cfg = { }, }; -static const struct pios_usart_cfg pios_usart_exbus_main_cfg = { - .regs = MAIN_USART_REGS, - .remap = MAIN_USART_REMAP, - .init = { - .USART_BaudRate = 125000, - .USART_WordLength = USART_WordLength_8b, - .USART_Parity = USART_Parity_No, - .USART_StopBits = USART_StopBits_1, - .USART_HardwareFlowControl = USART_HardwareFlowControl_None, - .USART_Mode = USART_Mode_Rx, - }, - .irq = { - .init = { - .NVIC_IRQChannel = MAIN_USART_IRQ, - .NVIC_IRQChannelPreemptionPriority = PIOS_IRQ_PRIO_HIGH, - .NVIC_IRQChannelSubPriority = 0, - .NVIC_IRQChannelCmd = ENABLE, - }, - }, - .rx = { - .gpio = MAIN_USART_RX_GPIO, - .init = { - .GPIO_Pin = MAIN_USART_RX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, - .tx = { - .gpio = MAIN_USART_TX_GPIO, - .init = { - .GPIO_Pin = MAIN_USART_TX_PIN, - .GPIO_Speed = GPIO_Speed_2MHz, - .GPIO_Mode = GPIO_Mode_AF, - .GPIO_OType = GPIO_OType_PP, - .GPIO_PuPd = GPIO_PuPd_UP - }, - }, -}; + #endif /* PIOS_INCLUDE_EXBUS */ /* * HK OSD diff --git a/flight/targets/boards/revonano/firmware/pios_board.c b/flight/targets/boards/revonano/firmware/pios_board.c index dfd9dd3fe..e03afbf38 100644 --- a/flight/targets/boards/revonano/firmware/pios_board.c +++ b/flight/targets/boards/revonano/firmware/pios_board.c @@ -576,27 +576,6 @@ void PIOS_Board_Init(void) } #endif break; - case HWSETTINGS_RM_MAINPORT_JETIEXBUS: -#if defined(PIOS_INCLUDE_EXBUS) - { - uint32_t pios_usart_exbus_id; - if (PIOS_USART_Init(&pios_usart_exbus_id, &pios_usart_exbus_main_cfg)) { - PIOS_Assert(0); - } - - uint32_t pios_exbus_id; - if (PIOS_EXBUS_Init(&pios_exbus_id, &pios_usart_com_driver, pios_usart_exbus_id)) { - PIOS_Assert(0); - } - - uint32_t pios_exbus_rcvr_id; - if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) { - PIOS_Assert(0); - } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_JETIEXBUSMAINPORT] = pios_exbus_rcvr_id; - } -#endif /* PIOS_INCLUDE_EXBUS */ - break; case HWSETTINGS_RM_MAINPORT_DSM: { // Force binding to zero on the main port @@ -687,7 +666,7 @@ void PIOS_Board_Init(void) } #endif break; - case HWSETTINGS_RM_FLEXIPORT_JETIEXBUS: + case HWSETTINGS_RM_FLEXIPORT_EXBUS: #if defined(PIOS_INCLUDE_EXBUS) { uint32_t pios_usart_exbus_id; @@ -704,7 +683,7 @@ void PIOS_Board_Init(void) if (PIOS_RCVR_Init(&pios_exbus_rcvr_id, &pios_exbus_rcvr_driver, pios_exbus_id)) { PIOS_Assert(0); } - pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_JETIEXBUSFLEXIPORT] = pios_exbus_rcvr_id; + pios_rcvr_group_map[MANUALCONTROLSETTINGS_CHANNELGROUPS_EXBUS] = pios_exbus_rcvr_id; } #endif /* PIOS_INCLUDE_EXBUS */ break; diff --git a/flight/targets/boards/revonano/pios_board.h b/flight/targets/boards/revonano/pios_board.h index 3c3e34fca..6c5a09d99 100644 --- a/flight/targets/boards/revonano/pios_board.h +++ b/flight/targets/boards/revonano/pios_board.h @@ -260,7 +260,7 @@ extern uint32_t pios_packet_handler; // ------------------------- // Receiver EX.Bus input // ------------------------- -#define PIOS_EXBUS_MAX_DEVS 2 +#define PIOS_EXBUS_MAX_DEVS 1 #define PIOS_EXBUS_NUM_INPUTS 16 // ------------------------- diff --git a/shared/uavobjectdefinition/hwsettings.xml b/shared/uavobjectdefinition/hwsettings.xml index 9339f4787..163e0dec0 100644 --- a/shared/uavobjectdefinition/hwsettings.xml +++ b/shared/uavobjectdefinition/hwsettings.xml @@ -3,7 +3,7 @@ Selection of optional hardware configurations. - + @@ -15,8 +15,8 @@ - - + + diff --git a/shared/uavobjectdefinition/manualcontrolsettings.xml b/shared/uavobjectdefinition/manualcontrolsettings.xml index b5cf0bf12..4d7a02a39 100644 --- a/shared/uavobjectdefinition/manualcontrolsettings.xml +++ b/shared/uavobjectdefinition/manualcontrolsettings.xml @@ -3,7 +3,7 @@ Settings to indicate how to decode receiver input by @ref ManualControlModule. + options="PWM,PPM,DSM (MainPort),DSM (FlexiPort),EX.Bus,S.Bus,SRXL,GCS,OPLink,None" defaultvalue="None"/> Monitors which receiver channels have been active within the last second. From 41b95231db32e1dc4bdfc27fd80fd3bed6e40852 Mon Sep 17 00:00:00 2001 From: Fredrik Arvidsson Date: Sat, 9 Jan 2016 19:13:17 +0100 Subject: [PATCH 19/19] LP-104 LP-196 Updated some constants after refactoring. --- ground/gcs/src/plugins/config/inputchannelform.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/ground/gcs/src/plugins/config/inputchannelform.cpp b/ground/gcs/src/plugins/config/inputchannelform.cpp index ad3aa440e..f7dc7b5b0 100644 --- a/ground/gcs/src/plugins/config/inputchannelform.cpp +++ b/ground/gcs/src/plugins/config/inputchannelform.cpp @@ -155,14 +155,14 @@ void InputChannelForm::groupUpdated() case ManualControlSettings::CHANNELGROUPS_DSMFLEXIPORT: count = 12; break; + case ManualControlSettings::CHANNELGROUPS_SRXL: + case ManualControlSettings::CHANNELGROUPS_EXBUS: + count = 16; + break; case ManualControlSettings::CHANNELGROUPS_SBUS: count = 18; break; - case ManualControlSettings::CHANNELGROUPS_SRXL: - case ManualControlSettings::CHANNELGROUPS_JETIEXBUSFLEXIPORT: - count = 16; - break; - case ManualControlSettings::CHANNELGROUPS_HOTTFLEXIPORT: + case ManualControlSettings::CHANNELGROUPS_HOTT: count = 32; break; case ManualControlSettings::CHANNELGROUPS_GCS: