diff --git a/sw/airborne/arm7/servos_ppm_hw.c b/sw/airborne/arm7/servos_ppm_hw.c new file mode 100644 index 0000000000..27ef34dee6 --- /dev/null +++ b/sw/airborne/arm7/servos_ppm_hw.c @@ -0,0 +1,61 @@ +/* + * $Id$ + * + * Copyright (C) 2008 Mark Griffin + * + * 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 servos_ppm_out.c + * \Efficient driving of MAT0.1 (SERVO_CLOCK_PIN) using TIMER0 to produce PPM + * \ for a R/C receiver which has a microcontroller to drive the servos + * \(not a 4015 or 4017 decade counter chip). + */ +#include "actuators.h" +#include "paparazzi.h" +#include "airframe.h" + +uint8_t servos_PPM_idx; +uint8_t ppm_pulse; +uint32_t servos_delay = SERVO_REFRESH_TICS; + +#define START_TIMEOUT 0xFFFF; + +void actuators_init ( void ) { + /* select ppm output pin as MAT0.1 output */ + SERVO_CLOCK_PINSEL |= SERVO_CLOCK_PINSEL_VAL << SERVO_CLOCK_PINSEL_BIT; + + /* enable match 1 interrupt */ + T0MCR |= TMCR_MR1_I; + + /* lower clock */ + T0EMR &= ~TEMR_EM1; + /* set high on match 1 */ + T0EMR |= TEMR_EMC11; + + /* set first pulse in a while */ + T0MR1 = START_TIMEOUT; + servos_PPM_idx = _PPM_NB_CHANNELS; + /* Set all servos to their midpoints */ + /* compulsory for unused servos */ + uint8_t i; + for( i=0 ; i < _PPM_NB_CHANNELS ; i++ ) + servos_values[i] = SERVOS_TICS_OF_USEC(1500); +} +uint16_t servos_values[_PPM_NB_CHANNELS]; + diff --git a/sw/airborne/arm7/servos_ppm_hw.h b/sw/airborne/arm7/servos_ppm_hw.h new file mode 100644 index 0000000000..427633b907 --- /dev/null +++ b/sw/airborne/arm7/servos_ppm_hw.h @@ -0,0 +1,84 @@ +/* + * $Id$ + * + * Copyright (C) 2008 Mark Griffin + * + * 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 servos_ppm_out.h + * \Efficient driving of the MAT0.1 pin (SERVO_CLOCK_PIN) using TIMER0 to produce + * \PPM for a R/C receiver which has a microcontroller to drive the servos + * \(not a 4015 or 4017 decade counter chip). + */ +#ifndef SERVOS_PPM_HW_H +#define SERVOS_PPM_HW_H + +#include "std.h" +#include "LPC21xx.h" +#include "sys_time.h" + +#include CONFIG + +#define SERVOS_TICS_OF_USEC(s) SYS_TICS_OF_USEC(s) +#define ChopServo(x,a,b) Chop(x, a, b) + +#define _PPM_NB_CHANNELS 8 +extern uint16_t servos_values[_PPM_NB_CHANNELS]; +#define Actuator(i) servos_values[i] + +#define ActuatorsCommit() {} +extern uint8_t servos_PPM_idx; +extern uint32_t servos_delay; +extern uint8_t ppm_pulse; /* 1=start of pulse, 0=end of pulse */ + +/* define the ppm frame rate. 22msec is typical for JR/Graupner */ +#define SERVO_REFRESH_TICS SERVOS_TICS_OF_USEC(22000) +/* define the ppm pulse width. 550usec is typical for JR/Graupner */ +#define PPM_WIDTH SERVOS_TICS_OF_USEC(550) + +#define ServosPPMMat_ISR() { \ + if (servos_PPM_idx == 0) { \ + servos_delay = SERVO_REFRESH_TICS; \ + } \ + if (servos_PPM_idx < _PPM_NB_CHANNELS ) { \ + if (ppm_pulse) { \ + T0MR1 += PPM_WIDTH; \ + servos_delay -= PPM_WIDTH; \ + } else { \ + T0MR1 += servos_values[servos_PPM_idx] - PPM_WIDTH; \ + servos_delay -= servos_values[servos_PPM_idx] - PPM_WIDTH; \ + servos_PPM_idx++; \ + } \ + } else { /* servos_PPM_idx=_PPM_NB_CHANNELS */ \ + if (ppm_pulse) { \ + T0MR1 += PPM_WIDTH; \ + servos_delay -= PPM_WIDTH; \ + } else { \ + servos_PPM_idx = 0; \ + T0MR1 += servos_delay - PPM_WIDTH; \ + } \ + } \ + if (!ppm_pulse) { \ + /* lower clock pin */ \ + T0EMR &= ~TEMR_EM1; \ + } \ + /* toggle ppm_pulse flag */ \ + ppm_pulse ^= 1; \ +} +#endif /* SERVOS_PPM_HW_H */ diff --git a/sw/airborne/arm7/sys_time_hw.c b/sw/airborne/arm7/sys_time_hw.c index b74acf6cb1..37433107c6 100644 --- a/sw/airborne/arm7/sys_time_hw.c +++ b/sw/airborne/arm7/sys_time_hw.c @@ -52,6 +52,13 @@ void TIMER0_ISR ( void ) { T0IR = TIR_MR1I; } #endif +#ifdef SERVOS_PPM_MAT + if (T0IR&TIR_MR1I) { + ServosPPMMat_ISR(); + /* clear interrupt */ + T0IR = TIR_MR1I; + } +#endif #ifdef MB_SCALE if (T0IR&TIR_CR3I) { MB_SCALE_ICP_ISR();