Atmega2560 Input Capture + Servo Library Issues

Hey Guys,
I am working on programming my own UAV using one of the "old" APM boards (I believe version 2.6).
Things are going pretty ok, and I have a git repository here here(still learning git lol).

If you are unfamiliar with the hardware topology of the APM, it features an Atmega32U4 in order to encode the PWM signals sent on the RC transmitter into a PPM signal for the main mcu (ATMEGA2560).
However, I have encountered some problems making the avr input capture and servo libraries play nicely together.

An important feature of a rc UAV is the ability to switch between manual control and automatic control. In my case, I would like to be able to control the aircraft while disabling individual channels and let the autopilot tune.

In an ideal world, the input capture ISR would write to all of its corresponding servo outputs unless that channel is under autopilot control.

My problem is: I would like to use the arduino Servo library and attach and detach servos as they are enabled and disabled on from PPM control. However, when I attach servo my ISR stops recording new values.

Here is the main.cpp for the project. I am compiling using platformio and atom. In the main loop, the program checks all the ppm values, and if the switch on channel 5 crosses a certain threshold, it should disable and re-enable channel 0.

main.cpp

Here is the cpp file for library. Note that if I comment out the servo.attach() call in the APM_PPM::halt() function (line 96) everything works as expected (except of course that the autopilot can't use the servo):

APM_PPM.cpp

Here is the header file for the library:

APM_PPM.h

Thank you ahead of time and let me know what you think!!!

Sorry to bump.

I have been working on this and I am still having the same issue.
I have compiled the whole thing into 1 arduino sketch to illustrate the problem.
The PPM input connected to ICP5 on the mega2560 drives the servo outputs.
Then I want to be able to turn off PPM control of the servos (by setting ppmFlag to zero).
This all works, but the second I attach a servo in my code, the whole thing seems to hang.
Do all the servo libraries use timer 5?

#include <Servo.h>
const int numChannel = 8;
const int servoPin[8] = {12,11,8,7,6,3,2,5};

volatile unsigned int count, lastCount;
volatile uint16_t _ppmIn_[numChannel];
volatile uint8_t _ppmFlag_;
volatile uint8_t _ppmPass_;

Servo test;

void setup() {
  _ppmPass_ = 0xFF;
  for(int j = 0; j<6; j++){
    pinMode(servoPin[j], OUTPUT);
  }
  pinMode(25, OUTPUT);
  cli();  //Disable interrupts
  TCCR5A = 0x00; // Clear confiugration registers
  TCCR5B = 0x00;

  TCCR5B = ( 1 << ICES5 ) | ( 1 << CS51); // use rising edge as trigger, prescale_clk/8
  TIMSK5 = ( 1 << ICIE5 ); // allow input capture interrupts

  // Clear TIMER5
  TCNT5H = 0x00;
  TCNT5L = 0x00;
  sei();  //Enable interrupts

  count = 0;
}

void loop() {
  delay(1000);
  _ppmPass_ = 0x00;
  test.attach(12);
  test.write(0);
  test.detach();
  delay(1000);
  _ppmPass_ = 0xFF;
  delay(1000);
  
}

ISR(TIMER3_COMPA_vect){
  digitalWrite(25, !digitalRead(25));
}


ISR(TIMER5_CAPT_vect){

  //Check how many clock cycles have elapsed since last time
  uint16_t ticks;
  ticks = (unsigned int)ICR5L;
  ticks |= (unsigned int)ICR5H << 8;

  //If it has been more than 10000 ticks than the signal is valid PPM input
  if( ticks < 10000){
    //Don't write to memory outside of the array
    if(count < numChannel){
     //Update pulsewidth values
     cli(); //Halt interrupts
     _ppmIn_[count] = ticks;
     _ppmFlag_ |= (1 << count);
     sei(); //Restart interrupts

     lastCount = count; //Required to drive the servo outputs low next cycle
   }
    count ++;
  }
  //If it has been more than 10000 ticks = 5mS
  //then reset the count
  else{
    count = 0;
  }

  //Update servo outputs
  if( (_ppmPass_ & ((uint8_t)1 << count)) == (1 << count)  ){
    digitalWrite(servoPin[count],HIGH);
  }

  if( (_ppmPass_ & ((uint8_t)1 << lastCount)) == (1 << lastCount)  ){
    digitalWrite(servoPin[lastCount],LOW);
  }

  //Reset the clock count
  TCNT5H = 0x00;
  TCNT5L = 0x00;
  // clear input capture flag
  TIFR5 = ( 1 << ICF5 );
}

I'm lazy and trying to figure out what all your register settings do is very too time consuming.

Write a narrative that explains how the different parts of your program are intended to work.

...R

Makes sense to me.

Because the PPM signal comes in on ICP5, I figured the best way to capture the signal would be to configure ICP5 on TIMER5.

In the setup I configure timer5 to run at 2Mhz (which seems a little fast in retrospect), set the ICP to trigger on rising edges, and I enable the input capture interrupt.

I use the input capture interrupt to read the elapsed time between PPM edges and save the pulse widths in the array ppmIn
In addition, as the interrupt iterates through the PPM signal, it will manually set and reset servo output pins (corresponding to the array servoPin).
This makes my servo outputs tightly bound to my PPM inputs, which I am happy with.

In addition, I can disable the PPM signals control by writing a zero value into the varaible ppmPass
(i.e. ppmPass &= ~(1 << 2) would disable PPM control of the servo on servoPin[2])
When this happens the value of the channel is still saved to ppmIn but the pins are left alone.

This all works and I am very happy with it.

However here is where the problem comes in:
When I shutdown ppm control of an individual channel, I would like to be able to control channel with the arduino Servo library.
My thinking was I could attach a servo when the channel was disabled from PPM and detach when I want PPM control.
However, whenever I attach any servo on any pin, my entire ICP interrupt hangs, and ppmIn does not produce new updated values.

#include <Servo.h>
const int numChannel = 8;
const int servoPin[8] = {12,11,8,7,6,3,2,5};

volatile unsigned int count, lastCount;
volatile uint16_t _ppmIn_[numChannel];
volatile uint8_t _ppmFlag_;
volatile uint8_t _ppmPass_;

Servo test;

void setup() {
  _ppmPass_ = 0xFF;
  for(int j = 0; j<6; j++){
    pinMode(servoPin[j], OUTPUT);
  }
  pinMode(25, OUTPUT);
  cli();  //Disable interrupts
  TCCR5A = 0x00; // Clear confiugration registers
  TCCR5B = 0x00;

  TCCR5B = ( 1 << ICES5 ) | ( 1 << CS51); // use rising edge as trigger, prescale_clk/8
  TIMSK5 = ( 1 << ICIE5 ); // allow input capture interrupts

  // Clear TIMER5
  TCNT5H = 0x00;
  TCNT5L = 0x00;
  sei();  //Enable interrupts

  count = 0;
}

void loop() {
  delay(1000);
  _ppmPass_ = 0x00;
  test.attach(12);
  test.write(0);
  test.detach();
  delay(1000);
  _ppmPass_ = 0xFF;
  delay(1000);
  
}


ISR(TIMER5_CAPT_vect){

  //Check how many clock cycles have elapsed since last time
  uint16_t ticks;
  ticks = (unsigned int)ICR5L;
  ticks |= (unsigned int)ICR5H << 8;

  //If it has been more than 10000 ticks than the signal is valid PPM input
  if( ticks < 10000){
    //Don't write to memory outside of the array
    if(count < numChannel){
     //Update pulsewidth values
     cli(); //Halt interrupts
     _ppmIn_[count] = ticks;
     _ppmFlag_ |= (1 << count);
     sei(); //Restart interrupts

     lastCount = count; //Required to drive the servo outputs low next cycle
   }
    count ++;
  }
  //If it has been more than 10000 ticks = 5mS
  //then reset the count
  else{
    count = 0;
  }

  //Update servo outputs
  if( (_ppmPass_ & ((uint8_t)1 << count)) == (1 << count)  ){
    digitalWrite(servoPin[count],HIGH);
  }

  if( (_ppmPass_ & ((uint8_t)1 << lastCount)) == (1 << lastCount)  ){
    digitalWrite(servoPin[lastCount],LOW);
  }

  //Reset the clock count
  TCNT5H = 0x00;
  TCNT5L = 0x00;
  // clear input capture flag
  TIFR5 = ( 1 << ICF5 );
}

waymond91:
However here is where the problem comes in:
When I shutdown ppm control of an individual channel, I would like to be able to control channel with the arduino Servo library.
My thinking was I could attach a servo when the channel was disabled from PPM and detach when I want PPM control.
However, whenever I attach any servo on any pin, my entire ICP interrupt hangs, and ppmIn does not produce new updated values.

AFAIK the Servo library uses Timer1. There is an alternative ServoTimer2 library in case that might be helpful.

I suspect you will have to study the detail of the code in the library to figure out where the conflict is.

…R