diff --git a/sw/airborne/booz/arch/stm32/radio_control/booz_radio_control_spektrum_arch.h b/sw/airborne/booz/arch/stm32/radio_control/booz_radio_control_spektrum_arch.h index b410f1e6e5..a55d6f2275 100644 --- a/sw/airborne/booz/arch/stm32/radio_control/booz_radio_control_spektrum_arch.h +++ b/sw/airborne/booz/arch/stm32/radio_control/booz_radio_control_spektrum_arch.h @@ -1,7 +1,7 @@ /* - * $Id$ + * Paparazzi $Id$ * - * Copyright (C) 2009 Eric Parsonage + * Copyright (C) 2009-2010 The Paparazzi Team * * This file is part of paparazzi. * @@ -18,14 +18,85 @@ * 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. - */ - -/* - *Function to bind Spektrum satellite receivers + * Boston, MA 02111-1307, USA. + * */ #ifndef BOOZ_RADIO_CONTROL_SPEKTRUM_ARCH_H #define BOOZ_RADIO_CONTROL_SPEKTRUM_ARCH_H -extern void radio_control_spektrum_try_bind( void ); -#endif /* BOOZ_RADIO_CONTROL_SPEKTRUM_ARCH_H */ + +#include "std.h" +#include "uart.h" + +#include RADIO_CONTROL_SPEKTRUM_MODEL_H + +#define RC_SPK_SYNC_1 0x03 + +#define RC_SPK_STA_UNINIT 0 +#define RC_SPK_STA_GOT_SYNC_1 1 +#define RC_SPK_STA_GOT_SYNC_2 2 + +extern bool_t rc_spk_parser_status; +extern uint8_t rc_spk_parser_idx; +extern uint8_t rc_spk_parser_buf[RADIO_CONTROL_NB_CHANNEL*2]; + +#define MAX_SPK 344 + + +extern const int16_t rc_spk_throw[RADIO_CONTROL_NB_CHANNEL]; + +#define __RcLink(dev, _x) dev##_x +#define _RcLink(dev, _x) __RcLink(dev, _x) +#define RcLink(_x) _RcLink(RADIO_CONTROL_LINK, _x) + +#define RcLinkChAvailable() RcLink(ChAvailable()) +#define RcLinkGetCh() RcLink(Getch()) + +#define _RadioControlEvent(_received_frame_handler) { \ + while (RcLinkChAvailable()) { \ + int8_t c = RcLinkGetCh(); \ + switch (rc_spk_parser_status) { \ + case RC_SPK_STA_UNINIT: \ + if (c==RC_SPK_SYNC_1) \ + rc_spk_parser_status = RC_SPK_STA_GOT_SYNC_1; \ + break; \ + case RC_SPK_STA_GOT_SYNC_1: \ + if (c==RC_SPK_SYNC_2) { \ + rc_spk_parser_status = RC_SPK_STA_GOT_SYNC_2; \ + rc_spk_parser_idx = 0; \ + } \ + else \ + rc_spk_parser_status = RC_SPK_STA_UNINIT; \ + break; \ + case RC_SPK_STA_GOT_SYNC_2: \ + rc_spk_parser_buf[rc_spk_parser_idx] = c; \ + rc_spk_parser_idx++; \ + if (rc_spk_parser_idx >= 2*RADIO_CONTROL_NB_CHANNEL) { \ + rc_spk_parser_status = RC_SPK_STA_UNINIT; \ + radio_control.frame_cpt++; \ + radio_control.time_since_last_frame = 0; \ + radio_control.status = RADIO_CONTROL_OK; \ + uint8_t i; \ + for (i=0;i> 10;*/ \ + const int16_t val = (tmp&0x03FF) - 512; \ + radio_control.values[i] = val; \ + radio_control.values[i] *= rc_spk_throw[i]; \ + if (i==RADIO_CONTROL_THROTTLE) { \ + radio_control.values[i] += MAX_PPRZ; \ + radio_control.values[i] /= 2; \ + } \ + } \ + _received_frame_handler(); \ + } \ + break; \ + default: \ + rc_spk_parser_status = RC_SPK_STA_UNINIT; \ + } \ + } \ + } + + +#endif /* BOOZ_RADIO_CONTROL_SPEKTRUM_ARCH_H */