From 34151cbe9110b328f6b8b23c717ecdafbdfb9b1e Mon Sep 17 00:00:00 2001 From: Olivier ADLER Date: Sat, 27 Oct 2012 23:37:44 +0200 Subject: [PATCH] ArduPPM: Redundancy mode Fixed logic problem in the decoder (PPM channel increment) --- Tools/ArduPPM/Libraries/PPM_Encoder_v3.h | 813 +++++++++++------------ 1 file changed, 398 insertions(+), 415 deletions(-) diff --git a/Tools/ArduPPM/Libraries/PPM_Encoder_v3.h b/Tools/ArduPPM/Libraries/PPM_Encoder_v3.h index 24205e81ce..1221ff294b 100644 --- a/Tools/ArduPPM/Libraries/PPM_Encoder_v3.h +++ b/Tools/ArduPPM/Libraries/PPM_Encoder_v3.h @@ -13,6 +13,8 @@ // This mode Can be used as well if a PPM conversion is needed (Futaba 16 channels 760us mode to APM mode) // ------------------------------------------------------------- +// Not for production - Work in progress + #ifndef _PPM_ENCODER_H_ #define _PPM_ENCODER_H_ @@ -609,135 +611,107 @@ ISR( SERVO_INT_VECTOR ) 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 lenght - static uint16_t ppm1_width[ 16 ] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; - // PPM2 pulse lenght - 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; - - // Last channels flags - static bool last_channel_ch1 = false; - static bool last_channel_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 mode - variables Init + // ------------------------------------- - // ----------------------------------- - // PPM redundancy - decoder - // ----------------------------------- - -CHECK_START: // Start of PPM inputs check + // 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 lenght + static uint16_t ppm1_width[ 16 ] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; + // PPM2 pulse lenght + 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; + // 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 + // 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 lenght - ppm1_prepulse_width = servo_time - ppm1_prepulse_start; - if ( true ) //Todo optionnal: We could add a test here for channel pre pulse lenght check - { - //We have a valid pre pulse - } - 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 - last_channel_ch1 = false; // Reset last channel flag - } - 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) + // Check if we have a pin change on PPM channel 1 + if( servo_change & 1 ) { - ppm1_width[ ppm1_channel ] = servo_time - ppm1_start[ ppm1_channel ]; // Calculate channel pulse lenght, or sync symbol lenght - if(sync_ch1 == true) // Are we synchronized ? + // ----------------------------------------------------------------------------------------------------------------------- + // Check if we've got a high level (raising edge, channel start or sync symbol end) + if( servo_pins & 1 ) { - // Check channel pulse lenght 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 lenght + // Check for pre pulse lenght + ppm1_prepulse_width = servo_time - ppm1_prepulse_start; + if ( true ) //Todo optionnal: We could add a test here for channel pre pulse lenght check { - // Reset channel error flag - channel_error_ch1 = false; - + //We have a valid pre pulse if( ppm1_channel == channel_count_ch1 ) // Check for last channel { // We are at latest PPM channel - last_channel_ch1 = true; // Set last channel flag sync_ch1 = false; // Reset sync flag ppm1_channel = 0; // Reset PPM channel counter } @@ -747,103 +721,96 @@ CHECK_LOOP: // Input PPM pins check loop ppm1_channel++; } } - else // We do not have a valid channel lenght + 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 lenght, or sync symbol lenght + if(sync_ch1 == true) // Are we synchronized ? + { + // Check channel pulse lenght 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 lenght + { + // Reset channel error flag + channel_error_ch1 = false; + } + else // We do not have a valid channel lenght + { + 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 - if( channel_count_detected_ch1 == false ) // Check if 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 - ppm1_channel = 0; // Reset PPM channel counter - last_channel_ch1 = false; // Reset last channel flag - } - else // Channel count had been detected before - { - //We should not have a sync symbol here - sync_error_ch1 = true; // Set sync error flag - last_channel_ch1 = false; // Reset last channel flag - sync_ch1 = false; // Reset sync flag - ppm1_channel = 0; // Reset PPM channel counter - } + sync_error_ch1 = false; // Reset sync error flag + sync_ch1 = true; // Set sync flag } - else // We do not have a valid sync symbol + else // We did not find a valid sync symbol { - channel_error_ch1 = true; // Set channel error flag + sync_error_ch1 = true; // Set sync error flag + sync_ch1 = false; // Reset sync flag } - } - + ppm1_channel = 0; // Reset PPM channel counter + } } - // ------------------------------------------------------------------------------ - 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 - last_channel_ch1 = false; // Reset last channel flag - } + ppm1_prepulse_start = servo_time; // Store prepulse start time + // ----------------------------------------------------------------------------------------------------------------------- } - ppm1_prepulse_start = servo_time; // Store prepulse start time - // ----------------------------------------------------------------------------------------------------------------------- - } - // ----------------------------------- - // PPM redundancy - Ch2 decoding - // ----------------------------------- + // ----------------------------------- + // 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 lenght - ppm2_prepulse_width = servo_time - ppm2_prepulse_start; - if ( true ) //Todo optionnal: We could add a test here for channel pre pulse lenght check - { - //We have a valid pre pulse - } - 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 - last_channel_ch2 = false; // Reset last channel flag - } - 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) + // Check if we have a pin change on PPM channel 2 + if( servo_change & 2 ) { - ppm2_width[ ppm2_channel ] = servo_time - ppm2_start[ ppm2_channel ]; // Calculate channel pulse lenght, or sync symbol lenght - if(sync_ch2 == true) // Are we synchronized ? + // ----------------------------------------------------------------------------------------------------------------------- + // Check if we've got a high level (raising edge, channel start or sync symbol end) + if( servo_pins & 2 ) { - // Check channel pulse lenght 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 lenght + // Check for pre pulse lenght + ppm2_prepulse_width = servo_time - ppm2_prepulse_start; + if ( true ) //Todo optionnal: We could add a test here for channel pre pulse lenght check { - // Reset channel error flag - channel_error_ch2 = false; - + //We have a valid pre pulse if( ppm2_channel == channel_count_ch2 ) // Check for last channel { // We are at latest PPM channel - last_channel_ch2 = true; // Set last channel flag sync_ch2 = false; // Reset sync flag ppm2_channel = 0; // Reset PPM channel counter } @@ -853,260 +820,276 @@ CHECK_LOOP: // Input PPM pins check loop ppm2_channel++; } } - else // We do not have a valid channel lenght + 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 lenght, or sync symbol lenght + if(sync_ch2 == true) // Are we synchronized ? + { + // Check channel pulse lenght 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 lenght + { + // Reset channel error flag + channel_error_ch2 = false; + } + else // We do not have a valid channel lenght + { + 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 - if( channel_count_detected_ch2 == false ) // Check if 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 - ppm2_channel = 0; // Reset PPM channel counter - last_channel_ch2 = false; // Reset last channel flag - } - else // Channel count had been detected before - { - //We should not have a sync symbol here - sync_error_ch2 = true; // Set sync error flag - last_channel_ch2 = false; // Reset last channel flag - sync_ch2 = false; // Reset sync flag - ppm2_channel = 0; // Reset PPM channel counter - } + sync_error_ch2 = false; // Reset sync error flag + sync_ch2 = true; // Set sync flag } - else // We do not have a valid sync symbol + else // We did not find a valid sync symbol { - channel_error_ch2 = true; // Set channel error flag + 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 + switchover_ch2 == true; // Switch to PPM2 } } - } - // ------------------------------------------------------------------------------ - else // We are not yet synchronized + else // Check for PPM1 selected { - if( ppm2_width[ ppm2_channel ] > PPM_CH2_MIN_SYNC_LENGHT ) || ( ppm2_width[ ppm2_channel ] < PPM_CH2_MAX_SYNC_LENGHT ) //Check for sync symbol + if ( switchover_ch2 == false ) // PPM1 is selected { - // We have a valid sync symbol - sync_error_ch2 = false; // Reset sync error flag - sync_ch2 = true; // Set sync flag + // Load PPM Output with PPM1 + ppm[ ppm1_channel ] = ppm1_width; } - else // We did not find a valid sync symbol + else // PPM1 is not selected { - sync_error_ch2 = true; // Set sync error flag - sync_ch2 = false; // Reset sync flag + // Delay switchover 2 to 1 here + switchover_ch2 == false; // Switch to PPM1 } - ppm2_channel = 0; // Reset PPM channel counter - last_channel_ch2 = false; // Reset last channel flag - } + } } - 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 + else // PPM1 is not valid { - // Check for PPM2 validity - if ( sync_error_ch2 == false ) && ( channel_error_ch2 == false) // PPM2 is valid + // Check for ppm2 validity + if ( sync_error_ch2 == false ) && ( channel_error_ch2 == false) // PPM2 is valid { - // Check for PPM2 selected + // Check PPM2 selected if ( switchover_ch2 == true ) // PPM2 is selected { - // Do nothing + // Load PPM Output with PPM2 + ppm[ ppm2_channel ] = ppm2_width; } - else + else // Switch to PPM2 { - // Switch to PPM2 + // Delay switchover 1 to 2 here switchover_ch2 == true; // Switch to PPM2 } + + } + else // PPM2 is not valid + { + // load PPM output with failsafe values } } - else // Check for PPM1 selected + + // ----------------------------------- + // 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 { - if ( switchover_ch2 == false ) // PPM1 is selected + // Do nothing + } + else // Do we have a channel count detection ready ? + { + if ( channel_count_ready_ch1 == true ) // If channel count is ready { - // Load PPM Output with PPM1 - ppm[ ppm1_channel ] = ppm1_width; - } - else // PPM1 is not selected - { - // Delay switchover 2 to 1 here - switchover_ch2 == false; // Switch to PPM1 + // 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 } } - } - else // PPM1 is not valid - { - // Check for ppm2 validity - if ( sync_error_ch2 == false ) && ( channel_error_ch2 == false) // PPM2 is valid + + // Ch2 + + if ( channel_count_detected_ch2 == true ) // Channel count for ch2 was detected { - // Check PPM2 selected - if ( switchover_ch2 == true ) // PPM2 is selected + // Do nothing + } + else // Do we have a channel count detection ready ? + { + if ( channel_count_ready_ch2 == true ) // If channel count is ready { - // Load PPM Output with PPM2 - ppm[ ppm2_channel ] = ppm2_width; - } - else // Switch to PPM2 - { - // Delay switchover 1 to 2 here - switchover_ch2 == true; // Switch to PPM2 + // 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 } + } - } - 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; - /* - //Reset throttle failsafe timeout - if( ppm1_channel == 5 ) throttle_timeout = 0; - -CHECK_ERROR: + 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 ); + #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) + // Delay LED toggle 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; - } - + #endif - //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; - - } + 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 // -------------------------