-
Notifications
You must be signed in to change notification settings - Fork 0
/
ppm.h
54 lines (40 loc) · 1.29 KB
/
ppm.h
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
#ifndef PPM_H
#define PPM_H
#include "msp430f5529.h"
#include "settings.h"
unsigned int PPM[PPM_ARRAY_LEN];
unsigned int PPM_IND;
unsigned int rising_count;
unsigned int cctl;
int rising = 0;
int falling = 0;
int ppm_setup();
void PPM_ISR(void);
int ppm_setup(){
// configure pin
P1SEL |= BIT2; // pin 1.2 in function mode
P1DIR &= ~BIT2; // pin 1.2 in input mode to use as capture signal
P1REN |= BIT2; // enable pull resistor
P1OUT |= BIT2; // pull up
// configure timer
TA0CCTL1 = CM_3 + CCIS_0 + SCS + CAP + CCIE; // timer capture ctl, capture on both edges, uses CCI0A (P1.2), synchronize capture signal, interrupt enabled
TA0CTL = TASSEL_1 + MC_2 + ID_0 + TACLR; // timer config, continuous mode, 32768Hz, reset timer
PPM_IND = 0;
return 0;
}
#pragma vector=TIMER0_A1_VECTOR
__interrupt void PPM_ISR(void){
cctl = TA0CCTL1;
if (cctl & CCI) { // if capture signal is currently high i.e. rising edge
rising++;
rising_count = TA0CCR1;
} else { // else, falling edge
falling++;
PPM[PPM_IND] = TA0CCR1-rising_count; // compute difference and store
PPM_IND++; // increment index
PPM_IND = PPM_IND & PPM_ARRAY_MOD; // make circular
}
// need to clear interrupt flag?
TA0IV = 0x00;
}
#endif