mirror of https://github.com/ArduPilot/ardupilot
ArduPPM : Redundancy mode
Work in progress on a new Redundancy dual PPM sum mode for PPM encoder. - New library PPM_Encoder_v3.h and new manual manual_v3.txt - New format conversion capability between input and output PPM frame timings and channel count. This will be experimental until heavily tested. The main goal is to allow the use of low cost satellite receivers in a high safety setup, and allow a new teacher / student RC mode without link between the two pilot transmitters.
This commit is contained in:
Olivier ADLER
parent
4e649a381c
commit
5e65e5ef8a
|
|
@ -0,0 +1,473 @@
|
|||
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
// ArduPPM Version v0.9.87
|
||||
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
// ARDUCOPTER 2 : PPM ENCODER for AT Mega 328p and APM v1.4 Boards
|
||||
// By:John Arne Birkeland - 2011
|
||||
//
|
||||
// By: Olivier ADLER - 2011 - APM v1.4 adaptation and testing
|
||||
//
|
||||
// Compiled with Atmel AVR Studio 4.0 / AVR GCC
|
||||
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
|
||||
// Changelog //
|
||||
//
|
||||
// Code based on John Arne PPM v1 encoder. Mux / Led / Failsafe control from Olivier ADLER.
|
||||
// Adaptation to APM v1.4 / ATMEGA 328p by Olivier ADLER, with great code base, help and advices from John Arne.
|
||||
//
|
||||
// 0.9.0 -> 0.9.4 : experimental versions. Not publicly available. Jitter problems, good reliability.
|
||||
//
|
||||
// New PPM code base V2 from John Arne designed for 32u2 AVRs
|
||||
//
|
||||
// 0.9.5 : first reliable and jitter free version based on new John PPM V2 code and Olivier interrupt nesting idea.
|
||||
// 0.9.6 : enhanced jitter free version with non bloking servo interrupt and ultra fast ppm generator interrupt(John's ideas)
|
||||
// 0.9.7 : mux (passthrough mode) switchover reliability enhancements and error reporting improvements.
|
||||
// 0.9.75 : implemented ppm_encoder.h library with support for both atmega328p and atmega32u2 chips
|
||||
// 0.9.76 : timers 0 and 2 replaced by a delayed loop for simplicity. Timer 0 and 2 are now free for use.
|
||||
// reworked error detection with settable time window, errors threshold and Led delay
|
||||
// 0.9.77 : Implemented ppm_encoder.h into latest version.
|
||||
// 0.9.78 : Implemented optimzed assembly compare interrupt
|
||||
// 0.9.79 : Removed Non Blocking attribute for servo input interrupt
|
||||
// 0.9.80 : Removed non blocking for compare interrupt, added optionnal jitter filter and optionnal non blocking attribute for assembly version of compare interrupt
|
||||
// 0.9.81 : Added PPM PASSTROUGH Mode and LED Codes function to report special modes
|
||||
// 0.9.82 : LED codes function simplification
|
||||
// 0.9.83 : Implemented PPM passtrough failsafe
|
||||
// 0.9.84 : Corrected pin and port names in Encoder-PPM.c according to #define for Mega32U2 compatibility
|
||||
// 0.9.85 : Added brownout reset detection flag
|
||||
// 0.9.86 : Added a #define to disable Radio Passthrough mode (hardware failsafe for Arduplane)
|
||||
// 0.9.87 : #define correction for radio passthrough (was screwed up).
|
||||
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
// PREPROCESSOR DIRECTIVES
|
||||
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
|
||||
#include "..\Libraries\ppm_encoder.h"
|
||||
#include <util/delay.h>
|
||||
|
||||
|
||||
#define ERROR_THRESHOLD 2 // Number of servo input errors before alerting
|
||||
#define ERROR_DETECTION_WINDOW 3000 * LOOP_TIMER_10MS // Detection window for error detection (default to 30s)
|
||||
#define ERROR_CONDITION_DELAY 500 * LOOP_TIMER_10MS // Servo error condition LED delay (LED blinking duration)
|
||||
|
||||
#define PASSTHROUGH_MODE_ENABLED // Comment this line to remove CH8 radio passthrough mode support (hardware failsafe for Arduplane)
|
||||
#define PASSTHROUGH_CHANNEL 8 * 2 // Channel for passthrough mode selection
|
||||
#define PASSTHROUGH_CHANNEL_OFF_US ONE_US * 1600 - PPM_PRE_PULSE // Passthrough off threshold
|
||||
#define PASSTHROUGH_CHANNEL_ON_US ONE_US * 1800 - PPM_PRE_PULSE // Passthrough on threshold
|
||||
|
||||
#define THROTTLE_CHANNEL 3 * 2 // Throttle Channel
|
||||
#define THROTTLE_CHANNEL_LED_TOGGLE_US ONE_US * 1200 - PPM_PRE_PULSE // Throttle Channel Led toggle threshold
|
||||
#define LED_LOW_BLINKING_RATE 125 * LOOP_TIMER_10MS // Led blink rate for low throttle position (half period)
|
||||
|
||||
// Timers
|
||||
|
||||
#define TIMER0_10MS 156 // Timer0 ticks for 10 ms duration
|
||||
#define TIMER1_10MS 20000 // Timer1 ticks for 10 ms duration
|
||||
#define TIMER2_100MS 1562 // Timer2 ticks for 100 ms duration
|
||||
#define LOOP_TIMER_10MS 10 // Loop timer ticks for 10 ms duration
|
||||
|
||||
// LED Code
|
||||
|
||||
#define SPACE_SHORT_DURATION 40 * LOOP_TIMER_10MS // Space after short symbol
|
||||
#define SPACE_LONG_DURATION 75 * LOOP_TIMER_10MS // Space after long symbol
|
||||
#define SYMBOL_SHORT_DURATION 20 * LOOP_TIMER_10MS // Short symbol duration
|
||||
#define SYMBOL_LONG_DURATION 100 * LOOP_TIMER_10MS // Long symbol duration
|
||||
#define INTER_CODE_DURATION 150 * LOOP_TIMER_10MS // Inter code duration
|
||||
|
||||
#define INTER_CODE 0 // Symbols value for coding
|
||||
#define SHORT_SYMBOL 1
|
||||
#define LONG_SYMBOL 2
|
||||
#define SHORT_SPACE 3
|
||||
#define LONG_SPACE 4
|
||||
#define LOOP 5
|
||||
|
||||
|
||||
|
||||
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
// PPM ENCODER INIT AND AUXILIARY TASKS
|
||||
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
|
||||
int main(void)
|
||||
{
|
||||
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
// LOCAL VARIABLES
|
||||
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
bool init = true; // We are inside init sequence
|
||||
bool mux_passthrough = false; // Mux passthrough mode status Flag : passthrough is off
|
||||
uint16_t led_acceleration; // Led acceleration based on throttle stick position
|
||||
bool servo_error_condition = false; // Servo signal error condition
|
||||
|
||||
static uint16_t servo_error_detection_timer=0; // Servo error detection timer
|
||||
static uint16_t servo_error_condition_timer=0; // Servo error condition timer
|
||||
static uint16_t blink_led_timer = 0; // Blink led timer
|
||||
|
||||
#ifdef PASSTHROUGH_MODE_ENABLED
|
||||
static uint8_t mux_timer = 0; // Mux timer
|
||||
static uint8_t mux_counter = 0; // Mux counter
|
||||
static int8_t mux_check = 0;
|
||||
static uint16_t mux_ppm = 500;
|
||||
#endif
|
||||
|
||||
static uint16_t led_code_timer = 0; // Blink Code Timer
|
||||
static uint8_t led_code_symbol = 0; // Blink Code current symbol
|
||||
|
||||
|
||||
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
// LOCAL FUNCTIONS
|
||||
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// Led blinking (non blocking) function
|
||||
// ------------------------------------------------------------------------------
|
||||
|
||||
uint8_t blink_led ( uint16_t half_period ) // ( half_period max = 65 s )
|
||||
{
|
||||
|
||||
blink_led_timer++;
|
||||
|
||||
if ( blink_led_timer < half_period ) // If half period has not been reached
|
||||
{
|
||||
return 0; // Exit timer function and return 0
|
||||
}
|
||||
else // half period reached - LED Toggle
|
||||
{
|
||||
PPM_PORT ^= ( 1 << PB0 ); // Toggle status LED
|
||||
blink_led_timer = 0; // Blink led timer reset
|
||||
|
||||
return 1; // half period reached - Exit timer function and return 1
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// Led code (non blocking) function
|
||||
// ------------------------------------------------------------------------------
|
||||
|
||||
void blink_code_led ( uint8_t code )
|
||||
{
|
||||
|
||||
const uint8_t coding[2][14] = {
|
||||
|
||||
// PPM_PASSTROUGH_MODE
|
||||
{ INTER_CODE, LONG_SYMBOL, LONG_SPACE, SHORT_SYMBOL, SHORT_SPACE, SHORT_SYMBOL, LOOP },
|
||||
|
||||
// JETI_MODE
|
||||
{ INTER_CODE, LONG_SYMBOL, LONG_SPACE, SHORT_SYMBOL, SHORT_SPACE, SHORT_SYMBOL, SHORT_SPACE, SHORT_SYMBOL,LOOP }
|
||||
|
||||
};
|
||||
|
||||
led_code_timer++;
|
||||
|
||||
|
||||
switch ( coding [ code - 2 ] [ led_code_symbol ] )
|
||||
{
|
||||
case INTER_CODE:
|
||||
|
||||
if ( led_code_timer < ( INTER_CODE_DURATION ) ) return;
|
||||
else PPM_PORT |= ( 1 << PB0 ); // Enable status LED
|
||||
break;
|
||||
|
||||
case LONG_SYMBOL: // Long symbol
|
||||
|
||||
if ( led_code_timer < ( SYMBOL_LONG_DURATION ) ) return;
|
||||
else PPM_PORT &= ~( 1 << PB0 ); // Disable status LED
|
||||
break;
|
||||
|
||||
case SHORT_SYMBOL: // Short symbol
|
||||
|
||||
if ( led_code_timer < ( SYMBOL_SHORT_DURATION ) ) return;
|
||||
else PPM_PORT &= ~( 1 << PB0 ); // Disable status LED
|
||||
break;
|
||||
|
||||
case SHORT_SPACE: // Short space
|
||||
|
||||
if ( led_code_timer < ( SPACE_SHORT_DURATION ) ) return;
|
||||
else PPM_PORT |= ( 1 << PB0 ); // Enable status LED
|
||||
break;
|
||||
|
||||
case LONG_SPACE: // Long space
|
||||
|
||||
if ( led_code_timer < ( SPACE_LONG_DURATION ) ) return;
|
||||
else PPM_PORT |= ( 1 << PB0 ); // Enable status LED
|
||||
break;
|
||||
|
||||
case LOOP: // Loop to code start
|
||||
led_code_symbol = 0;
|
||||
return;
|
||||
break;
|
||||
|
||||
}
|
||||
|
||||
led_code_timer = 0; // Code led timer reset
|
||||
led_code_symbol++; // Next symbol
|
||||
|
||||
return; // LED code function return
|
||||
|
||||
}
|
||||
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// ppm reading helper - interrupt safe and non blocking function
|
||||
// ------------------------------------------------------------------------------
|
||||
uint16_t ppm_read( uint8_t channel )
|
||||
{
|
||||
uint16_t ppm_tmp = ppm[ channel ];
|
||||
while( ppm_tmp != ppm[ channel ] ) ppm_tmp = ppm[ channel ];
|
||||
|
||||
return ppm_tmp;
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
// INITIALISATION CODE
|
||||
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// Reset Source checkings
|
||||
// ------------------------------------------------------------------------------
|
||||
if (MCUSR & 1) // Power-on Reset
|
||||
{
|
||||
MCUSR=0; // Clear MCU Status register
|
||||
// custom code here
|
||||
}
|
||||
else if (MCUSR & 2) // External Reset
|
||||
{
|
||||
MCUSR=0; // Clear MCU Status register
|
||||
// custom code here
|
||||
}
|
||||
else if (MCUSR & 4) // Brown-Out Reset
|
||||
{
|
||||
MCUSR=0; // Clear MCU Status register
|
||||
brownout_reset=true;
|
||||
}
|
||||
else // Watchdog Reset
|
||||
{
|
||||
MCUSR=0; // Clear MCU Status register
|
||||
// custom code here
|
||||
}
|
||||
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// Servo input and PPM generator init
|
||||
// ------------------------------------------------------------------------------
|
||||
ppm_encoder_init();
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// Outputs init
|
||||
// ------------------------------------------------------------------------------
|
||||
PPM_DDR |= ( 1 << PB0 ); // Set LED pin (PB0) to output
|
||||
PPM_DDR |= ( 1 << PB1 ); // Set MUX pin (PB1) to output
|
||||
PPM_DDR |= ( 1 << PPM_OUTPUT_PIN ); // Set PPM pin (PPM_OUTPUT_PIN, OC1B) to output
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// Timer0 init (normal mode) used for LED control and custom code
|
||||
// ------------------------------------------------------------------------------
|
||||
TCCR0A = 0x00; // Clock source: System Clock
|
||||
TCCR0B = 0x05; // Set 1024x prescaler - Clock value: 15.625 kHz - 16 ms max time
|
||||
TCNT0 = 0x00;
|
||||
OCR0A = 0x00; // OC0x outputs: Disconnected
|
||||
OCR0B = 0x00;
|
||||
TIMSK0 = 0x00; // Timer 1 interrupt disable
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// Enable global interrupt
|
||||
// ------------------------------------------------------------------------------
|
||||
sei(); // Enable Global interrupt flag
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// Disable radio passthrough (mux chip A/B control)
|
||||
// ------------------------------------------------------------------------------
|
||||
PPM_PORT |= ( 1 << PB1 ); // Set PIN B1 to disable Radio passthrough (mux)
|
||||
|
||||
|
||||
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// Check for first valid servo signal
|
||||
// ------------------------------------------------------------------------------
|
||||
while( 1 )
|
||||
{
|
||||
if ( servo_error_condition || servo_input_missing ) // We have an error
|
||||
{
|
||||
blink_led ( 6 * LOOP_TIMER_10MS ); // Status LED very fast blink if invalid servo input or missing signal
|
||||
}
|
||||
else // We are running normally
|
||||
{
|
||||
init = false; // initialisation is done,
|
||||
switch ( servo_input_mode )
|
||||
{
|
||||
case SERVO_PWM_MODE: // Normal PWM mode
|
||||
goto PWM_LOOP;
|
||||
break;
|
||||
|
||||
case PPM_PASSTROUGH_MODE: // PPM_PASSTROUGH_MODE
|
||||
goto PPM_PASSTHROUGH_LOOP;
|
||||
break;
|
||||
|
||||
default: // Normal PWM mode
|
||||
goto PWM_LOOP;
|
||||
break;
|
||||
}
|
||||
}
|
||||
_delay_us (970); // Slow down while loop
|
||||
}
|
||||
|
||||
|
||||
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
// AUXILIARY TASKS
|
||||
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
|
||||
PWM_LOOP: // SERVO_PWM_MODE
|
||||
while( 1 )
|
||||
{
|
||||
#ifdef PASSTHROUGH_MODE_ENABLED
|
||||
// ------------------------------------------------------------------------------
|
||||
// Radio passthrough control (mux chip A/B control)
|
||||
// ------------------------------------------------------------------------------
|
||||
|
||||
mux_timer++; // Increment mux timer
|
||||
|
||||
if ( mux_timer > ( 3 * LOOP_TIMER_10MS ) ) // Check Passthrough Channel every 30ms
|
||||
{
|
||||
|
||||
mux_timer = 0; // Reset mux timer
|
||||
|
||||
|
||||
if ( mux_counter++ < 5) // Check passthrough channel position 5 times
|
||||
{
|
||||
|
||||
mux_ppm = ppm_read( PASSTHROUGH_CHANNEL - 1 ); // Safely read passthrough channel ppm position
|
||||
|
||||
if ( mux_ppm < ( PASSTHROUGH_CHANNEL_OFF_US ) ) // Check ppm value and update validation counter
|
||||
{
|
||||
mux_check -= 1;
|
||||
}
|
||||
else if ( mux_ppm > ( PASSTHROUGH_CHANNEL_ON_US ) )
|
||||
{
|
||||
mux_check += 1;
|
||||
}
|
||||
|
||||
}
|
||||
else // Check
|
||||
{
|
||||
switch ( mux_check ) // If all 5 checks are the same, update mux status flag
|
||||
{
|
||||
case -5:
|
||||
mux_passthrough = false;
|
||||
PPM_PORT |= ( 1 << PB1 ); // Set PIN B1 (Mux) to disable Radio passthrough
|
||||
break;
|
||||
|
||||
case 5:
|
||||
mux_passthrough = true;
|
||||
PPM_PORT &= ~( 1 << PB1 ); // Reset PIN B1 (Mux) to enable Radio passthrough
|
||||
break;
|
||||
}
|
||||
mux_check = 0; // Reset mux validation counter
|
||||
mux_counter = 0; // Reset mux counter
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
#endif
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// Status LED control
|
||||
// ------------------------------------------------------------------------------
|
||||
if ( servo_error_condition || servo_input_missing ) // We have an error
|
||||
{
|
||||
blink_led ( 6 * LOOP_TIMER_10MS ); // Status LED very fast blink if invalid servo input or missing signal
|
||||
}
|
||||
else // We are running normally
|
||||
{
|
||||
if ( mux_passthrough == false ) // Normal mode : status LED toggle speed from throttle position
|
||||
{
|
||||
led_acceleration = ( ppm[THROTTLE_CHANNEL - 1] - ( PPM_SERVO_MIN ) ) / 2;
|
||||
blink_led ( LED_LOW_BLINKING_RATE - led_acceleration );
|
||||
}
|
||||
else // Passthrough mode : status LED never flashing
|
||||
{
|
||||
// Enable status LED if throttle > THROTTLE_CHANNEL_LED_TOGGLE_US
|
||||
if ( ppm[THROTTLE_CHANNEL - 1] > ( THROTTLE_CHANNEL_LED_TOGGLE_US ) )
|
||||
{
|
||||
PPM_PORT |= ( 1 << PB0 );
|
||||
}
|
||||
// Disable status LED if throttle <= THROTTLE_CHANNEL_LED_TOGGLE_US
|
||||
else if ( ppm[THROTTLE_CHANNEL - 1] <= ( THROTTLE_CHANNEL_LED_TOGGLE_US ) )
|
||||
{
|
||||
PPM_PORT &= ~( 1 << PB0 );
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// Servo input error detection
|
||||
// ------------------------------------------------------------------------------
|
||||
|
||||
// If there are too many errors during the detection time window, then trig servo error condition
|
||||
|
||||
if ( servo_input_errors > 0 ) // Start error rate checking if an error did occur
|
||||
{
|
||||
if ( servo_error_detection_timer > ( ERROR_DETECTION_WINDOW ) ) // If 10s delay reached
|
||||
{
|
||||
servo_error_detection_timer = 0; // Reset error detection timer
|
||||
|
||||
servo_input_errors = 0; // Reset servo input error counter
|
||||
|
||||
}
|
||||
else // If 10s delay is not reached
|
||||
{
|
||||
servo_error_detection_timer++; // Increment servo error timer value
|
||||
|
||||
if ( servo_input_errors >= ( ERROR_THRESHOLD ) ) // If there are too many errors
|
||||
{
|
||||
servo_error_condition = true; // Enable error condition flag
|
||||
servo_input_errors = 0; // Reset servo input error counter
|
||||
servo_error_detection_timer = 0; // Reset servo error detection timer
|
||||
servo_error_condition_timer = 0; // Reset servo error condition timer
|
||||
}
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
|
||||
// Servo error condition flag (will control blinking LED)
|
||||
|
||||
if ( servo_error_condition == true ) // We are in error condition
|
||||
{
|
||||
|
||||
if ( servo_error_condition_timer > ( ERROR_CONDITION_DELAY ) ) // If 3s delay reached
|
||||
{
|
||||
servo_error_condition_timer = 0; // Reset servo error condition timer
|
||||
|
||||
servo_error_condition = false; // Reset servo error condition flag (Led will stop very fast blink)
|
||||
}
|
||||
|
||||
else servo_error_condition_timer++; // If 3s delay is not reached update servo error condition timer value
|
||||
}
|
||||
|
||||
_delay_us (950); // Slow down while loop
|
||||
|
||||
} // PWM Loop end
|
||||
|
||||
|
||||
|
||||
PPM_PASSTHROUGH_LOOP: // PPM_PASSTROUGH_MODE
|
||||
|
||||
while (1)
|
||||
{
|
||||
// ------------------------------------------------------------------------------
|
||||
// Status LED control
|
||||
// ------------------------------------------------------------------------------
|
||||
|
||||
if ( servo_input_missing ) // We have an error
|
||||
{
|
||||
blink_led ( 6 * LOOP_TIMER_10MS ); // Status LED very fast blink if invalid servo input or missing signal
|
||||
}
|
||||
else // We are running normally
|
||||
{
|
||||
blink_code_led ( PPM_PASSTROUGH_MODE ); // Blink LED according to mode 2 code (one long, two shorts).
|
||||
}
|
||||
|
||||
_delay_us (970); // Slow down this loop
|
||||
|
||||
|
||||
} // PPM_PASSTHROUGH Loop end
|
||||
|
||||
} // main function end
|
||||
|
||||
|
||||
|
|
@ -0,0 +1,785 @@
|
|||
// -------------------------------------------------------------
|
||||
// PPM ENCODER V3.0.0 (12-10-2012)
|
||||
// -------------------------------------------------------------
|
||||
// By: John Arne Birkeland - 2012
|
||||
// By Olivier ADLER : PPM redundancy mode - APM v1.x adaptation and "difficult" receiver testing - 2012
|
||||
//
|
||||
// -------------------------------------------------------------
|
||||
// See changelog_v3 for a complete descrition of changes
|
||||
// -------------------------------------------------------------
|
||||
//
|
||||
// 12-10-2012
|
||||
// V3.0.0 - Added dual input PPM redundancy mode with auto switchover. This is mainly for dual PPM receivers setup.
|
||||
// This mode Can be used as well if a PPM conversion is needed (Futaba 16 channels 760us mode to APM mode)
|
||||
// -------------------------------------------------------------
|
||||
|
||||
#ifndef _PPM_ENCODER_H_
|
||||
#define _PPM_ENCODER_H_
|
||||
|
||||
#include <avr/io.h>
|
||||
|
||||
// -------------------------------------------------------------
|
||||
|
||||
#include <avr/interrupt.h>
|
||||
#include <avr/wdt.h>
|
||||
#include <util/delay.h>
|
||||
|
||||
|
||||
#ifndef F_CPU
|
||||
#define F_CPU 16000000UL
|
||||
#endif
|
||||
|
||||
#ifndef true
|
||||
#define true 1
|
||||
#endif
|
||||
|
||||
#ifndef false
|
||||
#define false 0
|
||||
#endif
|
||||
|
||||
#ifndef bool
|
||||
#define bool _Bool
|
||||
#endif
|
||||
|
||||
// -------------------------------------------------------------
|
||||
// INPUT MODE (by jumper selection)
|
||||
// -------------------------------------------------------------
|
||||
|
||||
#define JUMPER_SELECT_MODE 0 // Default - PPM passtrough mode selected if channel 2&3 shorted. Normal servo input (pwm) if not shorted.
|
||||
#define SERVO_PWM_MODE 1 // Normal 8 channel servo (pwm) input
|
||||
#define PPM_PASSTROUGH_MODE 2 // PPM signal passtrough on channel 1
|
||||
#define PPM_REDUNDANCY_MODE 3 // PPM redundancy on channels 1 and 2
|
||||
#define SPEKTRUM_MODE 4 // Spektrum satelitte on channel 1 (reserved but not implemented yet)
|
||||
|
||||
volatile uint8_t servo_input_mode = JUMPER_SELECT_MODE;
|
||||
|
||||
// -------------------------------------------------------------
|
||||
// PPM REDUNDANCY MODE SETTINGS
|
||||
// -------------------------------------------------------------
|
||||
|
||||
#define _TEACHER_STUDENT_MODE_ // Enable teacher student mode. See manual.
|
||||
#define TEACHER_STUDENT_CHANNEL 8 // PPM Channel to force receiver 2
|
||||
|
||||
#define RECEIVER_1_FRAME_FORMAT 0 // 0 for standard PPM, 1 for PPMv2 (Futaba 760 us 16 Channels), 2 for PPMv3 (Jeti 1050 us 16 channels), 3 for Hitec 9 channels
|
||||
#define RECEIVER_2_FRAME_FORMAT 0 // See manual for details
|
||||
|
||||
#define SWITCHOVER_1_to_2_DELAY_MS 50 // Delay for switching to receiver 2
|
||||
#define SWITCHOVER_2_to_1_DELAY_MS 200 // Delay for switching back to receiver 1
|
||||
|
||||
// -------------------------------------------------------------
|
||||
// PWM INPUT FILTERS
|
||||
// -------------------------------------------------------------
|
||||
// Using both filters is not recommended and may reduce servo input resolution
|
||||
|
||||
// #define _AVERAGE_FILTER_ // Average filter to smooth servo input capture jitter
|
||||
// #define _JITTER_FILTER_ // Cut filter to remove 0,5us servo input capture jitter
|
||||
|
||||
// -------------------------------------------------------------
|
||||
// GLOBAL SETTINGS
|
||||
// -------------------------------------------------------------
|
||||
|
||||
// Number of Timer1 ticks for 1 microsecond
|
||||
#define ONE_US F_CPU / 8 / 1000 / 1000
|
||||
|
||||
// -------------------------------------------------------------
|
||||
// PWM inputs used channels, limits and center positions
|
||||
// -------------------------------------------------------------
|
||||
|
||||
// Number of input PWM channels
|
||||
#define SERVO_CHANNELS 8
|
||||
|
||||
// Servo minimum position
|
||||
#define PWM_SERVO_MIN ONE_US * 900 - PPM_PRE_PULSE
|
||||
|
||||
// Servo center position
|
||||
#define PPM_SERVO_CENTER ONE_US * 1500 - PPM_PRE_PULSE
|
||||
|
||||
// Servo maximum position
|
||||
#define PWM_SERVO_MAX ONE_US * 2100 - PPM_PRE_PULSE
|
||||
|
||||
|
||||
// -------------------------------------------------------------
|
||||
// PPM output default values
|
||||
// -------------------------------------------------------------
|
||||
|
||||
// Throttle default at power on
|
||||
#define PPM_THROTTLE_DEFAULT ONE_US * 1100 - PPM_PRE_PULSE
|
||||
|
||||
// Throttle during failsafe
|
||||
#define PPM_THROTTLE_FAILSAFE ONE_US * 900 - PPM_PRE_PULSE
|
||||
|
||||
// CH5 power on values (mode selection channel)
|
||||
#define PPM_CH5_MODE_4 ONE_US * 1555 - PPM_PRE_PULSE
|
||||
|
||||
// -------------------------------------------------------------
|
||||
// PPM output frame format
|
||||
// -------------------------------------------------------------
|
||||
|
||||
// Number of output PPM channels
|
||||
#define PPM_CHANNELS 8
|
||||
|
||||
// 400us PPM pre pulse
|
||||
#define PPM_PRE_PULSE ONE_US * 400
|
||||
|
||||
// PPM period 18.5ms - 26.5ms (54hz - 37Hz)
|
||||
#define PPM_PERIOD ONE_US * ( 22500 - ( PPM_CHANNELS * 1500 ) )
|
||||
|
||||
// Size of ppm[..] data array
|
||||
#define PPM_ARRAY_MAX PPM_CHANNELS * 2 + 2
|
||||
|
||||
// -------------------------------------------------------------
|
||||
// PPM output frame variables
|
||||
// -------------------------------------------------------------
|
||||
|
||||
// Data array for storing ppm (8 channels) pulse widths.
|
||||
|
||||
volatile uint16_t ppm[ PPM_ARRAY_MAX ] =
|
||||
{
|
||||
PPM_PRE_PULSE,
|
||||
PPM_SERVO_CENTER, // Channel 1
|
||||
PPM_PRE_PULSE,
|
||||
PPM_SERVO_CENTER, // Channel 2
|
||||
PPM_PRE_PULSE,
|
||||
PPM_THROTTLE_DEFAULT, // Channel 3 (throttle)
|
||||
PPM_PRE_PULSE,
|
||||
PPM_SERVO_CENTER, // Channel 4
|
||||
PPM_PRE_PULSE,
|
||||
PPM_CH5_MODE_4, // Channel 5
|
||||
PPM_PRE_PULSE,
|
||||
PPM_SERVO_CENTER, // Channel 6
|
||||
PPM_PRE_PULSE,
|
||||
PPM_SERVO_CENTER, // Channel 7
|
||||
PPM_PRE_PULSE,
|
||||
PPM_SERVO_CENTER, // Channel 8
|
||||
PPM_PRE_PULSE,
|
||||
PPM_PERIOD
|
||||
};
|
||||
|
||||
// SERVO FAILSAFE VALUES
|
||||
|
||||
const uint16_t failsafe_ppm[ PPM_ARRAY_MAX ] =
|
||||
{
|
||||
PPM_PRE_PULSE,
|
||||
PPM_SERVO_CENTER, // Channel 1
|
||||
PPM_PRE_PULSE,
|
||||
PPM_SERVO_CENTER, // Channel 2
|
||||
PPM_PRE_PULSE,
|
||||
PPM_THROTTLE_FAILSAFE, // Channel 3 (throttle)
|
||||
PPM_PRE_PULSE,
|
||||
PPM_SERVO_CENTER, // Channel 4
|
||||
PPM_PRE_PULSE,
|
||||
PPM_CH5_MODE_4, // Channel 5
|
||||
PPM_PRE_PULSE,
|
||||
PPM_SERVO_CENTER, // Channel 6
|
||||
PPM_PRE_PULSE,
|
||||
PPM_SERVO_CENTER, // Channel 7
|
||||
PPM_PRE_PULSE,
|
||||
PPM_SERVO_CENTER, // Channel 8
|
||||
PPM_PRE_PULSE,
|
||||
PPM_PERIOD
|
||||
};
|
||||
// -------------------------------------------------------------
|
||||
|
||||
// AVR parameters for PhoneDrone and APM2 boards using ATmega32u2
|
||||
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
|
||||
|
||||
#define SERVO_DDR DDRB
|
||||
#define SERVO_PORT PORTB
|
||||
#define SERVO_INPUT PINB
|
||||
#define SERVO_INT_VECTOR PCINT0_vect
|
||||
#define SERVO_INT_MASK PCMSK0
|
||||
#define SERVO_INT_CLEAR_FLAG PCIF0
|
||||
#define SERVO_INT_ENABLE PCIE0
|
||||
#define SERVO_TIMER_CNT TCNT1
|
||||
|
||||
#define PPM_DDR DDRC
|
||||
#define PPM_PORT PORTC
|
||||
#define PPM_OUTPUT_PIN PC6
|
||||
#define PPM_INT_VECTOR TIMER1_COMPA_vect
|
||||
#define PPM_COMPARE OCR1A
|
||||
#define PPM_COMPARE_FLAG COM1A0
|
||||
#define PPM_COMPARE_ENABLE OCIE1A
|
||||
|
||||
#define USB_DDR DDRC
|
||||
#define USB_PORT PORTC
|
||||
#define USB_PIN PC2
|
||||
|
||||
// true if we have received a USB device connect event
|
||||
static bool usb_connected;
|
||||
|
||||
// USB connected event
|
||||
void EVENT_USB_Device_Connect(void)
|
||||
{
|
||||
// Toggle USB pin high if USB is connected
|
||||
USB_PORT |= (1 << USB_PIN);
|
||||
|
||||
usb_connected = true;
|
||||
|
||||
// this unsets the pin connected to PA1 on the 2560
|
||||
// when the bit is clear, USB is connected
|
||||
PORTD &= ~1;
|
||||
}
|
||||
|
||||
// USB disconnect event
|
||||
void EVENT_USB_Device_Disconnect(void)
|
||||
{
|
||||
// toggle USB pin low if USB is disconnected
|
||||
USB_PORT &= ~(1 << USB_PIN);
|
||||
|
||||
usb_connected = false;
|
||||
|
||||
// this sets the pin connected to PA1 on the 2560
|
||||
// when the bit is clear, USB is connected
|
||||
PORTD |= 1;
|
||||
}
|
||||
|
||||
// AVR parameters for ArduPilot MEGA v1.4 PPM Encoder (ATmega328P)
|
||||
#elif defined (__AVR_ATmega328P__) || defined (__AVR_ATmega328__)
|
||||
|
||||
#define SERVO_DDR DDRD
|
||||
#define SERVO_PORT PORTD
|
||||
#define SERVO_INPUT PIND
|
||||
#define SERVO_INT_VECTOR PCINT2_vect
|
||||
#define SERVO_INT_MASK PCMSK2
|
||||
#define SERVO_INT_CLEAR_FLAG PCIF2
|
||||
#define SERVO_INT_ENABLE PCIE2
|
||||
#define SERVO_TIMER_CNT TCNT1
|
||||
|
||||
#define PPM_DDR DDRB
|
||||
#define PPM_PORT PORTB
|
||||
#define PPM_OUTPUT_PIN PB2
|
||||
#define PPM_INT_VECTOR TIMER1_COMPB_vect
|
||||
#define PPM_COMPARE OCR1B
|
||||
#define PPM_COMPARE_FLAG COM1B0
|
||||
#define PPM_COMPARE_ENABLE OCIE1B
|
||||
|
||||
#else
|
||||
#error NO SUPPORTED DEVICE FOUND! (ATmega16u2 / ATmega32u2 / ATmega328p)
|
||||
#endif
|
||||
|
||||
// Used to indicate invalid SERVO input signals
|
||||
volatile uint8_t servo_input_errors = 0;
|
||||
|
||||
// Used to indicate missing SERVO input signals
|
||||
volatile bool servo_input_missing = true;
|
||||
|
||||
// Used to indicate if PPM generator is active
|
||||
volatile bool ppm_generator_active = false;
|
||||
|
||||
// Used to indicate a brownout restart
|
||||
volatile bool brownout_reset = false;
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// PPM GENERATOR START - TOGGLE ON COMPARE INTERRUPT ENABLE
|
||||
// ------------------------------------------------------------------------------
|
||||
void ppm_start( void )
|
||||
{
|
||||
// Prevent reenabling an already active PPM generator
|
||||
if( ppm_generator_active ) return;
|
||||
|
||||
// Store interrupt status and register flags
|
||||
uint8_t SREG_tmp = SREG;
|
||||
|
||||
// Stop interrupts
|
||||
cli();
|
||||
|
||||
|
||||
// Make sure initial output state is low
|
||||
PPM_PORT &= ~(1 << PPM_OUTPUT_PIN);
|
||||
|
||||
// Wait for output pin to settle
|
||||
//_delay_us( 1 );
|
||||
|
||||
// Set initial compare toggle to maximum (32ms) to give other parts of the system time to start
|
||||
SERVO_TIMER_CNT = 0;
|
||||
PPM_COMPARE = 0xFFFF;
|
||||
|
||||
// Set toggle on compare output
|
||||
TCCR1A = (1 << PPM_COMPARE_FLAG);
|
||||
|
||||
// Set TIMER1 8x prescaler
|
||||
TCCR1B = ( 1 << CS11 );
|
||||
|
||||
// Enable output compare interrupt
|
||||
TIMSK1 |= (1 << PPM_COMPARE_ENABLE);
|
||||
|
||||
// Indicate that PPM generator is active
|
||||
ppm_generator_active = true;
|
||||
|
||||
// Restore interrupt status and register flags
|
||||
SREG = SREG_tmp;
|
||||
|
||||
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
|
||||
// Turn on TX led if PPM generator is active
|
||||
PORTD &= ~( 1<< PD5 );
|
||||
#endif
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// PPM GENERATOR STOP - TOGGLE ON COMPARE INTERRUPT DISABLE
|
||||
// ------------------------------------------------------------------------------
|
||||
void ppm_stop( void )
|
||||
{
|
||||
// Store interrupt status and register flags
|
||||
uint8_t SREG_tmp = SREG;
|
||||
|
||||
// Stop interrupts
|
||||
cli();
|
||||
|
||||
// Disable output compare interrupt
|
||||
TIMSK1 &= ~(1 << PPM_COMPARE_ENABLE);
|
||||
|
||||
// Reset TIMER1 registers
|
||||
TCCR1A = 0;
|
||||
TCCR1B = 0;
|
||||
|
||||
// Indicate that PPM generator is not active
|
||||
ppm_generator_active = false;
|
||||
|
||||
// Restore interrupt status and register flags
|
||||
SREG = SREG_tmp;
|
||||
|
||||
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
|
||||
// Turn off TX led if PPM generator is off
|
||||
PORTD |= ( 1<< PD5 );
|
||||
#endif
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// Watchdog Interrupt (interrupt only mode, no reboot)
|
||||
// ------------------------------------------------------------------------------
|
||||
ISR( WDT_vect ) // If watchdog is triggered then enable missing signal flag and copy failsafe values
|
||||
{
|
||||
// Use failsafe values if PPM generator is active or if chip has been reset from a brown-out
|
||||
if ( ppm_generator_active || brownout_reset )
|
||||
{
|
||||
// Copy failsafe values to ppm[..]
|
||||
for ( uint8_t i = 0; i < PPM_ARRAY_MAX; 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( ( servo_input_mode == PPM_PASSTROUGH_MODE && servo_input_missing == false ) || brownout_reset )
|
||||
{
|
||||
// Start PPM generator
|
||||
ppm_start();
|
||||
brownout_reset = false;
|
||||
}
|
||||
|
||||
// Set missing receiver signal flag
|
||||
servo_input_missing = true;
|
||||
|
||||
// Reset servo input error flag
|
||||
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
|
||||
}
|
||||
// ------------------------------------------------------------------------------
|
||||
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// SERVO/PPM INPUT - PIN CHANGE INTERRUPT
|
||||
// ------------------------------------------------------------------------------
|
||||
ISR( SERVO_INT_VECTOR )
|
||||
{
|
||||
// Servo pulse start timing
|
||||
static uint16_t servo_start[ SERVO_CHANNELS ] = { 0, 0, 0, 0, 0, 0, 0, 0 };
|
||||
|
||||
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
|
||||
// Toggle LED delay
|
||||
static uint8_t led_delay = 0;
|
||||
#endif
|
||||
|
||||
// Missing throttle signal failsafe
|
||||
static uint8_t throttle_timeout = 0;
|
||||
|
||||
// Servo input pin storage
|
||||
static uint8_t servo_pins_old = 0;
|
||||
|
||||
// Used to store current servo input pins
|
||||
uint8_t servo_pins;
|
||||
|
||||
// Read current servo pulse change time
|
||||
uint16_t servo_time = SERVO_TIMER_CNT;
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// PPM passtrough mode ( signal passtrough from channel 1 to ppm output pin)
|
||||
// ------------------------------------------------------------------------------
|
||||
if( servo_input_mode == PPM_PASSTROUGH_MODE )
|
||||
{
|
||||
// Has watchdog timer failsafe started PPM generator? If so we need to stop it.
|
||||
if( ppm_generator_active )
|
||||
{
|
||||
// Stop PPM generator
|
||||
ppm_stop();
|
||||
}
|
||||
|
||||
// PPM (channel 1) input pin is high
|
||||
if( SERVO_INPUT & 1 )
|
||||
{
|
||||
// Set PPM output pin high
|
||||
PPM_PORT |= (1 << PPM_OUTPUT_PIN);
|
||||
}
|
||||
// PPM (channel 1) input pin is low
|
||||
else
|
||||
{
|
||||
// Set PPM output pin low
|
||||
PPM_PORT &= ~(1 << PPM_OUTPUT_PIN);
|
||||
}
|
||||
|
||||
// Reset Watchdog Timer
|
||||
wdt_reset();
|
||||
|
||||
// Set servo input missing flag false to indicate that we have received servo input signals
|
||||
servo_input_missing = false;
|
||||
|
||||
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
|
||||
// Toggle TX LED at PPM passtrough
|
||||
if( ++led_delay > 128 ) // Toggle every 128th pulse
|
||||
{
|
||||
// Toggle TX led
|
||||
PIND |= ( 1<< PD5 );
|
||||
led_delay = 0;
|
||||
}
|
||||
#endif
|
||||
|
||||
// Leave interrupt
|
||||
return;
|
||||
}
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// SERVO PWM MODE
|
||||
// ------------------------------------------------------------------------------
|
||||
CHECK_PINS_START: // Start of servo input check
|
||||
|
||||
// Store current servo input pins
|
||||
servo_pins = SERVO_INPUT;
|
||||
|
||||
// Calculate servo input pin change mask
|
||||
uint8_t servo_change = servo_pins ^ servo_pins_old;
|
||||
|
||||
// Set initial servo pin and channel
|
||||
uint8_t servo_pin = 1;
|
||||
uint8_t servo_channel = 0;
|
||||
|
||||
CHECK_PINS_LOOP: // Input servo pin check loop
|
||||
|
||||
// Check for pin change on current servo channel
|
||||
if( servo_change & servo_pin )
|
||||
{
|
||||
// High (raising edge)
|
||||
if( servo_pins & servo_pin )
|
||||
{
|
||||
servo_start[ servo_channel ] = servo_time;
|
||||
}
|
||||
else
|
||||
{
|
||||
|
||||
// Get servo pulse width
|
||||
uint16_t servo_width = servo_time - servo_start[ servo_channel ] - PPM_PRE_PULSE;
|
||||
|
||||
// Check that servo pulse signal is valid before sending to ppm encoder
|
||||
if( servo_width > PWM_SERVO_MAX ) goto CHECK_PINS_ERROR;
|
||||
if( servo_width < PWM_SERVO_MIN ) goto CHECK_PINS_ERROR;
|
||||
|
||||
// Calculate servo channel position in ppm[..]
|
||||
uint8_t _ppm_channel = ( servo_channel << 1 ) + 1;
|
||||
|
||||
//Reset throttle failsafe timeout
|
||||
if( _ppm_channel == 5 ) throttle_timeout = 0;
|
||||
|
||||
#ifdef _AVERAGE_FILTER_
|
||||
// Average filter to smooth input jitter
|
||||
servo_width += ppm[ _ppm_channel ];
|
||||
servo_width >>= 1;
|
||||
#endif
|
||||
|
||||
#ifdef _JITTER_FILTER_
|
||||
// 0.5us cut filter to remove input jitter
|
||||
int16_t ppm_tmp = ppm[ _ppm_channel ] - servo_width;
|
||||
if( ppm_tmp == 1 ) goto CHECK_PINS_NEXT;
|
||||
if( ppm_tmp == -1 ) goto CHECK_PINS_NEXT;
|
||||
#endif
|
||||
|
||||
// Update ppm[..]
|
||||
ppm[ _ppm_channel ] = servo_width;
|
||||
}
|
||||
}
|
||||
|
||||
CHECK_PINS_NEXT:
|
||||
|
||||
// Select next servo pin
|
||||
servo_pin <<= 1;
|
||||
|
||||
// Select next servo channel
|
||||
servo_channel++;
|
||||
|
||||
// Check channel and process if needed
|
||||
if( servo_channel < SERVO_CHANNELS ) goto CHECK_PINS_LOOP;
|
||||
|
||||
goto CHECK_PINS_DONE;
|
||||
|
||||
CHECK_PINS_ERROR:
|
||||
|
||||
// Used to indicate invalid servo input signals
|
||||
servo_input_errors++;
|
||||
|
||||
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
|
||||
// Delay LED toggle
|
||||
led_delay = 0;
|
||||
#endif
|
||||
|
||||
goto CHECK_PINS_NEXT;
|
||||
|
||||
// All servo input pins has now been processed
|
||||
|
||||
CHECK_PINS_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_PINS_START;
|
||||
|
||||
// Clear interrupt event from already processed pin changes
|
||||
PCIFR |= (1 << SERVO_INT_CLEAR_FLAG);
|
||||
}
|
||||
// ------------------------------------------------------------------------------
|
||||
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// PPM OUTPUT - TIMER1 COMPARE INTERRUPT
|
||||
// ------------------------------------------------------------------------------
|
||||
ISR( PPM_INT_VECTOR )
|
||||
{
|
||||
// Current active ppm channel
|
||||
static uint8_t ppm_channel = PPM_ARRAY_MAX - 1;
|
||||
|
||||
// Update timing for next PPM output pin toggle
|
||||
PPM_COMPARE += ppm[ ppm_channel ];
|
||||
|
||||
// Select the next ppm channel
|
||||
if( ++ppm_channel >= PPM_ARRAY_MAX )
|
||||
{
|
||||
ppm_channel = 0;
|
||||
|
||||
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
|
||||
// Blink TX LED when PPM generator has finished a pulse train
|
||||
PIND |= ( 1<< PD5 );
|
||||
#endif
|
||||
}
|
||||
}
|
||||
// ------------------------------------------------------------------------------
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// PPM READ - INTERRUPT SAFE PPM SERVO CHANNEL READ
|
||||
// ------------------------------------------------------------------------------
|
||||
uint16_t ppm_read_channel( uint8_t channel )
|
||||
{
|
||||
// Limit channel to valid value
|
||||
uint8_t _channel = channel;
|
||||
if( _channel == 0 ) _channel = 1;
|
||||
if( _channel > SERVO_CHANNELS ) _channel = SERVO_CHANNELS;
|
||||
|
||||
// Calculate ppm[..] position
|
||||
uint8_t ppm_index = ( _channel << 1 ) + 1;
|
||||
|
||||
// Read ppm[..] in a non blocking interrupt safe manner
|
||||
uint16_t ppm_tmp = ppm[ ppm_index ];
|
||||
while( ppm_tmp != ppm[ ppm_index ] ) ppm_tmp = ppm[ ppm_index ];
|
||||
|
||||
// Return as normal servo value
|
||||
return ppm_tmp + PPM_PRE_PULSE;
|
||||
}
|
||||
// ------------------------------------------------------------------------------
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// PPM ENCODER INIT
|
||||
// ------------------------------------------------------------------------------
|
||||
void ppm_encoder_init( void )
|
||||
{
|
||||
// ATmegaXXU2 only init code
|
||||
// ------------------------------------------------------------------------------
|
||||
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
|
||||
// ------------------------------------------------------------------------------
|
||||
// Reset Source check
|
||||
// ------------------------------------------------------------------------------
|
||||
if (MCUSR & 1) // Power-on Reset
|
||||
{
|
||||
MCUSR=0; // Clear MCU Status register
|
||||
// custom code here
|
||||
}
|
||||
else if (MCUSR & 2) // External Reset
|
||||
{
|
||||
MCUSR=0; // Clear MCU Status register
|
||||
// custom code here
|
||||
}
|
||||
else if (MCUSR & 4) // Brown-Out Reset
|
||||
{
|
||||
MCUSR=0; // Clear MCU Status register
|
||||
brownout_reset=true;
|
||||
}
|
||||
else // Watchdog Reset
|
||||
{
|
||||
MCUSR=0; // Clear MCU Status register
|
||||
// custom code here
|
||||
}
|
||||
|
||||
// APM USB connection status UART MUX selector pin
|
||||
// ------------------------------------------------------------------------------
|
||||
USB_DDR |= (1 << USB_PIN); // Set USB pin to output
|
||||
#endif
|
||||
|
||||
|
||||
// USE JUMPER TO CHECK FOR INPUT MODE (pin 2&3 or 3&4 shorted)
|
||||
// ------------------------------------------------------------------------------
|
||||
if( servo_input_mode == JUMPER_SELECT_MODE )
|
||||
{
|
||||
// channel 3 status counter
|
||||
uint8_t channel_2_status = 0;
|
||||
uint8_t channel_4_status = 0;
|
||||
|
||||
// Set channel 2 to input
|
||||
SERVO_DDR &= ~(1 << 1);
|
||||
// Enable channel 2 pullup
|
||||
SERVO_PORT |= (1 << 1);
|
||||
|
||||
// Set channel 4 to input
|
||||
SERVO_DDR &= ~(1 << 3);
|
||||
// Enable channel 4 pullup
|
||||
SERVO_PORT |= (1 << 3);
|
||||
|
||||
|
||||
// Set channel 3 to output
|
||||
SERVO_DDR |= (1 << 2);
|
||||
// Set channel 3 output low
|
||||
SERVO_PORT &= ~(1 << 2);
|
||||
|
||||
_delay_us (10);
|
||||
|
||||
|
||||
// Increment channel_2_status if channel 2 is set low by channel 3
|
||||
if( ( SERVO_INPUT & (1 << 1) ) == 0 ) channel_2_status++;
|
||||
// Increment channel_4_status if channel 4 is set low by channel 3
|
||||
if( ( SERVO_INPUT & (1 << 3) ) == 0 ) channel_4_status++;
|
||||
|
||||
// Set channel 3 output high
|
||||
SERVO_PORT |= (1 << 2);
|
||||
|
||||
_delay_us (10);
|
||||
|
||||
// Increment channel_2_status if channel 2 is set high by channel 3
|
||||
if( ( SERVO_INPUT & (1 << 1) ) != 0 ) channel_2_status++;
|
||||
// Increment channel_4_status if channel 4 is set high by channel 3
|
||||
if( ( SERVO_INPUT & (1 << 3) ) != 0 ) channel_4_status++;
|
||||
|
||||
// Set channel 3 output low
|
||||
SERVO_PORT &= ~(1 << 2);
|
||||
|
||||
_delay_us (10);
|
||||
|
||||
// Increment channel_2_status if channel 2 is set low by channel 3
|
||||
if( ( SERVO_INPUT & (1 << 1) ) == 0 ) channel_2_status++;
|
||||
// Increment channel_4_status if channel 4 is set low by channel 3
|
||||
if( ( SERVO_INPUT & (1 << 3) ) == 0 ) channel_4_status++;
|
||||
|
||||
|
||||
// Set servo input mode based on channel_2_status
|
||||
if( channel_2_status == 3 ) servo_input_mode = PPM_PASSTROUGH_MODE;
|
||||
if( channel_4_status == 3 ) servo_input_mode = PPM_REDUNDANCY_MODE;
|
||||
else servo_input_mode = SERVO_PWM_MODE;
|
||||
|
||||
}
|
||||
|
||||
// RESET SERVO/PPM PINS AS INPUTS WITH PULLUPS
|
||||
// ------------------------------------------------------------------------------
|
||||
SERVO_DDR = 0;
|
||||
SERVO_PORT |= 0xFF;
|
||||
|
||||
#if defined (__AVR_ATmega16U2__) || defined (__AVR_ATmega32U2__)
|
||||
// on 32U2 set PD0 to be an output, and clear the bit. This tells
|
||||
// the 2560 that USB is connected. The USB connection event fires
|
||||
// later to set the right value
|
||||
DDRD |= 1;
|
||||
if (usb_connected) {
|
||||
PORTD &= ~1;
|
||||
} else {
|
||||
PORTD |= 1;
|
||||
}
|
||||
#endif
|
||||
|
||||
// SERVO/PPM INPUT - PIN CHANGE INTERRUPT
|
||||
// ------------------------------------------------------------------------------
|
||||
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 )
|
||||
{
|
||||
// Set servo input interrupt pin mask to servo input channel 1
|
||||
SERVO_INT_MASK = 0x01;
|
||||
}
|
||||
|
||||
// Enable servo input interrupt
|
||||
PCICR |= (1 << SERVO_INT_ENABLE);
|
||||
|
||||
// PPM OUTPUT PIN
|
||||
// ------------------------------------------------------------------------------
|
||||
// Set PPM pin to output
|
||||
PPM_DDR |= (1 << PPM_OUTPUT_PIN);
|
||||
|
||||
// ------------------------------------------------------------------------------
|
||||
// Enable watchdog interrupt mode
|
||||
// ------------------------------------------------------------------------------
|
||||
// Disable watchdog
|
||||
wdt_disable();
|
||||
// Reset watchdog timer
|
||||
wdt_reset();
|
||||
// Start timed watchdog setup sequence
|
||||
WDTCSR |= (1<<WDCE) | (1<<WDE );
|
||||
// Set 250 ms watchdog timeout and enable interrupt
|
||||
WDTCSR = (1<<WDIE) | (1<<WDP2);
|
||||
|
||||
|
||||
|
||||
}
|
||||
// ------------------------------------------------------------------------------
|
||||
|
||||
#endif // _PPM_ENCODER_H_
|
||||
|
||||
|
|
@ -0,0 +1,67 @@
|
|||
// -------------------------------------------------------------
|
||||
// PPM ENCODER V3.0.0 (12-10-2012)
|
||||
// -------------------------------------------------------------
|
||||
// Improved servo to ppm for ArduPilot MEGA v1.x (ATmega328p),
|
||||
// PhoneDrone and APM2.x (ATmega32u2)
|
||||
|
||||
// By John Arne Birkeland - 2012
|
||||
// By Olivier ADLER : PPM redundancy mode - APM v1.x adaptation and "difficult" receiver testing - 2012
|
||||
// -------------------------------------------------------------
|
||||
// Changelog:
|
||||
|
||||
// 01-08-2011
|
||||
// V2.2.3 - Changed back to BLOCKING interrupts.
|
||||
// Assembly PPM compare interrupt can be switch back to non-blocking, but not recommended.
|
||||
// V2.2.3 - Implemented 0.5us cut filter to remove servo input capture jitter.
|
||||
|
||||
// 04-08-2011
|
||||
// V2.2.4 - Implemented PPM passtrough funtion.
|
||||
// Shorting channel 2&3 enabled ppm passtrough on channel 1.
|
||||
|
||||
// 04-08-2011
|
||||
// V2.2.5 - Implemented simple average filter to smooth servo input capture jitter.
|
||||
// Takes fewer clocks to execute and has better performance then cut filter.
|
||||
|
||||
// 05-08-2011
|
||||
// V2.2.51 - Minor bug fixes.
|
||||
|
||||
// 06-08-2011
|
||||
// V2.2.6 - PPM passtrough failsafe implemented.
|
||||
// The PPM generator will be activated and output failsafe values while ppm passtrough signal is missing.
|
||||
|
||||
// 01-09-2011
|
||||
// V2.2.61 - Temporary MUX pin always high patch for APM beta board
|
||||
|
||||
// 22-09-2011
|
||||
// V2.2.62 - ATmegaXXU2 USB connection status pin (PC2) for APM UART MUX selection (removed temporary high patch)
|
||||
// - Removed assembly optimized PPM generator (not usable for production release)
|
||||
|
||||
// 23-09-2011
|
||||
// V2.2.63 - Average filter disabled
|
||||
|
||||
// 24-09-2011
|
||||
// V2.2.64 - Added distincts Power on / Failsafe PPM values
|
||||
// - Changed CH5 (mode selection) PPM Power on and Failsafe values to 1555 (Flight mode 4)
|
||||
// - Added brownout detection : Failsafe values are copied after a brownout reset instead of power on values
|
||||
|
||||
// 25-09-2011
|
||||
// V2.2.65 - Implemented PPM output delay until input signal is detected (PWM and PPM pass-trough mode)
|
||||
// - Changed brownout detection and FailSafe handling to work with XXU2 chips
|
||||
// - Minor variable and define naming changes to enhance readability
|
||||
|
||||
// 15-03-2012
|
||||
// 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)
|
||||
// - <TX>: <OFF> = ppm output not started, <FAST TOGGLE> = normal PWM->PPM output or PPM passtrough failsafe, <SLOW TOGGLE> = PPM passtrough
|
||||
|
||||
// 03-06-2012
|
||||
// V2.2.67 - Implemented detection and failsafe (throttle = 900us) for missing throttle signal.
|
||||
|
||||
// 04-06-2012
|
||||
// V2.2.68 - Fixed possible logic flaw in throttle failsafe reset if _JITTER_FILTER_ is enabled
|
||||
|
||||
// 12-10-2012
|
||||
// V3.0.0 - Added dual input PPM redundancy mode with auto switchover. This is mainly for dual PPM receivers setup.
|
||||
// This mode Can be used as well if a PPM conversion is needed (Futaba 16 channels 760us mode to APM mode)
|
||||
|
||||
// -------------------------------------------------------------
|
||||
|
|
@ -0,0 +1,145 @@
|
|||
|
||||
|
||||
ArduPPM is a 8 channels ppm encoder firmware for the APM project.
|
||||
|
||||
It is compatible with APM v1.x board (ATMEGA 328p), APM 2.x, Phonedrone (using ATmega32u2) and futur boards.
|
||||
|
||||
Emphasis has been put on code simplicity, performance and reliability.
|
||||
|
||||
|
||||
|
||||
--------------------------------------------------------------------------------------------------
|
||||
Manual
|
||||
--------------------------------------------------------------------------------------------------
|
||||
|
||||
--------------------------------------------------
|
||||
Warning
|
||||
--------------------------------------------------
|
||||
|
||||
|
||||
Carefully check that your RC radio system is working perfectly before you fly.
|
||||
|
||||
|
||||
On APM 1.4, If you see the blue status LED blinking very fast very often, this is an indication that something is wrong in the decoding. Rare decoding errors do not hurt.
|
||||
|
||||
|
||||
If you have problems, please report the problem and what brand/modell receiver you are using.
|
||||
|
||||
|
||||
------------------------------------------
|
||||
Normal mode
|
||||
------------------------------------------
|
||||
|
||||
Normal mode :
|
||||
|
||||
Blue status LED is used for status reports :
|
||||
|
||||
- slow to fast blinking according to throttle channel position
|
||||
|
||||
- very fast blinking if missing receiver servo signals, or if the servo signals are wrong (invalid pulse widths)
|
||||
|
||||
|
||||
------------------------------------------
|
||||
Radio Passthrough mode (mux)
|
||||
------------------------------------------
|
||||
|
||||
This mode is described as hardware failsafe in Arduplane terminology.
|
||||
|
||||
Radio Passthrough mode is trigged by channel 8 > 1800 us.
|
||||
|
||||
Blue status LED has different behavior in passthrough mode :
|
||||
|
||||
- If throttle position < 1200 us, status LED is off
|
||||
|
||||
- If throttle position > 1200 us, status LED is on
|
||||
|
||||
------------------------------------------
|
||||
Failsafe mode
|
||||
------------------------------------------
|
||||
|
||||
If a receiver servo channel is lost, the last know channel position is used.
|
||||
If all contact with the receiver is lost, an internal failsafe is trigged after 250ms.
|
||||
|
||||
Default failsafe values are :
|
||||
|
||||
Throttle channel low (channel 3 = 900 us)
|
||||
|
||||
Mode Channel set to flight mode 4 (channel 5 = 1555 us)
|
||||
|
||||
All other channels set to midstick (1500 us)
|
||||
|
||||
------------------------------------------
|
||||
PPM passtrough mode
|
||||
------------------------------------------
|
||||
|
||||
If your receiver has a PPM sum signal output, it is now possible to pass on the PPM signal from servo channel 1 to the PPM pin (APM atmega1280/2560 PPM decoder).
|
||||
To enable PPM passtrough, short the servo input channel 2 & 3 signal pins together using a jumper strap and use the channel 1 signal pin as PPM input.
|
||||
Please note that the PPM sum signal must be standard 8 channel PPM to work with the APM PPM decoder.
|
||||
|
||||
In this mode, the blue LED will blink like this : Long - Short - short
|
||||
|
||||
|
||||
------------------------------------------
|
||||
PPM redundancy mode
|
||||
------------------------------------------
|
||||
|
||||
This mode is designed for dual PPM receiver setups, effectively enhancing reliability.
|
||||
|
||||
It is possible to use it for teacher / student use. In this case, each receiver needs to be paired with a separate transmitter.
|
||||
|
||||
To enable this mode, short the servo input channel 3 & 4 signal pins together using a jumper strap and use the channels 1 and 2 signal pins as PPM inputs.
|
||||
|
||||
- Channel 1 is for the primary receiver
|
||||
|
||||
- Channel 2 is for the secondary receiver
|
||||
|
||||
|
||||
In this mode, the blue LED will blink like this on the APM 1.4 : Long - Short - short - short
|
||||
|
||||
|
||||
How redundancy does work :
|
||||
|
||||
|
||||
- If channel 1 receiver has a valid PPM signal and no failsafe indication, then it will be selected regardless channel 2 status
|
||||
|
||||
- If channel 1 receiver has a corrupted PPM signal or failsafe indication, and channel 2 receiver has a valid PPM signal, then channel 2 will be selected.
|
||||
|
||||
|
||||
It is recommended to use the more reliable receiver on channel 1 (the one with reliable error detection on it like most 2.4 Ghz RC radio systems).
|
||||
|
||||
The channel 1 receiver failsafe needs to be set on channel 8 or 16 at max PWM value (1900 us). This will be used for safely switching to channel 2.
|
||||
|
||||
Another solution is to program your receiver 1 to have no output signal in failsafe condition. In this case ch8 and 16 stay free for other uses (disabling CH8 / 16 failsafe needs a recompilation).
|
||||
|
||||
|
||||
|
||||
Note :
|
||||
|
||||
Each receiver needs to output a standard 8 channels PPM sum signal at 50 Hz :
|
||||
|
||||
Technically this translate to this PPM frame design :
|
||||
|
||||
- 1520 us central point
|
||||
- +/- 400 us PWM range (1100 us - 1900 us)
|
||||
- 20 ms frame period
|
||||
|
||||
|
||||
It is possible to use non standard receivers using those PPM frames formats :
|
||||
|
||||
|
||||
- Futaba PPMv2 mode 16 channels : 760 us +/- 250 us 20 ms period
|
||||
|
||||
- Jeti PPMv3 mode 16 channels : 1050 us +/- 250 us 25 ms period
|
||||
|
||||
- Hitec PPM mode 9 channels : 1520 us +/- 400 us 23 ms period
|
||||
|
||||
|
||||
|
||||
The PPM encoder will translate the input format so that it can be understood by the APM PPM decoder.
|
||||
|
||||
|
||||
Using one of those alternate frame formats ask for a firmware recompilation.
|
||||
|
||||
|
||||
|
||||
--------------------------------------------------------------------------------------------------
|
||||
Loading…
Reference in New Issue