Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-18 14:48:28 -04:00
Code Issues Packages Projects Releases Wiki Activity

ArduPPM v2.3.12 ATMega32u2 (APM 2.x)

Browse Source 
---------------------------------------------
- New improved fail-safe detection and handeling for single or multible signal loss and receiver malfuntion
- Improved LED status for APM 2.x
- Improved jitter performance (PPM output using nested interrupts)

-------------------------------------------------------------
ARDUPPM OPERATIONAL DESCRIPTION
-------------------------------------------------------------

APM 2.x LED STATUS:
-------------------
RX - OFF         = No input signal detected
RX - SLOW TOGGLE = Input signal OK
RX - FAST TOGGLE = Invalid input signal(s) detected
RX - ON          = Input signal(s) lost during flight and fail-safe activated
TX - OFF         = PPM output disabled
TX - FAST TOGGLE = PPM output enabled
TX - SLOW TOGGLE = PPM pass-trough mode

SERVO INPUT (PWM) MODE:
 -----------------------
- PPM output will not be enabled unless a input signal has been detected and verified
- Verified inputs are lost during operaton (lose servo wire or receiver malfunction):
  + The PPM output channel for the lost input will be set to the default fail-safe value
  + PPM throttle output (ch3) will be permanently set to fail-safe (900us)
- Lost channel signal is restored:
  + PPM output for the restored channel will be updated with the valid signal
  + PPM throttle output (ch3) will not be restored, and will continue to output fail-safe (900us)

PPM PASS-THROUGH MODE (signal pin 2&3 shorted):
-----------------------------------------------
- PPM output will not be enabled unless a input signal has been detected
- Active signal on input channel 1 has been detected:
  + Any input level changes will be passed directly to the PPM output (PPM pass-trough)
  + If no input level changes are detected withing 250ms:
    + PPM output is enabled and default fail-safe values for all eight channels transmitted
    + Input level change detected again, PPM fail-safe output is terminated and normal PPM pass-through operation is restored
This commit is contained in:
John Arne Birkeland 2012-11-23 21:53:35 +01:00
parent 6d7eb2a0db
commit 3783d0b6de
2 changed files with 240 additions and 134 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
Tools/ArduPPM/ATMega32U2/Projects/arduino-usbserial/makefile
View File

@ -243,6 +243,7 @@ CPPDEFS += $(LUFA_OPTS)
CFLAGS = -g$(DEBUG) CFLAGS = -g$(DEBUG)
CFLAGS += $(CDEFS) CFLAGS += $(CDEFS)
CFLAGS += -O$(OPT) CFLAGS += -O$(OPT)
#CFLAGS += -mcall-prologues
CFLAGS += -funsigned-char CFLAGS += -funsigned-char
CFLAGS += -funsigned-bitfields CFLAGS += -funsigned-bitfields
CFLAGS += -ffunction-sections CFLAGS += -ffunction-sections

357
Tools/ArduPPM/Libraries/PPM_Encoder.h
View File

@ -1,12 +1,47 @@
// ------------------------------------------------------------- // -------------------------------------------------------------
// PPM ENCODER V2.2.69 (02-11-2012) // ArduPPM (PPM Encoder) V2.3.12
// ------------------------------------------------------------- // -------------------------------------------------------------
// Improved servo to ppm for ArduPilot MEGA v1.x (ATmega328p), // Improved servo to ppm for ArduPilot MEGA v1.x (ATmega328p),
// PhoneDrone and APM2 (ATmega32u2) // PhoneDrone and APM2.x (ATmega32u2)
// By: John Arne Birkeland - 2012 // By: John Arne Birkeland - 2012
// APM v1.x adaptation and "difficult" receiver testing by Olivier ADLER // APM v1.x adaptation and "difficult" receiver testing by Olivier ADLER
// ------------------------------------------------------------- // -------------------------------------------------------------
// -------------------------------------------------------------
// ARDUPPM OPERATIONAL DESCRIPTION
// -------------------------------------------------------------
// APM 2.x LED STATUS:
// -------------------
// RX - OFF = No input signal detected
// RX - SLOW TOGGLE = Input signal OK
// RX - FAST TOGGLE = Invalid input signal(s) detected
// RX - ON = Input signal(s) lost during flight and fail-safe activated
// TX - OFF = PPM output disabled
// TX - FAST TOGGLE = PPM output enabled
// TX - SLOW TOGGLE = PPM pass-trough mode
// SERVO INPUT (PWM) MODE:
// -----------------------
// - PPM output will not be enabled unless a input signal has been detected and verified
// - Verified inputs are lost during operaton (lose servo wire or receiver malfunction):
// + The PPM output channel for the lost input will be set to the default fail-safe value
// + PPM throttle output (ch3) will be permanently set to fail-safe (900us)
// - Lost channel signal is restored:
// + PPM output for the restored channel will be updated with the valid signal
// + PPM throttle output (ch3) will not be restored, and will continue to output fail-safe (900us)
// PPM PASS-THROUGH MODE (signal pin 2&3 shorted):
// -----------------------------------------------
// - PPM output will not be enabled unless a input signal has been detected
// - Active signal on input channel 1 has been detected:
// + Any input level changes will be passed directly to the PPM output (PPM pass-trough)
// + If no input level changes are detected withing 250ms:
// + PPM output is enabled and default fail-safe values for all eight channels transmitted
// + Input level change detected again, PPM fail-safe output is terminated and normal PPM pass-through operation is restored
// Changelog: // Changelog:
// 01-08-2011 // 01-08-2011
@ -51,8 +86,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.
@ -69,6 +104,20 @@
// 16-11-2012 // 16-11-2012
// V2.3.1 - Improved watchdog timer reset, so that only valid input signals will prevent the watchdog timer from triggering // V2.3.1 - Improved watchdog timer reset, so that only valid input signals will prevent the watchdog timer from triggering
// 22-11-2012
// 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
// 23-11-2012 !VERY EXPERIMENTAL!
// V2.3.11 - Nested interrupt PWM output interrupt (PPM generator) for greatly improved jitter performance
// - Active input channel test during init removed
// - Implemented dynamic testing of active input channels
// 23-12-2012
// V2.3.12 - New improved fail-safe detection and handeling for single or multible signal loss and receiver malfuntion
// - Improved LED status for APM 2.x
// - Improved jitter performance (PPM output using nested interrupts)
// ------------------------------------------------------------- // -------------------------------------------------------------
#ifndef _PPM_ENCODER_H_ #ifndef _PPM_ENCODER_H_
@ -84,16 +133,17 @@
// ------------------------------------------------------------- // -------------------------------------------------------------
// SERVO INPUT FILTERS // SERVO INPUT FILTERS AND PARAMETERS
// ------------------------------------------------------------- // -------------------------------------------------------------
// Using both filters is not recommended and may reduce servo input resolution // Using both filters is not recommended and may reduce servo input resolution
// #define _AVERAGE_FILTER_ // Average filter to smooth servo input capture jitter //#define _AVERAGE_FILTER_ // Average filter to smooth servo input capture jitter
// #define _JITTER_FILTER_ // Cut filter to remove 0,5us servo input capture jitter #define _JITTER_FILTER_ 2 // Cut filter to remove servo input capture jitter (1 unit = 0.5us)
#define _INPUT_ERROR_TRIGGER_ 100 // Number of invalid input signals allowed before triggering alarm
// ------------------------------------------------------------- // -------------------------------------------------------------
#ifndef F_CPU #ifndef F_CPU
#define F_CPU 16000000UL #define F_CPU 16000000UL
#endif #endif
#ifndef true #ifndef true
@ -109,7 +159,7 @@
#endif #endif
// Version stamp for firmware hex file ( decode hex file using <avr-objdump -s file.hex> and look for "ArduPPM" string ) // Version stamp for firmware hex file ( decode hex file using <avr-objdump -s file.hex> and look for "ArduPPM" string )
const char ver[13] = "ArduPPMv2.3.0"; const char ver[15] = "ArduPPMv2.3.12";
// ------------------------------------------------------------- // -------------------------------------------------------------
// INPUT MODE // INPUT MODE
@ -157,7 +207,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
@ -172,7 +222,7 @@ volatile uint8_t servo_input_mode = JUMPER_SELECT_MODE;
// ------------------------------------------------------------- // -------------------------------------------------------------
// SERVO FAILSAFE VALUES // SERVO FAILSAFE VALUES
// ------------------------------------------------------------- // -------------------------------------------------------------
const uint16_t failsafe_ppm[ PPM_ARRAY_MAX ] = volatile uint16_t failsafe_ppm[ PPM_ARRAY_MAX ] =
{ {
PPM_PRE_PULSE, PPM_PRE_PULSE,
PPM_SERVO_CENTER, // Channel 1 PPM_SERVO_CENTER, // Channel 1
@ -225,6 +275,8 @@ volatile uint16_t ppm[ PPM_ARRAY_MAX ] =
#define PPM_TIMEOUT_VALUE 40 // ~1sec before triggering missing channel detection #define PPM_TIMEOUT_VALUE 40 // ~1sec before triggering missing channel detection
volatile uint8_t ppm_timeout[ PPM_ARRAY_MAX ]; volatile uint8_t ppm_timeout[ PPM_ARRAY_MAX ];
// Servo input channel connected flag
volatile bool servo_input_connected[ PPM_ARRAY_MAX ];
// AVR parameters for PhoneDrone and APM2 boards using ATmega32u2 // AVR parameters for PhoneDrone and APM2 boards using ATmega32u2
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__) #if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
@ -305,6 +357,9 @@ void EVENT_USB_Device_Disconnect(void)
#error NO SUPPORTED DEVICE FOUND! (ATmega16u2 / ATmega32u2 / ATmega328p) #error NO SUPPORTED DEVICE FOUND! (ATmega16u2 / ATmega32u2 / ATmega328p)
#endif #endif
// Used to force throttle fail-safe mode (RTL)
volatile bool throttle_failsafe_force = false;
// Used to indicate invalid SERVO input signals // Used to indicate invalid SERVO input signals
volatile uint8_t servo_input_errors = 0; volatile uint8_t servo_input_errors = 0;
@ -317,6 +372,7 @@ volatile bool ppm_generator_active = false;
// Used to indicate a brownout restart // Used to indicate a brownout restart
volatile bool brownout_reset = false; volatile bool brownout_reset = false;
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------
// PPM GENERATOR START - TOGGLE ON COMPARE INTERRUPT ENABLE // PPM GENERATOR START - TOGGLE ON COMPARE INTERRUPT ENABLE
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------
@ -326,12 +382,12 @@ void ppm_start( void )
if( ppm_generator_active ) return; if( ppm_generator_active ) return;
// Store interrupt status and register flags // Store interrupt status and register flags
uint8_t SREG_tmp = SREG; volatile uint8_t SREG_tmp = SREG;
// 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
@ -347,7 +403,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
@ -358,13 +414,13 @@ void ppm_start( void )
// Indicate that PPM generator is active // Indicate that PPM generator is active
ppm_generator_active = true; ppm_generator_active = true;
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
// Turn on TX led if PPM generator is active
PORTD &= ~( 1<< PD5 );
#endif
// 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__)
// Turn on TX led if PPM generator is active
PORTD &= ~( 1<< PD5 );
#endif
} }
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------
@ -373,7 +429,7 @@ void ppm_start( void )
void ppm_stop( void ) void ppm_stop( void )
{ {
// Store interrupt status and register flags // Store interrupt status and register flags
uint8_t SREG_tmp = SREG; volatile uint8_t SREG_tmp = SREG;
// Stop interrupts // Stop interrupts
cli(); cli();
@ -388,13 +444,13 @@ void ppm_stop( void )
// Indicate that PPM generator is not active // Indicate that PPM generator is not active
ppm_generator_active = false; ppm_generator_active = false;
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
// Turn off TX led if PPM generator is off
PORTD |= ( 1<< PD5 );
#endif
// 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__)
// Turn off TX led if PPM generator is off
PORTD |= ( 1<< PD5 );
#endif
} }
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------
@ -406,12 +462,11 @@ ISR( WDT_vect ) // If watchdog is triggered then enable missing signal flag and
if ( ppm_generator_active || brownout_reset ) if ( ppm_generator_active || brownout_reset )
{ {
// Copy failsafe values to ppm[..] // Copy failsafe values to ppm[..]
for ( uint8_t i = 0; i < PPM_ARRAY_MAX; i++ ) for( volatile uint8_t i = 0; i < PPM_ARRAY_MAX; i++ )
{ {
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 )
@ -419,7 +474,15 @@ 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;
} }
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
// Turn on RX led if signal is missing
if( !servo_input_missing )
{
PORTD &= ~( 1<< PD4 );
}
#endif
// Set missing receiver signal flag // Set missing receiver signal flag
servo_input_missing = true; servo_input_missing = true;
@ -427,10 +490,6 @@ 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__)
// Turn on RX led if failsafe has triggered after ppm generator i active
if( ppm_generator_active ) PORTD &= ~( 1<< PD4 );
#endif
} }
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------
@ -441,19 +500,23 @@ ISR( WDT_vect ) // If watchdog is triggered then enable missing signal flag and
ISR( SERVO_INT_VECTOR ) 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[ 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
uint8_t servo_pins; uint8_t servo_pins;
uint8_t servo_change;
uint8_t servo_pin;
uint8_t ppm_channel;
// Read current servo pulse change time // Read current servo pulse change time
uint16_t servo_time = SERVO_TIMER_CNT; uint16_t servo_time = SERVO_TIMER_CNT;
@ -488,93 +551,117 @@ 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;
} }
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------
// SERVO PWM MODE // SERVO PWM MODE
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------
CHECK_PINS_START: // Start of servo input check CHECK_PINS_START: // Start of servo input check
// Store current servo input pins // Store current servo input 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; servo_change = servo_pins ^ servo_pins_old;
// Set initial servo pin and channel // Set initial servo pin and ppm[..] index
uint8_t servo_pin = 1; servo_pin = 1;
uint8_t servo_channel = 0; ppm_channel = 1;
CHECK_PINS_LOOP: // Input servo pin check loop CHECK_PINS_LOOP: // Input servo pin check loop
// Check for pin change on current servo channel // Check for pin change on current servo channel
if( servo_change & servo_pin ) if( servo_change & servo_pin )
{ {
// Remove processed pin change from bitmask
servo_change &= ~servo_pin;
// High (raising edge) // High (raising edge)
if( servo_pins & servo_pin ) if( servo_pins & servo_pin )
{ {
servo_start[ servo_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[ 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();
// Calculate servo channel position in ppm[..] // Set servo input missing flag false to indicate that we have received servo input signals
uint8_t _ppm_channel = ( servo_channel << 1 ) + 1; servo_input_missing = false;
//Reset ppm single channel fail-safe timeout // Channel has received a valid signal, so it must be connected
ppm_timeout[ _ppm_channel ] = 0; servo_input_connected [ ppm_channel ] = true;
//Reset ppm single channel fail-safe timeout
ppm_timeout[ ppm_channel ] = 0;
// Check for forced throttle fail-safe
if( throttle_failsafe_force )
{
if( ppm_channel == 5 )
{
// Force throttle fail-safe
servo_width = PPM_THROTTLE_FAILSAFE;
}
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
// Turn on RX LED if throttle fail-safe is forced
PORTD &= ~(1 << PD4);
#endif
}
#ifdef _AVERAGE_FILTER_ #ifdef _AVERAGE_FILTER_
// Average filter to smooth input jitter // Average filter to smooth input jitter
servo_width += ppm[ _ppm_channel ]; servo_width += ppm[ ppm_channel ];
servo_width >>= 1; servo_width >>= 1;
#endif #endif
#ifdef _JITTER_FILTER_ #ifdef _JITTER_FILTER_
// 0.5us cut filter to remove input jitter // 0.5us cut filter to remove input jitter
int16_t ppm_tmp = ppm[ _ppm_channel ] - servo_width; int16_t ppm_tmp = ppm[ ppm_channel ] - servo_width;
if( ppm_tmp == 1 ) goto CHECK_PINS_NEXT; if( ppm_tmp <= _JITTER_FILTER_ && ppm_tmp >= -_JITTER_FILTER_ ) goto CHECK_PINS_NEXT;
if( ppm_tmp == -1 ) goto CHECK_PINS_NEXT;
#endif #endif
// Update ppm[..] // Update ppm[..]
ppm[ _ppm_channel ] = servo_width; ppm[ ppm_channel ] = servo_width;
} }
} }
CHECK_PINS_NEXT: CHECK_PINS_NEXT:
// Select next servo pin // Are we done processing pins?
servo_pin <<= 1; if( servo_change )
{
// Select next ppm[..] index
ppm_channel += 2;
// Select next servo channel // Select next servo pin
servo_channel++; servo_pin <<= 1;
// Check channel and process if needed // Check next pin
if( servo_channel < SERVO_CHANNELS ) goto CHECK_PINS_LOOP; goto CHECK_PINS_LOOP;
}
// All changed pins have been checked
goto CHECK_PINS_DONE; goto CHECK_PINS_DONE;
CHECK_PINS_ERROR: CHECK_PINS_ERROR:
@ -582,40 +669,37 @@ 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__)
// Delay LED toggle
led_delay = 0;
#endif
goto CHECK_PINS_NEXT; goto CHECK_PINS_NEXT;
// All servo input pins has now been processed // Processing done, cleanup and exit
CHECK_PINS_DONE: CHECK_PINS_DONE:
// Set servo input missing flag false to indicate that we have received servo input signals // Start PPM generator if not already running
servo_input_missing = false; if( !ppm_generator_active ) ppm_start();
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
// Toggle RX LED when finished receiving servo pulses
if( ++led_delay == 0 ) // Toggle led every 128th or 255th interval
{
PIND |= ( 1<< PD4 );
// Fast toggle on servo input errors
if( servo_input_errors > _INPUT_ERROR_TRIGGER_ )
{
led_delay = 223;
}
}
#endif
// Store current servo input pins for next check // Store current servo input pins for next check
servo_pins_old = servo_pins; 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
//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;
// Clear interrupt event from already processed pin changes // Clear interrupt event from already processed pin changes
PCIFR |= (1 << SERVO_INT_CLEAR_FLAG); //PCIFR |= (1 << SERVO_INT_CLEAR_FLAG);
} }
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------
@ -623,36 +707,51 @@ CHECK_PINS_DONE:
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------
// PPM OUTPUT - TIMER1 COMPARE INTERRUPT // PPM OUTPUT - TIMER1 COMPARE INTERRUPT
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------
ISR( PPM_INT_VECTOR ) // Current active ppm channel
volatile uint8_t ppm_out_channel = PPM_ARRAY_MAX - 1;
ISR( PPM_INT_VECTOR, ISR_NOBLOCK )
{ {
// Current active ppm channel // ------------------------------------------------------------------------------
static uint8_t ppm_channel = PPM_ARRAY_MAX - 1; // !! NESTED INTERRUPT !!
// - ALL VARIABLES SHOULD BE GLOBAL VOLATILE
// - ACCESSING VARIABLES >8BIT MUST BE DONE ATOMIC USING CLI/SEI
// ------------------------------------------------------------------------------
// Update timing for next compare toggle with either current ppm input value, or fail-safe value if there is a channel timeout. // Update timing for next compare toggle with either current ppm input value, or fail-safe value if there is a channel timeout.
if( ppm_timeout[ ppm_channel ] > PPM_TIMEOUT_VALUE ) if( ppm_timeout[ ppm_out_channel ] > PPM_TIMEOUT_VALUE )
{ {
// Use ppm fail-safe value // Use ppm fail-safe value
PPM_COMPARE += failsafe_ppm[ ppm_channel ]; cli();
PPM_COMPARE += failsafe_ppm[ ppm_out_channel ];
sei();
// Did we lose an active servo input channel? If so force throttle fail-safe (RTL)
if( servo_input_connected[ ppm_out_channel ] )
{
throttle_failsafe_force = true;
}
} }
else else
{ {
// Use latest ppm input value // Use latest ppm input value
PPM_COMPARE += ppm[ ppm_channel ]; cli();
PPM_COMPARE += ppm[ ppm_out_channel ];
sei();
// Increment channel timeout (reset to zero in input interrupt each time a valid signal is detected) // Increment channel timeout (reset to zero in input interrupt each time a valid signal is detected)
ppm_timeout[ ppm_channel ] ++; ppm_timeout[ ppm_out_channel ] ++;
} }
// Select the next ppm channel // Select the next ppm channel
if( ++ppm_channel >= PPM_ARRAY_MAX ) if( ++ppm_out_channel >= PPM_ARRAY_MAX )
{ {
ppm_channel = 0; ppm_out_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
} }
} }
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------
@ -713,6 +812,12 @@ void ppm_encoder_init( void )
// APM USB connection status UART MUX selector pin // APM USB connection status UART MUX selector pin
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------
USB_DDR |= (1 << USB_PIN); // Set USB pin to output USB_DDR |= (1 << USB_PIN); // Set USB pin to output
DDRD |= (1<<PD4); // RX LED OUTPUT
DDRD |= (1<<PD5); // TX LED OUTPUT
PORTD |= (1<<PD4); // RX LED OFF
PORTD |= (1<<PD5); // TX LED OFF
#endif #endif
@ -777,31 +882,34 @@ 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 )
{
// Interrupt on all input pins
SERVO_INT_MASK = 0xFF;
}
// 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);
@ -816,9 +924,6 @@ void ppm_encoder_init( void )
WDTCSR |= (1<<WDCE) | (1<<WDE ); WDTCSR |= (1<<WDCE) | (1<<WDE );
// Set 250 ms watchdog timeout and enable interrupt // Set 250 ms watchdog timeout and enable interrupt
WDTCSR = (1<<WDIE) | (1<<WDP2); WDTCSR = (1<<WDIE) | (1<<WDP2);
} }
// ------------------------------------------------------------------------------ // ------------------------------------------------------------------------------