mirror of
https://github.com/ArduPilot/ardupilot
synced 2025-01-03 22:48:29 -04:00
5e65e5ef8a
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.
474 lines
18 KiB
C
474 lines
18 KiB
C
// ------------------------------------------------------------------------------------------------------------------------------------------------------------
|
|
// 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
|
|
|
|
|