Paparazzi UAS
v5.18.0_stable
Paparazzi is a free software Unmanned Aircraft System.
Main Page
Related Pages
Modules
Data Structures
Data Structures
Data Structure Index
Data Fields
All
_
a
b
c
d
e
f
g
h
i
j
k
l
m
n
o
p
q
r
s
t
u
v
w
x
y
z
Functions
Variables
_
a
b
c
d
e
f
g
h
i
j
k
l
m
n
o
p
q
r
s
t
u
v
w
x
y
z
Files
File List
Globals
All
_
a
b
c
d
e
f
g
h
i
j
k
l
m
n
o
p
q
r
s
t
u
v
w
x
y
z
Functions
_
a
b
c
d
e
f
g
h
i
j
k
l
m
n
o
p
q
r
s
t
u
v
w
x
y
Variables
_
a
b
c
d
e
f
g
h
i
j
k
l
m
n
o
p
q
r
s
t
u
v
w
x
y
z
Typedefs
a
b
c
d
e
f
g
h
i
j
m
n
p
r
s
t
u
v
w
Enumerations
a
b
c
d
e
f
g
h
i
j
l
m
n
o
p
r
s
t
u
v
w
z
Enumerator
a
b
c
d
e
f
g
h
i
j
l
m
n
o
p
r
s
t
u
v
w
x
y
z
Macros
_
a
b
c
d
e
f
g
h
i
j
k
l
m
n
o
p
q
r
s
t
u
v
w
x
y
z
•
All
Data Structures
Namespaces
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
29
uint16_t
servos_values
[
_4015_NB_CHANNELS
];
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
{
48
/* PWM selected as IRQ */
49
VICIntSelect
&= ~
VIC_BIT
(
VIC_PWM
);
50
/* PWM interrupt enabled */
51
VICIntEnable
=
VIC_BIT
(
VIC_PWM
);
52
_VIC_CNTL
(
PWM_VIC_SLOT
) =
VIC_ENABLE
|
VIC_PWM
;
53
/* address of the ISR */
54
_VIC_ADDR
(
PWM_VIC_SLOT
) = (
uint32_t
)
PWM_ISR
;
55
/* set clock, data and reset pins as output */
56
IO0DIR
|= _BV(SERV0_CLOCK_PIN);
57
IO1DIR
|= _BV(SERV0_DATA_PIN) | _BV(SERV0_RESET_PIN);
58
IO0DIR
|= _BV(SERV1_CLOCK_PIN);
59
IO1DIR
|= _BV(SERV1_DATA_PIN) | _BV(SERV1_RESET_PIN);
60
/* Configure the two servo clocks as PWM */
61
SERV0_CLOCK_PINSEL |= SERV0_CLOCK_PINSEL_VAL << SERV0_CLOCK_PINSEL_BIT;
62
SERV1_CLOCK_PINSEL |= SERV1_CLOCK_PINSEL_VAL << SERV1_CLOCK_PINSEL_BIT;
63
64
/* set first pulse to be very long */
65
PWMMR0
= 0XFFFFFF;
66
PWMMR_SERV1
= 0XFFF;
67
PWMMR_SERV0
= 0X0;
68
/* commit above changes */
69
PWMLER
=
PWMLER_LATCH0
|
PWMLER_LATCH_SERV0
|
PWMLER_LATCH_SERV1
;
70
/* enable interrupt on serv1 PWM match */
71
PWMMCR
=
PWMMCR_MR0R
|
PWMMCR_MRI_SERV1
;
72
/* enable PWM ouptputs */
73
PWMPCR
=
PWMPCR_ENA_SERV0
|
PWMPCR_ENA_SERV1
;
74
75
/* Prescaler */
76
PWMPR
= PWM_PRESCALER - 1;
77
78
/* enable PWM timer counter and PWM mode */
79
PWMTCR
=
PWMTCR_COUNTER_ENABLE
|
PWMTCR_PWM_ENABLE
;
80
/* Load failsafe values */
81
/* Set all servos at their midpoints */
82
/* compulsory for unaffected servos */
83
uint8_t
i;
84
for
(i = 0 ; i <
_4015_NB_CHANNELS
; i++) {
85
servos_values
[i] =
SERVOS_TICS_OF_USEC
(1500);
86
}
87
#ifdef SERVO_MOTOR
88
servos_values
[SERVO_MOTOR] =
SERVOS_TICS_OF_USEC
(SERVO_MOTOR_NEUTRAL);
89
#endif
90
}
91
92
93
94
#define SERVO_REFRESH_TICS SERVOS_TICS_OF_USEC(25000)
95
96
97
static
uint8_t
servos_idx
= 0;
98
static
uint32_t
servos_delay
;
99
100
void
PWM_ISR
(
void
)
101
{
102
ISR_ENTRY
();
103
if
(
servos_idx
== 0) {
104
/* lower serv0 reset */
105
IO1CLR
= _BV(SERV0_RESET_PIN);
106
/* raise serv0 data */
107
IO1SET
= _BV(SERV0_DATA_PIN);
108
/* start pulsing serv0 */
109
PWMMR_SERV0
= 0XFFF;
110
/* stop pulsing serv1 */
111
PWMMR_SERV1
= 0X0;
112
PWMMR0
=
servos_values
[
servos_idx
];
113
servos_delay
=
SERVO_REFRESH_TICS
-
servos_values
[
servos_idx
];
114
PWMLER
=
PWMLER_LATCH0
|
PWMLER_LATCH_SERV0
|
PWMLER_LATCH_SERV1
;
115
/* enable serv0 match interrupt, disable serv1 match interrupt */
116
PWMMCR
=
PWMMCR_MR0R
|
PWMMCR_MRI_SERV0
;
117
servos_idx
++;
118
}
else
if
(
servos_idx
< 4) {
119
/* lower serv0 data */
120
IO1CLR
= _BV(SERV0_DATA_PIN);
121
PWMMR0
=
servos_values
[
servos_idx
];
122
servos_delay
-=
servos_values
[
servos_idx
];
123
PWMLER
=
PWMLER_LATCH0
;
124
servos_idx
++;
125
}
else
if
(
servos_idx
== 4) {
126
/* raise serv0 reset */
127
IO1SET
= _BV(SERV1_RESET_PIN);
128
/* lower serv1 reset */
129
IO1CLR
= _BV(SERV1_RESET_PIN);
130
/* raise serv1 data */
131
IO1SET
= _BV(SERV1_DATA_PIN);
132
/* stop pulsing serv0 */
133
PWMMR_SERV0
= 0;
134
/* start pulsing serv1 */
135
PWMMR_SERV1
= 0XFFF;
136
/* disable serv0 interrupt, enable serv1 match interrupt */
137
PWMMCR
=
PWMMCR_MR0R
|
PWMMCR_MRI_SERV1
;
138
/* fill next servo value */
139
PWMMR0
=
servos_values
[
servos_idx
];
140
servos_delay
-=
servos_values
[
servos_idx
];
141
/* latch PWM values */
142
PWMLER
=
PWMLER_LATCH0
|
PWMLER_LATCH_SERV0
|
PWMLER_LATCH_SERV1
;
143
servos_idx
++;
144
}
else
if
(
servos_idx
<
_4015_NB_CHANNELS
) {
145
/* clear serv1 data */
146
IO1CLR
= _BV(SERV1_DATA_PIN);
147
PWMMR0
=
servos_values
[
servos_idx
];
148
servos_delay
-=
servos_values
[
servos_idx
];
149
PWMLER
=
PWMLER_LATCH0
;
150
servos_idx
++;
151
}
else
{
152
/* raise serv1 reset */
153
IO1SET
= _BV(SERV1_RESET_PIN);
154
/* command the delay */
155
PWMMR0
=
servos_delay
;
156
PWMLER
=
PWMLER_LATCH0
;
157
servos_idx
= 0;
158
}
159
/* clear the interrupt */
160
PWMIR
=
PWMIR_MRI_SERV1
;
161
PWMIR
=
PWMIR_MRI_SERV0
;
162
VICVectAddr
= 0x00000000;
163
ISR_EXIT
();
164
}
uint16_t
unsigned short uint16_t
Definition:
types.h:16
VIC_PWM
#define VIC_PWM
Definition:
lpcVIC.h:76
PWMPCR_ENA_SERV0
#define PWMPCR_ENA_SERV0
Definition:
servos_4015_hw_new.c:37
PWMTCR
#define PWMTCR
Definition:
LPC21xx.h:95
PWMPCR
#define PWMPCR
Definition:
LPC21xx.h:107
servos_delay
static uint32_t servos_delay
Definition:
servos_4015_hw_new.c:98
PWM_ISR
void PWM_ISR(void)
Definition:
servos_4015_hw_new.c:100
PWMMCR_MRI_SERV0
#define PWMMCR_MRI_SERV0
Definition:
servos_4015_hw_new.c:35
armVIC.h
VICIntSelect
#define VICIntSelect
Definition:
LPC21xx.h:430
PWMLER_LATCH0
#define PWMLER_LATCH0
Definition:
lpcTMR.h:124
PWMMR0
#define PWMMR0
Definition:
LPC21xx.h:100
uint32_t
unsigned long uint32_t
Definition:
types.h:18
servos_values
uint16_t servos_values[_4015_NB_CHANNELS]
Definition:
servos_4015_hw_new.c:29
PWMIR
#define PWMIR
Definition:
LPC21xx.h:94
_4015_NB_CHANNELS
#define _4015_NB_CHANNELS
Definition:
servos_4015_hw.h:36
servos_idx
static uint8_t servos_idx
Definition:
servos_4015_hw_new.c:97
_VIC_ADDR
#define _VIC_ADDR(idx)
Definition:
armVIC.h:20
VIC_BIT
#define VIC_BIT(chan)
Definition:
lpcVIC.h:105
PWMMCR_MR0R
#define PWMMCR_MR0R
Definition:
lpcTMR.h:89
PWM_VIC_SLOT
#define PWM_VIC_SLOT
Definition:
servos_4015_hw_new.c:43
PWMLER
#define PWMLER
Definition:
LPC21xx.h:108
PWMMCR
#define PWMMCR
Definition:
LPC21xx.h:99
PWMIR_MRI_SERV0
#define PWMIR_MRI_SERV0
Definition:
servos_4015_hw_new.c:39
sys_time.h
Architecture independent timing functions.
SERVOS_TICS_OF_USEC
#define SERVOS_TICS_OF_USEC(s)
Definition:
actuators_pwm_arch.h:41
uint8_t
unsigned char uint8_t
Definition:
types.h:14
VICVectAddr
#define VICVectAddr
Definition:
LPC21xx.h:436
IO0DIR
#define IO0DIR
Definition:
LPC21xx.h:335
actuators_4015_init
void actuators_4015_init(void)
Definition:
servos_4015_hw_new.c:46
PWMLER_LATCH_SERV0
#define PWMLER_LATCH_SERV0
Definition:
servos_4015_hw_new.c:33
PWMLER_LATCH_SERV1
#define PWMLER_LATCH_SERV1
Definition:
servos_4015_hw_new.c:34
ISR_ENTRY
#define ISR_ENTRY()
Definition:
armVIC.h:40
PWMMR_SERV1
#define PWMMR_SERV1
Definition:
servos_4015_hw_new.c:32
ISR_EXIT
#define ISR_EXIT()
Definition:
armVIC.h:61
PWMTCR_COUNTER_ENABLE
#define PWMTCR_COUNTER_ENABLE
Definition:
lpcTMR.h:83
PWMPCR_ENA_SERV1
#define PWMPCR_ENA_SERV1
Definition:
servos_4015_hw_new.c:38
IO1CLR
#define IO1CLR
Definition:
LPC21xx.h:340
PWMPR
#define PWMPR
Definition:
LPC21xx.h:97
PWMMR_SERV0
#define PWMMR_SERV0
Definition:
servos_4015_hw_new.c:31
PWMMCR_MRI_SERV1
#define PWMMCR_MRI_SERV1
Definition:
servos_4015_hw_new.c:36
VIC_ENABLE
#define VIC_ENABLE
Definition:
lpcVIC.h:102
VICIntEnable
#define VICIntEnable
Definition:
LPC21xx.h:431
IO1SET
#define IO1SET
Definition:
LPC21xx.h:338
PWMIR_MRI_SERV1
#define PWMIR_MRI_SERV1
Definition:
servos_4015_hw_new.c:40
_VIC_CNTL
#define _VIC_CNTL(idx)
Definition:
armVIC.h:19
IO1DIR
#define IO1DIR
Definition:
LPC21xx.h:339
SERVO_REFRESH_TICS
#define SERVO_REFRESH_TICS
Definition:
servos_4015_hw_new.c:94
PWMTCR_PWM_ENABLE
#define PWMTCR_PWM_ENABLE
Definition:
lpcTMR.h:85
actuators.h
sw
airborne
arch
lpc21
subsystems
actuators
servos_4015_hw_new.c
Generated on Tue Feb 1 2022 13:51:13 for Paparazzi UAS by
1.8.17