#define CHANNELS 8 //set the number of chanels
#define CENTER_VAL 1500 //set the default servo value
#define PPM_FRLEN 22500 //set the PPM frame length in microseconds (1ms = 1000µs)
#define PPM_PULSELEN 300 //set the pulse length
#define ONSTATE 0 //set polarity of the pulses: 1 is positive, 0 is negative
#define SIGPIN 9 //set PPM signal output pin on the Arduino
int ppm_val[CHANNELS];
int input_pins[CHANNELS] = {14,15,16,17,2,3,4,5};
void setup(){
for (int i = 0; i < CHANNELS; ++i) {
if (i < 4) {
pinMode(input_pins[i], INPUT);
ppm_val[i] = map(analogRead(input_pins[i]), 0, 1024, 1000, 2000);
} else {
pinMode(input_pins[i], INPUT_PULLUP);
ppm_val[i] = map(digitalRead(input_pins[i]), 0, 1, 1000, 2000);
}
if(i>5) ppm_val[i]=CENTER_VAL;
}
pinMode(SIGPIN, OUTPUT);
digitalWrite(SIGPIN, !ONSTATE);
cli();
TCCR1A = 0; // set entire TCCR1 register to 0
TCCR1B = 0;
OCR1A = 100; // compare match register
TCCR1B |= (1 << WGM12); // turn on CTC mode
TCCR1B |= (1 << CS11); // 8 prescaler: 0,5 microseconds at 16mhz
TIMSK1 |= (1 << OCIE1A); // enable timer compare interrupt
sei();
}
void loop(){
for (int i = 0; i < CHANNELS; ++i) {
if (i < 4) {
ppm_val[i] = map(analogRead(input_pins[i]), 0, 1024, 1000, 2000);
} else {
ppm_val[i] = map(digitalRead(input_pins[i]), 0, 1, 1000, 2000);
}
if(i>5) ppm_val[i]=CENTER_VAL;
}
}
ISR(TIMER1_COMPA_vect){
static boolean bState = true;
TCNT1 = 0;
if(bState) {
digitalWrite(SIGPIN, ONSTATE);
OCR1A = PPM_PULSELEN * 2;
bState = false;
} else {
static byte bCurChan;
static unsigned int iPPMRest;
digitalWrite(SIGPIN, !ONSTATE);
bState = true;
if(bCurChan >= CHANNELS){
bCurChan = 0;
iPPMRest = iPPMRest + PPM_PULSELEN;//
OCR1A = (PPM_FRLEN - iPPMRest) * 2;
iPPMRest = 0;
} else {
OCR1A = (ppm_val[bCurChan] - PPM_PULSELEN) * 2;
iPPMRest = iPPMRest + ppm_val[bCurChan];
bCurChan++;
}
}
}