// ------------------------------------------------------------- // PPM ENCODER V3.0.0 (12-10-2012) // ------------------------------------------------------------- // By: John Arne Birkeland - 2012 // By Olivier ADLER : PPM redundancy mode - APM v1.x adaptation and "difficult" receiver testing - 2012 // // ------------------------------------------------------------- // See changelog_v3 for a complete descrition of changes // ------------------------------------------------------------- // // 12-10-2012 // V3.0.0 - Added dual input PPM redundancy mode with auto switchover. This is mainly for dual PPM receivers setup. // // ------------------------------------------------------------- // Not for production - Work in progress #pragma once #include #include #include #include #ifndef F_CPU #define F_CPU 16000000UL #endif #ifndef true #define true 1 #endif #ifndef false #define false 0 #endif #ifndef bool #define bool _Bool #endif // ------------------------------------------------------------- // GLOBAL SETTINGS // ------------------------------------------------------------- // Number of Timer1 ticks for 1 microsecond #define TICKS_FOR_ONE_US F_CPU / 8 / 1000 / 1000 // ------------------------------------------------------------- // INPUT MODE (by jumper selection) // ------------------------------------------------------------- #define JUMPER_SELECT_MODE 0 // Default - PPM passtrough mode selected if input pins 2&3 shorted. Normal servo input (pwm) if not shorted. #define SERVO_PWM_MODE 1 // Normal 8 channel servo (pwm) input #define PPM_PASSTROUGH_MODE 2 // PPM signal passtrough on channel 1 if input pins 2&3 shorted #define PPM_REDUNDANCY_MODE 3 // PPM redundancy on channels 1 and 2 if input pins 3&4 shorted #define SPEKTRUM_MODE 4 // Spektrum satelitte on channel 1 (reserved - not yet implemented) volatile uint8_t servo_input_mode = JUMPER_SELECT_MODE; // ------------------------------------------------------------- // PPM REDUNDANCY MODE SETTINGS // ------------------------------------------------------------- #define SWITCHOVER_CHANNEL_A 9 // Receiver 1 PPM channel to force receiver 2. Use 0 for no switchover channel // Must be chosen between 6 to 16. Preferabily from 9 to 16 so that APM can use // channels 1 to 8. #define SWITCHOVER_1_to_2_DELAY_MS 50 // Delay for switching to receiver 2 #define SWITCHOVER_2_to_1_DELAY_MS 200 // Delay for switching back to receiver 1 #define CHANNEL_COUNT_DETECTION_THRESHOLD 10 // Valid frames detected before channel count validation // PPM input frame mode receiver 1 // ------------------------------------------------------------- #define PPM_CH1_STANDARD // Standard PPM : 1520 us +/- 600 us - 8 channels - 20 ms frame period //#define PPM_CH1_STANDARD_EXTENDED // 9 channels : 1520 us +/- 600 us - 9 channels - 22.1 ms slower frame period //#define PPM_CH1_V2 // PPMv2 : 760 us +/- 300 us - 16 Channels - normal 20 ms frame period //#define PPM_CH1_V3 // PPMv3 16 channels with long sync symbol : 1050 us +/- 300 us - 25 ms frame period // PPM input frame mode receiver 2 // ------------------------------------------------------------- #define PPM_CH2_STANDARD //#define PPM_CH2_STANDARD_EXTENDED //#define PPM_CH2_V2 //#define PPM_CH2_V3 // PPM1 input : frame formats definitions // ------------------------------------------------------------- #if defined (PPM_CH1_STANDARD) || defined (PPM_CH1_STANDARD_EXTENDED) #ifdef PPM_CH1_STANDARD_EXTENDED // PPM channel count limits #define PPM_CH1_MIN_CHANNELS 4 #define PPM_CH1_MAX_CHANNELS 9 // Frame period #define PPM_CH1_FRAME_PERIOD 22500 // frame period (microseconds) #else // PPM channel count limits #define PPM_CH1_MIN_CHANNELS 4 #define PPM_CH1_MAX_CHANNELS 8 // Frame period #define PPM_CH1_FRAME_PERIOD 20000 // frame period (microseconds) #endif // PPM channels limits #define PPM_CH1_VAL_MIN TICKS_FOR_ONE_US * 920 #define PPM_CH1_VAL_MAX TICKS_FOR_ONE_US * 2120 #define PPM_CH1_VAL_CENTER TICKS_FOR_ONE_US * 1520 #define PPM_CH1_FORCE_VAL_MIN 1800 // PPM channel pre pulse length #define PPM_CH1_CHANNEL_PREPULSE_LENGHT 400 // PPM frame sync symbol limits #define PPM_CH1_MIN_SYNC_LENGHT PPM_CH1_FRAME_PERIOD - ( PPM_CH1_MAX_CHANNELS * PPM_CH1_VAL_MAX ) - PPM_CH1_CHANNEL_PREPULSE_LENGHT // Sync symbol detection #define PPM_CH1_MAX_SYNC_LENGHT PPM_CH1_FRAME_PERIOD - ( PPM_CH1_MIN_CHANNELS * PPM_CH1_VAL_MIN ) - PPM_CH1_CHANNEL_PREPULSE_LENGHT // Sync timeout #endif #ifdef PPM_CH1_V2 (PPMv2 is a 50 Hz 16 channels mode) // PPM channel count limits #define PPM_CH1_MIN_CHANNELS 4 #define PPM_CH1_MAX_CHANNELS 16 // Frame period #define PPM_CH1_FRAME_PERIOD 20000 // frame period (microseconds) // PPM channels limits #define PPM_CH1_VAL_MIN TICKS_FOR_ONE_US * 460 #define PPM_CH1_VAL_MAX TICKS_FOR_ONE_US * 1060 #define PPM_CH1_VAL_CENTER TICKS_FOR_ONE_US * 760 #define PPM_CH1_FORCE_VAL_MIN 900 // PPM channel pre pulse length #define PPM_CH1_CHANNEL_PREPULSE_LENGHT 200 // PPM frame sync symbol limits #define PPM_CH1_MIN_SYNC_LENGHT PPM_CH1_FRAME_PERIOD - ( PPM_CH1_MAX_CHANNELS * PPM_CH1_VAL_MAX ) - PPM_CH1_CHANNEL_PREPULSE_LENGHT // Sync symbol detection #define PPM_CH1_MAX_SYNC_LENGHT PPM_CH1_FRAME_PERIOD - ( PPM_CH1_MIN_CHANNELS * PPM_CH1_VAL_MIN ) - PPM_CH1_CHANNEL_PREPULSE_LENGHT // Sync timeout #endif #ifdef PPM_CH1_V3 (PPMv3 is a 40 Hz slower refresh rate 16 channels mode) // PPM channel count limits #define PPM_CH1_MIN_CHANNELS 4 #define PPM_CH1_MAX_CHANNELS 16 // Frame period #define PPM_CH1_FRAME_PERIOD 25000 // frame period (microseconds) // PPM channels limits #define PPM_CH1_VAL_MIN TICKS_FOR_ONE_US * 750 #define PPM_CH1_VAL_MAX TICKS_FOR_ONE_US * 1350 #define PPM_CH1_VAL_CENTER TICKS_FOR_ONE_US * 1050 #define PPM_CH1_FORCE_VAL_MIN 1260 // PPM channel pre pulse length #define PPM_CH1_CHANNEL_PREPULSE_LENGHT 400 // PPM frame sync symbol limits #define PPM_CH1_MIN_SYNC_LENGHT PPM_CH1_FRAME_PERIOD - ( PPM_CH1_MAX_CHANNELS * PPM_CH1_VAL_MAX ) - PPM_CH1_CHANNEL_PREPULSE_LENGHT // Sync symbol detection #define PPM_CH1_MAX_SYNC_LENGHT PPM_CH1_FRAME_PERIOD - ( PPM_CH1_MIN_CHANNELS * PPM_CH1_VAL_MIN ) - PPM_CH1_CHANNEL_PREPULSE_LENGHT // Sync timeout #endif // PPM2 input : frame format definitions // ------------------------------------------------------------- #if defined (PPM_CH2_STANDARD) || defined (PPM_CH2_STANDARD_EXTENDED) #ifdef PPM_CH2_STANDARD_EXTENDED // PPM channel count limits #define PPM_CH1_MIN_CHANNELS 4 #define PPM_CH1_MAX_CHANNELS 9 // Frame period #define PPM_CH2_FRAME_PERIOD 22500 // frame period (microseconds) #else // PPM channel count limits #define PPM_CH1_MIN_CHANNELS 4 #define PPM_CH1_MAX_CHANNELS 8 // Frame period #define PPM_CH2_FRAME_PERIOD 20000 // frame period (microseconds) #endif // PPM channels limits #define PPM_CH2_VAL_MIN TICKS_FOR_ONE_US * 920 #define PPM_CH2_VAL_MAX TICKS_FOR_ONE_US * 2120 #define PPM_CH2_VAL_CENTER TICKS_FOR_ONE_US * 1520 // PPM channel pre pulse length #define PPM_CH1_CHANNEL_PREPULSE_LENGHT 400 // PPM frame sync symbol limits #define PPM_CH2_MIN_SYNC_LENGHT PPM_CH2_FRAME_PERIOD - ( PPM_CH2_MAX_CHANNELS * PPM_CH2_VAL_MAX ) - PPM_CH2_CHANNEL_PREPULSE_LENGHT // Sync symbol detection #define PPM_CH2_MAX_SYNC_LENGHT PPM_CH2_FRAME_PERIOD - ( PPM_CH2_MIN_CHANNELS * PPM_CH2_VAL_MIN ) - PPM_CH2_CHANNEL_PREPULSE_LENGHT // Sync timeout #endif #ifdef PPM_CH2_V2 (PPMv2 is a 50 Hz 16 channels mode) // PPM channel count limits #define PPM_CH1_MIN_CHANNELS 4 #define PPM_CH1_MAX_CHANNELS 16 // Frame period #define PPM_CH2_FRAME_PERIOD 20000 // frame period (microseconds) // PPM channels limits #define PPM_CH2_VAL_MIN TICKS_FOR_ONE_US * 460 #define PPM_CH2_VAL_MAX TICKS_FOR_ONE_US * 1060 #define PPM_CH2_VAL_CENTER TICKS_FOR_ONE_US * 760 // PPM channel pre pulse length #define PPM_CH1_CHANNEL_PREPULSE_LENGHT 200 // PPM sync symbol limits #define PPM_CH2_MIN_SYNC_LENGHT PPM_CH2_FRAME_PERIOD - ( PPM_CH2_MAX_CHANNELS * PPM_CH2_VAL_MAX ) - PPM_CH2_CHANNEL_PREPULSE_LENGHT // Sync symbol detection #define PPM_CH2_MAX_SYNC_LENGHT PPM_CH2_FRAME_PERIOD - ( PPM_CH2_MIN_CHANNELS * PPM_CH2_VAL_MIN ) - PPM_CH2_CHANNEL_PREPULSE_LENGHT // Sync timeout #endif #ifdef PPM_CH2_V3 (PPMv3 is a 40 Hz slower refresh rate 16 channels mode) // PPM channel count limits #define PPM_CH1_MIN_CHANNELS 4 #define PPM_CH1_MAX_CHANNELS 16 // Frame period #define PPM_CH2_FRAME_PERIOD 25000 // frame period (microseconds) // PPM channels limits #define PPM_CH2_VAL_MIN TICKS_FOR_ONE_US * 750 #define PPM_CH2_VAL_MAX TICKS_FOR_ONE_US * 1350 #define PPM_CH2_VAL_CENTER TICKS_FOR_ONE_US * 1050 // PPM channel pre pulse length #define PPM_CH1_CHANNEL_PREPULSE_LENGHT 400 // PPM sync symbol limits #define PPM_CH2_MIN_SYNC_LENGHT PPM_CH2_FRAME_PERIOD - ( PPM_CH2_MAX_CHANNELS * PPM_CH2_VAL_MAX ) - PPM_CH2_CHANNEL_PREPULSE_LENGHT // Sync symbol detection #define PPM_CH2_MAX_SYNC_LENGHT PPM_CH2_FRAME_PERIOD - ( PPM_CH2_MIN_CHANNELS * PPM_CH2_VAL_MIN ) - PPM_CH2_CHANNEL_PREPULSE_LENGHT // Sync timeout #endif // ------------------------------------------------------------- // SERVO PWM MODE input settings // ------------------------------------------------------------- // Number of input PWM channels #define PWM_CHANNELS 8 // PWM channels minimum values #define PWM_VAL_MIN TICKS_FOR_ONE_US * 920 - PPM_PRE_PULSE // PWM channels maximum values #define PWM_VAL_MAX TICKS_FOR_ONE_US * 2120 - PPM_PRE_PULSE // PWM input filters // Using both filters is not recommended and may reduce servo input resolution // #define _AVERAGE_FILTER_ // Average filter to smooth servo input capture jitter // #define _JITTER_FILTER_ // Cut filter to remove 0,5us servo input capture jitter // ------------------------------------------------------------- // PPM output frame format // ------------------------------------------------------------- // Number of output PPM channels #define PPM_CHANNELS 8 // 400us PPM pre pulse #define PPM_PRE_PULSE TICKS_FOR_ONE_US * 400 // PPM channels center positions #define PPM_VAL_CENTER TICKS_FOR_ONE_US * 1520 - PPM_PRE_PULSE // PPM period 18.5ms - 26.5ms (54hz - 37Hz) #define PPM_PERIOD TICKS_FOR_ONE_US * ( 22500 - ( PPM_CHANNELS * 1520 ) ) // Size of ppm[..] data array #define PPM_ARRAY_MAX PPM_CHANNELS * 2 + 2 // ------------------------------------------------------------- // PPM output default values // ------------------------------------------------------------- // Throttle default at power on #define PPM_THROTTLE_DEFAULT TICKS_FOR_ONE_US * 1100 - PPM_PRE_PULSE // Throttle during failsafe #define PPM_THROTTLE_FAILSAFE TICKS_FOR_ONE_US * 920 - PPM_PRE_PULSE // CH5 power on values (mode selection channel) #define PPM_CH5_MODE_4 TICKS_FOR_ONE_US * 1555 - PPM_PRE_PULSE // ------------------------------------------------------------- // PPM output frame variables // ------------------------------------------------------------- // Data array for storing output PPM (8 channels) pulse widths. volatile uint16_t ppm[ PPM_ARRAY_MAX ] = { PPM_PRE_PULSE, PPM_VAL_CENTER, // Channel 1 PPM_PRE_PULSE, PPM_VAL_CENTER, // Channel 2 PPM_PRE_PULSE, PPM_THROTTLE_DEFAULT, // Channel 3 (throttle) PPM_PRE_PULSE, PPM_VAL_CENTER, // Channel 4 PPM_PRE_PULSE, PPM_CH5_MODE_4, // Channel 5 (flight mode) PPM_PRE_PULSE, PPM_VAL_CENTER, // Channel 6 PPM_PRE_PULSE, PPM_VAL_CENTER, // Channel 7 PPM_PRE_PULSE, PPM_VAL_CENTER, // Channel 8 PPM_PRE_PULSE, PPM_PERIOD }; // Output PPM FAILSAFE values const uint16_t failsafe_ppm[ PPM_ARRAY_MAX ] = { PPM_PRE_PULSE, PPM_VAL_CENTER, // Channel 1 PPM_PRE_PULSE, PPM_VAL_CENTER, // Channel 2 PPM_PRE_PULSE, PPM_THROTTLE_FAILSAFE, // Channel 3 (throttle) PPM_PRE_PULSE, PPM_VAL_CENTER, // Channel 4 PPM_PRE_PULSE, PPM_CH5_MODE_4, // Channel 5 PPM_PRE_PULSE, PPM_VAL_CENTER, // Channel 6 PPM_PRE_PULSE, PPM_VAL_CENTER, // Channel 7 PPM_PRE_PULSE, PPM_VAL_CENTER, // Channel 8 PPM_PRE_PULSE, PPM_PERIOD }; // ------------------------------------------------------------- // AVR parameters for PhoneDrone and APM2 boards using ATmega32u2 #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) #define SERVO_DDR DDRB #define SERVO_PORT PORTB #define SERVO_INPUT PINB #define SERVO_INT_VECTOR PCINT0_vect #define SERVO_INT_MASK PCMSK0 #define SERVO_INT_CLEAR_FLAG PCIF0 #define SERVO_INT_ENABLE PCIE0 #define SERVO_TIMER_CNT TCNT1 #define PPM_DDR DDRC #define PPM_PORT PORTC #define PPM_OUTPUT_PIN PC6 #define PPM_INT_VECTOR TIMER1_COMPA_vect #define PPM_COMPARE OCR1A #define PPM_COMPARE_FLAG COM1A0 #define PPM_COMPARE_ENABLE OCIE1A #define USB_DDR DDRC #define USB_PORT PORTC #define USB_PIN PC2 // true if we have received a USB device connect event static bool usb_connected; // USB connected event void EVENT_USB_Device_Connect(void) { // Toggle USB pin high if USB is connected USB_PORT |= (1 << USB_PIN); usb_connected = true; // this unsets the pin connected to PA1 on the 2560 // when the bit is clear, USB is connected PORTD &= ~1; } // USB disconnect event void EVENT_USB_Device_Disconnect(void) { // toggle USB pin low if USB is disconnected USB_PORT &= ~(1 << USB_PIN); usb_connected = false; // this sets the pin connected to PA1 on the 2560 // when the bit is clear, USB is connected PORTD |= 1; } // AVR parameters for ArduPilot MEGA v1.4 PPM Encoder (ATmega328P) #elif defined (__AVR_ATmega328P__) || defined (__AVR_ATmega328__) #define SERVO_DDR DDRD #define SERVO_PORT PORTD #define SERVO_INPUT PIND #define SERVO_INT_VECTOR PCINT2_vect #define SERVO_INT_MASK PCMSK2 #define SERVO_INT_CLEAR_FLAG PCIF2 #define SERVO_INT_ENABLE PCIE2 #define SERVO_TIMER_CNT TCNT1 #define PPM_DDR DDRB #define PPM_PORT PORTB #define PPM_OUTPUT_PIN PB2 #define PPM_INT_VECTOR TIMER1_COMPB_vect #define PPM_COMPARE OCR1B #define PPM_COMPARE_FLAG COM1B0 #define PPM_COMPARE_ENABLE OCIE1B #else #error NO SUPPORTED DEVICE FOUND! (ATmega16u2 / ATmega32u2 / ATmega328p) #endif // Invalid SERVO input signals count volatile uint8_t servo_input_errors = 0; // Missing SERVO input signals flag volatile bool servo_input_missing = true; // PPM generator active flag volatile bool ppm_generator_active = false; // Brownout restart flag volatile bool brownout_reset = false; // ------------------------------------------------------------------------------ // PPM GENERATOR START - TOGGLE ON COMPARE INTERRUPT ENABLE // ------------------------------------------------------------------------------ void ppm_start( void ) { // Prevent reenabling an already active PPM generator if( ppm_generator_active ) return; // Store interrupt status and register flags uint8_t SREG_tmp = SREG; // Stop interrupts cli(); // Make sure initial output state is low PPM_PORT &= ~(1 << PPM_OUTPUT_PIN); // Wait for output pin to settle //_delay_us( 1 ); // Set initial compare toggle to maximum (32ms) to give other parts of the system time to start SERVO_TIMER_CNT = 0; PPM_COMPARE = 0xFFFF; // Set toggle on compare output TCCR1A = (1 << PPM_COMPARE_FLAG); // Set TIMER1 8x prescaler TCCR1B = ( 1 << CS11 ); // Enable output compare interrupt TIMSK1 |= (1 << PPM_COMPARE_ENABLE); // Indicate that PPM generator is active ppm_generator_active = true; // Restore interrupt status and register flags SREG = SREG_tmp; #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) // Turn on TX led if PPM generator is active PORTD &= ~( 1<< PD5 ); #endif } // ------------------------------------------------------------------------------ // PPM GENERATOR STOP - TOGGLE ON COMPARE INTERRUPT DISABLE // ------------------------------------------------------------------------------ void ppm_stop( void ) { // Store interrupt status and register flags uint8_t SREG_tmp = SREG; // Stop interrupts cli(); // Disable output compare interrupt TIMSK1 &= ~(1 << PPM_COMPARE_ENABLE); // Reset TIMER1 registers TCCR1A = 0; TCCR1B = 0; // Indicate that PPM generator is not active ppm_generator_active = false; // Restore interrupt status and register flags SREG = SREG_tmp; #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) // Turn off TX led if PPM generator is off PORTD |= ( 1<< PD5 ); #endif } // ------------------------------------------------------------------------------ // Watchdog Interrupt (interrupt only mode, no reboot) // ------------------------------------------------------------------------------ ISR( WDT_vect ) // If watchdog is triggered then enable missing signal flag and copy failsafe values { // Use failsafe values if PPM generator is active or if chip has been reset from a brown-out if ( ppm_generator_active || brownout_reset ) { // Copy failsafe values to ppm[..] for ( uint8_t i = 0; i < PPM_ARRAY_MAX; i++ ) { ppm[ i ] = failsafe_ppm[ i ]; } } // If we are in PPM passtrough mode and a input signal has been detected, or if chip has been reset from a brown_out then start the PPM generator. if( ( servo_input_mode == PPM_PASSTROUGH_MODE && servo_input_missing == false ) || brownout_reset ) { // Start PPM generator ppm_start(); brownout_reset = false; } // Set missing receiver signal flag servo_input_missing = true; // Reset servo input error flag servo_input_errors = 0; #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) // Turn on RX led if failsafe has triggered after ppm generator i active if( ppm_generator_active ) PORTD &= ~( 1<< PD4 ); #endif } // ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------ // SERVO/PPM INPUT - PIN CHANGE INTERRUPT // ------------------------------------------------------------------------------ ISR( SERVO_INT_VECTOR ) { // To store current servo input pins uint8_t servo_pins; // Servo input pin storage static uint8_t servo_pins_old = 0; // PWM Mode pulse start time static uint16_t servo_start[ servo_channel ] = { 0, 0, 0, 0, 0, 0, 0, 0 }; // Missing throttle signal failsafe static uint8_t throttle_timeout = 0; #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) // Toggle LED delay static uint8_t led_delay = 0; #endif // Read current servo timer uint16_t servo_time = SERVO_TIMER_CNT; // ------------------------------------------------------------------------------ // PPM redundancy mode // ------------------------------------------------------------------------------ //--------------------------------------------------------------------- // Todo : Refine sync symbol limits after channel count detection // Todo : Conversion to PPM output format (would be better if the main APM could follow format changes) // Todo : sync between PPM input and output after switchover // Todo : rework code from line 950 to end of redundancy mode // Todo : Add optional delay inside switchover algo // Todo : Add LED code for APM 1.4 //------------------------------------------------------------------------- if( servo_input_mode == PPM_REDUNDANCY_MODE ) { // ------------------------------------- // PPM redundancy mode - variables Init // ------------------------------------- // PPM1 prepulse start static uint16_t ppm1_prepulse_start; // PPM2 prepulse start static uint16_t ppm2_prepulse_start; // PPM1 prepulse width static uint16_t ppm1_prepulse_width; // PPM2 prepulse width static uint16_t ppm2_prepulse_width; // PPM1 pulse start time static uint16_t ppm1_start[ 16 ] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; // PPM2 pulse start time static uint16_t ppm2_start[ 16 ] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; // PPM1 pulse length static uint16_t ppm1_width[ 16 ] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; // PPM2 pulse length static uint16_t ppm2_width[ 16 ] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; // Reset PPM channels ( 0 = Sync Symbol ) static uint8_t ppm1_channel = 0; // Channel 0 = sync symbol static uint8_t ppm2_channel = 0; // Channel 0 = sync symbol // Frame sync flag static bool sync_ch1 = false; static bool sync_ch2 = false; // Channel error flags static bool channel_error_ch1 = true; static bool channel_error_ch2 = true; // Sync error flags static bool sync_error_ch1 = true; static bool sync_error_ch2 = true; //--------------------------------------------------------- // ch2 switchover flag static bool switchover_ch2 = false; //--------------------------------------------------------- // Channel count detection ready flag static bool channel_count_ready_ch1 = false; static bool channel_count_ready_ch2 = false; // Channel count detected flag static bool channel_count_detected_ch1 = false; static bool channel_count_detected_ch2 = false; // Detected Channel count static uint8_t channel_count_ch1 = PPM_CH1_MAX_CHANNELS; static uint8_t channel_count_ch2 = PPM_CH2_MAX_CHANNELS; // Detected Channel count previous value static uint8_t previous_channel_count_ch1 = 0; static uint8_t previous_channel_count_ch2 = 0; // Channel count detection counter static uint8_t channel_count_detection_counter_ch1 = 0; static uint8_t channel_count_detection_counter_ch1 = 0; // ----------------------------------- // PPM redundancy - decoder // ----------------------------------- CHECK_START: // Start of PPM inputs check // Store current PPM inputs pins servo_pins = SERVO_INPUT; // Calculate servo input pin change mask uint8_t servo_change = servo_pins ^ servo_pins_old; // ----------------------------------- // PPM redundancy - Ch1 decoding // ----------------------------------- CHECK_LOOP: // Input PPM pins check loop // Check if we have a pin change on PPM channel 1 if( servo_change & 1 ) { // ----------------------------------------------------------------------------------------------------------------------- // Check if we've got a high level (raising edge, channel start or sync symbol end) if( servo_pins & 1 ) { // Check for pre pulse length ppm1_prepulse_width = servo_time - ppm1_prepulse_start; if ( true ) //Todo optionnal: We could add a test here for channel pre pulse length check { //We have a valid pre pulse if( ppm1_channel == channel_count_ch1 ) // Check for last channel { // We are at latest PPM channel sync_ch1 = false; // Reset sync flag ppm1_channel = 0; // Reset PPM channel counter } else // We do not have yet reached the last channel { // Increment channel counter ppm1_channel++; } } else { //We do not have a valid pre pulse sync_error_ch1 = true; // Set sync error flag sync_ch1 = false; // Reset sync flag ppm1_channel = 0; // Reset PPM channel counter } ppm1_start[ ppm1_channel ] = servo_time; // Store pulse start time for PPM1 input } // ----------------------------------------------------------------------------------------------------------------------- else // We've got a low level (falling edge, channel end or sync symbol start) { ppm1_width[ ppm1_channel ] = servo_time - ppm1_start[ ppm1_channel ]; // Calculate channel pulse length, or sync symbol length if(sync_ch1 == true) // Are we synchronized ? { // Check channel pulse length validity if( ppm1_width[ ppm1_channel ] > ( PPM_CH1_VAL_MAX - PPM_CH1_CHANNEL_PREPULSE_LENGHT ) ) || ( ppm1_width[ ppm1_channel ] < ( PPM_CH1_VAL_MIN - PPM_CH1_CHANNEL_PREPULSE_LENGHT ) ) // If we have a valid pulse length { // Reset channel error flag channel_error_ch1 = false; } else // We do not have a valid channel length { if( ppm1_width[ ppm1_channel ] > PPM_CH1_MIN_SYNC_LENGHT ) || ( ppm1_width[ ppm1_channel ] < PPM_CH1_MAX_SYNC_LENGHT ) //Check for sync symbol { // We have a valid sync symbol if( channel_count_detected_ch1 == false ) // Check if we do not have yet channel count detected { // We have detected channels count channel_count_ch1 = ppm1_channel; // Store PPM1 channel count channel_count_ready_ch1 = true; // Set PPM1 channel count detection ready flag sync_error_ch1 = false; // Reset sync error flag sync_ch1 = true; // Set sync flag } else // Channel count had been detected before { //We should not have a sync symbol here sync_error_ch1 = true; // Set sync error flag sync_ch1 = false; // Reset sync flag } ppm1_channel = 0; // Reset PPM channel counter } else // We do not have a valid sync symbol { channel_error_ch1 = true; // Set channel error flag } } } // ------------------------------------------------------------------------------ else // We are not yet synchronized { if( ppm1_width[ ppm1_channel ] > PPM_CH1_MIN_SYNC_LENGHT ) || ( ppm1_width[ ppm1_channel ] < PPM_CH1_MAX_SYNC_LENGHT ) //Check for sync symbol { // We have a valid sync symbol sync_error_ch1 = false; // Reset sync error flag sync_ch1 = true; // Set sync flag } else // We did not find a valid sync symbol { sync_error_ch1 = true; // Set sync error flag sync_ch1 = false; // Reset sync flag } ppm1_channel = 0; // Reset PPM channel counter } } ppm1_prepulse_start = servo_time; // Store prepulse start time // ----------------------------------------------------------------------------------------------------------------------- } // ----------------------------------- // PPM redundancy - Ch2 decoding // ----------------------------------- // Check if we have a pin change on PPM channel 2 if( servo_change & 2 ) { // ----------------------------------------------------------------------------------------------------------------------- // Check if we've got a high level (raising edge, channel start or sync symbol end) if( servo_pins & 2 ) { // Check for pre pulse length ppm2_prepulse_width = servo_time - ppm2_prepulse_start; if ( true ) //Todo optionnal: We could add a test here for channel pre pulse length check { //We have a valid pre pulse if( ppm2_channel == channel_count_ch2 ) // Check for last channel { // We are at latest PPM channel sync_ch2 = false; // Reset sync flag ppm2_channel = 0; // Reset PPM channel counter } else // We do not have yet reached the last channel { // Increment channel counter ppm2_channel++; } } else { //We do not have a valid pre pulse sync_error_ch2 = true; // Set sync error flag sync_ch2 = false; // Reset sync flag ppm2_channel = 0; // Reset PPM channel counter } ppm2_start[ ppm2_channel ] = servo_time; // Store pulse start time for PPM2 input } // ----------------------------------------------------------------------------------------------------------------------- else // We've got a low level (falling edge, channel end or sync symbol start) { ppm2_width[ ppm2_channel ] = servo_time - ppm2_start[ ppm2_channel ]; // Calculate channel pulse length, or sync symbol length if(sync_ch2 == true) // Are we synchronized ? { // Check channel pulse length validity if( ppm2_width[ ppm2_channel ] > ( PPM_CH2_VAL_MAX - PPM_CH2_CHANNEL_PREPULSE_LENGHT ) ) || ( ppm2_width[ ppm2_channel ] < ( PPM_CH2_VAL_MIN - PPM_CH2_CHANNEL_PREPULSE_LENGHT ) ) // If we have a valid pulse length { // Reset channel error flag channel_error_ch2 = false; } else // We do not have a valid channel length { if( ppm2_width[ ppm2_channel ] > PPM_CH2_MIN_SYNC_LENGHT ) || ( ppm2_width[ ppm2_channel ] < PPM_CH2_MAX_SYNC_LENGHT ) //Check for sync symbol { // We have a valid sync symbol if( channel_count_detected_ch2 == false ) // Check if we do not have yet channel count detected { // We have detected channels count channel_count_ch2 = ppm2_channel; // Store PPM2 channel count channel_count_ready_ch2 = true; // Set PPM2 channel count detection ready flag sync_error_ch2 = false; // Reset sync error flag sync_ch2 = true; // Set sync flag } else // Channel count had been detected before { //We should not have a sync symbol here sync_error_ch2 = true; // Set sync error flag sync_ch2 = false; // Reset sync flag } ppm2_channel = 0; // Reset PPM channel counter } else // We do not have a valid sync symbol { channel_error_ch2 = true; // Set channel error flag } } } // ------------------------------------------------------------------------------ else // We are not yet synchronized { if( ppm2_width[ ppm2_channel ] > PPM_CH2_MIN_SYNC_LENGHT ) || ( ppm2_width[ ppm2_channel ] < PPM_CH2_MAX_SYNC_LENGHT ) //Check for sync symbol { // We have a valid sync symbol sync_error_ch2 = false; // Reset sync error flag sync_ch2 = true; // Set sync flag } else // We did not find a valid sync symbol { sync_error_ch2 = true; // Set sync error flag sync_ch2 = false; // Reset sync flag } ppm2_channel = 0; // Reset PPM channel counter } } ppm2_prepulse_start = servo_time; // Store prepulse start time // ----------------------------------------------------------------------------------------------------------------------- } // ----------------------------------- // PPM redundancy - Post processing // ----------------------------------- // Could be eventually run in the main loop for performances improvements if necessary // sync mode between input and ouptput should clear performance problems // ----------------- // Switchover code // ----------------- // Check for PPM1 validity if ( sync_error_ch1 == false ) && ( channel_error_ch1 == false) // PPM1 is valid { // check for PPM2 forcing (through PPM1 force channel) if ( ppm1_width [ SWITCHOVER_CHANNEL ] > PPM_CH1_FORCE_VAL_MIN ) // Channel 2 forcing is alive { // Check for PPM2 validity if ( sync_error_ch2 == false ) && ( channel_error_ch2 == false) // PPM2 is valid { // Check for PPM2 selected if ( switchover_ch2 == true ) // PPM2 is selected { // Do nothing } else { // Switch to PPM2 without delay if ( ppm2_channel == channel_count_ch2 ) // Check for last PPM2 channel before switching { switchover_ch2 == true; // Switch to PPM2 } } } } else // Check for PPM1 selected { if ( switchover_ch2 == false ) // PPM1 is selected { // Load PPM Output with PPM1 ppm[ ppm1_channel ] = ppm1_width; } else // PPM1 is not selected { // To Enhance : Optional switchover delay 2 to 1 here if ( ppm1_channel == channel_count_ch1 ) // Check for last PPM1 channel before switching { switchover_ch2 == false; // Switch to PPM1 } } } } else // PPM1 is not valid { // Check for ppm2 validity if ( sync_error_ch2 == false ) && ( channel_error_ch2 == false) // PPM2 is valid { // Check PPM2 selected if ( switchover_ch2 == true ) // PPM2 is selected { // Load PPM Output with PPM2 ppm[ ppm2_channel ] = ppm2_width; } else // Switch to PPM2 { // To Enhance : Optional switchover delay 1 to 2 here if ( ppm2_channel == channel_count_ch2 ) // Check for last PPM2 channel before switching { switchover_ch2 == true; // Switch to PPM2 } } } else // PPM2 is not valid { // load PPM output with failsafe values } } // ----------------------------------- // Channel count post processing code // ----------------------------------- // To enhance: possible global detection flag to avoid 2 channel_count_detected tests // Ch1 if ( channel_count_detected_ch1 == true ) // Channel count for Ch1 was detected { // Do nothing } else // Do we have a channel count detection ready ? { if ( channel_count_ready_ch1 == true ) // If channel count is ready { // Check for detection counter if ( channel_count_detection_counter_ch1 < CHANNEL_COUNT_DETECTION_THRESHOLD ) // Detection counter < Threshold { // Compare channel count with previous value if ( channel_count_ch1 == previous_channel_count_ch1 ) // We have the same value { channel_count_detection_counter_ch1++; // Increment detection counter } else // We do not have the same value { channel_count_detection_counter_ch1 = 0; // Reset detection counter } previous_channel_count_ch1 = channel_count_ch1; // Load previous channel count with channel count } else // Detection counter >= Threshold { channel_count_detected_ch1 = true; // Channel count is now detected } channel_count_ready_ch1 = false; // Reset channel count detection ready flag } } // Ch2 if ( channel_count_detected_ch2 == true ) // Channel count for ch2 was detected { // Do nothing } else // Do we have a channel count detection ready ? { if ( channel_count_ready_ch2 == true ) // If channel count is ready { // Check for detection counter if ( channel_count_detection_counter_ch2 < CHANNEL_COUNT_DETECTION_THRESHOLD ) // Detection counter < Threshold { // Compare channel count with previous value if ( channel_count_ch2 == previous_channel_count_ch2 ) // We have the same value { channel_count_detection_counter_ch2++; // Increment detection counter } else // We do not have the same value { channel_count_detection_counter_ch2 = 0; // Reset detection counter } previous_channel_count_ch2 = channel_count_ch2; // Load previous channel count with channel count } else // Detection counter >= Threshold { channel_count_detected_ch2 = true; // Channel count is now detected } channel_count_ready_ch2 = false; // Reset channel count detection ready flag } } /* //Reset throttle failsafe timeout if( ppm1_channel == 5 ) throttle_timeout = 0; CHECK_ERROR: #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) // Delay LED toggle led_delay = 0; #endif */ CHECK_DONE: // Reset Watchdog Timer wdt_reset(); // Set servo input missing flag false to indicate that we have received servo input signals servo_input_missing = false; // Store current servo input pins for next check servo_pins_old = servo_pins; // Start PPM generator if not already running if( ppm_generator_active == false ) ppm_start(); /* #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) // Toggle RX LED when finished receiving servo pulses if( ++led_delay > 64 ) // Toggle led every 64th time { PIND |= ( 1<< PD4 ); led_delay = 0; } #endif // Throttle failsafe if( throttle_timeout++ >= 128 ) { // Reset throttle timeout throttle_timeout = 0; // Set throttle failsafe value ppm[ 5 ] = PPM_THROTTLE_FAILSAFE; } //Has servo input changed while processing pins, if so we need to re-check pins if( servo_pins != SERVO_INPUT ) goto CHECK_START; */ // Clear interrupt event from already processed pin changes // PCIFR |= (1 << SERVO_INT_CLEAR_FLAG); // Leave interrupt return; } // ------------------------- // PPM redundancy mode END // ------------------------- // ------------------------------------------------------------------------------ // PPM passtrough mode ( signal passtrough from channel 1 to ppm output pin) // ------------------------------------------------------------------------------ if( servo_input_mode == PPM_PASSTROUGH_MODE ) { // Has watchdog timer failsafe started PPM generator? If so we need to stop it. if( ppm_generator_active ) { // Stop PPM generator ppm_stop(); } // PPM (channel 1) input pin is high if( SERVO_INPUT & 1 ) { // Set PPM output pin high PPM_PORT |= (1 << PPM_OUTPUT_PIN); } // PPM (channel 1) input pin is low else { // Set PPM output pin low PPM_PORT &= ~(1 << PPM_OUTPUT_PIN); } // Reset Watchdog Timer wdt_reset(); // Set servo input missing flag false to indicate that we have received servo input signals servo_input_missing = false; #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) // Toggle TX LED at PPM passtrough if( ++led_delay > 128 ) // Toggle every 128th pulse { // Toggle TX led PIND |= ( 1<< PD5 ); led_delay = 0; } #endif // Leave interrupt return; } // ------------------------------------------------------------------------------ // PWM MODE (8 channels inputs) // ------------------------------------------------------------------------------ CHECK_PINS_START: // Start of servo input check // Store current servo input pins servo_pins = SERVO_INPUT; // Calculate servo input pin change mask uint8_t servo_change = servo_pins ^ servo_pins_old; // Set initial servo pin and channel uint8_t servo_pin = 1; uint8_t servo_channel = 0; CHECK_PINS_LOOP: // Input servo pin check loop // Check for pin change on current servo channel if( servo_change & servo_pin ) { // High (raising edge) if( servo_pins & servo_pin ) { servo_start[ servo_channel ] = servo_time; } else // Low (falling edge) { // Get servo pulse width uint16_t servo_width = servo_time - servo_start[ servo_channel ] - PPM_PRE_PULSE; // Check that servo pulse signal is valid before sending to ppm encoder if( servo_width > PWM_VAL_MAX ) goto CHECK_PINS_ERROR; if( servo_width < PWM_VAL_MIN ) goto CHECK_PINS_ERROR; // Calculate servo channel position in ppm[..] uint8_t _ppm_channel = ( servo_channel << 1 ) + 1; //Reset throttle failsafe timeout if( _ppm_channel == 5 ) throttle_timeout = 0; #ifdef _AVERAGE_FILTER_ // Average filter to smooth input jitter servo_width += ppm[ _ppm_channel ]; servo_width >>= 1; #endif #ifdef _JITTER_FILTER_ // 0.5us cut filter to remove input jitter int16_t ppm_tmp = ppm[ _ppm_channel ] - servo_width; if( ppm_tmp == 1 ) goto CHECK_PINS_NEXT; if( ppm_tmp == -1 ) goto CHECK_PINS_NEXT; #endif // Update ppm[..] ppm[ _ppm_channel ] = servo_width; } } CHECK_PINS_NEXT: // Select next servo pin servo_pin <<= 1; // Select next servo channel servo_channel++; // Check channel and process if needed if( servo_channel < PWM_CHANNELS ) goto CHECK_PINS_LOOP; goto CHECK_PINS_DONE; CHECK_PINS_ERROR: // Used to indicate invalid servo input signals servo_input_errors++; #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) // Delay LED toggle led_delay = 0; #endif goto CHECK_PINS_NEXT; // All servo input pins has now been processed CHECK_PINS_DONE: // Reset Watchdog Timer wdt_reset(); // Set servo input missing flag false to indicate that we have received servo input signals servo_input_missing = false; // Store current servo input pins for next check servo_pins_old = servo_pins; // Start PPM generator if not already running if( ppm_generator_active == false ) ppm_start(); #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) // Toggle RX LED when finished receiving servo pulses if( ++led_delay > 64 ) // Toggle led every 64th time { PIND |= ( 1<< PD4 ); led_delay = 0; } #endif // Throttle failsafe if( throttle_timeout++ >= 128 ) { // Reset throttle timeout throttle_timeout = 0; // Set throttle failsafe value ppm[ 5 ] = PPM_THROTTLE_FAILSAFE; } //Has servo input changed while processing pins, if so we need to re-check pins if( servo_pins != SERVO_INPUT ) goto CHECK_PINS_START; // Clear interrupt event from already processed pin changes PCIFR |= (1 << SERVO_INT_CLEAR_FLAG); } // ------------------------------------------------------------------------------ // PWM MODE END // ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------ // PPM OUTPUT - TIMER1 COMPARE INTERRUPT // ------------------------------------------------------------------------------ ISR( PPM_INT_VECTOR ) { // Current active ppm channel static uint8_t ppm_channel = PPM_ARRAY_MAX - 1; // Update timing for next PPM output pin toggle PPM_COMPARE += ppm[ ppm_channel ]; // Select the next ppm channel if( ++ppm_channel >= PPM_ARRAY_MAX ) { ppm_channel = 0; #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) // Blink TX LED when PPM generator has finished a pulse train PIND |= ( 1<< PD5 ); #endif } } // ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------ // PPM READ - INTERRUPT SAFE PPM SERVO CHANNEL READ // ------------------------------------------------------------------------------ uint16_t ppm_read_channel( uint8_t channel ) { // Limit channel to valid value uint8_t _channel = channel; if( _channel == 0 ) _channel = 1; if( _channel > PWM_CHANNELS ) _channel = PWM_CHANNELS; // Calculate ppm[..] position uint8_t ppm_index = ( _channel << 1 ) + 1; // Read ppm[..] in a non blocking interrupt safe manner uint16_t ppm_tmp = ppm[ ppm_index ]; while( ppm_tmp != ppm[ ppm_index ] ) ppm_tmp = ppm[ ppm_index ]; // Return as normal servo value return ppm_tmp + PPM_PRE_PULSE; } // ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------ // PPM ENCODER INIT // ------------------------------------------------------------------------------ void ppm_encoder_init( void ) { // ATmegaXXU2 only init code // ------------------------------------------------------------------------------ #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) // ------------------------------------------------------------------------------ // Reset Source check // ------------------------------------------------------------------------------ if (MCUSR & 1) // Power-on Reset { MCUSR=0; // Clear MCU Status register // custom code here } else if (MCUSR & 2) // External Reset { MCUSR=0; // Clear MCU Status register // custom code here } else if (MCUSR & 4) // Brown-Out Reset { MCUSR=0; // Clear MCU Status register brownout_reset=true; } else // Watchdog Reset { MCUSR=0; // Clear MCU Status register // custom code here } // APM USB connection status UART MUX selector pin // ------------------------------------------------------------------------------ USB_DDR |= (1 << USB_PIN); // Set USB pin to output #endif // USE JUMPER TO CHECK FOR INPUT MODE (pins 2&3 or 3&4 shorted) // ------------------------------------------------------------------------------ if( servo_input_mode == JUMPER_SELECT_MODE ) { // channel 3 status counter uint8_t channel_2_status = 0; uint8_t channel_4_status = 0; // Set channel 2 to input SERVO_DDR &= ~(1 << 1); // Enable channel 2 pullup SERVO_PORT |= (1 << 1); // Set channel 4 to input SERVO_DDR &= ~(1 << 3); // Enable channel 4 pullup SERVO_PORT |= (1 << 3); // Set channel 3 to output SERVO_DDR |= (1 << 2); // Set channel 3 output low SERVO_PORT &= ~(1 << 2); _delay_us (10); // Increment channel_2_status if channel 2 is set low by channel 3 if( ( SERVO_INPUT & (1 << 1) ) == 0 ) channel_2_status++; // Increment channel_4_status if channel 4 is set low by channel 3 if( ( SERVO_INPUT & (1 << 3) ) == 0 ) channel_4_status++; // Set channel 3 output high SERVO_PORT |= (1 << 2); _delay_us (10); // Increment channel_2_status if channel 2 is set high by channel 3 if( ( SERVO_INPUT & (1 << 1) ) != 0 ) channel_2_status++; // Increment channel_4_status if channel 4 is set high by channel 3 if( ( SERVO_INPUT & (1 << 3) ) != 0 ) channel_4_status++; // Set channel 3 output low SERVO_PORT &= ~(1 << 2); _delay_us (10); // Increment channel_2_status if channel 2 is set low by channel 3 if( ( SERVO_INPUT & (1 << 1) ) == 0 ) channel_2_status++; // Increment channel_4_status if channel 4 is set low by channel 3 if( ( SERVO_INPUT & (1 << 3) ) == 0 ) channel_4_status++; // Set servo input mode based on channel_2_status if( channel_2_status == 3 ) servo_input_mode = PPM_PASSTROUGH_MODE; if( channel_4_status == 3 ) servo_input_mode = PPM_REDUNDANCY_MODE; else servo_input_mode = SERVO_PWM_MODE; } // RESET SERVO/PPM PINS AS INPUTS WITH PULLUPS // ------------------------------------------------------------------------------ SERVO_DDR = 0; SERVO_PORT |= 0xFF; #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) // on 32U2 set PD0 to be an output, and clear the bit. This tells // the 2560 that USB is connected. The USB connection event fires // later to set the right value DDRD |= 1; if (usb_connected) { PORTD &= ~1; } else { PORTD |= 1; } #endif // SERVO/PPM INPUT - PIN CHANGE INTERRUPT // ------------------------------------------------------------------------------ if( servo_input_mode == SERVO_PWM_MODE ) { // Set servo input interrupt pin mask to all 8 servo input channels SERVO_INT_MASK = 0b11111111; } if( servo_input_mode == PPM_PASSTROUGH_MODE ) { // Set servo input interrupt pin mask to servo input channel 1 SERVO_INT_MASK = 0b00000001; } if( servo_input_mode == PPM_REDUNDANCY_MODE ) { // Set servo input interrupt pin mask to servo input channel 1 and 2 SERVO_INT_MASK = 0b00000011; } // Enable servo input interrupt PCICR |= (1 << SERVO_INT_ENABLE); // PPM OUTPUT PIN // ------------------------------------------------------------------------------ // Set PPM pin to output PPM_DDR |= (1 << PPM_OUTPUT_PIN); // ------------------------------------------------------------------------------ // Enable watchdog interrupt mode // ------------------------------------------------------------------------------ // Disable watchdog wdt_disable(); // Reset watchdog timer wdt_reset(); // Start timed watchdog setup sequence WDTCSR |= (1<