[Top][All Lists]
[Date Prev][Date Next][Thread Prev][Thread Next][Date Index][Thread Index]
[paparazzi-commits] [6164] New radio control system
|
From: |
Gautier Hattenberger |
|
Subject: |
[paparazzi-commits] [6164] New radio control system |
|
Date: |
Mon, 18 Oct 2010 14:27:51 +0000 |
Revision: 6164
http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=6164
Author: gautier
Date: 2010-10-18 14:27:50 +0000 (Mon, 18 Oct 2010)
Log Message:
-----------
New radio control system
result of the old one and booz radio
only for fixed wing (booz coming soon)
ppm and rc_datalink are available on lpc21 and stm32 (not tested on stm32)
the decoding of ppm frame is done with IIR filter (old FIR should not work for
now)
Modified Paths:
--------------
paparazzi3/trunk/conf/autopilot/sitl_jsbsim.makefile
paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/radio_control_ppm.makefile
paparazzi3/trunk/sw/airborne/arch/lpc21/radio_control/ppm_arch.c
paparazzi3/trunk/sw/airborne/arch/lpc21/radio_control/ppm_arch.h
paparazzi3/trunk/sw/airborne/arch/lpc21/sys_time_hw.c
paparazzi3/trunk/sw/airborne/arch/sim/jsbsim_hw.c
paparazzi3/trunk/sw/airborne/datalink.c
paparazzi3/trunk/sw/airborne/fbw_downlink.h
paparazzi3/trunk/sw/airborne/firmwares/fixedwing/autopilot.h
paparazzi3/trunk/sw/airborne/inter_mcu.h
paparazzi3/trunk/sw/airborne/main_ap.c
paparazzi3/trunk/sw/airborne/main_fbw.c
paparazzi3/trunk/sw/airborne/radio_control/ppm.c
paparazzi3/trunk/sw/airborne/radio_control/ppm.h
paparazzi3/trunk/sw/airborne/radio_control.c
paparazzi3/trunk/sw/airborne/radio_control.h
paparazzi3/trunk/sw/airborne/rc_settings.h
paparazzi3/trunk/sw/tools/gen_radio.ml
Added Paths:
-----------
paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile
paparazzi3/trunk/sw/airborne/arch/sim/radio_control/
paparazzi3/trunk/sw/airborne/arch/sim/radio_control/ppm_arch.c
paparazzi3/trunk/sw/airborne/arch/sim/radio_control/ppm_arch.h
paparazzi3/trunk/sw/airborne/arch/sim/radio_control/rc_datalink.c
paparazzi3/trunk/sw/airborne/arch/stm32/radio_control/
paparazzi3/trunk/sw/airborne/arch/stm32/radio_control/ppm_arch.c
paparazzi3/trunk/sw/airborne/arch/stm32/radio_control/ppm_arch.h
paparazzi3/trunk/sw/airborne/radio_control/rc_datalink.c
paparazzi3/trunk/sw/airborne/radio_control/rc_datalink.h
Modified: paparazzi3/trunk/conf/autopilot/sitl_jsbsim.makefile
===================================================================
--- paparazzi3/trunk/conf/autopilot/sitl_jsbsim.makefile 2010-10-17
20:12:05 UTC (rev 6163)
+++ paparazzi3/trunk/conf/autopilot/sitl_jsbsim.makefile 2010-10-18
14:27:50 UTC (rev 6164)
@@ -24,10 +24,13 @@
jsbsim.LDFLAGS += -L$(JSBSIM_LIB) -lJSBSim
endif
-jsbsim.CFLAGS += -DSITL -DAP -DFBW -DRADIO_CONTROL -DINTER_MCU -DDOWNLINK
-DDOWNLINK_TRANSPORT=IvyTransport -DINFRARED -DNAV -DLED -DWIND_INFO
+jsbsim.CFLAGS += -DSITL -DAP -DFBW -DINTER_MCU -DDOWNLINK
-DDOWNLINK_TRANSPORT=IvyTransport -DINFRARED -DNAV -DLED -DWIND_INFO
-Ifirmwares/fixedwing
jsbsim.srcs = $(SRC_ARCH)/jsbsim_hw.c $(SRC_ARCH)/jsbsim_gps.c
$(SRC_ARCH)/jsbsim_ir.c $(SRC_ARCH)/jsbsim_transport.c
$(SRC_ARCH)/ivy_transport.c
-jsbsim.srcs += latlong.c radio_control.c downlink.c commands.c gps.c
inter_mcu.c infrared.c fw_h_ctl.c fw_v_ctl.c nav.c estimator.c sys_time.c
main_fbw.c main_ap.c datalink.c
+jsbsim.srcs += latlong.c downlink.c commands.c gps.c inter_mcu.c infrared.c
fw_h_ctl.c fw_v_ctl.c nav.c estimator.c sys_time.c main_fbw.c main_ap.c
datalink.c
jsbsim.srcs += $(SIMDIR)/sim_ac_jsbsim.c
# Choose in your airframe file type of airframe
# jsbsim.srcs += $(SIMDIR)/sim_ac_fw.c
# jsbsim.srcs += $(SIMDIR)/sim_ac_booz.c
+
+#jsbsim.CFLAGS += -DRADIO_CONTROL
-DRADIO_CONTROL_TYPE_H=\"radio_control/ppm.h\" -DRADIO_CONTROL_TYPE_PPM
+#jsbsim.srcs += radio_control.c radio_control/ppm.c
$(SRC_ARCH)/radio_control/ppm_arch.c
Added:
paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile
===================================================================
---
paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile
(rev 0)
+++
paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/radio_control_datalink.makefile
2010-10-18 14:27:50 UTC (rev 6164)
@@ -0,0 +1,19 @@
+
+NORADIO = False
+
+ifeq ($(BOARD),classix)
+ ifeq ($(TARGET),ap)
+ NORADIO = True
+ endif
+endif
+
+
+ifeq ($(NORADIO), False)
+ $(TARGET).CFLAGS += -DRADIO_CONTROL
+ $(TARGET).CFLAGS +=
-DRADIO_CONTROL_TYPE_H=\"radio_control/rc_datalink.h\"
+ $(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_DATALINK
+ $(TARGET).srcs += $(SRC_FIXEDWING)/radio_control.c
+ $(TARGET).srcs += $(SRC_FIXEDWING)/radio_control/rc_datalink.c
+ # arch only with sim target for compatibility (empty functions)
+ sim.srcs +=
$(SRC_ARCH)/radio_control/rc_datalink.c
+endif
Modified:
paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/radio_control_ppm.makefile
===================================================================
---
paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/radio_control_ppm.makefile
2010-10-17 20:12:05 UTC (rev 6163)
+++
paparazzi3/trunk/conf/autopilot/subsystems/fixedwing/radio_control_ppm.makefile
2010-10-18 14:27:50 UTC (rev 6164)
@@ -25,9 +25,12 @@
endif
ifeq ($(NORADIO), False)
- $(TARGET).CFLAGS += -DRADIO_CONTROL
- $(TARGET).srcs += $(SRC_FIXEDWING)/radio_control.c
+ $(TARGET).CFLAGS += -DRADIO_CONTROL
+ $(TARGET).CFLAGS +=
-DRADIO_CONTROL_TYPE_H=\"radio_control/ppm.h\"
+ $(TARGET).CFLAGS += -DRADIO_CONTROL_TYPE_PPM
+ $(TARGET).srcs += $(SRC_FIXEDWING)/radio_control.c
+ $(TARGET).srcs += $(SRC_FIXEDWING)/radio_control/ppm.c
ifneq ($(ARCH),jsbsim)
- $(TARGET).srcs += $(SRC_ARCH)/ppm_hw.c
+ $(TARGET).srcs += $(SRC_ARCH)/radio_control/ppm_arch.c
endif
endif
Modified: paparazzi3/trunk/sw/airborne/arch/lpc21/radio_control/ppm_arch.c
===================================================================
--- paparazzi3/trunk/sw/airborne/arch/lpc21/radio_control/ppm_arch.c
2010-10-17 20:12:05 UTC (rev 6163)
+++ paparazzi3/trunk/sw/airborne/arch/lpc21/radio_control/ppm_arch.c
2010-10-18 14:27:50 UTC (rev 6164)
@@ -22,9 +22,11 @@
*/
#include "radio_control.h"
+#include "radio_control/ppm.h"
uint8_t ppm_cur_pulse;
uint32_t ppm_last_pulse_time;
+bool_t ppm_data_valid;
void ppm_arch_init ( void ) {
/* select pin for capture */
@@ -35,9 +37,10 @@
#elif defined PPM_PULSE_TYPE && PPM_PULSE_TYPE == PPM_PULSE_TYPE_NEGATIVE
T0CCR = PPM_CCR_CRF | PPM_CCR_CRI;
#else
-#error "ppm_hw.h: Unknown PM_PULSE_TYPE"
+#error "ppm_arch.h: Unknown PM_PULSE_TYPE"
#endif
ppm_last_pulse_time = 0;
ppm_cur_pulse = RADIO_CONTROL_NB_CHANNEL;
+ ppm_data_valid = FALSE;
ppm_frame_available = FALSE;
}
Modified: paparazzi3/trunk/sw/airborne/arch/lpc21/radio_control/ppm_arch.h
===================================================================
--- paparazzi3/trunk/sw/airborne/arch/lpc21/radio_control/ppm_arch.h
2010-10-17 20:12:05 UTC (rev 6163)
+++ paparazzi3/trunk/sw/airborne/arch/lpc21/radio_control/ppm_arch.h
2010-10-18 14:27:50 UTC (rev 6164)
@@ -39,50 +39,11 @@
#define PPM_NB_CHANNEL RADIO_CONTROL_NB_CHANNEL
-extern uint8_t ppm_cur_pulse;
-extern uint32_t ppm_last_pulse_time;
-
-/**
- * A valid ppm frame:
- * - synchro blank
- * - correct number of channels
- * - synchro blank
- */
#define PPM_IT PPM_CRI
-#define PPM_ISR() { \
- static uint8_t ppm_data_valid = FALSE; \
- \
- uint32_t now = PPM_CR; \
- uint32_t length = now - ppm_last_pulse_time;
\
- ppm_last_pulse_time = now;
\
- \
- if (ppm_cur_pulse == PPM_NB_CHANNEL) { \
- if (length > SYS_TICS_OF_USEC(PPM_SYNC_MIN_LEN) && \
- length < SYS_TICS_OF_USEC(PPM_SYNC_MAX_LEN)) { \
- if (ppm_data_valid) { \
- ppm_frame_available = TRUE; \
- ppm_data_valid = FALSE; \
- } \
- ppm_cur_pulse = 0; \
- } \
- else { \
- ppm_data_valid = FALSE; \
- } \
- } \
- else { \
- if (length > SYS_TICS_OF_USEC(PPM_DATA_MIN_LEN) && \
- length < SYS_TICS_OF_USEC(PPM_DATA_MAX_LEN)) { \
- ppm_pulses[ppm_cur_pulse] = length; \
- ppm_cur_pulse++; \
- if (ppm_cur_pulse == PPM_NB_CHANNEL) { \
- ppm_data_valid = TRUE; \
- } \
- } \
- else { \
- ppm_cur_pulse = PPM_NB_CHANNEL;
\
- ppm_data_valid = FALSE; \
- } \
- } \
+
+#define PPM_ISR() { \
+ uint32_t now = PPM_CR; \
+ DecodePpmFrame(now); \
}
Modified: paparazzi3/trunk/sw/airborne/arch/lpc21/sys_time_hw.c
===================================================================
--- paparazzi3/trunk/sw/airborne/arch/lpc21/sys_time_hw.c 2010-10-17
20:12:05 UTC (rev 6163)
+++ paparazzi3/trunk/sw/airborne/arch/lpc21/sys_time_hw.c 2010-10-18
14:27:50 UTC (rev 6164)
@@ -13,8 +13,8 @@
#define ACTUATORS_IT 0x00
#endif /* ACTUATORS */
-#ifdef RADIO_CONTROL
-#include "ppm.h"
+#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_PPM
+#include "radio_control.h"
#else
#define PPM_IT 0x00
#endif
@@ -86,7 +86,7 @@
}
#endif /* ACTUATORS && (SERVOS_4017 || SERVOS_4015_MAT || SERVOS_PPM_MAT) */
-#ifdef RADIO_CONTROL
+#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_PPM
if (T0IR&PPM_IT) {
PPM_ISR();
T0IR = PPM_IT;
Modified: paparazzi3/trunk/sw/airborne/arch/sim/jsbsim_hw.c
===================================================================
--- paparazzi3/trunk/sw/airborne/arch/sim/jsbsim_hw.c 2010-10-17 20:12:05 UTC
(rev 6163)
+++ paparazzi3/trunk/sw/airborne/arch/sim/jsbsim_hw.c 2010-10-18 14:27:50 UTC
(rev 6164)
@@ -22,8 +22,8 @@
//bool_t gpio1_status;
uint16_t adc_generic_val1;
uint16_t adc_generic_val2;
-uint16_t ppm_pulses[ PPM_NB_PULSES ];
-volatile bool_t ppm_valid;
+//uint16_t ppm_pulses[ PPM_NB_PULSES ];
+//volatile bool_t ppm_valid;
uint8_t ac_id;
Added: paparazzi3/trunk/sw/airborne/arch/sim/radio_control/ppm_arch.c
===================================================================
--- paparazzi3/trunk/sw/airborne/arch/sim/radio_control/ppm_arch.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/arch/sim/radio_control/ppm_arch.c
2010-10-18 14:27:50 UTC (rev 6164)
@@ -0,0 +1,64 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 The Paparazzi Team
+ *
+ * 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.
+ */
+
+#include "sys_time.h"
+#include "radio_control.h"
+#include "radio_control/ppm.h"
+
+#include <inttypes.h>
+#include <caml/mlvalues.h>
+
+uint8_t ppm_cur_pulse;
+uint32_t ppm_last_pulse_time;
+bool_t ppm_data_valid;
+
+void ppm_arch_init ( void ) {
+ ppm_last_pulse_time = 0;
+ ppm_cur_pulse = RADIO_CONTROL_NB_CHANNEL;
+ ppm_data_valid = FALSE;
+ ppm_frame_available = FALSE;
+}
+
+#ifdef RADIO_CONTROL
+
+value update_rc_channel(value c, value v) {
+ ppm_pulses[Int_val(c)] = Double_val(v);
+ return Val_unit;
+}
+
+value send_ppm(value unit) {
+ ppm_frame_available = TRUE;
+ return unit;
+}
+
+#else // RADIO_CONTROL
+
+value update_rc_channel(value c __attribute__ ((unused)), value v
__attribute__ ((unused))) {
+ return Val_unit;
+}
+
+value send_ppm(value unit) {
+ return unit;
+}
+
+#endif // RADIO_CONTROL
Added: paparazzi3/trunk/sw/airborne/arch/sim/radio_control/ppm_arch.h
===================================================================
--- paparazzi3/trunk/sw/airborne/arch/sim/radio_control/ppm_arch.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/arch/sim/radio_control/ppm_arch.h
2010-10-18 14:27:50 UTC (rev 6164)
@@ -0,0 +1,39 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 The Paparazzi Team
+ *
+ * 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 PPM_ARCH_H
+#define PPM_ARCH_H
+
+
+/**
+ * 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(_x) (_x)
+#define RC_PPM_SIGNED_TICS_OF_USEC(_x) (_x)
+
+#define PPM_NB_CHANNEL RADIO_CONTROL_NB_CHANNEL
+
+#endif /* PPM_ARCH_H */
Added: paparazzi3/trunk/sw/airborne/arch/sim/radio_control/rc_datalink.c
===================================================================
--- paparazzi3/trunk/sw/airborne/arch/sim/radio_control/rc_datalink.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/arch/sim/radio_control/rc_datalink.c
2010-10-18 14:27:50 UTC (rev 6164)
@@ -0,0 +1,36 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 The Paparazzi Team
+ *
+ * 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.
+ */
+
+#include "radio_control.h"
+
+#include <inttypes.h>
+#include <caml/mlvalues.h>
+
+value update_rc_channel(value c __attribute__ ((unused)), value v
__attribute__ ((unused))) {
+ return Val_unit;
+}
+
+value send_ppm(value unit) {
+ return unit;
+}
+
Added: paparazzi3/trunk/sw/airborne/arch/stm32/radio_control/ppm_arch.c
===================================================================
--- paparazzi3/trunk/sw/airborne/arch/stm32/radio_control/ppm_arch.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/arch/stm32/radio_control/ppm_arch.c
2010-10-18 14:27:50 UTC (rev 6164)
@@ -0,0 +1,117 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 The Paparazzi Team
+ *
+ * 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.
+ */
+
+#include "radio_control.h"
+#include "radio_control/ppm.h"
+
+#include <stm32/rcc.h>
+#include <stm32/gpio.h>
+#include <stm32/tim.h>
+#include <stm32/misc.h>
+
+#include "sys_time.h"
+
+/*
+ *
+ * This a radio control ppm driver for stm32
+ * signal on PA1 TIM2/CH2 (uart1 trig on lisa/L)
+ *
+ */
+uint8_t ppm_cur_pulse;
+uint32_t ppm_last_pulse_time;
+bool_t ppm_data_valid;
+static uint32_t timer_rollover_cnt;
+
+void tim2_irq_handler(void);
+
+void ppm_arch_init ( void ) {
+
+ /* TIM2 channel 2 pin (PA.01) configuration */
+ GPIO_InitTypeDef GPIO_InitStructure;
+ GPIO_InitStructure.GPIO_Pin = GPIO_Pin_1;
+ GPIO_InitStructure.GPIO_Mode = GPIO_Mode_IN_FLOATING;
+ GPIO_InitStructure.GPIO_Speed = GPIO_Speed_50MHz;
+ GPIO_Init(GPIOA, &GPIO_InitStructure);
+
+ /* TIM2 clock enable */
+ RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2, ENABLE);
+
+ /* 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,
+ ------------------------------------------------------------ */
+ TIM_ICInitTypeDef TIM_ICInitStructure;
+ TIM_ICInitStructure.TIM_Channel = TIM_Channel_2;
+ 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 = 0x00;
+ TIM_ICInit(TIM2, &TIM_ICInitStructure);
+
+ /* Enable the TIM2 global Interrupt */
+ NVIC_InitTypeDef NVIC_InitStructure;
+ NVIC_InitStructure.NVIC_IRQChannel = TIM2_IRQn;
+ NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority = 2;
+ NVIC_InitStructure.NVIC_IRQChannelSubPriority = 1;
+ NVIC_InitStructure.NVIC_IRQChannelCmd = ENABLE;
+ NVIC_Init(&NVIC_InitStructure);
+
+ /* TIM2 enable counter */
+ TIM_Cmd(TIM2, ENABLE);
+
+ /* Enable the CC2 Interrupt Request */
+ TIM_ITConfig(TIM2, TIM_IT_CC2|TIM_IT_Update, ENABLE);
+
+ ppm_last_pulse_time = 0;
+ ppm_cur_pulse = RADIO_CONTROL_NB_CHANNEL;
+ timer_rollover_cnt = 0;
+
+}
+
+
+void tim2_irq_handler(void) {
+
+ if(TIM_GetITStatus(TIM2, TIM_IT_CC2) == SET) {
+ TIM_ClearITPendingBit(TIM2, TIM_IT_CC2);
+
+ uint32_t now = TIM_GetCapture2(TIM2) + timer_rollover_cnt;
+ DecodePpmFrame(now);
+ }
+ else if(TIM_GetITStatus(TIM2, TIM_IT_Update) == SET) {
+ timer_rollover_cnt+=(1<<16);
+ TIM_ClearITPendingBit(TIM2, TIM_IT_Update);
+ }
+
+}
Added: paparazzi3/trunk/sw/airborne/arch/stm32/radio_control/ppm_arch.h
===================================================================
--- paparazzi3/trunk/sw/airborne/arch/stm32/radio_control/ppm_arch.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/arch/stm32/radio_control/ppm_arch.h
2010-10-18 14:27:50 UTC (rev 6164)
@@ -0,0 +1,36 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 The Paparazzi Team
+ *
+ * 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.
+ */
+
+/** \file booz_radio_control_ppm_hw.h
+ * \brief STM32 ppm decoder
+ *
+ */
+
+/**
+ * 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)
Modified: paparazzi3/trunk/sw/airborne/datalink.c
===================================================================
--- paparazzi3/trunk/sw/airborne/datalink.c 2010-10-17 20:12:05 UTC (rev
6163)
+++ paparazzi3/trunk/sw/airborne/datalink.c 2010-10-18 14:27:50 UTC (rev
6164)
@@ -204,6 +204,15 @@
DL_RC_3CH_pitch(dl_buffer));
} else
#endif // USE_RC_TELEMETRY
+#if defined RADIO_CONTROL && defined RADIO_CONTROL_TYPE_DATALINK
+ if (msg_id == DL_RC_3CH /*&& DL_RC_3CH_ac_id(dl_buffer) == TX_ID*/) {
+LED_TOGGLE(3);
+ parse_rc_datalink(
+ DL_RC_3CH_throttle_mode(dl_buffer),
+ DL_RC_3CH_roll(dl_buffer),
+ DL_RC_3CH_pitch(dl_buffer));
+ } else
+#endif // RC_DATALINK
{ /* Last else */
/* Parse modules datalink */
modules_parse_datalink(msg_id);
Modified: paparazzi3/trunk/sw/airborne/fbw_downlink.h
===================================================================
--- paparazzi3/trunk/sw/airborne/fbw_downlink.h 2010-10-17 20:12:05 UTC (rev
6163)
+++ paparazzi3/trunk/sw/airborne/fbw_downlink.h 2010-10-18 14:27:50 UTC (rev
6164)
@@ -53,9 +53,13 @@
#define PERIODIC_SEND_COMMANDS(_chan) DOWNLINK_SEND_COMMANDS(_chan,
COMMANDS_NB, commands)
#ifdef RADIO_CONTROL
-#define PERIODIC_SEND_FBW_STATUS(_chan) DOWNLINK_SEND_FBW_STATUS(_chan,
&rc_status, &fbw_mode, &fbw_vsupply_decivolt, &fbw_current_milliamp)
-#define PERIODIC_SEND_PPM(_chan) DOWNLINK_SEND_PPM(_chan, &last_ppm_cpt,
PPM_NB_PULSES, ppm_pulses)
-#define PERIODIC_SEND_RC(_chan) DOWNLINK_SEND_RC(_chan, PPM_NB_PULSES,
rc_values)
+#define PERIODIC_SEND_FBW_STATUS(_chan) DOWNLINK_SEND_FBW_STATUS(_chan,
&(radio_control.status), &fbw_mode, &fbw_vsupply_decivolt,
&fbw_current_milliamp)
+#ifdef RADIO_CONTROL_TYPE_PPM
+#define PERIODIC_SEND_PPM(_chan) DOWNLINK_SEND_PPM(_chan,
&(radio_control.frame_rate), PPM_NB_CHANNEL, ppm_pulses)
+#else
+#define PERIODIC_SEND_PPM(_chan) {}
+#endif
+#define PERIODIC_SEND_RC(_chan) DOWNLINK_SEND_RC(_chan,
RADIO_CONTROL_NB_CHANNEL, radio_control.values)
#else // RADIO_CONTROL
#define PERIODIC_SEND_FBW_STATUS(_chan) { uint8_t dummy = 0;
DOWNLINK_SEND_FBW_STATUS(_chan, &dummy, &fbw_mode, &fbw_vsupply_decivolt,
&fbw_current_milliamp); }
#define PERIODIC_SEND_PPM(_chan) {}
Modified: paparazzi3/trunk/sw/airborne/firmwares/fixedwing/autopilot.h
===================================================================
--- paparazzi3/trunk/sw/airborne/firmwares/fixedwing/autopilot.h
2010-10-17 20:12:05 UTC (rev 6163)
+++ paparazzi3/trunk/sw/airborne/firmwares/fixedwing/autopilot.h
2010-10-18 14:27:50 UTC (rev 6164)
@@ -90,12 +90,12 @@
})
void periodic_task( void );
-void telecommand_task(void);
+//void telecommand_task(void);
#ifdef RADIO_CONTROL
#include "radio_control.h"
static inline void autopilot_process_radio_control ( void ) {
- pprz_mode = PPRZ_MODE_OF_PULSE(rc_values[RADIO_MODE], 0);
+ pprz_mode = PPRZ_MODE_OF_PULSE(radio_control.values[RADIO_MODE], 0);
}
#endif
Modified: paparazzi3/trunk/sw/airborne/inter_mcu.h
===================================================================
--- paparazzi3/trunk/sw/airborne/inter_mcu.h 2010-10-17 20:12:05 UTC (rev
6163)
+++ paparazzi3/trunk/sw/airborne/inter_mcu.h 2010-10-18 14:27:50 UTC (rev
6164)
@@ -38,9 +38,6 @@
#include <inttypes.h>
#include "std.h"
-#if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1
-#include "radio.h"
-#endif
#include "paparazzi.h"
#include "airframe.h"
@@ -50,7 +47,7 @@
/** Data structure shared by fbw and ap processes */
struct fbw_state {
#if defined RADIO_CONTROL || RADIO_CONTROL_AUTO1
- pprz_t channels[RADIO_CTL_NB];
+ pprz_t channels[RADIO_CONTROL_NB_CHANNEL];
uint8_t ppm_cpt;
#endif
uint8_t status;
@@ -101,26 +98,20 @@
#ifdef RADIO_CONTROL
uint8_t i;
- for(i = 0; i < RADIO_CTL_NB; i++)
- fbw_state->channels[i] = rc_values[i];
+ for(i = 0; i < RADIO_CONTROL_NB_CHANNEL; i++)
+ fbw_state->channels[i] = radio_control.values[i];
- fbw_state->ppm_cpt = last_ppm_cpt;
+ fbw_state->ppm_cpt = radio_control.frame_rate;
- status = (rc_status == RC_OK ? _BV(STATUS_RADIO_OK) : 0);
- status |= (rc_status == RC_REALLY_LOST ? _BV(STATUS_RADIO_REALLY_LOST) : 0);
+ status = (radio_control.status == RC_OK ? _BV(STATUS_RADIO_OK) : 0);
+ status |= (radio_control.status == RC_REALLY_LOST ?
_BV(STATUS_RADIO_REALLY_LOST) : 0);
+ status |= (radio_control.status == RC_OK ? _BV(AVERAGED_CHANNELS_SENT) : 0);
// Any valid frame contains averaged channels
#endif // RADIO_CONTROL
status |= (fbw_mode == FBW_MODE_AUTO ? _BV(STATUS_MODE_AUTO) : 0);
status |= (fbw_mode == FBW_MODE_FAILSAFE ? _BV(STATUS_MODE_FAILSAFE) : 0);
fbw_state->status = status;
-#ifdef RADIO_CONTROL
- if (rc_values_contains_avg_channels) {
- fbw_state->status |= _BV(AVERAGED_CHANNELS_SENT);
- rc_values_contains_avg_channels = FALSE;
- }
-#endif // RADIO_CONTROL
-
fbw_state->vsupply = fbw_vsupply_decivolt;
fbw_state->current = fbw_current_milliamp;
}
Modified: paparazzi3/trunk/sw/airborne/main_ap.c
===================================================================
--- paparazzi3/trunk/sw/airborne/main_ap.c 2010-10-17 20:12:05 UTC (rev
6163)
+++ paparazzi3/trunk/sw/airborne/main_ap.c 2010-10-18 14:27:50 UTC (rev
6164)
@@ -255,7 +255,7 @@
#endif
/** \brief Function to be called when a message from FBW is available */
-inline void telecommand_task( void ) {
+static inline void telecommand_task( void ) {
uint8_t mode_changed = FALSE;
copy_from_to_fbw();
Modified: paparazzi3/trunk/sw/airborne/main_fbw.c
===================================================================
--- paparazzi3/trunk/sw/airborne/main_fbw.c 2010-10-17 20:12:05 UTC (rev
6163)
+++ paparazzi3/trunk/sw/airborne/main_fbw.c 2010-10-18 14:27:50 UTC (rev
6164)
@@ -108,8 +108,7 @@
SetCommands(commands_failsafe);
#endif
#ifdef RADIO_CONTROL
- ppm_init();
- // radio_control_init();
+ radio_control_init();
#endif
#ifdef INTER_MCU
inter_mcu_init();
@@ -132,22 +131,19 @@
SetCommands(commands_failsafe);
}
+#ifdef RADIO_CONTROL
+static inline void handle_rc_frame( void ) {
+ fbw_mode = FBW_MODE_OF_PPRZ(radio_control.values[RADIO_MODE]);
+ if (fbw_mode == FBW_MODE_MANUAL)
+ SetCommandsFromRC(commands, radio_control.values);
+}
+#endif
/********** EVENT ************************************************************/
void event_task_fbw( void) {
#ifdef RADIO_CONTROL
- if (ppm_valid) {
- ppm_valid = FALSE;
- radio_control_event_task();
- if (rc_status == RC_OK) {
- if (rc_values_contains_avg_channels) {
- fbw_mode = FBW_MODE_OF_PPRZ(rc_values[RADIO_MODE]);
- }
- if (fbw_mode == FBW_MODE_MANUAL)
- SetCommandsFromRC(commands, rc_values);
- }
- }
+ RadioControlEvent(handle_rc_frame);
#endif
@@ -204,17 +200,10 @@
#ifdef RADIO_CONTROL
radio_control_periodic_task();
- if (fbw_mode == FBW_MODE_MANUAL && rc_status == RC_REALLY_LOST) {
+ if (fbw_mode == FBW_MODE_MANUAL && radio_control.status == RC_REALLY_LOST) {
fbw_mode = FBW_MODE_AUTO;
}
-#ifdef RADIO_LED
- if (rc_status != RC_OK) {
- LED_OFF(RADIO_LED);
- } else {
- LED_ON(RADIO_LED);
- }
#endif
-#endif
#ifdef INTER_MCU
inter_mcu_periodic_task();
Modified: paparazzi3/trunk/sw/airborne/radio_control/ppm.c
===================================================================
--- paparazzi3/trunk/sw/airborne/radio_control/ppm.c 2010-10-17 20:12:05 UTC
(rev 6163)
+++ paparazzi3/trunk/sw/airborne/radio_control/ppm.c 2010-10-18 14:27:50 UTC
(rev 6164)
@@ -22,9 +22,9 @@
*/
#include "radio_control.h"
+#include "radio_control/ppm.h"
-
-uint16_t ppm_pulses[ RADIO_CONTROL_NB_CHANNEL ];
+uint16_t ppm_pulses[ PPM_NB_CHANNEL ];
volatile bool_t ppm_frame_available;
void radio_control_impl_init(void) {
Modified: paparazzi3/trunk/sw/airborne/radio_control/ppm.h
===================================================================
--- paparazzi3/trunk/sw/airborne/radio_control/ppm.h 2010-10-17 20:12:05 UTC
(rev 6163)
+++ paparazzi3/trunk/sw/airborne/radio_control/ppm.h 2010-10-18 14:27:50 UTC
(rev 6164)
@@ -21,15 +21,17 @@
* Boston, MA 02111-1307, USA.
*/
-#ifndef PPM_H
-#define PPM_H
+#ifndef RC_PPM_H
+#define RC_PPM_H
+#include "std.h"
+
/**
* Architecture dependant code
*/
#include "radio_control/ppm_arch.h"
/* must be implemented by arch dependant code */
-extern void ppm_arch_init ( void );
+extern void ppm_arch_init(void);
/**
* Generated code holding the description of a given
@@ -38,6 +40,12 @@
#include "radio.h"
/**
+ * Define number of channels
+ * Using generated code radio.h
+ */
+#define RADIO_CONTROL_NB_CHANNEL RADIO_CTL_NB
+
+/**
* ppm pulse type : futaba is falling edge clocked whereas JR is rising edge
*/
#define PPM_PULSE_TYPE_POSITIVE 0
@@ -46,19 +54,70 @@
extern uint16_t ppm_pulses[ RADIO_CONTROL_NB_CHANNEL ];
extern volatile bool_t ppm_frame_available;
+/**
+ * Event macro with handler callback
+ * PPM frame are normalize using the IIR filter
+ */
+#define RadioControlEvent(_received_frame_handler) { \
+ if (ppm_frame_available) { \
+ radio_control.frame_cpt++; \
+ radio_control.time_since_last_frame = 0; \
+ if (radio_control.radio_ok_cpt > 0) { \
+ radio_control.radio_ok_cpt--; \
+ } else { \
+ radio_control.status = RC_OK; \
+ NormalizePpmIIR(ppm_pulses,radio_control); \
+ _received_frame_handler(); \
+ } \
+ ppm_frame_available = FALSE; \
+ } \
+}
-#define RadioControlEvent(_received_frame_handler) { \
- if (ppm_frame_available) { \
- radio_control.frame_cpt++; \
- radio_control.time_since_last_frame = 0; \
- if (radio_control.radio_ok_cpt > 0) radio_control.radio_ok_cpt--; \
- else { \
- radio_control.status = RADIO_CONTROL_OK; \
- NormalizePpm();
\
- _received_frame_handler(); \
- }
\
- ppm_frame_available = FALSE; \
- } \
- }
+/**
+ * State machine for decoding ppm frames
+ */
+extern uint8_t ppm_cur_pulse;
+extern uint32_t ppm_last_pulse_time;
+extern bool_t ppm_data_valid;
-#endif /* PPM_H */
+/**
+ * A valid ppm frame:
+ * - synchro blank
+ * - correct number of channels
+ * - synchro blank
+ */
+#define DecodePpmFrame(_ppm_time) { \
+ uint32_t length = _ppm_time - ppm_last_pulse_time; \
+ ppm_last_pulse_time = _ppm_time; \
+ \
+ if (ppm_cur_pulse == PPM_NB_CHANNEL) { \
+ if (length > RC_PPM_TICS_OF_USEC(PPM_SYNC_MIN_LEN) && \
+ length < RC_PPM_TICS_OF_USEC(PPM_SYNC_MAX_LEN)) { \
+ if (ppm_data_valid) { \
+ ppm_frame_available = TRUE; \
+ ppm_data_valid = FALSE; \
+ } \
+ ppm_cur_pulse = 0; \
+ } \
+ else { \
+ ppm_data_valid = FALSE; \
+ } \
+ } \
+ else { \
+ if (length > RC_PPM_TICS_OF_USEC(PPM_DATA_MIN_LEN) && \
+ length < RC_PPM_TICS_OF_USEC(PPM_DATA_MAX_LEN)) { \
+ ppm_pulses[ppm_cur_pulse] = length; \
+ ppm_cur_pulse++; \
+ if (ppm_cur_pulse == PPM_NB_CHANNEL) { \
+ ppm_data_valid = TRUE; \
+ } \
+ } \
+ else { \
+ ppm_cur_pulse = PPM_NB_CHANNEL; \
+ ppm_data_valid = FALSE; \
+ } \
+ } \
+}
+
+#endif /* RC_PPM_H */
+
Added: paparazzi3/trunk/sw/airborne/radio_control/rc_datalink.c
===================================================================
--- paparazzi3/trunk/sw/airborne/radio_control/rc_datalink.c
(rev 0)
+++ paparazzi3/trunk/sw/airborne/radio_control/rc_datalink.c 2010-10-18
14:27:50 UTC (rev 6164)
@@ -0,0 +1,51 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 The Paparazzi Team
+ *
+ * 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.
+ */
+
+#include "radio_control/rc_datalink.h"
+#include "radio_control.h"
+
+int8_t rc_dl_values[ RC_DL_NB_CHANNEL ];
+volatile bool_t rc_dl_frame_available;
+
+
+void radio_control_impl_init(void) {
+ rc_dl_frame_available = FALSE;
+}
+
+
+void parse_rc_datalink( uint8_t throttle_mode,
+ int8_t roll,
+ int8_t pitch)
+{
+ uint8_t throttle = throttle_mode & 0xFC;
+ uint8_t mode = throttle_mode & 0x03;
+
+ rc_dl_values[RADIO_ROLL] = roll;
+ rc_dl_values[RADIO_PITCH] = pitch;
+ rc_dl_values[RADIO_THROTTLE] = (int8_t)throttle;
+ rc_dl_values[RADIO_MODE] = (int8_t)mode;
+
+ rc_dl_frame_available = TRUE;
+}
+
+
Added: paparazzi3/trunk/sw/airborne/radio_control/rc_datalink.h
===================================================================
--- paparazzi3/trunk/sw/airborne/radio_control/rc_datalink.h
(rev 0)
+++ paparazzi3/trunk/sw/airborne/radio_control/rc_datalink.h 2010-10-18
14:27:50 UTC (rev 6164)
@@ -0,0 +1,84 @@
+/*
+ * $Id$
+ *
+ * Copyright (C) 2010 The Paparazzi Team
+ *
+ * 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_DATALINK_H
+#define RC_DATALINK_H
+
+#include "std.h"
+
+#define RC_DL_NB_CHANNEL 5
+#define RADIO_CONTROL_NB_CHANNEL RC_DL_NB_CHANNEL
+
+/**
+ * Redefining RADIO_*
+ * Do not use with radio.h (ppm rc)
+ */
+#define RADIO_ROLL 0
+#define RADIO_PITCH 1
+#define RADIO_YAW 2
+#define RADIO_THROTTLE 3
+#define RADIO_MODE 4
+
+extern int8_t rc_dl_values[ RC_DL_NB_CHANNEL ];
+extern volatile bool_t rc_dl_frame_available;
+
+/**
+ * Decode datalink message to get rc values
+ */
+extern void parse_rc_datalink(
+ uint8_t throttle_mode,
+ int8_t roll,
+ int8_t pitch);
+
+/**
+ * Macro that normalize rc_dl_values to radio values
+ */
+#define NormalizeRcDl(_in, _out) { \
+ _out[RADIO_ROLL] = (MAX_PPRZ/128) * _in[RADIO_ROLL]; \
+ Bound(_out[RADIO_ROLL], MIN_PPRZ, MAX_PPRZ); \
+ _out[RADIO_PITCH] = (MAX_PPRZ/128) * _in[RADIO_PITCH]; \
+ Bound(_out[RADIO_PITCH], MIN_PPRZ, MAX_PPRZ); \
+ _out[RADIO_YAW] = 0; \
+ Bound(_out[RADIO_YAW], MIN_PPRZ, MAX_PPRZ); \
+ _out[RADIO_THROTTLE] = ((MAX_PPRZ/64) * _in[RADIO_THROTTLE]); \
+ Bound(_out[RADIO_THROTTLE], 0, MAX_PPRZ); \
+ _out[RADIO_MODE] = MAX_PPRZ * (_in[RADIO_MODE] - 1); \
+ Bound(_out[RADIO_MODE], MIN_PPRZ, MAX_PPRZ); \
+}
+
+/**
+ * Event macro with handler callback
+ */
+#define RadioControlEvent(_received_frame_handler) { \
+ if (rc_dl_frame_available) { \
+ radio_control.frame_cpt++; \
+ radio_control.time_since_last_frame = 0; \
+ radio_control.radio_ok_cpt = 0; \
+ radio_control.status = RC_OK; \
+ NormalizeRcDl(rc_dl_values,radio_control.values); \
+ _received_frame_handler(); \
+ rc_dl_frame_available = FALSE; \
+ } \
+}
+
+#endif /* RC_DATALINK_H */
Modified: paparazzi3/trunk/sw/airborne/radio_control.c
===================================================================
--- paparazzi3/trunk/sw/airborne/radio_control.c 2010-10-17 20:12:05 UTC
(rev 6163)
+++ paparazzi3/trunk/sw/airborne/radio_control.c 2010-10-18 14:27:50 UTC
(rev 6164)
@@ -24,10 +24,5 @@
#include "radio_control.h"
-pprz_t rc_values[PPM_NB_PULSES];
-uint8_t rc_status;
-int32_t avg_rc_values[PPM_NB_PULSES];
-uint8_t rc_values_contains_avg_channels = FALSE;
-uint8_t time_since_last_ppm;
-uint8_t ppm_cpt, last_ppm_cpt, radio_ok_cpt;
+struct RadioControl radio_control;
Modified: paparazzi3/trunk/sw/airborne/radio_control.h
===================================================================
--- paparazzi3/trunk/sw/airborne/radio_control.h 2010-10-17 20:12:05 UTC
(rev 6163)
+++ paparazzi3/trunk/sw/airborne/radio_control.h 2010-10-18 14:27:50 UTC
(rev 6164)
@@ -27,55 +27,50 @@
#if defined RADIO_CONTROL
-#define RadioControlEventCheckAndHandle(_user_callback) { \
- if (ppm_valid) { \
- ppm_valid = FALSE; \
- radio_control_event_task(); \
- _user_callback(); \
- } \
- }
-
-
-
#include "led.h"
-#include "sys_time.h"
-#include "ppm.h"
-#include "radio.h"
#include "airframe.h"
#include "paparazzi.h"
-#define RC_AVG_PERIOD 8
+/* underlying hardware */
+#include RADIO_CONTROL_TYPE_H
+/* must be defined by underlying hardware */
+extern void radio_control_impl_init(void);
+/* RADIO_CONTROL_NB_CHANNEL has to be defined by the implementation */
+
+/* timeouts - for now assumes 60Hz periodic */
+#define RC_AVG_PERIOD 8 /* TODO remove if IIR filter is used */
#define RC_LOST_TIME 30 /* 500ms with a 60Hz timer */
#define RC_REALLY_LOST_TIME 60 /* ~1s */
-// Number of valid ppm frame to go back to RC OK
+/* Number of valid frames before going back to RC OK */
#define RC_OK_CPT 15
#define RC_OK 0
#define RC_LOST 1
#define RC_REALLY_LOST 2
-extern pprz_t rc_values[PPM_NB_PULSES];
-extern uint8_t rc_status;
-extern int32_t avg_rc_values[PPM_NB_PULSES];
-extern uint8_t rc_values_contains_avg_channels;
-extern uint8_t time_since_last_ppm;
-extern uint8_t ppm_cpt, last_ppm_cpt, radio_ok_cpt;
+struct RadioControl {
+ uint8_t status;
+ uint8_t time_since_last_frame;
+ uint8_t radio_ok_cpt;
+ uint8_t frame_rate;
+ uint8_t frame_cpt;
+ pprz_t values[RADIO_CONTROL_NB_CHANNEL];
+};
-/*
- * 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 struct RadioControl radio_control;
/************* INIT ******************************************************/
static inline void radio_control_init ( void ) {
- rc_status = RC_REALLY_LOST;
- time_since_last_ppm = RC_REALLY_LOST_TIME;
- radio_ok_cpt = 0;
+ uint8_t i;
+ for (i=0; i<RADIO_CONTROL_NB_CHANNEL; i++)
+ radio_control.values[i] = 0;
+ radio_control.status = RC_REALLY_LOST;
+ radio_control.time_since_last_frame = RC_REALLY_LOST_TIME;
+ radio_control.radio_ok_cpt = 0;
+ radio_control.frame_rate = 0;
+ radio_control.frame_cpt = 0;
+ radio_control_impl_init();
}
/************* PERIODIC ******************************************************/
@@ -85,44 +80,34 @@
if (_1Hz >= 60) {
_1Hz = 0;
- last_ppm_cpt = ppm_cpt;
- ppm_cpt = 0;
+ radio_control.frame_rate = radio_control.frame_cpt;
+ radio_control.frame_cpt = 0;
}
- if (time_since_last_ppm >= RC_REALLY_LOST_TIME) {
- rc_status = RC_REALLY_LOST;
+ if (radio_control.time_since_last_frame >= RC_REALLY_LOST_TIME) {
+ radio_control.status = RC_REALLY_LOST;
} else {
- if (time_since_last_ppm >= RC_LOST_TIME) {
- rc_status = RC_LOST;
- radio_ok_cpt = RC_OK_CPT;
+ if (radio_control.time_since_last_frame >= RC_LOST_TIME) {
+ radio_control.status = RC_LOST;
+ radio_control.radio_ok_cpt = RC_OK_CPT;
}
- time_since_last_ppm++;
+ radio_control.time_since_last_frame++;
}
-#if defined RC_LED
- if (rc_status == RC_OK) {
- LED_ON(RC_LED);
+#if defined RADIO_CONTROL_LED
+ if (radio_control.status == RC_OK) {
+ LED_ON(RADIO_CONTROL_LED);
}
else {
- LED_OFF(RC_LED);
+ LED_OFF(RADIO_CONTROL_LED);
}
#endif
}
/********** EVENT ************************************************************/
-static inline void radio_control_event_task ( void ) {
- ppm_cpt++;
- time_since_last_ppm = 0;
+// Implemented in radio_control/*.h
- /* Wait for enough valid frame to switch back to RC_OK */
- if (radio_ok_cpt > 0) radio_ok_cpt--;
- else {
- rc_status = RC_OK;
- /** From ppm values to normalised rc_values */
- NormalizePpm();
- }
-}
#endif /* RADIO_CONTROL */
Modified: paparazzi3/trunk/sw/airborne/rc_settings.h
===================================================================
--- paparazzi3/trunk/sw/airborne/rc_settings.h 2010-10-17 20:12:05 UTC (rev
6163)
+++ paparazzi3/trunk/sw/airborne/rc_settings.h 2010-10-18 14:27:50 UTC (rev
6164)
@@ -37,6 +37,8 @@
#ifndef RC_SETTINGS_H
+#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
+
#include "std.h"
#include "radio.h"
@@ -44,8 +46,6 @@
#define RC_SETTINGS_MODE_DOWN 1
#define RC_SETTINGS_MODE_UP 2
-#if defined RADIO_CALIB && defined RADIO_CONTROL_SETTINGS
-
#define RcSettingsOff() (rc_settings_mode==RC_SETTINGS_MODE_NONE)
#define RC_SETTINGS_MODE_OF_PULSE(pprz) (pprz < TRESHOLD1 ?
RC_SETTINGS_MODE_DOWN : \
Modified: paparazzi3/trunk/sw/tools/gen_radio.ml
===================================================================
--- paparazzi3/trunk/sw/tools/gen_radio.ml 2010-10-17 20:12:05 UTC (rev
6163)
+++ paparazzi3/trunk/sw/tools/gen_radio.ml 2010-10-18 14:27:50 UTC (rev
6164)
@@ -74,19 +74,19 @@
else
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";
+let gen_normalize_ppm_fir = fun channels ->
+ printf "#define NormalizePpmFIR(_ppm, _rc) {\\\n";
printf " static uint8_t avg_cpt = 0; /* Counter for averaging */\\\n";
printf " int16_t tmp_radio;\\\n";
List.iter
(fun c ->
let value, min_pprz = norm1_ppm c in
if c.averaged then begin
- printf " avg_rc_values[RADIO_%s] += ppm_pulses[RADIO_%s];\\\n" c.name
c.name
+ printf " _rc.avg_values[RADIO_%s] += _ppm[RADIO_%s];\\\n" c.name
c.name
end else begin
- printf " tmp_radio = ppm_pulses[RADIO_%s] -
RC_PPM_TICS_OF_USEC(%d);\\\n" c.name c.neutral;
- printf " rc_values[RADIO_%s] = %s;\\\n" c.name value;
- printf " Bound(rc_values[RADIO_%s], %s, MAX_PPRZ); \\\n\\\n" c.name
min_pprz;
+ printf " tmp_radio = _ppm[RADIO_%s] - RC_PPM_TICS_OF_USEC(%d);\\\n"
c.name c.neutral;
+ printf " _rc.values[RADIO_%s] = %s;\\\n" c.name value;
+ printf " Bound(_rc.values[RADIO_%s], %s, MAX_PPRZ); \\\n\\\n" c.name
min_pprz;
end
)
channels;
@@ -97,14 +97,13 @@
(fun c ->
if c.averaged then begin
let value, min_pprz = norm1_ppm c in
- printf " tmp_radio = avg_rc_values[RADIO_%s] / RC_AVG_PERIOD -
RC_PPM_TICS_OF_USEC(%d);\\\n" c.name c.neutral;
- printf " rc_values[RADIO_%s] = %s;\\\n" c.name value;
- printf " avg_rc_values[RADIO_%s] = 0;\\\n" c.name;
- printf " Bound(rc_values[RADIO_%s], %s, MAX_PPRZ); \\\n\\\n" c.name
min_pprz;
+ printf " tmp_radio = _rc.avg_values[RADIO_%s] / RC_AVG_PERIOD -
RC_PPM_TICS_OF_USEC(%d);\\\n" c.name c.neutral;
+ printf " _rc.values[RADIO_%s] = %s;\\\n" c.name value;
+ printf " _rc.avg_values[RADIO_%s] = 0;\\\n" c.name;
+ printf " Bound(_rc.values[RADIO_%s], %s, MAX_PPRZ); \\\n\\\n"
c.name min_pprz;
end
)
channels;
- printf " rc_values_contains_avg_channels = TRUE;\\\n";
printf " }\\\n";
printf "}\n"
@@ -114,8 +113,8 @@
else
sprintf "(tmp_radio >=0 ? (tmp_radio * MAX_PPRZ) /
(RC_PPM_SIGNED_TICS_OF_USEC(%d-%d)) : (tmp_radio * MIN_PPRZ) /
(RC_PPM_SIGNED_TICS_OF_USEC(%d-%d)))" c.max c.neutral c.min c.neutral,
"MIN_PPRZ"
-let gen_normalize_ppm2 = fun channels ->
- printf "#define NormalizePpm2(_ppm, _rc) {\\\n";
+let gen_normalize_ppm_iir = fun channels ->
+ printf "#define NormalizePpmIIR(_ppm, _rc) {\\\n";
printf " int32_t tmp_radio;\\\n";
printf " int32_t tmp_value;\\\n\\\n";
List.iter
@@ -125,12 +124,12 @@
printf " tmp_value = %s;\\\n" value;
printf " Bound(tmp_value, %s, MAX_PPRZ); \\\n" min_pprz;
if c.averaged then
- printf " _rc[RADIO_%s] = (pprz_t)((RADIO_FILTER * _rc[RADIO_%s] +
tmp_value) / (RADIO_FILTER + 1));\\\n\\\n" c.name c.name
+ printf " _rc.values[RADIO_%s] = (pprz_t)((RADIO_FILTER *
_rc.values[RADIO_%s] + tmp_value) / (RADIO_FILTER + 1));\\\n\\\n" c.name c.name
else
- printf " _rc[RADIO_%s] = (pprz_t)(tmp_value);\\\n\\\n" c.name
+ printf " _rc.values[RADIO_%s] = (pprz_t)(tmp_value);\\\n\\\n" c.name
)
channels;
- printf " rc_values_contains_avg_channels = TRUE;\\\n";
+ (*printf " rc_values_contains_avg_channels = TRUE;\\\n";*)
printf "}\n"
@@ -151,6 +150,7 @@
Xml2h.warning ("RADIO MODEL: "^n);
define_string "RADIO_NAME" n;
nl ();
+ (*define "RADIO_CONTROL_NB_CHANNEL" (string_of_int (List.length channels));*)
define "RADIO_CTL_NB" (string_of_int (List.length channels));
nl ();
define "RADIO_FILTER" "7";
@@ -183,10 +183,10 @@
printf "#define PPM_SYNC_MAX_LEN (%sul)\n" ppm_sync_max;
nl ();
- gen_normalize_ppm channels_params;
+ gen_normalize_ppm_fir channels_params;
nl ();
-
- gen_normalize_ppm2 channels_params;
+ gen_normalize_ppm_iir channels_params;
+ nl ();
printf "\n#endif // %s\n" h_name
| [Prev in Thread] |
Current Thread |
[Next in Thread] |
- [paparazzi-commits] [6164] New radio control system,
Gautier Hattenberger <=