mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-11 02:18:29 -04:00
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)
|
// 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_
|
#ifndef _PPM_ENCODER_H_
|
||||||
#define _PPM_ENCODER_H_
|
#define _PPM_ENCODER_H_
|
||||||
|
|
||||||
@ -609,135 +611,107 @@ ISR( SERVO_INT_VECTOR )
|
|||||||
|
|
||||||
if( servo_input_mode == PPM_REDUNDANCY_MODE )
|
if( servo_input_mode == PPM_REDUNDANCY_MODE )
|
||||||
{
|
{
|
||||||
// -------------------------------------
|
// -------------------------------------
|
||||||
// PPM redundancy mode - variables Init
|
// 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;
|
|
||||||
|
|
||||||
|
|
||||||
// -----------------------------------
|
// PPM1 prepulse start
|
||||||
// PPM redundancy - decoder
|
static uint16_t ppm1_prepulse_start;
|
||||||
// -----------------------------------
|
// PPM2 prepulse start
|
||||||
|
static uint16_t ppm2_prepulse_start;
|
||||||
CHECK_START: // Start of PPM inputs check
|
|
||||||
|
// 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
|
// Store current PPM inputs pins
|
||||||
servo_pins = SERVO_INPUT;
|
servo_pins = SERVO_INPUT;
|
||||||
|
|
||||||
// Calculate servo input pin change mask
|
// Calculate servo input pin change mask
|
||||||
uint8_t servo_change = servo_pins ^ servo_pins_old;
|
uint8_t servo_change = servo_pins ^ servo_pins_old;
|
||||||
|
|
||||||
// -----------------------------------
|
// -----------------------------------
|
||||||
// PPM redundancy - Ch1 decoding
|
// PPM redundancy - Ch1 decoding
|
||||||
// -----------------------------------
|
// -----------------------------------
|
||||||
|
|
||||||
CHECK_LOOP: // Input PPM pins check loop
|
CHECK_LOOP: // Input PPM pins check loop
|
||||||
|
|
||||||
// Check if we have a pin change on PPM channel 1
|
// Check if we have a pin change on PPM channel 1
|
||||||
if( servo_change & 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)
|
|
||||||
{
|
{
|
||||||
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
|
// Check for pre pulse lenght
|
||||||
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
|
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
|
//We have a valid pre pulse
|
||||||
channel_error_ch1 = false;
|
|
||||||
|
|
||||||
if( ppm1_channel == channel_count_ch1 ) // Check for last channel
|
if( ppm1_channel == channel_count_ch1 ) // Check for last channel
|
||||||
{
|
{
|
||||||
// We are at latest PPM channel
|
// We are at latest PPM channel
|
||||||
last_channel_ch1 = true; // Set last channel flag
|
|
||||||
sync_ch1 = false; // Reset sync flag
|
sync_ch1 = false; // Reset sync flag
|
||||||
ppm1_channel = 0; // Reset PPM channel counter
|
ppm1_channel = 0; // Reset PPM channel counter
|
||||||
}
|
}
|
||||||
@ -747,103 +721,96 @@ CHECK_LOOP: // Input PPM pins check loop
|
|||||||
ppm1_channel++;
|
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
|
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
|
// We have a valid sync symbol
|
||||||
if( channel_count_detected_ch1 == false ) // Check if do not have yet channel count detected
|
sync_error_ch1 = false; // Reset sync error flag
|
||||||
{
|
sync_ch1 = true; // Set sync flag
|
||||||
// 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
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
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
|
||||||
|
}
|
||||||
}
|
}
|
||||||
// ------------------------------------------------------------------------------
|
ppm1_prepulse_start = servo_time; // Store prepulse start time
|
||||||
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
|
|
||||||
// -----------------------------------------------------------------------------------------------------------------------
|
|
||||||
}
|
|
||||||
|
|
||||||
// -----------------------------------
|
// -----------------------------------
|
||||||
// PPM redundancy - Ch2 decoding
|
// PPM redundancy - Ch2 decoding
|
||||||
// -----------------------------------
|
// -----------------------------------
|
||||||
|
|
||||||
// Check if we have a pin change on PPM channel 2
|
// Check if we have a pin change on PPM channel 2
|
||||||
if( servo_change & 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)
|
|
||||||
{
|
{
|
||||||
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
|
// Check for pre pulse lenght
|
||||||
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
|
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
|
//We have a valid pre pulse
|
||||||
channel_error_ch2 = false;
|
|
||||||
|
|
||||||
if( ppm2_channel == channel_count_ch2 ) // Check for last channel
|
if( ppm2_channel == channel_count_ch2 ) // Check for last channel
|
||||||
{
|
{
|
||||||
// We are at latest PPM channel
|
// We are at latest PPM channel
|
||||||
last_channel_ch2 = true; // Set last channel flag
|
|
||||||
sync_ch2 = false; // Reset sync flag
|
sync_ch2 = false; // Reset sync flag
|
||||||
ppm2_channel = 0; // Reset PPM channel counter
|
ppm2_channel = 0; // Reset PPM channel counter
|
||||||
}
|
}
|
||||||
@ -853,260 +820,276 @@ CHECK_LOOP: // Input PPM pins check loop
|
|||||||
ppm2_channel++;
|
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
|
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
|
// We have a valid sync symbol
|
||||||
if( channel_count_detected_ch2 == false ) // Check if do not have yet channel count detected
|
sync_error_ch2 = false; // Reset sync error flag
|
||||||
{
|
sync_ch2 = true; // Set sync flag
|
||||||
// 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
|
|
||||||
}
|
|
||||||
}
|
}
|
||||||
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 // Check for PPM1 selected
|
||||||
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
|
if ( switchover_ch2 == false ) // PPM1 is selected
|
||||||
{
|
{
|
||||||
// We have a valid sync symbol
|
// Load PPM Output with PPM1
|
||||||
sync_error_ch2 = false; // Reset sync error flag
|
ppm[ ppm1_channel ] = ppm1_width;
|
||||||
sync_ch2 = true; // Set sync flag
|
|
||||||
}
|
}
|
||||||
else // We did not find a valid sync symbol
|
else // PPM1 is not selected
|
||||||
{
|
{
|
||||||
sync_error_ch2 = true; // Set sync error flag
|
// Delay switchover 2 to 1 here
|
||||||
sync_ch2 = false; // Reset sync flag
|
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
|
else // PPM1 is not valid
|
||||||
// -----------------------------------------------------------------------------------------------------------------------
|
|
||||||
}
|
|
||||||
|
|
||||||
// -----------------------------------
|
|
||||||
// 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
|
// Check for ppm2 validity
|
||||||
if ( sync_error_ch2 == false ) && ( channel_error_ch2 == false) // PPM2 is valid
|
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
|
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
|
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
|
// Check for detection counter
|
||||||
ppm[ ppm1_channel ] = ppm1_width;
|
if ( channel_count_detection_counter_ch1 < CHANNEL_COUNT_DETECTION_THRESHOLD ) // Detection counter < Threshold
|
||||||
}
|
{
|
||||||
else // PPM1 is not selected
|
// Compare channel count with previous value
|
||||||
{
|
if ( channel_count_ch1 == previous_channel_count_ch1 ) // We have the same value
|
||||||
// Delay switchover 2 to 1 here
|
{
|
||||||
switchover_ch2 == false; // Switch to PPM1
|
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
|
// Ch2
|
||||||
{
|
|
||||||
// Check for ppm2 validity
|
if ( channel_count_detected_ch2 == true ) // Channel count for ch2 was detected
|
||||||
if ( sync_error_ch2 == false ) && ( channel_error_ch2 == false) // PPM2 is valid
|
|
||||||
{
|
{
|
||||||
// Check PPM2 selected
|
// Do nothing
|
||||||
if ( switchover_ch2 == true ) // PPM2 is selected
|
}
|
||||||
|
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
|
// Check for detection counter
|
||||||
ppm[ ppm2_channel ] = ppm2_width;
|
if ( channel_count_detection_counter_ch2 < CHANNEL_COUNT_DETECTION_THRESHOLD ) // Detection counter < Threshold
|
||||||
}
|
{
|
||||||
else // Switch to PPM2
|
// Compare channel count with previous value
|
||||||
{
|
if ( channel_count_ch2 == previous_channel_count_ch2 ) // We have the same value
|
||||||
// Delay switchover 1 to 2 here
|
{
|
||||||
switchover_ch2 == true; // Switch to PPM2
|
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
|
//Reset throttle failsafe timeout
|
||||||
{
|
if( ppm1_channel == 5 ) throttle_timeout = 0;
|
||||||
// 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
|
|
||||||
}
|
|
||||||
}
|
|
||||||
|
|
||||||
/*
|
CHECK_ERROR:
|
||||||
//Reset throttle failsafe timeout
|
|
||||||
if( ppm1_channel == 5 ) throttle_timeout = 0;
|
|
||||||
|
|
||||||
CHECK_ERROR:
|
|
||||||
|
|
||||||
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
|
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
|
||||||
// Delay LED toggle
|
// 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;
|
led_delay = 0;
|
||||||
}
|
#endif
|
||||||
#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
|
CHECK_DONE:
|
||||||
// PCIFR |= (1 << SERVO_INT_CLEAR_FLAG);
|
|
||||||
|
// Reset Watchdog Timer
|
||||||
// Leave interrupt
|
wdt_reset();
|
||||||
return;
|
|
||||||
|
// 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 redundancy mode END
|
||||||
// -------------------------
|
// -------------------------
|
||||||
|
|||||||
Loading…
Reference in New Issue
Block a user