diff --git a/conf/firmwares/subsystems/shared/radio_control_sbus.makefile b/conf/firmwares/subsystems/shared/radio_control_sbus.makefile index 67970f298a..433a151085 100644 --- a/conf/firmwares/subsystems/shared/radio_control_sbus.makefile +++ b/conf/firmwares/subsystems/shared/radio_control_sbus.makefile @@ -20,4 +20,5 @@ $(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/sbus.h\" $(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_SBUS $(TARGET).srcs += $(SRC_SUBSYSTEMS)/radio_control.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/radio_control/sbus.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/radio_control/sbus_common.c diff --git a/conf/firmwares/subsystems/shared/radio_control_sbus_dual.makefile b/conf/firmwares/subsystems/shared/radio_control_sbus_dual.makefile index 28579ee528..fadeecd5bf 100644 --- a/conf/firmwares/subsystems/shared/radio_control_sbus_dual.makefile +++ b/conf/firmwares/subsystems/shared/radio_control_sbus_dual.makefile @@ -31,4 +31,5 @@ $(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_H=\"subsystems/radio_control/sbus_dual. $(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_SBUS $(TARGET).srcs += $(SRC_SUBSYSTEMS)/radio_control.c $(TARGET).srcs += $(SRC_SUBSYSTEMS)/radio_control/sbus_dual.c +$(TARGET).srcs += $(SRC_SUBSYSTEMS)/radio_control/sbus_common.c diff --git a/sw/airborne/subsystems/radio_control/sbus.c b/sw/airborne/subsystems/radio_control/sbus.c index 7dff0b4467..b486139951 100644 --- a/sw/airborne/subsystems/radio_control/sbus.c +++ b/sw/airborne/subsystems/radio_control/sbus.c @@ -21,39 +21,12 @@ /** @file subsystems/radio_control/sbus.c * - * Futaba SBUS decoder + * Single SBUS radio_control */ #include "subsystems/radio_control.h" #include "subsystems/radio_control/sbus.h" #include BOARD_CONFIG -#include "mcu_periph/uart.h" -#include "mcu_periph/gpio.h" -#include - -/* - * SBUS protocol and state machine status - */ -#define SBUS_START_BYTE 0x0f -#define SBUS_END_BYTE 0x00 -#define SBUS_BIT_PER_CHANNEL 11 -#define SBUS_BIT_PER_BYTE 8 -#define SBUS_FLAGS_BYTE 22 -#define SBUS_FRAME_LOST_BIT 2 - -#define SBUS_STATUS_UNINIT 0 -#define SBUS_STATUS_GOT_START 1 - -/** Set polarity using RC_POLARITY_GPIO. - * SBUS signal has a reversed polarity compared to normal UART - * this allows to using hardware UART peripheral by changing - * the input signal polarity. - * Setting this gpio ouput high inverts the signal, - * output low sets it to normal polarity. - */ -#ifndef RC_SET_POLARITY -#define RC_SET_POLARITY gpio_set -#endif /** SBUS struct */ @@ -78,18 +51,7 @@ static void send_sbus(void) { // Init function void radio_control_impl_init(void) { - sbus.frame_available = FALSE; - sbus.status = SBUS_STATUS_UNINIT; - - // Set UART parameters (100K, 8 bits, 2 stops, even parity) - uart_periph_set_bits_stop_parity(&SBUS_UART_DEV, UBITS_8, USTOP_2, UPARITY_EVEN); - uart_periph_set_baudrate(&SBUS_UART_DEV, B100000); - - // Set polarity -#ifdef RC_POLARITY_GPIO_PORT - gpio_setup_output(RC_POLARITY_GPIO_PORT, RC_POLARITY_GPIO_PIN); - RC_SET_POLARITY(RC_POLARITY_GPIO_PORT, RC_POLARITY_GPIO_PIN); -#endif + sbus_common_init(&sbus, &SBUS_UART_DEV); // Register telemetry message #if PERIODIC_TELEMETRY @@ -98,73 +60,10 @@ void radio_control_impl_init(void) { } -/** Decode the raw buffer */ -static void decode_sbus_buffer (const uint8_t *src, uint16_t *dst, bool_t *available, uint16_t *dstppm) -{ - // reset counters - uint8_t byteInRawBuf = 0; - uint8_t bitInRawBuf = 0; - uint8_t channel = 0; - uint8_t bitInChannel = 0; - - // clear bits - memset (dst, 0, SBUS_NB_CHANNEL*sizeof(uint16_t)); - - // decode sbus data - for (uint8_t i=0; i< (SBUS_NB_CHANNEL*SBUS_BIT_PER_CHANNEL); i++) { - if (src[byteInRawBuf] & (1< + +/* + * SBUS protocol and state machine status + */ +#define SBUS_START_BYTE 0x0f +#define SBUS_END_BYTE 0x00 +#define SBUS_BIT_PER_CHANNEL 11 +#define SBUS_BIT_PER_BYTE 8 +#define SBUS_FLAGS_BYTE 22 +#define SBUS_FRAME_LOST_BIT 2 + +#define SBUS_STATUS_UNINIT 0 +#define SBUS_STATUS_GOT_START 1 + +/** Set polarity using RC_POLARITY_GPIO. + * SBUS signal has a reversed polarity compared to normal UART + * this allows to using hardware UART peripheral by changing + * the input signal polarity. + * Setting this gpio ouput high inverts the signal, + * output low sets it to normal polarity. + */ +#ifndef RC_SET_POLARITY +#define RC_SET_POLARITY gpio_set +#endif + + +void sbus_common_init(struct _sbus* sbus_p, struct uart_periph* dev) { + + sbus_p->frame_available = FALSE; + sbus_p->status = SBUS_STATUS_UNINIT; + + // Set UART parameters (100K, 8 bits, 2 stops, even parity) + uart_periph_set_bits_stop_parity(dev, UBITS_8, USTOP_2, UPARITY_EVEN); + uart_periph_set_baudrate(dev, B100000); + + // Set polarity +#ifdef RC_POLARITY_GPIO_PORT + gpio_setup_output(RC_POLARITY_GPIO_PORT, RC_POLARITY_GPIO_PIN); + RC_SET_POLARITY(RC_POLARITY_GPIO_PORT, RC_POLARITY_GPIO_PIN); +#endif + +} + + +/** Decode the raw buffer */ +static void decode_sbus_buffer (const uint8_t *src, uint16_t *dst, bool_t *available, uint16_t *dstppm) +{ + // reset counters + uint8_t byteInRawBuf = 0; + uint8_t bitInRawBuf = 0; + uint8_t channel = 0; + uint8_t bitInChannel = 0; + + // clear bits + memset (dst, 0, SBUS_NB_CHANNEL*sizeof(uint16_t)); + + // decode sbus data + for (uint8_t i=0; i< (SBUS_NB_CHANNEL*SBUS_BIT_PER_CHANNEL); i++) { + if (src[byteInRawBuf] & (1<status) { + case SBUS_STATUS_UNINIT: + // Wait for the start byte + if (rbyte == SBUS_START_BYTE) { + sbus_p->status++; + sbus_p->idx = 0; + } + break; + case SBUS_STATUS_GOT_START: + // Store buffer + sbus_p->buffer[sbus_p->idx] = rbyte; + sbus_p->idx++; + if (sbus_p->idx == SBUS_BUF_LENGTH) { + // Decode if last byte is the correct end byte + if (rbyte == SBUS_END_BYTE) { + decode_sbus_buffer(sbus_p->buffer, sbus_p->pulses, &sbus_p->frame_available, sbus_p->ppm); + } + sbus_p->status = SBUS_STATUS_UNINIT; + } + break; + default: + break; + } + } while (uart_char_available(dev)); + } +} diff --git a/sw/airborne/subsystems/radio_control/sbus_common.h b/sw/airborne/subsystems/radio_control/sbus_common.h new file mode 100644 index 0000000000..795c8f59f2 --- /dev/null +++ b/sw/airborne/subsystems/radio_control/sbus_common.h @@ -0,0 +1,85 @@ +/* + * Copyright (C) 2013 Alexandre Bustico, Gautier Hattenberger + * + * This file is part of paparazzi. + * + * paparazzi 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 2, or (at your option) + * any later version. + * + * paparazzi 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 paparazzi; see the file COPYING. If not, write to + * the Free Software Foundation, 59 Temple Place - Suite 330, + * Boston, MA 02111-1307, USA. + */ + +#ifndef RC_SBUS_COMMON_H +#define RC_SBUS_COMMON_H + +/** @file subsystems/radio_control/sbus_common.h + * + * Futaba SBUS decoder + */ + +#include "std.h" +#include "mcu_periph/uart.h" + +/** + * Macro to use radio.h file + * + * SBUS: 0..1024..2047 (sweep 2048) + * PPM: 880..1520..2160 (sweep 1280) + */ +#define RC_PPM_TICKS_OF_USEC(_v) ((((_v) - 880) * 8) / 5) +#define RC_PPM_SIGNED_TICKS_OF_USEC(_v) (((_v) * 8) / 5) +#define USEC_OF_RC_PPM_TICKS(_v) ((((_v) * 5) / 8) + 880) + +/** + * Generated code holding the description of a given + * transmitter + */ +#include "generated/radio.h" + +/** + * Define number of channels. + * + * SBUS frame always have 16 channels + * but only the X first one will be available + * depending of the RC transmitter. + * The radio XML file is used to assign the + * input values to RC channels. + */ +#define SBUS_BUF_LENGTH 24 +#define SBUS_NB_CHANNEL 16 +#define RADIO_CONTROL_NB_CHANNEL SBUS_NB_CHANNEL + +/** + * SBUS structure + */ +struct _sbus { + uint16_t pulses[SBUS_NB_CHANNEL]; ///< decoded values + uint16_t ppm[SBUS_NB_CHANNEL]; ///< decoded and converted values + bool_t frame_available; ///< new frame available + uint8_t buffer[SBUS_BUF_LENGTH]; ///< input buffer + uint8_t idx; ///< input index + uint8_t status; ///< decoder state machine status +}; + +/** + * Init function + */ +void sbus_common_init(struct _sbus* sbus, struct uart_periph* dev); + +/** + * Decoding event function + */ +void sbus_common_decode_event(struct _sbus* sbus, struct uart_periph* dev); + + +#endif /* RC_SBUS_H */ diff --git a/sw/airborne/subsystems/radio_control/sbus_dual.c b/sw/airborne/subsystems/radio_control/sbus_dual.c index 809833b571..7281fec1c2 100644 --- a/sw/airborne/subsystems/radio_control/sbus_dual.c +++ b/sw/airborne/subsystems/radio_control/sbus_dual.c @@ -1,5 +1,5 @@ /* - * Copyright (C) 2013 Alexandre Bustico, Gautier Hattenberger + * Copyright (C) 2014 Christophe De Wagter * * This file is part of paparazzi. * @@ -19,9 +19,9 @@ * Boston, MA 02111-1307, USA. */ -/** @file subsystems/radio_control/sbus.c +/** @file subsystems/radio_control/sbus_dual.c * - * Futaba SBUS decoder + * Dual SBUS radio_control */ #include "subsystems/radio_control.h" @@ -31,29 +31,6 @@ #include "mcu_periph/gpio.h" #include -/* - * SBUS protocol and state machine status - */ -#define SBUS_START_BYTE 0x0f -#define SBUS_END_BYTE 0x00 -#define SBUS_BIT_PER_CHANNEL 11 -#define SBUS_BIT_PER_BYTE 8 -#define SBUS_FLAGS_BYTE 22 -#define SBUS_FRAME_LOST_BIT 2 - -#define SBUS_STATUS_UNINIT 0 -#define SBUS_STATUS_GOT_START 1 - -/** Set polarity using RC_POLARITY_GPIO. - * SBUS signal has a reversed polarity compared to normal UART - * this allows to using hardware UART peripheral by changing - * the input signal polarity. - * Setting this gpio ouput high inverts the signal, - * output low sets it to normal polarity. - */ -#ifndef RC_SET_POLARITY -#define RC_SET_POLARITY gpio_set -#endif /** SBUS struct */ @@ -78,22 +55,8 @@ static void send_sbus(void) { // Init function void radio_control_impl_init(void) { - sbus1.frame_available = FALSE; - sbus1.status = SBUS_STATUS_UNINIT; - sbus2.frame_available = FALSE; - sbus2.status = SBUS_STATUS_UNINIT; - - // Set UART parameters (100K, 8 bits, 2 stops, even parity) - uart_periph_set_bits_stop_parity(&SBUS1_UART_DEV, UBITS_8, USTOP_2, UPARITY_EVEN); - uart_periph_set_baudrate(&SBUS1_UART_DEV, B100000); - uart_periph_set_bits_stop_parity(&SBUS2_UART_DEV, UBITS_8, USTOP_2, UPARITY_EVEN); - uart_periph_set_baudrate(&SBUS2_UART_DEV, B100000); - - // Set polarity -#ifdef RC_POLARITY_GPIO_PORT - gpio_setup_output(RC_POLARITY_GPIO_PORT, RC_POLARITY_GPIO_PIN); - RC_SET_POLARITY(RC_POLARITY_GPIO_PORT, RC_POLARITY_GPIO_PIN); -#endif + sbus_common_init(&sbus1, &SBUS1_UART_DEV); + sbus_common_init(&sbus2, &SBUS2_UART_DEV); // Register telemetry message #if PERIODIC_TELEMETRY @@ -101,79 +64,7 @@ void radio_control_impl_init(void) { #endif } - -/** Decode the raw buffer */ -static void decode_sbus_buffer (const uint8_t *src, uint16_t *dst, bool_t *available, uint16_t *dstppm) -{ - // reset counters - uint8_t byteInRawBuf = 0; - uint8_t bitInRawBuf = 0; - uint8_t channel = 0; - uint8_t bitInChannel = 0; - - // clear bits - memset (dst, 0, SBUS_NB_CHANNEL*sizeof(uint16_t)); - - // decode sbus data - for (uint8_t i=0; i< (SBUS_NB_CHANNEL*SBUS_BIT_PER_CHANNEL); i++) { - if (src[byteInRawBuf] & (1<status) { - case SBUS_STATUS_UNINIT: - // Wait for the start byte - if (rbyte == SBUS_START_BYTE) { - sbus->status++; - sbus->idx = 0; - } - break; - case SBUS_STATUS_GOT_START: - // Store buffer - sbus->buffer[sbus->idx] = rbyte; - sbus->idx++; - if (sbus->idx == SBUS_BUF_LENGTH) { - // Decode if last byte is the correct end byte - if (rbyte == SBUS_END_BYTE) { - decode_sbus_buffer(sbus->buffer, sbus->pulses, &sbus->frame_available, sbus->ppm); - } - sbus->status = SBUS_STATUS_UNINIT; - } - break; - default: - break; - } - } while (uart_char_available(dev)); - } -} - void sbus_dual_decode_event(void) { - sbus_decode_event(&sbus1, &SBUS1_UART_DEV); - sbus_decode_event(&sbus2, &SBUS2_UART_DEV); + sbus_common_decode_event(&sbus1, &SBUS1_UART_DEV); + sbus_common_decode_event(&sbus2, &SBUS2_UART_DEV); } diff --git a/sw/airborne/subsystems/radio_control/sbus_dual.h b/sw/airborne/subsystems/radio_control/sbus_dual.h index d0794052c7..b8f827a930 100644 --- a/sw/airborne/subsystems/radio_control/sbus_dual.h +++ b/sw/airborne/subsystems/radio_control/sbus_dual.h @@ -1,5 +1,5 @@ /* - * Copyright (C) 2013 Alexandre Bustico, Gautier Hattenberger + * Copyright (C) 2014 Christophe De Wagter * * This file is part of paparazzi. * @@ -19,56 +19,16 @@ * Boston, MA 02111-1307, USA. */ -#ifndef RC_SBUS_H -#define RC_SBUS_H +#ifndef RC_SBUS_DUAL_H +#define RC_SBUS_DUAL_H -/** @file subsystems/radio_control/sbus.h +/** @file subsystems/radio_control/sbus_dual.h * - * Futaba SBUS decoder + * Dual SBUS radio_control */ -#include "std.h" +#include "subsystems/radio_control/sbus_common.h" -/** - * Macro to use radio.h file - * - * SBUS: 0..1024..2047 (sweep 2048) - * PPM: 880..1520..2160 (sweep 1280) - */ -#define RC_PPM_TICKS_OF_USEC(_v) ((((_v) - 880) * 8) / 5) -#define RC_PPM_SIGNED_TICKS_OF_USEC(_v) (((_v) * 8) / 5) -#define USEC_OF_RC_PPM_TICKS(_v) ((((_v) * 5) / 8) + 880) - -/** - * Generated code holding the description of a given - * transmitter - */ -#include "generated/radio.h" - -/** - * Define number of channels. - * - * SBUS frame always have 16 channels - * but only the X first one will be available - * depending of the RC transmitter. - * The radio XML file is used to assign the - * input values to RC channels. - */ -#define SBUS_BUF_LENGTH 24 -#define SBUS_NB_CHANNEL 16 -#define RADIO_CONTROL_NB_CHANNEL SBUS_NB_CHANNEL - -/** - * SBUS structure - */ -struct _sbus { - uint16_t pulses[SBUS_NB_CHANNEL]; ///< decoded values - uint16_t ppm[SBUS_NB_CHANNEL]; ///< decoded and converted values - bool_t frame_available; ///< new frame available - uint8_t buffer[SBUS_BUF_LENGTH]; ///< input buffer - uint8_t idx; ///< input index - uint8_t status; ///< decoder state machine status -}; extern struct _sbus sbus1, sbus2;