Paparazzi UAS  v5.0.5_stable-7-g4b8bbb7
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Modules Pages
servos_4015_hw_new.c
Go to the documentation of this file.
1 /*
2  * Copyright (C) 2006 Antoine Drouin
3  *
4  * This file is part of paparazzi.
5  *
6  * paparazzi is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2, or (at your option)
9  * any later version.
10  *
11  * paparazzi is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with paparazzi; see the file COPYING. If not, write to
18  * the Free Software Foundation, 59 Temple Place - Suite 330,
19  * Boston, MA 02111-1307, USA.
20  *
21  */
22 #include "subsystems/actuators.h"
23 #include "armVIC.h"
24 
25 #include "generated/airframe.h"
26 #include "mcu_periph/sys_time.h"
27 
28 
30 
31 #define PWMMR_SERV0 PWMMR5
32 #define PWMMR_SERV1 PWMMR2
33 #define PWMLER_LATCH_SERV0 PWMLER_LATCH5
34 #define PWMLER_LATCH_SERV1 PWMLER_LATCH2
35 #define PWMMCR_MRI_SERV0 PWMMCR_MR5I
36 #define PWMMCR_MRI_SERV1 PWMMCR_MR2I
37 #define PWMPCR_ENA_SERV0 PWMPCR_ENA5
38 #define PWMPCR_ENA_SERV1 PWMPCR_ENA2
39 #define PWMIR_MRI_SERV0 PWMIR_MR5I
40 #define PWMIR_MRI_SERV1 PWMIR_MR2I
41 
42 #ifndef PWM_VIC_SLOT
43 #define PWM_VIC_SLOT 3
44 #endif
45 
46 void actuators_4015_init ( void ) {
47  /* PWM selected as IRQ */
49  /* PWM interrupt enabled */
52  /* address of the ISR */
54  /* set clock, data and reset pins as output */
55  IO0DIR |= _BV(SERV0_CLOCK_PIN);
56  IO1DIR |= _BV(SERV0_DATA_PIN) | _BV(SERV0_RESET_PIN);
57  IO0DIR |= _BV(SERV1_CLOCK_PIN);
58  IO1DIR |= _BV(SERV1_DATA_PIN) | _BV(SERV1_RESET_PIN);
59  /* Configure the two servo clocks as PWM */
60  SERV0_CLOCK_PINSEL |= SERV0_CLOCK_PINSEL_VAL << SERV0_CLOCK_PINSEL_BIT;
61  SERV1_CLOCK_PINSEL |= SERV1_CLOCK_PINSEL_VAL << SERV1_CLOCK_PINSEL_BIT;
62 
63  /* set first pulse to be very long */
64  PWMMR0 = 0XFFFFFF;
65  PWMMR_SERV1 = 0XFFF;
66  PWMMR_SERV0 = 0X0;
67  /* commit above changes */
69  /* enable interrupt on serv1 PWM match */
71  /* enable PWM ouptputs */
73 
74  /* Prescaler */
75  PWMPR = PWM_PRESCALER-1;
76 
77  /* enable PWM timer counter and PWM mode */
79  /* Load failsafe values */
80  /* Set all servos at their midpoints */
81  /* compulsory for unaffected servos */
82  uint8_t i;
83  for( i=0 ; i < _4015_NB_CHANNELS ; i++ )
85 #ifdef SERVO_GAZ
86  servos_values[SERVO_GAZ] = SERVOS_TICS_OF_USEC(SERVO_GAZ_NEUTRAL);
87 #endif
88 }
89 
90 
91 
92 #define SERVO_REFRESH_TICS SERVOS_TICS_OF_USEC(25000)
93 
94 
95 static uint8_t servos_idx = 0;
97 
98 void PWM_ISR ( void ) {
99  ISR_ENTRY();
100  if (servos_idx == 0) {
101  /* lower serv0 reset */
102  IO1CLR = _BV(SERV0_RESET_PIN);
103  /* raise serv0 data */
104  IO1SET = _BV(SERV0_DATA_PIN);
105  /* start pulsing serv0 */
106  PWMMR_SERV0 = 0XFFF;
107  /* stop pulsing serv1 */
108  PWMMR_SERV1 = 0X0;
112  /* enable serv0 match interrupt, disable serv1 match interrupt */
114  servos_idx++;
115  }
116  else if (servos_idx < 4) {
117  /* lower serv0 data */
118  IO1CLR = _BV(SERV0_DATA_PIN);
122  servos_idx++;
123  }
124  else if (servos_idx == 4) {
125  /* raise serv0 reset */
126  IO1SET = _BV(SERV1_RESET_PIN);
127  /* lower serv1 reset */
128  IO1CLR = _BV(SERV1_RESET_PIN);
129  /* raise serv1 data */
130  IO1SET = _BV(SERV1_DATA_PIN);
131  /* stop pulsing serv0 */
132  PWMMR_SERV0 = 0;
133  /* start pulsing serv1 */
134  PWMMR_SERV1 = 0XFFF;
135  /* disable serv0 interrupt, enable serv1 match interrupt */
137  /* fill next servo value */
140  /* latch PWM values */
142  servos_idx++;
143  }
144  else if (servos_idx < _4015_NB_CHANNELS) {
145  /* clear serv1 data */
146  IO1CLR = _BV(SERV1_DATA_PIN);
150  servos_idx++;
151  }
152  else {
153  /* raise serv1 reset */
154  IO1SET = _BV(SERV1_RESET_PIN);
155  /* command the delay */
158  servos_idx = 0;
159  }
160  /* clear the interrupt */
163  VICVectAddr = 0x00000000;
164  ISR_EXIT();
165 }
#define VICIntSelect
Definition: LPC21xx.h:430
unsigned short uint16_t
Definition: types.h:16
#define IO1SET
Definition: LPC21xx.h:338
uint16_t servos_values[_4015_NB_CHANNELS]
#define PWMMR_SERV0
#define SERVOS_TICS_OF_USEC(s)
#define PWMIR
Definition: LPC21xx.h:94
static uint8_t servos_idx
#define SERVO_REFRESH_TICS
#define PWMMR0
Definition: LPC21xx.h:100
#define _VIC_CNTL(idx)
Definition: armVIC.h:19
#define PWMTCR
Definition: LPC21xx.h:95
#define PWMPCR
Definition: LPC21xx.h:107
#define PWMLER_LATCH_SERV1
void actuators_4015_init(void)
#define VIC_PWM
Definition: lpcVIC.h:76
#define PWMMCR_MRI_SERV1
#define _VIC_ADDR(idx)
Definition: armVIC.h:20
#define PWMTCR_COUNTER_ENABLE
Definition: lpcTMR.h:83
#define _4015_NB_CHANNELS
#define PWMMCR
Definition: LPC21xx.h:99
#define PWMIR_MRI_SERV0
void PWM_ISR(void)
#define PWMIR_MRI_SERV1
Architecture independent timing functions.
#define PWMLER
Definition: LPC21xx.h:108
#define VICVectAddr
Definition: LPC21xx.h:436
unsigned long uint32_t
Definition: types.h:18
#define PWMMCR_MR0R
Definition: lpcTMR.h:89
#define PWM_VIC_SLOT
Hardware independent API for actuators (servos, motor controllers).
#define PWMMCR_MRI_SERV0
#define VIC_BIT(chan)
Definition: lpcVIC.h:105
#define IO0DIR
Definition: LPC21xx.h:335
#define PWMPR
Definition: LPC21xx.h:97
#define PWMTCR_PWM_ENABLE
Definition: lpcTMR.h:85
unsigned char uint8_t
Definition: types.h:14
#define PWMLER_LATCH0
Definition: lpcTMR.h:124
#define ISR_EXIT()
Definition: armVIC.h:61
#define PWMPCR_ENA_SERV0
#define VICIntEnable
Definition: LPC21xx.h:431
static uint32_t servos_delay
#define IO1CLR
Definition: LPC21xx.h:340
#define PWMLER_LATCH_SERV0
#define PWMPCR_ENA_SERV1
#define ISR_ENTRY()
Definition: armVIC.h:40
#define VIC_ENABLE
Definition: lpcVIC.h:102
#define IO1DIR
Definition: LPC21xx.h:339
#define PWMMR_SERV1