From 40038d280286a6894c635e0f99cd08f5544bf473 Mon Sep 17 00:00:00 2001 From: Antoine Drouin Date: Thu, 26 Aug 2010 13:17:44 +0000 Subject: [PATCH] a ppm decoder for rotorcraft --- conf/autopilot/lisa_l_test_progs.makefile | 2 +- .../radio_control_spektrum.makefile | 2 +- .../booz_radio_control_ppm_arch.h | 10 ++++ .../booz_radio_control_ppm_arch.c | 53 ++++++++++--------- .../booz_radio_control_ppm_arch.h | 8 +++ .../booz/test/booz2_test_radio_control.c | 34 ++++++------ sw/airborne/stm32/adc_hw.c | 15 +++--- sw/tools/gen_conf_radio_control_ppm.ml | 8 +-- 8 files changed, 77 insertions(+), 55 deletions(-) diff --git a/conf/autopilot/lisa_l_test_progs.makefile b/conf/autopilot/lisa_l_test_progs.makefile index f7888b9eba..7742d9396a 100644 --- a/conf/autopilot/lisa_l_test_progs.makefile +++ b/conf/autopilot/lisa_l_test_progs.makefile @@ -153,7 +153,7 @@ test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_TYPE_H=\"radio_control/booz_radio_con test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_MODEL_H=\"radio_control/booz_radio_control_spektrum_dx7se.h\" test_rc_spektrum.CFLAGS += -DRADIO_CONTROL_LINK=$(RADIO_CONTROL_LINK) #test_rc_spektrum.CFLAGS += -DUSE_$(RADIO_CONTROL_LINK) -D$(RADIO_CONTROL_LINK)_BAUD=B115200 -test_rc_spektrum.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_LINK)_IRQ_HANDLER +test_rc_spektrum.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_LINK)_IRQ_HANDLER -DUSE_TIM1_UP_IRQ test_rc_spektrum.srcs += $(SRC_BOOZ)/booz_radio_control.c \ $(SRC_BOOZ)/radio_control/booz_radio_control_spektrum.c \ $(SRC_BOOZ_ARCH)/radio_control/booz_radio_control_spektrum_arch.c diff --git a/conf/autopilot/subsystems/rotorcraft/radio_control_spektrum.makefile b/conf/autopilot/subsystems/rotorcraft/radio_control_spektrum.makefile index 7f2aa35734..61f2b82784 100644 --- a/conf/autopilot/subsystems/rotorcraft/radio_control_spektrum.makefile +++ b/conf/autopilot/subsystems/rotorcraft/radio_control_spektrum.makefile @@ -11,7 +11,7 @@ ap.CFLAGS += -DRADIO_CONTROL_SPEKTRUM_MODEL_H=$(RADIO_CONTROL_SPEKTRUM_MODEL) ap.CFLAGS += -DRADIO_CONTROL_LED=$(RADIO_CONTROL_LED) ap.CFLAGS += -DRADIO_CONTROL_LINK=$(RADIO_CONTROL_LINK) #ap.CFLAGS += -DUSE_$(RADIO_CONTROL_LINK) -D$(RADIO_CONTROL_LINK)_BAUD=B115200 -ap.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_LINK)_IRQ_HANDLER +ap.CFLAGS += -DOVERRIDE_$(RADIO_CONTROL_LINK)_IRQ_HANDLER -DUSE_TIM1_UP_IRQ ap.srcs += $(SRC_BOOZ)/booz_radio_control.c \ $(SRC_BOOZ)/radio_control/booz_radio_control_spektrum.c \ diff --git a/sw/airborne/booz/arch/lpc21/radio_control/booz_radio_control_ppm_arch.h b/sw/airborne/booz/arch/lpc21/radio_control/booz_radio_control_ppm_arch.h index bd42f99bcb..4532c86513 100644 --- a/sw/airborne/booz/arch/lpc21/radio_control/booz_radio_control_ppm_arch.h +++ b/sw/airborne/booz/arch/lpc21/radio_control/booz_radio_control_ppm_arch.h @@ -28,6 +28,16 @@ #include "LPC21xx.h" #include BOARD_CONFIG +/** + * On tiny (and booz) the ppm counter is running at the same speed as + * the systic counter. There is no reason for this to be true. + * Let's add a pair of macros to make it possible for them to be different. + * + */ +#define RC_PPM_TICS_OF_USEC SYS_TICS_OF_USEC +#define RC_PPM_SIGNED_TICS_OF_USEC SIGNED_SYS_TICS_OF_USEC + + extern uint8_t booz_radio_control_ppm_cur_pulse; extern uint32_t booz_radio_control_ppm_last_pulse_time; diff --git a/sw/airborne/booz/arch/stm32/radio_control/booz_radio_control_ppm_arch.c b/sw/airborne/booz/arch/stm32/radio_control/booz_radio_control_ppm_arch.c index 30980fae91..223213f22f 100644 --- a/sw/airborne/booz/arch/stm32/radio_control/booz_radio_control_ppm_arch.c +++ b/sw/airborne/booz/arch/stm32/radio_control/booz_radio_control_ppm_arch.c @@ -30,9 +30,6 @@ #include "sys_time.h" -#include "my_debug_servo.h" - - /* * * This a radio control ppm driver for stm32 @@ -41,8 +38,7 @@ */ uint8_t booz_radio_control_ppm_cur_pulse; uint32_t booz_radio_control_ppm_last_pulse_time; - -uint32_t debug_len; +static uint32_t timer_rollover_cnt; void tim2_irq_handler(void); @@ -60,7 +56,16 @@ void booz_radio_control_ppm_arch_init ( void ) { /* GPIOA clock enable */ RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA, ENABLE); - + + /* Time Base configuration */ + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; + TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); + TIM_TimeBaseStructure.TIM_Period = 0xFFFF; + TIM_TimeBaseStructure.TIM_Prescaler = 0x8; + TIM_TimeBaseStructure.TIM_ClockDivision = 0x0; + TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up; + TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure); + /* TIM2 configuration: Input Capture mode --------------------- The external signal is connected to TIM2 CH2 pin (PA.01) The Rising edge is used as active edge, @@ -70,10 +75,9 @@ void booz_radio_control_ppm_arch_init ( void ) { TIM_ICInitStructure.TIM_ICPolarity = TIM_ICPolarity_Rising; TIM_ICInitStructure.TIM_ICSelection = TIM_ICSelection_DirectTI; TIM_ICInitStructure.TIM_ICPrescaler = TIM_ICPSC_DIV1; - TIM_ICInitStructure.TIM_ICFilter = 0x0; + TIM_ICInitStructure.TIM_ICFilter = 0x00; TIM_ICInit(TIM2, &TIM_ICInitStructure); - /* Enable the TIM2 global Interrupt */ NVIC_InitTypeDef NVIC_InitStructure; NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn; @@ -86,48 +90,47 @@ void booz_radio_control_ppm_arch_init ( void ) { TIM_Cmd(TIM2, ENABLE); /* Enable the CC2 Interrupt Request */ - TIM_ITConfig(TIM2, TIM_IT_CC2, ENABLE); + TIM_ITConfig(TIM2, TIM_IT_CC2|TIM_IT_Update, ENABLE); booz_radio_control_ppm_last_pulse_time = 0; booz_radio_control_ppm_cur_pulse = RADIO_CONTROL_NB_CHANNEL; - - DEBUG_SERVO2_INIT(); + timer_rollover_cnt = 0; } void tim2_irq_handler(void) { - - DEBUG_S4_ON(); - + if(TIM_GetITStatus(TIM2, TIM_IT_CC2) == SET) { TIM_ClearITPendingBit(TIM2, TIM_IT_CC2); - uint32_t now = TIM_GetCapture2(TIM2); + uint32_t now = TIM_GetCapture2(TIM2) + timer_rollover_cnt; uint32_t length = now - booz_radio_control_ppm_last_pulse_time; - debug_len = length; booz_radio_control_ppm_last_pulse_time = now; - + if (booz_radio_control_ppm_cur_pulse == RADIO_CONTROL_NB_CHANNEL) { - if (length > SYS_TICS_OF_USEC(PPM_SYNC_MIN_LEN) && - length < SYS_TICS_OF_USEC(PPM_SYNC_MAX_LEN)) { - booz_radio_control_ppm_cur_pulse = 0; + if (length > RC_PPM_TICS_OF_USEC(PPM_SYNC_MIN_LEN) && + length < RC_PPM_TICS_OF_USEC(PPM_SYNC_MAX_LEN)) { + booz_radio_control_ppm_cur_pulse = 0; } } else { - if (length > SYS_TICS_OF_USEC(PPM_DATA_MIN_LEN) && - length < SYS_TICS_OF_USEC(PPM_DATA_MAX_LEN)) { + if (length > RC_PPM_TICS_OF_USEC(PPM_DATA_MIN_LEN) && + length < RC_PPM_TICS_OF_USEC(PPM_DATA_MAX_LEN)) { booz_radio_control_ppm_pulses[booz_radio_control_ppm_cur_pulse] = length; booz_radio_control_ppm_cur_pulse++; if (booz_radio_control_ppm_cur_pulse == RADIO_CONTROL_NB_CHANNEL) { booz_radio_control_ppm_frame_available = TRUE; } } - else + else { booz_radio_control_ppm_cur_pulse = RADIO_CONTROL_NB_CHANNEL; + } } } - - DEBUG_S4_OFF(); + else if(TIM_GetITStatus(TIM2, TIM_IT_Update) == SET) { + timer_rollover_cnt+=(1<<16); + TIM_ClearITPendingBit(TIM2, TIM_IT_Update); + } } diff --git a/sw/airborne/booz/arch/stm32/radio_control/booz_radio_control_ppm_arch.h b/sw/airborne/booz/arch/stm32/radio_control/booz_radio_control_ppm_arch.h index 85457192d1..431f72b5f0 100644 --- a/sw/airborne/booz/arch/stm32/radio_control/booz_radio_control_ppm_arch.h +++ b/sw/airborne/booz/arch/stm32/radio_control/booz_radio_control_ppm_arch.h @@ -26,3 +26,11 @@ * */ +/** + * On tiny (and booz) the ppm counter is running at the same speed as + * the systic counter. There is no reason for this to be true. + * Let's add a pair of macros to make it possible for them to be different. + * + */ +#define RC_PPM_TICS_OF_USEC(_v) SYS_TICS_OF_USEC(_v/9) +#define RC_PPM_SIGNED_TICS_OF_USEC(_v) SIGNED_SYS_TICS_OF_USEC(_v/9) diff --git a/sw/airborne/booz/test/booz2_test_radio_control.c b/sw/airborne/booz/test/booz2_test_radio_control.c index facea3b287..f6142d6f13 100644 --- a/sw/airborne/booz/test/booz2_test_radio_control.c +++ b/sw/airborne/booz/test/booz2_test_radio_control.c @@ -1,22 +1,22 @@ /* * $Id$ * - * Copyright (C) 2008-2009 Antoine Drouin + * Copyright (C) 2008-2010 The Paparazzi Team * - * This file is part of paparazzi. + * This file is part of Paparazzi. * - * paparazzi is free software; you can redistribute it and/or modify + * 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, + * 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 + * 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. */ @@ -63,17 +63,19 @@ static inline void main_periodic_task( void ) { RunOnceEvery(10, {radio_control_periodic();}); int16_t foo = 0; - RunOnceEvery(10, {DOWNLINK_SEND_BOOZ2_RADIO_CONTROL(DefaultChannel, \ - &radio_control.values[RADIO_CONTROL_ROLL], \ - &radio_control.values[RADIO_CONTROL_PITCH], \ - &radio_control.values[RADIO_CONTROL_YAW], \ - &radio_control.values[RADIO_CONTROL_THROTTLE], \ - &radio_control.values[RADIO_CONTROL_MODE], \ - &foo, \ - &radio_control.status);}); - // uint8_t bar; - // RunOnceEvery(10, { DOWNLINK_SEND_CHRONO(DefaultChannel, &bar, &debug_len)}); - + RunOnceEvery(10, + {DOWNLINK_SEND_BOOZ2_RADIO_CONTROL(DefaultChannel, \ + &radio_control.values[RADIO_CONTROL_ROLL], \ + &radio_control.values[RADIO_CONTROL_PITCH], \ + &radio_control.values[RADIO_CONTROL_YAW], \ + &radio_control.values[RADIO_CONTROL_THROTTLE], \ + &radio_control.values[RADIO_CONTROL_MODE], \ + &foo, \ + &radio_control.status);}); +#ifdef RADIO_CONTROL_TYPE_PPM + RunOnceEvery(10, + {uint8_t blaa = 0; DOWNLINK_SEND_PPM(DefaultChannel,&blaa, 8, booz_radio_control_ppm_pulses);}); +#endif LED_PERIODIC(); } diff --git a/sw/airborne/stm32/adc_hw.c b/sw/airborne/stm32/adc_hw.c index 84d008f66b..1a0c0be86e 100644 --- a/sw/airborne/stm32/adc_hw.c +++ b/sw/airborne/stm32/adc_hw.c @@ -1,7 +1,7 @@ /* - * $Id: adc_hw.c 5313 2010-08-11 18:46:20Z flixr $ + * $Id$ * - * Copyright (C) 2008 Pascal Brisset, Antoine Drouin + * Copyright (C) 2010 The Paparazzi Team * * This file is part of paparazzi. * @@ -23,6 +23,10 @@ */ /** + * + * This is the driver for the analog to digital converters + * on STM32 + * * Usage: * Define flags for ADCs to use and their channels: * @@ -30,10 +34,6 @@ * * would enable ADC1 and it's channels 1 and 3. * - * - * - * - * */ #include "adc.h" @@ -204,8 +204,6 @@ static inline void adc_init_rcc( void ) rcc_apb = RCC_APB1Periph_TIM2; #endif - TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; - RCC_ADCCLKConfig(RCC_PCLK2_Div2); RCC_APB1PeriphClockCmd(rcc_apb, ENABLE); RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOB | @@ -218,6 +216,7 @@ static inline void adc_init_rcc( void ) #endif /* Time Base configuration */ + TIM_TimeBaseInitTypeDef TIM_TimeBaseStructure; TIM_TimeBaseStructInit(&TIM_TimeBaseStructure); TIM_TimeBaseStructure.TIM_Period = 0xFF; TIM_TimeBaseStructure.TIM_Prescaler = 0x8; diff --git a/sw/tools/gen_conf_radio_control_ppm.ml b/sw/tools/gen_conf_radio_control_ppm.ml index 910a9a97b7..7ca59bad5b 100644 --- a/sw/tools/gen_conf_radio_control_ppm.ml +++ b/sw/tools/gen_conf_radio_control_ppm.ml @@ -70,9 +70,9 @@ let parse_channel = let norm1_ppm = fun c -> if c.neutral = c.min then - sprintf "tmp_radio * (MAX_PPRZ / (float)(SIGNED_SYS_TICS_OF_USEC(%d-%d)))" c.max c.min, "0" + sprintf "tmp_radio * (MAX_PPRZ / (float)(RC_PPM_SIGNED_TICS_OF_USEC(%d-%d)))" c.max c.min, "0" else - sprintf "tmp_radio * (tmp_radio >=0 ? (MAX_PPRZ/(float)(SIGNED_SYS_TICS_OF_USEC(%d-%d))) : (MIN_PPRZ/(float)(SIGNED_SYS_TICS_OF_USEC(%d-%d))))" c.max c.neutral c.min c.neutral, "MIN_PPRZ" + sprintf "tmp_radio * (tmp_radio >=0 ? (MAX_PPRZ/(float)(RC_PPM_SIGNED_TICS_OF_USEC(%d-%d))) : (MIN_PPRZ/(float)(RC_PPM_SIGNED_TICS_OF_USEC(%d-%d))))" c.max c.neutral c.min c.neutral, "MIN_PPRZ" let gen_normalize_ppm = fun channels -> printf "#define NormalizePpm() {\\\n"; @@ -82,7 +82,7 @@ let gen_normalize_ppm = fun channels -> (fun c -> let value, min_pprz = norm1_ppm c in begin - printf " tmp_radio = booz_radio_control_ppm_pulses[RADIO_CONTROL_%s] - SYS_TICS_OF_USEC(%d);\\\n" c.name c.neutral; + printf " tmp_radio = booz_radio_control_ppm_pulses[RADIO_CONTROL_%s] - RC_PPM_TICS_OF_USEC(%d);\\\n" c.name c.neutral; printf " radio_control.values[RADIO_CONTROL_%s] = %s;\\\n" c.name value; printf " Bound(radio_control.values[RADIO_CONTROL_%s], %s, MAX_PPRZ); \\\n\\\n" c.name min_pprz; end @@ -96,7 +96,7 @@ let gen_normalize_ppm = fun channels -> (fun c -> if c.averaged then begin let value, min_pprz = norm1_ppm c in - printf " tmp_radio = avg_rc_values[RADIO_CONTROL_%s] / RC_AVG_PERIOD - SYS_TICS_OF_USEC(%d);\\\n" c.name c.neutral; + printf " tmp_radio = avg_rc_values[RADIO_CONTROL_%s] / RC_AVG_PERIOD - RC_PPM_TICS_OF_USEC(%d);\\\n" c.name c.neutral; printf " rc_values[RADIO_CONTROL_%s] = %s;\\\n" c.name value; printf " avg_rc_values[RADIO_CONTROL_%s] = 0;\\\n" c.name; printf " Bound(rc_values[RADIO_CONTROL_%s], %s, MAX_PPRZ); \\\n\\\n" c.name min_pprz;