Mirror
/
ardupilot
mirror of https://github.com/ArduPilot/ardupilot
5
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

ArduPPM(experimental): Active input channel detection during init

This commit is contained in:
John Arne Birkeland 2012-11-22 16:54:04 +01:00
parent 6372e8591e
commit 147fb8c17a
1 changed files with 200 additions and 108 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

276
Tools/ArduPPM/WorkBasket/Experimental/PPM_Encoder.h
View File

@ -53,8 +53,8 @@
// 15-03-2012 // 15-03-2012
// V2.2.66 - Added APM2 (ATmega32U2) support for using TX and RX status leds to indicate PWM and PPM traffic // V2.2.66 - Added APM2 (ATmega32U2) support for using TX and RX status leds to indicate PWM and PPM traffic
// - <RX>: <OFF> = no pwm input detected, <TOGGLE> = speed of toggle indicate how many channel are active, <ON> = input lost (failsafe) // - <RX>: <OFF> = no pwm input detected, <TOGGLE> = speed of toggle indicate how many channel are active, <ON> = input lost (failsafe)
// - <TX>: <OFF> = ppm output not started, <FAST TOGGLE> = normal PWM->PPM output or PPM passtrough failsafe, <SLOW TOGGLE> = PPM passtrough // - <TX>: <OFF> = ppm output not started, <FAST TOGGLE> = normal PWM->PPM output or PPM passtrough failsafe, <SLOW TOGGLE> = PPM passtrough
// 03-06-2012 // 03-06-2012
// V2.2.67 - Implemented detection and failsafe (throttle = 900us) for missing throttle signal. // V2.2.67 - Implemented detection and failsafe (throttle = 900us) for missing throttle signal.
@ -73,6 +73,7 @@
// 22-11-2012 // 22-11-2012
// V2.3.11 - Very experimental test forcing throttle fail-safe (RTL) on single channel loss. !DO NOT RELEASE TO PUBLIC! // V2.3.11 - Very experimental test forcing throttle fail-safe (RTL) on single channel loss. !DO NOT RELEASE TO PUBLIC!
// - Test for active input channels during init
// ------------------------------------------------------------- // -------------------------------------------------------------
@ -98,7 +99,7 @@
// ------------------------------------------------------------- // -------------------------------------------------------------
#ifndef F_CPU #ifndef F_CPU
#define F_CPU 16000000UL #define F_CPU 16000000UL
#endif #endif
#ifndef true #ifndef true
@ -162,7 +163,7 @@ volatile uint8_t servo_input_mode = JUMPER_SELECT_MODE;
// ------------------------------------------------------------- // -------------------------------------------------------------
// PPM OUTPUT SETTINGS // PPM OUTPUT SETTINGS
// ------------------------------------------------------------- // -------------------------------------------------------------
// #define _POSITIVE_PPM_FRAME_ // Switch to positive pulse PPM // #define _POSITIVE_PPM_FRAME_ // Switch to positive pulse PPM
// (the actual timing is encoded in the length of the low between two pulses) // (the actual timing is encoded in the length of the low between two pulses)
// Number of servo input channels // Number of servo input channels
@ -342,7 +343,7 @@ void ppm_start( void )
// Stop interrupts // Stop interrupts
cli(); cli();
// Make sure initial output state is low // Make sure initial output state is low
PPM_PORT &= ~(1 << PPM_OUTPUT_PIN); PPM_PORT &= ~(1 << PPM_OUTPUT_PIN);
// Wait for output pin to settle // Wait for output pin to settle
@ -358,7 +359,7 @@ void ppm_start( void )
// Set TIMER1 8x prescaler // Set TIMER1 8x prescaler
TCCR1B = ( 1 << CS11 ); TCCR1B = ( 1 << CS11 );
#if defined (_POSITIVE_PPM_FRAME_) #if defined (_POSITIVE_PPM_FRAME_)
// Force output compare to reverse polarity // Force output compare to reverse polarity
TCCR1C |= (1 << PPM_COMPARE_FORCE_MATCH); TCCR1C |= (1 << PPM_COMPARE_FORCE_MATCH);
#endif #endif
@ -372,10 +373,10 @@ void ppm_start( void )
// Restore interrupt status and register flags // Restore interrupt status and register flags
SREG = SREG_tmp; SREG = SREG_tmp;
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
// Turn on TX led if PPM generator is active // Turn on TX led if PPM generator is active
PORTD &= ~( 1<< PD5 ); PORTD &= ~( 1<< PD5 );
#endif #endif
} }
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------
@ -402,10 +403,10 @@ void ppm_stop( void )
// Restore interrupt status and register flags // Restore interrupt status and register flags
SREG = SREG_tmp; SREG = SREG_tmp;
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
// Turn off TX led if PPM generator is off // Turn off TX led if PPM generator is off
PORTD |= ( 1<< PD5 ); PORTD |= ( 1<< PD5 );
#endif #endif
} }
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------
@ -422,7 +423,7 @@ ISR( WDT_vect ) // If watchdog is triggered then enable missing signal flag and
ppm[ i ] = failsafe_ppm[ i ]; ppm[ i ] = failsafe_ppm[ i ];
} }
} }
// If we are in PPM passtrough mode and a input signal has been detected, or if chip has been reset from a brown_out then start the PPM generator. // If we are in PPM passtrough mode and a input signal has been detected, or if chip has been reset from a brown_out then start the PPM generator.
if( ( servo_input_mode == PPM_PASSTROUGH_MODE && servo_input_missing == false ) || brownout_reset ) if( ( servo_input_mode == PPM_PASSTROUGH_MODE && servo_input_missing == false ) || brownout_reset )
@ -430,7 +431,7 @@ ISR( WDT_vect ) // If watchdog is triggered then enable missing signal flag and
// Start PPM generator // Start PPM generator
ppm_start(); ppm_start();
brownout_reset = false; brownout_reset = false;
} }
// Set missing receiver signal flag // Set missing receiver signal flag
servo_input_missing = true; servo_input_missing = true;
@ -438,10 +439,10 @@ ISR( WDT_vect ) // If watchdog is triggered then enable missing signal flag and
// Reset servo input error flag // Reset servo input error flag
servo_input_errors = 0; servo_input_errors = 0;
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
// Turn on RX led if failsafe has triggered after ppm generator i active // Turn on RX led if failsafe has triggered after ppm generator i active
if( ppm_generator_active ) PORTD &= ~( 1<< PD4 ); if( ppm_generator_active ) PORTD &= ~( 1<< PD4 );
#endif #endif
} }
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------
@ -453,15 +454,15 @@ ISR( SERVO_INT_VECTOR )
{ {
// Servo pulse start timing // Servo pulse start timing
//static uint16_t servo_start[ SERVO_CHANNELS ] = { 0, 0, 0, 0, 0, 0, 0, 0 }; //static uint16_t servo_start[ SERVO_CHANNELS ] = { 0, 0, 0, 0, 0, 0, 0, 0 };
static uint16_t servo_start[ PPM_ARRAY_MAX ] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; // We sacrefice some memory but save instructions by working with ppm index count (18) instead of input channel count (8) static uint16_t servo_start[ PPM_ARRAY_MAX ] = { 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 }; // We sacrefice some memory but save instructions by working with ppm index count (18) instead of input channel count (8)
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
// Toggle LED delay // Toggle LED delay
static uint8_t led_delay = 0; static uint8_t led_delay = 0;
#endif #endif
// Servo input pin storage // Servo input pin storage
static uint8_t servo_pins_old = 0; static uint8_t servo_pins_old = 0;
// Used to store current servo input pins // Used to store current servo input pins
@ -501,15 +502,15 @@ ISR( SERVO_INT_VECTOR )
// Set servo input missing flag false to indicate that we have received servo input signals // Set servo input missing flag false to indicate that we have received servo input signals
servo_input_missing = false; servo_input_missing = false;
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
// Toggle TX LED at PPM passtrough // Toggle TX LED at PPM passtrough
if( ++led_delay > 128 ) // Toggle every 128th pulse if( ++led_delay > 128 ) // Toggle every 128th pulse
{ {
// Toggle TX led // Toggle TX led
PIND |= ( 1<< PD5 ); PIND |= ( 1<< PD5 );
led_delay = 0; led_delay = 0;
} }
#endif #endif
// Leave interrupt // Leave interrupt
return; return;
@ -519,9 +520,9 @@ ISR( SERVO_INT_VECTOR )
// SERVO PWM MODE // SERVO PWM MODE
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------
uint8_t servo_change; uint8_t servo_change;
uint8_t servo_pin ; uint8_t servo_pin ;
//uint8_t servo_channel ; //uint8_t servo_channel ;
uint8_t ppm_channel; uint8_t ppm_channel;
CHECK_PINS_START: // Start of servo input check CHECK_PINS_START: // Start of servo input check
@ -534,7 +535,7 @@ CHECK_PINS_START: // Start of servo input check
// Set initial servo pin, channel and ppm[..] index // Set initial servo pin, channel and ppm[..] index
servo_pin = 1; servo_pin = 1;
//servo_channel = 0; //servo_channel = 0;
ppm_channel = 1; ppm_channel = 1;
CHECK_PINS_LOOP: // Input servo pin check loop CHECK_PINS_LOOP: // Input servo pin check loop
@ -545,36 +546,36 @@ CHECK_PINS_LOOP: // Input servo pin check loop
if( servo_pins & servo_pin ) if( servo_pins & servo_pin )
{ {
//servo_start[ servo_channel ] = servo_time; //servo_start[ servo_channel ] = servo_time;
servo_start[ ppm_channel ] = servo_time; servo_start[ ppm_channel ] = servo_time;
} }
else else
{ {
// Get servo pulse width // Get servo pulse width
//uint16_t servo_width = servo_time - servo_start[ servo_channel ] - PPM_PRE_PULSE; //uint16_t servo_width = servo_time - servo_start[ servo_channel ] - PPM_PRE_PULSE;
uint16_t servo_width = servo_time - servo_start[ ppm_channel ] - PPM_PRE_PULSE; uint16_t servo_width = servo_time - servo_start[ ppm_channel ] - PPM_PRE_PULSE;
// Check that servo pulse signal is valid before sending to ppm encoder // Check that servo pulse signal is valid before sending to ppm encoder
if( servo_width > PPM_SERVO_MAX ) goto CHECK_PINS_ERROR; if( servo_width > PPM_SERVO_MAX ) goto CHECK_PINS_ERROR;
if( servo_width < PPM_SERVO_MIN ) goto CHECK_PINS_ERROR; if( servo_width < PPM_SERVO_MIN ) goto CHECK_PINS_ERROR;
// Reset Watchdog Timer // Reset Watchdog Timer
wdt_reset(); wdt_reset();
// Channel has received a valid signal, so it must be connected // Channel has received a valid signal, so it must be connected
servo_input_connected [ ppm_channel ] = true; // servo_input_connected [ ppm_channel ] = true; // Removed - Check performed during init
// Check for forced throttle fail-safe // Check for forced throttle fail-safe
if( throttle_failsafe_force && ppm_channel == 5 ) if( throttle_failsafe_force && ppm_channel == 5 )
{ {
// Force throttle fail-safe // Force throttle fail-safe
ppm_timeout[ ppm_channel ] = 255; ppm_timeout[ ppm_channel ] = 255;
} }
else else
{ {
//Reset ppm single channel fail-safe timeout //Reset ppm single channel fail-safe timeout
ppm_timeout[ ppm_channel ] = 0; ppm_timeout[ ppm_channel ] = 0;
} }
#ifdef _AVERAGE_FILTER_ #ifdef _AVERAGE_FILTER_
// Average filter to smooth input jitter // Average filter to smooth input jitter
@ -602,12 +603,12 @@ CHECK_PINS_NEXT:
// Select next servo channel // Select next servo channel
//servo_channel++; //servo_channel++;
// Select next ppm[..] index // Select next ppm[..] index
ppm_channel += 2; ppm_channel += 2;
// Check channel and process if needed // Check channel and process if needed
//if( servo_channel < SERVO_CHANNELS ) goto CHECK_PINS_LOOP; //if( servo_channel < SERVO_CHANNELS ) goto CHECK_PINS_LOOP;
if( servo_pin ) goto CHECK_PINS_LOOP; // Bitshift wraps to zero (false) if( servo_pin ) goto CHECK_PINS_LOOP; // Bit shifts to zero when all pins have been checked
goto CHECK_PINS_DONE; goto CHECK_PINS_DONE;
@ -616,10 +617,10 @@ CHECK_PINS_ERROR:
// Used to indicate invalid servo input signals // Used to indicate invalid servo input signals
servo_input_errors++; servo_input_errors++;
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
// Delay LED toggle // Delay LED toggle
led_delay = 0; led_delay = 0;
#endif #endif
goto CHECK_PINS_NEXT; goto CHECK_PINS_NEXT;
@ -636,14 +637,14 @@ CHECK_PINS_DONE:
// Start PPM generator if not already running // Start PPM generator if not already running
if( ppm_generator_active == false ) ppm_start(); if( ppm_generator_active == false ) ppm_start();
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
// Toggle RX LED when finished receiving servo pulses // Toggle RX LED when finished receiving servo pulses
if( ++led_delay > 64 ) // Toggle led every 64th time if( ++led_delay > 64 ) // Toggle led every 64th time
{ {
PIND |= ( 1<< PD4 ); PIND |= ( 1<< PD4 );
led_delay = 0; led_delay = 0;
} }
#endif #endif
//Has servo input changed while processing pins, if so we need to re-check pins //Has servo input changed while processing pins, if so we need to re-check pins
if( servo_pins != SERVO_INPUT ) goto CHECK_PINS_START; if( servo_pins != SERVO_INPUT ) goto CHECK_PINS_START;
@ -668,11 +669,11 @@ ISR( PPM_INT_VECTOR )
// Use ppm fail-safe value // Use ppm fail-safe value
PPM_COMPARE += failsafe_ppm[ ppm_channel ]; PPM_COMPARE += failsafe_ppm[ ppm_channel ];
// Did we lose an active servo input channel? If so force throttle fail-safe (RTL) // Did we lose an active servo input channel? If so force throttle fail-safe (RTL)
if( servo_input_connected[ ppm_channel ] ) if( servo_input_connected[ ppm_channel ] )
{ {
throttle_failsafe_force = true; throttle_failsafe_force = true;
} }
} }
else else
{ {
@ -685,14 +686,14 @@ ISR( PPM_INT_VECTOR )
// Select the next ppm channel // Select the next ppm channel
if( ++ppm_channel >= PPM_ARRAY_MAX ) if( ++ppm_channel >= PPM_ARRAY_MAX )
{ {
ppm_channel = 0; ppm_channel = 0;
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
// Blink TX LED when PPM generator has finished a pulse train // Blink TX LED when PPM generator has finished a pulse train
PIND |= ( 1<< PD5 ); PIND |= ( 1<< PD5 );
#endif #endif
} }
} }
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------
@ -817,34 +818,125 @@ void ppm_encoder_init( void )
// later to set the right value // later to set the right value
DDRD |= 1; DDRD |= 1;
if (usb_connected) { if (usb_connected) {
PORTD &= ~1; PORTD &= ~1;
} else { } else {
PORTD |= 1; PORTD |= 1;
} }
#endif #endif
// SERVO/PPM INPUT - PIN CHANGE INTERRUPT // PPM PASS-THROUGH MODE
// ------------------------------------------------------------------------------
if( servo_input_mode == SERVO_PWM_MODE )
{
// Set servo input interrupt pin mask to all 8 servo input channels
SERVO_INT_MASK = 0xFF;
}
if( servo_input_mode == PPM_PASSTROUGH_MODE ) if( servo_input_mode == PPM_PASSTROUGH_MODE )
{ {
// Set servo input interrupt pin mask to servo input channel 1 // Set servo input interrupt pin mask to servo input channel 1
SERVO_INT_MASK = 0x01; SERVO_INT_MASK = 0x01;
} }
// SERVO PWM INPUT MODE
// ------------------------------------------------------------------------------
if( servo_input_mode == SERVO_PWM_MODE )
{
// CHECK FOR ACTIVE INPUT CHANNELS
// ------------------------------------------------------------------------------
// This will poll the input pins looking for pin level changes, and mark the inputs with active signal connections
uint16_t input_channel_count[ SERVO_CHANNELS ];
uint8_t input_current; // Input pin level mask
uint8_t input_previous; // Input pin level mask
uint8_t input_int_mask; // Active input pin interupt mask
INPUT_ACTIVE_CHANNEL_CHECK:
// ~100khz timed polling of input pins for ~0.5sec duration (1us = 16 clocks)
for( uint16_t t = 0; t < 50000; t++ ) // 10us * 1000ms * 0.5sec = 50000 delayed loops at 10us
{
// Get current input pin levels
input_current = SERVO_INPUT;
// Check input pins against previous levels
uint8_t input_change = input_current ^ input_previous;
// Do we need to check for changes?
if( input_change )
{
// Check for change on all input pins
uint8_t input_pin = 1;
uint8_t input_channel = 0;
do
{
// Check for change on current input pin
if( input_change & input_pin )
{
// Pin has changed, count it
input_channel_count[ input_channel ]++;
}
// Select next pin
input_pin <<= 1;
// Select next channel
input_channel++;
} while ( input_channel < SERVO_CHANNELS );
// Store checked input pin levels
input_previous = input_current;
}
// Delay the next input level check
_delay_us( 10 );
}
// Check input_channel_count[..] to see if channels are active.
for( uint8_t input_channel = 0; input_channel < SERVO_CHANNELS; input_channel++ )
{
// Is current channel active?
if( input_channel_count[ input_channel ] > 20 ) // 0.5sec * 40hz * 2 = 40 pulse edges - 50% margin = 20 level changes
{
// Set active channel in servo_input_connected[..]
servo_input_connected[ (input_channel << 1) + 1 ] = true;
// Set interupt pin mask for active channel
input_int_mask |= 1 << input_channel;
}
}
// if no active input channels are found, then re-check until found (might be caused by delayed receiver startup)
// Do not perform re-check if there was a brown-out reset
if( input_int_mask > 0 && !brownout_reset )
{
// Re-check active input channels
goto INPUT_ACTIVE_CHANNEL_CHECK;
}
// Set servo input interrupt pin mask
// Set to all input channel for safety in case of brown-out reset
if( brownout_reset)
{
// Interrupt on all input pins
SERVO_INT_MASK = 0xFF;
}
else
{
// Interrupt only on active input pins
SERVO_INT_MASK = input_int_mask;
}
}
// Enable servo input interrupt // Enable servo input interrupt
PCICR |= (1 << SERVO_INT_ENABLE); PCICR |= (1 << SERVO_INT_ENABLE);
// PPM OUTPUT PIN // PPM OUTPUT
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------
// PPM generator (PWM output timer/counter) is started either by pin change interrupt or by watchdog interrupt
// Set PPM pin to output // Set PPM pin to output
PPM_DDR |= (1 << PPM_OUTPUT_PIN); PPM_DDR |= (1 << PPM_OUTPUT_PIN);
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------
// Enable watchdog interrupt mode // Enable watchdog interrupt mode
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------