mirror of https://github.com/ArduPilot/ardupilot
ArduPPM: Redundancy mode
Fixed logic problem in the decoder (PPM channel increment)
This commit is contained in:
Olivier ADLER
parent
9c40c11e7b
commit
34151cbe91
|
|
@ -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
|
||||
// -------------------------
|
||||
|
|
|
|||
Loading…
Reference in New Issue