Paparazzi UAS  v4.2.2_stable-4-gcc32f65
Paparazzi is a free software Unmanned Aircraft System.
 All Data Structures Files Functions Variables Typedefs Enumerations Enumerator Macros Pages
servos_4015_hw_new.c
Go to the documentation of this file.
1 #include "actuators.h"
2 #include "armVIC.h"
3 
4 #include "generated/airframe.h"
5 #include "mcu_periph/sys_time.h"
6 
7 
9 
10 #define PWMMR_SERV0 PWMMR5
11 #define PWMMR_SERV1 PWMMR2
12 #define PWMLER_LATCH_SERV0 PWMLER_LATCH5
13 #define PWMLER_LATCH_SERV1 PWMLER_LATCH2
14 #define PWMMCR_MRI_SERV0 PWMMCR_MR5I
15 #define PWMMCR_MRI_SERV1 PWMMCR_MR2I
16 #define PWMPCR_ENA_SERV0 PWMPCR_ENA5
17 #define PWMPCR_ENA_SERV1 PWMPCR_ENA2
18 #define PWMIR_MRI_SERV0 PWMIR_MR5I
19 #define PWMIR_MRI_SERV1 PWMIR_MR2I
20 
21 
22 void actuators_init ( void ) {
23  /* PWM selected as IRQ */
25  /* PWM interrupt enabled */
28  /* address of the ISR */
30  /* set clock, data and reset pins as output */
31  IO0DIR |= _BV(SERV0_CLOCK_PIN);
32  IO1DIR |= _BV(SERV0_DATA_PIN) | _BV(SERV0_RESET_PIN);
33  IO0DIR |= _BV(SERV1_CLOCK_PIN);
34  IO1DIR |= _BV(SERV1_DATA_PIN) | _BV(SERV1_RESET_PIN);
35  /* Configure the two servo clocks as PWM */
38 
39  /* set first pulse to be very long */
40  PWMMR0 = 0XFFFFFF;
41  PWMMR_SERV1 = 0XFFF;
42  PWMMR_SERV0 = 0X0;
43  /* commit above changes */
45  /* enable interrupt on serv1 PWM match */
47  /* enable PWM ouptputs */
49 
50  /* Prescaler */
51  PWMPR = PWM_PRESCALER-1;
52 
53  /* enable PWM timer counter and PWM mode */
55  /* Load failsafe values */
56  /* Set all servos at their midpoints */
57  /* compulsory for unaffected servos */
58  uint8_t i;
59  for( i=0 ; i < _4015_NB_CHANNELS ; i++ )
61 #ifdef SERVO_GAZ
62  servos_values[SERVO_GAZ] = SERVOS_TICS_OF_USEC(SERVO_GAZ_NEUTRAL);
63 #endif
64 }
65 
66 
67 
68 #define SERVO_REFRESH_TICS SERVOS_TICS_OF_USEC(25000)
69 
70 
71 static uint8_t servos_idx = 0;
73 
74 void PWM_ISR ( void ) {
75  ISR_ENTRY();
76  if (servos_idx == 0) {
77  /* lower serv0 reset */
78  IO1CLR = _BV(SERV0_RESET_PIN);
79  /* raise serv0 data */
80  IO1SET = _BV(SERV0_DATA_PIN);
81  /* start pulsing serv0 */
82  PWMMR_SERV0 = 0XFFF;
83  /* stop pulsing serv1 */
84  PWMMR_SERV1 = 0X0;
88  /* enable serv0 match interrupt, disable serv1 match interrupt */
90  servos_idx++;
91  }
92  else if (servos_idx < 4) {
93  /* lower serv0 data */
94  IO1CLR = _BV(SERV0_DATA_PIN);
98  servos_idx++;
99  }
100  else if (servos_idx == 4) {
101  /* raise serv0 reset */
102  IO1SET = _BV(SERV1_RESET_PIN);
103  /* lower serv1 reset */
104  IO1CLR = _BV(SERV1_RESET_PIN);
105  /* raise serv1 data */
106  IO1SET = _BV(SERV1_DATA_PIN);
107  /* stop pulsing serv0 */
108  PWMMR_SERV0 = 0;
109  /* start pulsing serv1 */
110  PWMMR_SERV1 = 0XFFF;
111  /* disable serv0 interrupt, enable serv1 match interrupt */
113  /* fill next servo value */
116  /* latch PWM values */
118  servos_idx++;
119  }
120  else if (servos_idx < _4015_NB_CHANNELS) {
121  /* clear serv1 data */
122  IO1CLR = _BV(SERV1_DATA_PIN);
126  servos_idx++;
127  }
128  else {
129  /* raise serv1 reset */
130  IO1SET = _BV(SERV1_RESET_PIN);
131  /* command the delay */
134  servos_idx = 0;
135  }
136  /* clear the interrupt */
139  VICVectAddr = 0x00000000;
140  ISR_EXIT();
141 }
#define VICIntSelect
Definition: LPC21xx.h:398
unsigned short uint16_t
Definition: types.h:16
#define IO1SET
Definition: LPC21xx.h:306
uint16_t servos_values[_4015_NB_CHANNELS]
#define PWMMR_SERV0
#define PWMIR
Definition: LPC21xx.h:94
#define SERV0_CLOCK_PINSEL
Definition: tiny_sim.h:34
static uint8_t servos_idx
#define SERVO_REFRESH_TICS
#define PWMMR0
Definition: LPC21xx.h:100
#define VICVectAddr3
Definition: LPC21xx.h:409
#define SERV1_RESET_PIN
Definition: tiny_sim.h:50
#define PWMTCR
Definition: LPC21xx.h:95
#define PWMPCR
Definition: LPC21xx.h:107
#define SERVOS_TICS_OF_USEC(s)
#define PWMLER_LATCH_SERV1
#define SERV0_RESET_PIN
Definition: tiny_sim.h:40
#define SERV0_DATA_PIN
Definition: tiny_sim.h:38
#define VIC_PWM
Definition: lpcVIC.h:76
void actuators_init(void)
#define SERV0_CLOCK_PIN
Definition: tiny_sim.h:33
#define PWMMCR_MRI_SERV1
#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 SERV1_CLOCK_PINSEL_VAL
Definition: tiny_sim.h:45
#define PWMLER
Definition: LPC21xx.h:108
#define VICVectAddr
Definition: LPC21xx.h:404
unsigned long uint32_t
Definition: types.h:18
#define PWMMCR_MR0R
Definition: lpcTMR.h:89
#define SERV0_CLOCK_PINSEL_BIT
Definition: tiny_sim.h:36
#define PWMMCR_MRI_SERV0
#define SERV1_CLOCK_PINSEL_BIT
Definition: tiny_sim.h:46
#define VIC_BIT(chan)
Definition: lpcVIC.h:105
#define IO0DIR
Definition: LPC21xx.h:303
#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:399
#define SERV0_CLOCK_PINSEL_VAL
Definition: tiny_sim.h:35
static uint32_t servos_delay
#define SERV1_CLOCK_PINSEL
Definition: tiny_sim.h:44
#define VICVectCntl3
Definition: LPC21xx.h:425
#define IO1CLR
Definition: LPC21xx.h:308
#define PWMLER_LATCH_SERV0
#define SERV1_CLOCK_PIN
Definition: tiny_sim.h:43
#define SERV1_DATA_PIN
Definition: tiny_sim.h:48
#define PWMPCR_ENA_SERV1
#define ISR_ENTRY()
Definition: armVIC.h:40
#define VIC_ENABLE
Definition: lpcVIC.h:102
#define IO1DIR
Definition: LPC21xx.h:307
#define PWMMR_SERV1