From 13681612c2b53a21a31b8f7f562eb21dab9b29db Mon Sep 17 00:00:00 2001 From: Andrew Tridgell Date: Fri, 9 Sep 2011 18:57:09 +1000 Subject: [PATCH] imported PPMEncoder --- Tools/PPMEncoder/ap_ppm_encoder.aps | 1 + Tools/PPMEncoder/ap_ppm_encoder.aws | 1 + Tools/PPMEncoder/ap_ppm_encoder.c | 1287 ++++++++ Tools/PPMEncoder/default/Makefile | 77 + Tools/PPMEncoder/default/ap_ppm_encoder.eep | 5 + Tools/PPMEncoder/default/ap_ppm_encoder.elf | Bin 0 -> 16459 bytes Tools/PPMEncoder/default/ap_ppm_encoder.lss | 2636 +++++++++++++++++ Tools/PPMEncoder/default/ap_ppm_encoder.map | 419 +++ .../PPMEncoder/default/dep/ap_ppm_encoder.o.d | 46 + .../PPMEncoder/default/ppm_enconder_at328.bat | 17 + Tools/PPMEncoder/servo2ppm_settings.h | 128 + 11 files changed, 4617 insertions(+) create mode 100644 Tools/PPMEncoder/ap_ppm_encoder.aps create mode 100644 Tools/PPMEncoder/ap_ppm_encoder.aws create mode 100644 Tools/PPMEncoder/ap_ppm_encoder.c create mode 100644 Tools/PPMEncoder/default/Makefile create mode 100644 Tools/PPMEncoder/default/ap_ppm_encoder.eep create mode 100644 Tools/PPMEncoder/default/ap_ppm_encoder.elf create mode 100644 Tools/PPMEncoder/default/ap_ppm_encoder.lss create mode 100644 Tools/PPMEncoder/default/ap_ppm_encoder.map create mode 100644 Tools/PPMEncoder/default/dep/ap_ppm_encoder.o.d create mode 100644 Tools/PPMEncoder/default/ppm_enconder_at328.bat create mode 100644 Tools/PPMEncoder/servo2ppm_settings.h diff --git a/Tools/PPMEncoder/ap_ppm_encoder.aps b/Tools/PPMEncoder/ap_ppm_encoder.aps new file mode 100644 index 0000000000..f324a0d4a9 --- /dev/null +++ b/Tools/PPMEncoder/ap_ppm_encoder.aps @@ -0,0 +1 @@ +ap_ppm_encoder03-Dec-2009 19:19:1423-Jan-2010 22:43:53241003-Dec-2009 19:19:1444, 17, 0, 655AVR GCCdefault\ap_ppm_encoder.elfC:\Users\Jordi PRO\Desktop\DEV\ArduPilotMega_v10\Source\Trunk\ap_ppm_encoder\AVR SimulatorATmega328P.xmlfalseR00R01R02R03R04R05R06R07R08R09R10R11R12R13R14R15R16R17R18R19R20R21R22R23R24R25R26R27R28R29R30R31Auto000ap_ppm_encoder.cservo2ppm_settings.hdefault\ap_ppm_encoder.lssdefault\ap_ppm_encoder.mapdefaultNOatmega328p111ap_ppm_encoder.elfdefault\0-Wall -gdwarf-2 -std=gnu99 -DF_CPU=8000000UL -Os -funsigned-char -funsigned-bitfields -fpack-struct -fshort-enumsdefault1C:\WinAVR-20090313\bin\avr-gcc.exeC:\WinAVR-20090313\utils\bin\make.exeC:\Users\Jordi PRO\Desktop\DEV\ArduPilotMega_v10\Source\Trunk\ap_ppm_encoder\servo2ppm_settings.hC:\Users\Jordi PRO\Desktop\DEV\ArduPilotMega_v10\Source\Trunk\ap_ppm_encoder\ap_ppm_encoder.c00000ap_ppm_encoder.c25900001servo2ppm_settings.h1 diff --git a/Tools/PPMEncoder/ap_ppm_encoder.aws b/Tools/PPMEncoder/ap_ppm_encoder.aws new file mode 100644 index 0000000000..ac2139b58e --- /dev/null +++ b/Tools/PPMEncoder/ap_ppm_encoder.aws @@ -0,0 +1 @@ + diff --git a/Tools/PPMEncoder/ap_ppm_encoder.c b/Tools/PPMEncoder/ap_ppm_encoder.c new file mode 100644 index 0000000000..6e03261eb8 --- /dev/null +++ b/Tools/PPMEncoder/ap_ppm_encoder.c @@ -0,0 +1,1287 @@ + +/********************************************************************************************************* + Title : C file for the rc ppm encoder (servo2ppm_v4_2.c) + Author: Chris Efstathiou + E-mail: hendrix at vivodinet dot gr + Homepage: ........................ + Date: 3/Aug/2009 + Compiler: AVR-GCC with AVR-AS + MCU type: ATmega168 + Comments: This software is FREE. Use it at your own risk. +*********************************************************************************************************/ + + +/********************************************************************************************************/ +/* PREPROCESSOR DIRECTIVES */ +/********************************************************************************************************/ + + +#include +#include +#include +#include +#include +#include "servo2ppm_settings.h" + + + +/********************************************************************************************************/ +/* Normaly you shouldn't need to change anything below this line but who knows? */ +/********************************************************************************************************/ + + + +#define RC_SERVO_PORT D /* The port for the servo pulse input. */ + +#define RC_LED_PORT B /* The port for the PPM waveform and the led. */ +#define RC_LED_PIN 0 /* The led indicator pin. */ + +#define RC_PPM_PORT B +#define RC_PPM_PIN 2 /* The PPM waveform output pin */ + +#define RC_MUX_PORT B /* The port for the MUX controller. */ +#define RC_MUX_PIN 1 /* The pin to control the MUX. */ + +#define RC_SETUP_PORT B +#define RC_SETUP_PIN 4 + +#define RC_TIMER0_TIMSK TIMSK0 /* Timer0 registers */ +#define RC_TIMER0_TIFR TIFR0 +#define RC_TIMER0_PRESCALER_REG TCCR0B + +#define RC_TIMER1_TIMSK TIMSK1 /* Timer1 registers */ +#define RC_TIMER1_TIFR TIFR1 +#define RC_TIMER1_PRESCALER_REG TCCR1B +#define RC_TIMER1_MODE_REG TCCR1A +#define RC_TIMER1_COMP1_REG OCR1A +#define RC_TIMER1_COMP2_REG OCR1B + +#define RC_PIN_INT_EN_REG PCICR +#define RC_PIN_INT_EN_BIT PCIE2 +#define RC_PIN_INT_FLAG_REG PCIFR +#define RC_PIN_INT_FLAG_BIT PCIF2 +#define RC_PIN_INT_MASK_REG PCMSK2 + +#define RC_SERVO_INPUT_CHANNELS 8 + + +/* +You can copy the above avr type data and edit them as needed if you plan to use +a different avr type cpu having at least 2k bytes of flash memory. +*/ + + +#define _CONCAT2_(a, b) a ## b +#define CONCAT2(a, b) _CONCAT2_(a, b) + +#define RC_SERVO_PORT_OUT_REG CONCAT2(PORT, RC_SERVO_PORT) +#define RC_SERVO_PORT_DDR_REG CONCAT2(DDR, RC_SERVO_PORT) +#define RC_SERVO_PORT_PIN_REG CONCAT2(PIN, RC_SERVO_PORT) + +#define RC_LED_PORT_OUT_REG CONCAT2(PORT, RC_LED_PORT) +#define RC_LED_PORT_DDR_REG CONCAT2(DDR, RC_LED_PORT) +#define RC_LED_PORT_PIN_REG CONCAT2(PIN, RC_LED_PORT) + +#define RC_MUX_PORT_OUT_REG CONCAT2(PORT, RC_MUX_PORT) +#define RC_MUX_PORT_DDR_REG CONCAT2(DDR, RC_MUX_PORT) +#define RC_MUX_PORT_PIN_REG CONCAT2(PIN, RC_MUX_PORT) + +#define RC_PPM_PORT_OUT_REG CONCAT2(PORT, RC_PPM_PORT) +#define RC_PPM_PORT_DDR_REG CONCAT2(DDR, RC_PPM_PORT) +#define RC_PPM_PORT_PIN_REG CONCAT2(PIN, RC_PPM_PORT) + +#define RC_SETUP_PORT_OUT_REG CONCAT2(PORT, RC_SETUP_PORT) +#define RC_SETUP_DDR_REG CONCAT2(DDR, RC_SETUP_PORT) +#define RC_SETUP_PIN_REG CONCAT2(PIN, RC_SETUP_PORT) + + +#if RC_PPM_GEN_CHANNELS > 8 +#error PPM generator max number of channels is 8 +#endif +#if RC_PPM_GEN_CHANNELS == 0 +#error PPM generator channels cannot be zero! +#endif + +#if RC_PPM_GEN_CHANNELS < RC_SERVO_INPUT_CHANNELS +#undef RC_SERVO_INPUT_CHANNELS +#define RC_SERVO_INPUT_CHANNELS RC_PPM_GEN_CHANNELS +#warning servo channels = PPM generator channels +#endif + +#if RC_PPM_RESET_PW == 0 && RC_PPM_FRAME_LENGTH_MS == 0 + +#undef RC_PPM_FRAME_LENGTH_MS +#undef RC_PPM_RESET_PW +#define RC_PPM_RESET_PW (23500UL - (8 * RC_SERVO_CENTER_PW)) +#define RC_PPM_FRAME_LENGTH_MS (RC_PPM_RESET_PW + (RC_PPM_GEN_CHANNELS * RC_SERVO_CENTER_PW)) + +#elif RC_PPM_FRAME_LENGTH_MS == 0 && RC_PPM_RESET_PW > 0 + +#undef RC_PPM_FRAME_LENGTH_MS +#define RC_PPM_FRAME_LENGTH_MS ((RC_PPM_GEN_CHANNELS * RC_SERVO_CENTER_PW)+RC_PPM_RESET_PW) + +#elif RC_PPM_FRAME_LENGTH_MS > 0 && RC_PPM_RESET_PW == 0 + +#undef RC_PPM_RESET_PW +#define RC_PPM_RESET_PW (RC_PPM_FRAME_LENGTH_MS -(RC_PPM_GEN_CHANNELS * RC_SERVO_CENTER_PW)) + +#endif + +#if RC_PPM_FRAME_LENGTH_MS > 25000UL +#warning PPM frame period exceeds 25ms +#endif + +#if RC_PPM_RESET_PW <= (5000UL + RC_PPM_CHANNEL_SYNC_PW) +#warning PPM reset period lower than 5ms +#endif + + +/* Search for a suitable timer prescaler. Both timers must use the same prescaling factor. */ +#if ((F_CPU * (RC_MAX_TIMEOUT/1000))/1000) < 0xFFF0 + +#define TIMER0_PRESCALER 1 +#define TIMER0_PRESCALER_BITS ((0< 255) +#error RC_MAX_TIMEOUT is set too high! +#endif + +#define RC_LED_FREQUENCY_VAL (((1000000/RC_LED_FREQUENCY)/23500)/2) //each frame is 23,5 ms +#if RC_LED_FREQUENCY_VAL <= 0 +#undef RC_LED_FREQUENCY_VAL +#define RC_LED_FREQUENCY_VAL 1 +#warning LED FREQUENCY set to maximum +#endif + +//It is used when there is no servo signals received so the elapsed time is always RC_MAX_TIMEOUT ms. +#define RC_LED_FREQUENCY_VAL_1HZ (((1000000/1)/RC_MAX_TIMEOUT)/2) + +/* Macro command definitions. */ +#define LED_ON() { RC_LED_PORT_OUT_REG |= (1< 1 +#error RC_USE_FAILSAFE can be 0 or 1 +#endif +#if RC_PPM_OUTPUT_TYPE > 1 +#error RC_PPM_OUTPUT_TYPE can be 0 or 1 +#endif +#if RC_CONSTANT_PPM_FRAME_TIME > 1 +#error RC_CONSTANT_PPM_FRAME_TIME can be 0 or 1 +#endif +#if RC_REDUCE_LATENCY > 1 +#error RC_REDUCE_LATENCY can be 0 or 1 +#endif + + +/********************************************************************************************************/ +/* TYPE DEFINITIONS */ +/********************************************************************************************************/ + + + + + + + +/********************************************************************************************************/ +/* LOCAL FUNCTION PROTOTYPES */ +/********************************************************************************************************/ +void initialize_mcu(void); +void wait_for_rx(void); +unsigned char detect_connected_channels(void); +unsigned int get_channel_pw(unsigned char pin); +void check_for_setup_mode(void); +void write_default_values_to_eeprom(void); +void load_failsafe_values(void); +void load_values_from_eeprom(void); +static inline void ppm_off(void); +static inline void ppm_on(void); + + +/********************************************************************************************************/ +/* GLOBAL VARIABLES */ +/********************************************************************************************************/ + +volatile unsigned int isr_channel_pw[(RC_PPM_GEN_CHANNELS +1)]; // +1 is for the reset pulse. +/* +unsigned int fs_channel_pw[8]= { RC_FS_CH_1_TIMER_VAL, RC_FS_CH_2_TIMER_VAL, + RC_FS_CH_3_TIMER_VAL, RC_FS_CH_4_TIMER_VAL, + RC_FS_CH_5_TIMER_VAL, RC_FS_CH_6_TIMER_VAL, + RC_FS_CH_7_TIMER_VAL, RC_FS_CH_8_TIMER_VAL, + }; +*/ +const char version_info[]="Servo2ppm V4.20"; +volatile unsigned char pin_interrupt_detected = 0; +volatile unsigned char isr_channel_number = 0; +volatile unsigned int isr_timer0_16 = 0; +unsigned char channels_in_use = 0; +unsigned char channel_mask = 0; + +union word2byte{ + volatile unsigned int timer0_16; + volatile unsigned char timer0[2]; + } timer0; + +volatile unsigned int isr_timer0 = 0; + +#if RC_CONSTANT_PPM_FRAME_TIME == 1 +volatile unsigned int reset_pw = 0; +#endif + +//unsigned char failsafe_mode = 0; +unsigned int ppm_off_threshold = RC_PPM_OFF_THRESHOLD_VAL; +unsigned char rc_lost_channel = (RC_LOST_CHANNEL-1); +//Dummy variable array in order not to use the beggining of the eeprom for usefull data. +//The beggining of the eeprom and especially byte 0 is prone to data corruption. +// "EEMEM" is equal to "__attribute__((section(".eeprom")))" +unsigned int dummy_int[10] __attribute__((section(".eeprom")))= { 1, 1 ,1, 1, 1, 1, 1, 1, 1, 1 }; +unsigned int EEMEM ppm_off_threshold_e[11] = { RC_PPM_OFF_THRESHOLD_VAL, RC_PPM_OFF_THRESHOLD_VAL, RC_PPM_OFF_THRESHOLD_VAL, + RC_PPM_OFF_THRESHOLD_VAL, RC_PPM_OFF_THRESHOLD_VAL, RC_PPM_OFF_THRESHOLD_VAL, + RC_PPM_OFF_THRESHOLD_VAL, RC_PPM_OFF_THRESHOLD_VAL, RC_PPM_OFF_THRESHOLD_VAL, + RC_PPM_OFF_THRESHOLD_VAL, RC_PPM_OFF_THRESHOLD_VAL + }; + +unsigned char EEMEM rc_lost_channel_e[11] = { RC_LOST_CHANNEL-1, RC_LOST_CHANNEL-1, RC_LOST_CHANNEL-1, + RC_LOST_CHANNEL-1, RC_LOST_CHANNEL-1, RC_LOST_CHANNEL-1, + RC_LOST_CHANNEL-1, RC_LOST_CHANNEL-1, RC_LOST_CHANNEL-1, + RC_LOST_CHANNEL-1, RC_LOST_CHANNEL-1 + }; +//unsigned int EEMEM ch_failsafe_pw_e[RC_PPM_GEN_CHANNELS]; + +/********************************************************************************************************/ +/* LOCAL FUNCTION DEFINITIONS */ +/********************************************************************************************************/ + +/*11111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111111*/ + +void initialize_mcu(void) +{ +unsigned char x = 0; +unsigned int eep_address = 0; + +asm("cli"); + +STOP_TIMER0(); +RESET_TIMER0(); + +/* Enable pwm mode 15 (fast pwm with top=OCR1A) and stop the timer. */ +/* The timer compare module must be configured before the DDR register. */ +// THE PPM GENERATOR IS CONFIGURED HERE ! +#if RC_PPM_OUTPUT_TYPE == 0 // NEGATIVE PULSES +RC_TIMER1_MODE_REG = (1< 0 + +void wait_for_rx(void) +{ +unsigned int pw=0; +unsigned char x = 0; +unsigned char servo_connected = 0; + +RESET_START_TIMER0(); +servo_connected = 0; +do{ + + wdt_reset(); + RESET_TIMER0(); + x=0; + while(timer0.timer0[1] <= RC_MAX_TIMEOUT_VAL ) + { + if( (RC_SERVO_PORT_PIN_REG & (1<<(RC_RX_READY_CHANNEL-1))) ) { x=timer0.timer0[1]; } + if( (timer0.timer0[1] - x) >= RC_PULSE_TIMEOUT_VAL ){ servo_connected = 1; break; } + } + + }while(servo_connected < 1); + +//Now test the found channel for a proper servo pulse. +x = 0; +while(1) + { + pw=get_channel_pw( (RC_RX_READY_CHANNEL-1) ); + if( (pw > RC_SERVO_MIN_PW_VAL) && (pw < RC_SERVO_MAX_PW_VAL) ) { x++; }else{ x=0; } + if(x >= 3) { break; } + } + +return; +} + +#else + +void wait_for_rx(void) +{ +unsigned int pw=0; +unsigned char x = 0; +unsigned char servo_connected = 0; +unsigned char channel = 0; + +RESET_START_TIMER0(); +do{ + for(channel=0; channel < RC_SERVO_INPUT_CHANNELS; channel++) + { + wdt_reset(); + RESET_TIMER0(); + servo_connected = 0; + x=0; + while(timer0.timer0[1] <= RC_MAX_TIMEOUT_VAL ) + { + if(RC_SERVO_PORT_PIN_REG & (1<= RC_PULSE_TIMEOUT_VAL ){ servo_connected = 1; break; } + } + if(servo_connected >= 1){ break; } + } + + }while(servo_connected < 1); + +//Now test the found channel for a proper servo pulse. +x = 0; +while(1) + { + pw=get_channel_pw(channel); + if( (pw > RC_SERVO_MIN_PW_VAL) && (pw < RC_SERVO_MAX_PW_VAL) ) { x++; }else{ x=0; } + if(x >= 3) { break; } + } + +return; +} +#endif + +/*22222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222*/ +/*33333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333*/ + +unsigned char detect_connected_channels(void) +{ +unsigned char channel=0; +unsigned char servo_connected = 0; +unsigned char connected_channels = 0; +unsigned char x = 0; +unsigned char y = 0; +#if defined(RC_RX_READY_CHANNEL) && RC_RX_READY_CHANNEL > 0 +unsigned int pw = 0; +#endif + +/* +There must be no error in which channels are connected to the encoder +because this will have devastating effects later on. +*/ +wdt_reset(); +RESET_START_TIMER0(); + + for(channel=0; channel < RC_SERVO_INPUT_CHANNELS; channel++) + { + servo_connected = 0; + for(y=0; y<5; y++) + { + wdt_reset(); + RESET_TIMER0(); + x=0; + while(timer0.timer0[1] <= RC_MAX_TIMEOUT_VAL ) + { + if(RC_SERVO_PORT_PIN_REG & (1<= RC_PULSE_TIMEOUT_VAL ){ servo_connected++; break; } + } + if(servo_connected >= 3){ channel_mask |= (1< 0 + +connected_channels = 0; +for(channel=0; channel < RC_SERVO_INPUT_CHANNELS; channel++) + { + if(channel_mask & (1< RC_SERVO_MAX_PW_VAL) + { + channel_mask &= (~(1<= match_lower_limit ) + { + if( eeprom_buf_x < RC_SERVO_INPUT_CHANNELS ) + { + rc_lost_channel = eeprom_buf_x; //Load the stored value to throttle_thershold. + if(match < match_upper_limit) + { + for(x=0; x= match_lower_limit ) + { + if( (eeprom_buf_x >= RC_PPM_OFF_UPPER_WINDOW_VAL) && (eeprom_buf_x <= RC_SERVO_MAX_PW_VAL) ) + { + ppm_off_threshold = eeprom_buf_x; //Load the stored value to throttle_thershold. + if(match < match_upper_limit) + { + for(x=0; x < match_upper_limit; x++){ eeprom_write_word(&ppm_off_threshold_e[x], eeprom_buf_x); } + } + + }else if( (eeprom_buf_x <= RC_PPM_OFF_LOWER_WINDOW_VAL) && (eeprom_buf_x >= RC_SERVO_MIN_PW_VAL) ) + { + ppm_off_threshold = eeprom_buf_x; //Load the stored value to throttle_thershold. + if(match < match_upper_limit) + { + for(x=0; x < match_upper_limit; x++){ eeprom_write_word(&ppm_off_threshold_e[x], eeprom_buf_x); } + } + + }else{ match = 0; } + break; + } + } + +if( match < match_lower_limit ){ write_default_values_to_eeprom(); return; } + + +return; +} + +/*55555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555555*/ +/*66666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666*/ + +unsigned int get_channel_pw(unsigned char pin) +{ + +unsigned int pw = 0; +unsigned char pw_measurement_started = 0; + +wdt_reset(); +/* The servo input pins are already configured as inputs with pullup resistors. */ +// First we must disable the pin interrupt. +RC_PIN_INT_EN_REG &= (~(1<= RC_MAX_TIMEOUT_VAL ) + { + return(0); + } + + }while( pin_interrupt_detected == 0 ); + pin_interrupt_detected = 0; + if( RC_SERVO_PORT_PIN_REG & (1< RC_MAX_TIMEOUT_VAL ){setup_mode = 0; break; } + } + + }while(x < 100); + +if(setup_mode) + { +/****************************************************************************************************/ +/* FIRST WE MUST FIND WHICH CHANNEL WILL BE USED AS A TX SIGNAL LOST INDICATOR */ +/****************************************************************************************************/ + wdt_reset(); + success = 0; + if(channels_in_use > 1 ) + { + if( channel_mask & (1<<(RC_LOST_CHANNEL - 1)) ) + { + rc_lost_channel = (RC_LOST_CHANNEL - 1); + for(x=0; x < (sizeof(rc_lost_channel_e)/sizeof(char)); x++) + { + eeprom_write_byte(&rc_lost_channel_e[x], rc_lost_channel); + } + success += 1; + } + + }else if(channels_in_use == 1) + { + for(x=0; x < RC_SERVO_INPUT_CHANNELS; x++) + { + if(channel_mask & (1<= RC_SERVO_MIN_PW_VAL && pw <= RC_SERVO_MAX_PW_VAL) + { + pw_buffer += pw; + y++; + } + } + pw_buffer /= y; + wdt_reset(); + if( (pw_buffer >= RC_PPM_OFF_UPPER_WINDOW_VAL) && (pw_buffer <= RC_SERVO_MAX_PW_VAL) ) + { + for(x=0; x<(sizeof(ppm_off_threshold_e)/sizeof(int)); x++) + { + eeprom_write_word(&ppm_off_threshold_e[x], (pw_buffer+RC_PPM_OFF_OFFSET_VAL)); + } + success += 1; + + }else if( (pw_buffer <= RC_PPM_OFF_LOWER_WINDOW_VAL) && (pw_buffer >= RC_SERVO_MIN_PW_VAL) ) + { + for(x=0; x<(sizeof(ppm_off_threshold_e)/sizeof(int)); x++) + { + eeprom_write_word(&ppm_off_threshold_e[x], (pw_buffer-RC_PPM_OFF_OFFSET_VAL)); + } + success += 1; + } + + } + +/****************************************************************************************************/ +/* LASTLY WE MUST INDICATE TO THE USER IF THE SETUP PROCEDURE WAS SUCCESSFUL */ +/****************************************************************************************************/ + if(success == 2) + { + RC_SETUP_PORT_OUT_REG &= (~(1<= 2 +isr_channel_pw[1] = RC_FS_CH_2_TIMER_VAL; +#endif +#if RC_PPM_GEN_CHANNELS >= 3 +isr_channel_pw[2] = RC_FS_CH_3_TIMER_VAL; +#endif +#if RC_PPM_GEN_CHANNELS >= 4 +isr_channel_pw[3] = RC_FS_CH_4_TIMER_VAL; +#endif +#if RC_PPM_GEN_CHANNELS >= 5 +isr_channel_pw[4] = RC_FS_CH_5_TIMER_VAL; +#endif +#if RC_PPM_GEN_CHANNELS >= 6 +isr_channel_pw[5] = RC_FS_CH_6_TIMER_VAL; +#endif +#if RC_PPM_GEN_CHANNELS >= 7 +isr_channel_pw[6] = RC_FS_CH_7_TIMER_VAL; +#endif +#if RC_PPM_GEN_CHANNELS >= 8 +isr_channel_pw[7] = RC_FS_CH_8_TIMER_VAL; +#endif +#if RC_PPM_GEN_CHANNELS >= 9 +isr_channel_pw[8] = RC_FS_CH_8_TIMER_VAL; +#endif +isr_channel_pw[RC_PPM_GEN_CHANNELS] = RC_RESET_PW_TIMER_VAL; + + +return; +} +/* +void load_failsafe_values(void) +{ +unsigned char x = 0; + +for(x=0; x< RC_PPM_GEN_CHANNELS; x++) + { + isr_channel_pw[x] = fs_channel_pw[x]; + } + +// LOAD THE PPM FRAME RESET PULSE WIDTH. +isr_channel_pw[RC_PPM_GEN_CHANNELS] = RC_RESET_PW_TIMER_VAL; + + +return; + +} +*/ + +/*88888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888*/ +/*99999999999999999999999999999999999999999999999999999999999999999999999999999999999999999999999999*/ + + +static inline void ppm_on(void) +{ + + RC_TIMER1_PRESCALER_REG &= (~(TIMER1_PRESCALER_BITS)); + TCNT1 = 0; + isr_channel_number = RC_PPM_GEN_CHANNELS; + RC_TIMER1_COMP1_REG = RC_RESET_PW_TIMER_VAL; + RC_TIMER1_COMP2_REG = RC_PPM_SYNC_PW_VAL; + RC_TIMER1_TIFR |= ( (1<RC_MUX_MIN)&&(PULSE_WIDTHRC_MUX_MIN)&&(PULSE_WIDTH= RC_MAX_TIMEOUT_VAL ) + { + goto PPM_CONTROL; + } + + }while( pin_interrupt_detected == 0 ); + x=0; + y=1; + //Only pins that changed their state will be tested for a high or low level + pin_reg_buffer0 = (RC_SERVO_PORT_PIN_REG & channel_mask_buffer); + pin_interrupt_detected = 0; + channels_to_check = pin_reg_buffer1 ^ pin_reg_buffer0; + pin_reg_buffer1 = pin_reg_buffer0; + while(x RC_SERVO_MIN_PW_VAL) && (timer0_buffer < RC_SERVO_MAX_PW_VAL) ) + { +#if defined(RC_LOST_CHANNEL) && RC_LOST_CHANNEL > 0 + if(x == rc_lost_channel) + { + if(ppm_off_threshold > RC_SERVO_CENTER_PW_VAL) + { + if(timer0_buffer >= ppm_off_threshold) + { + channel_mask_buffer = 0xFF; + goto PPM_CONTROL; + } + + }else{ + if(timer0_buffer <= ppm_off_threshold) + { + channel_mask_buffer = 0xFF; + goto PPM_CONTROL; + } + } + } +#endif + if(servo_signals_lost == 0) + { + asm("cli"); //Atomic operation needed here. + isr_channel_pw[x] = timer0_buffer; + asm("sei"); + } + + } // End of " if( (timer0_buffer > RC_SERVO_MIN_PW_VAL) && ..." statement. + } // End of "if( channel_status & y )" statement. + + } // End of "if( (pin_reg_buffer0 & y) )...else..." statement + } // End of "if(channels_to_check & y)" statement. + x++; + y=(y<<1); + } + + } // End of "while(channel_mask_buffer)" loop. +PPM_CONTROL: + + led_counter++; + if( led_counter >= led_frequency ){ led_counter = 0; TOGGLE_LED(); } + +//We need 'RC_MAX_BAD_PPM_FRAMES" consecutive readings in order to change the PPM generator's status. + if( channel_mask_buffer == 0 ) //IF ALL CHANNELS HAVE BEEN MEASURED... + { + tx_signal_lost = 0; + if(servo_signals_lost == 1) //IF PREVIOUSLY THE SERVO SIGNAL WAS LOST... + { + tx_signal_detected++; + if(tx_signal_detected > RC_MAX_BAD_PPM_FRAMES) + { + ppm_on(); + servo_signals_lost = 0; + LED_ON(); + led_counter = 0; + led_frequency = RC_LED_FREQUENCY_VAL; + } + } + } + else{ //IF NOT ALL CHANNELS HAVE BEEN MEASURED... + pin_reg_buffer1 = (RC_SERVO_PORT_PIN_REG & channel_mask); + tx_signal_detected = 0; + if(servo_signals_lost == 0) + { + tx_signal_lost++; + if(tx_signal_lost > RC_MAX_BAD_PPM_FRAMES) + { + servo_signals_lost = 1; + led_counter = 0; + led_frequency = RC_LED_FREQUENCY_VAL_1HZ; +#if defined(RC_USE_FAILSAFE) && RC_USE_FAILSAFE == 1 + load_failsafe_values(); +#else + ppm_off(); +#endif + } + } //end of if(servo_signals_lost == 0) statement. + } //end of if( x > (channels_in_use/2) ){...}else{...} statement. + + +mux_control(); + + } //end of while(1) loop. + +} + +/********************************************************************************************************/ +/* INTERRUPT SERVICE ROUTINES */ +/********************************************************************************************************/ + +//ISR(TIMER0_OVF_vect, ISR_NAKED) +ISR(TIMER0_OVF_vect) +{ +timer0.timer0[1]++; + +return; +} + +/********************************************************************************************************/ +/* +By using the fast pwm mode 15 which uses the OCR1A value as TOP value and changing the values within the +OCR1B interrupt, timing is very accurate because the OCR1A and OCR1B registers are double buffered. +This means that although it looks like we modify both registers within the OCR1B interrupt we are actually +modifying their buffers and not the actual registers. +Both actual registers are updated when the timer reaches the OCR1A (TOP) value automatically. +This way the OCR1B interrupt can be delayed as needed without any loss of timing accuracy. +*/ +ISR(TIMER1_COMPB_vect) +{ +asm("sei"); +#if RC_CONSTANT_PPM_FRAME_TIME == 1 +isr_channel_number++; +if( isr_channel_number >= (RC_PPM_GEN_CHANNELS + 1) ) {isr_channel_number = 0; reset_pw = RC_PPM_FRAME_TIMER_VAL; } + +if(isr_channel_number < RC_PPM_GEN_CHANNELS) + { + RC_TIMER1_COMP1_REG = isr_channel_pw[isr_channel_number]; + reset_pw -= RC_TIMER1_COMP1_REG; + + }else{ + RC_TIMER1_COMP1_REG = reset_pw; + } + +#endif +#if RC_CONSTANT_PPM_FRAME_TIME == 0 + +isr_channel_number++; +if( isr_channel_number >= (RC_PPM_GEN_CHANNELS + 1) ) {isr_channel_number = 0; } + RC_TIMER1_COMP1_REG = isr_channel_pw[isr_channel_number]; + +#endif + +return; +} + +/********************************************************************************************************/ + +ISR(PCINT2_vect) +{ + +timer0.timer0[0]= TCNT0; +if( RC_TIMER0_TIFR & (1< $@ + +size: ${TARGET} + @echo + @avr-size -C --mcu=${MCU} ${TARGET} + +## Clean target +.PHONY: clean +clean: + -rm -rf $(OBJECTS) ap_ppm_encoder.elf dep/* ap_ppm_encoder.hex ap_ppm_encoder.eep ap_ppm_encoder.lss ap_ppm_encoder.map + + +## Other dependencies +-include $(shell mkdir dep 2>/dev/null) $(wildcard dep/*) + diff --git a/Tools/PPMEncoder/default/ap_ppm_encoder.eep b/Tools/PPMEncoder/default/ap_ppm_encoder.eep new file mode 100644 index 0000000000..cd488d1e5f --- /dev/null +++ b/Tools/PPMEncoder/default/ap_ppm_encoder.eep @@ -0,0 +1,5 @@ +:1000000001000100010001000100010001000100E8 +:1000100001000100E907E907E907E907E907E9073E +:10002000E907E907E907E907E90702020202020214 +:050030000202020202C1 +:00000001FF diff --git a/Tools/PPMEncoder/default/ap_ppm_encoder.elf b/Tools/PPMEncoder/default/ap_ppm_encoder.elf new file mode 100644 index 0000000000000000000000000000000000000000..4ee1ca8525259b4bed4ed566f498997b03594e84 GIT binary patch literal 16459 zcmcJ0d3;pmz4!B+$x0v#kPz82Bmu&b$-?5=ilV*Hs}jM5QtBbgghaDBNm#5`2MdUZ zGMPz$Ms8g{e4?+fg}N)?RJm4LHN5I;t+(OL0_VwoN%??kHKYKqI35rdJgOPP&%h8=jLXq1q*ZKubi;~`&j!0%#5(XTqOZ4Phqt$+ciN6gz0-P&cg*Y^`_$HBw?Dnf z3Py6}E%+TNZ#%jjo^oU`k}ZSBB35>^$l7{-ICbes##``Ac;C8xmMcEF?ZYz5j9FII zsYTYhqeT}C*mhfymbTe@<7HXXU9uxymRqb1`$AS zRu>HGJ~B{c&9h$Zk44Uqo6Fwo>hDVH>FWt~HHf=z4n-QoB*bAv3$tbwZ0iif>QGkd>%S}ZOo?tr0e@jsW@mIv=b>+#bt}(LO z`p^FQqG>0*;LE`mkDHSb_xiF->&c1~MB)dMY5 z2YESlwO-X#)k(Xa8cvLi3hR&kY2^n${i;9P3YKMd&z0*IN9NsnbX8P-GI&D@%^fvdSYx(is{1o6cU7IAbyxsG%PfkN$nYJUXry!ga z&Uzv%oKw6t!uYqE!3ci4vO-z=vqA--{2>;1M~+2KMb>l&yKa=_uP9<`~m&*40+3` zJEOMG|6cyS=(_UsScgixpsY$guQg@ARZmt=&#gV|gTYg|Q7=XxdW@IHJr2rk^H82{ zlqV(XJfDX@Ux;2E#j~Gn2|P(7C41yf8APAF4Sh2I~XMhU(|I^uMU+8hEe&?|Gob-}4YDr-wc~ zE&1}6^0e5b{&V|h?qFn(_Ol1r&uDA-=J3|=-QnlL{o(h*$HJ$=@qTgbkv8l9$dBZV zZETap`!2w{D)#s<%XVASkCa=MYANRp7P2;vxhpmOAV9%=N2thK%^dHT9J2s%7X(%(ccWV{M$oP z?2es2#V|Y>A?u~gKTO;*`d^dMV-NPTH#oI@Ds(LLUZ_9xT7+ne>n+#boMw_YNrvRq`OMo+g(}AVHq2p|3 z_sw{oJ`%x5KCs*1d+hd``5tQvogv%8|LFZTZO_z6@xH%dpEop8TA^$i3+2fA;#Z2F zEq=H7jqZ7eAMgq5yiex)=2%O7JC5Gwi&#JR#oy#(N`H)zgHKIbY&j^tAD=$4cF*-i zd`F(?tDJI3=4`9Ro4Lx`SoX6?=_k*LZY;aCXkjdfcVr0f$ee9EBX>sXr~9HCOE(t% z==jF6U{8LL^9H@6YiC!^_L6aDXS_W4E_>-@abESwJ3jd^Z-ezkiZ|xVJwMqS8y7v2 zcOoTv@?G!3lbD@J=Y3G|Gw;vVbhFQn+%~mrLHXs$IkEbzr~7k?(qs4a8_}s{1;?_? zVYwx-4STw*+oo=KT39(nYdWQFccu$0YkH!ILpK8vWjP1FXM<4Q-oCY3;Ur zXxy{oE8sf?Z>tOwEq%8uw(zrh=arEbDo2AlMV?*(+bgyy1)PQ~gW)fmN-Q zZEelot7iKu%f+8l4Xir8{qN7Ir~7BbFD^+8+yV<9Ub}FAK1`A7rvyq{KRR8A53#ao zY-#LhtZr(&!EZL#c8aCY*7-a9wH;<{YfB5zUuV`fRD*147f(RD(hrp_FxyrO=@DXe zbz_HF-x@FjYeW_Jrq=2@v%b2qslB@1Z?39t>h!mZ6;L#HuA$qGKx>n@0laChLJmk+ zF;{FI!dHq1-25EzG}zUF#ty$(=dZ8sZ0gW{&5l;n?{5pVHj6PRb8Uma_Bs_yyT7Be z&1`P1^NX*cP%+t|nDwxB5?zTET61+{i+D5@mC+t>m+c5dtNelX#?}_Iv8BFMdcwxpq%AA3lgV7;mGW4_ltjy%gpBL z_Upto5ZFn?^+P;}^ zo-Uo~7XtktAtj}_vZ1m2N|G!BFSmlb3j{mW&J6tEGGxjuh&?gTo*T|olC%m)l8j_# z>-iUIZ>*xiNWB`2k(9g`{mw{AT}&=5`BHLesh5IFK110}mp=!lyhOzen`R`&Ot}Yg zkxFlA%HLvmn30rrwlx~Q?)BjxBO@W%5V>B*JLk=l44!ksY8yB0sj2cD~;7h|%)upOSD zb;$by*!)TO;dRDLa-_yk&HEs3nDK$nKLC0L?y(fgdNEF3%6w1@6ubo!SaKW~zeX6B zOV+b&oNychW3gn_&&NHHUYEkQh4!1|c~BQvpbLJFAK|4s2&$LrJETh`CZR$gJSCND z=GR2uAumUlyo*T-gSi%d%($eYrN8jcOl8>IwT#VU(=FrIqBq%e9lZaTc8?n>RC@l# z_RfK-2oJ9?9#gj0A%|iNV8)XM3w0ASe21n_*{)bIIlsQ2Yc{l6MsSC3IrSbqI`Jj@X{YuzpCVFqe>*1LD-Jkckrc22J)Q zNvcY0b&0%y!dH_d1vfdu3t8Hak|YJcwT0tkAuZ1|Bn2hOphI2`j*}7W*{XCC-$ypD zLv(~mzGr-e1OjtV7QxaoZJWttPWmbp{WH}4n*7y@&s)aQFI6;~;YT)w(k@ffL_W}0Ag>jy z(3M6nN>s2LvT2S`J=I6y7S%%IYhk#G#@8CZ88mi;%6K@k*L8Rg44lTfksP_3f+e#50g z#kM1@;YpILRK7(}v0IQq;rS?FE-Ac?X)|o@stI2(!{JFg;dl$oSox4t z$AzotZ=}urq;L&&qik+jAsAtdwz(D4=hAXxQV3J78GPJ7MIjgPJvuRICV6Gumo$SO zpMlYrxQ*^4FNcCPs0>vhdy=L(v^^f+Ib`&_)!QzW@6yK+a2iK0PF$2YO_Zj*%rjvV_@qVxcBoT6URFwLXrhx9sC4SW#gQ*=KamN=p_C{l0qQq>(`g}oL{5hslP>0cp^Vuk(_tcHj+~r1D#MdGE;C1tl-cO`MzP4q_&!9K z=i~`b<~bSXW-J_q36Pc}b@CYL$;`-rCRsw0GQu`XrdjG7K}eHw;V3H8CxI_RR~Tle z$0Wsv)J!{dxHp3)!;^(4jRGbd0Y+)bWM&~25?MM;#?;{pN7<>ebtnA;yL?u~tg@!YnzEI(wPijZ z|3g*Q*izfni8asv#Fz<%U}}3MS>0xGq2X_-#TqK$s}(3qdq*8+^|l`U+Sr9S ze-G>f<^mrE<^y*F#{nM$dV!n{)&idb(rzzs3~)biJn(s79`Jz1*MLRf4+4{nv6`ai z*R{o)KzjNkkoqG)=J=tOpVY{xX~#)Dk*`kXJXZ7Lfy{ZL=Div@K2Tn)`6(JpfJ{GK z^PFpw_i4To$aGv4Fh8yZh*dzw$vJb9^z6D>f>{oRiLU{&8ft({T&Izf>m-8}*9IyR z7^Fmc`c(4uV3>R(kjd`^GI>ZNC*Mg%10-BP(DD~RM)Qc~cWeGBjZt7W^e+Ngn_Pm? z{-8#Vz0B!NApLRnejV@~U?K32K-!-KGLkQWX8^y}@-*}-wqibzcGH15z)B$PxQrME z{DGG9tvm-j=kYr<(yj$O^IrpGKASaS{$kgF#$Jt&09pHcflPk@$o#qfBfbfwA4}^$ z(0mNYdiWGbyDx#vmkR>V(SKnZ8hA_4^++IH=L1Ip#{!vTww7O{`NcpMnfoDZQLZXW zfGt|jWdPF!fgY^+l8yDCd_b-pJYf$-Bx%G64RHsM@!tbvd=G1UOygdS2Q%e84>Z46V-t}1 za@9iI0;K=2*6-5zgvPx<=F0_*=Y<a5=L1Vl!RjzqJma-72N(p=xd^vC;uoB4VE(Fra zB^s{+(tbIx09dExT+tx52XAC1ZD8!!IzZ8U6_Byr0HnJcHF6Pz*xph-APj~*a|e)l zKc$Vh9D?sVmG1*!=1*45Yvjrck@ToYxCBE^pQxM`gW)4nV_V#zEVv4T#b?SQ2<9wUtOv4` zn}MEsR@a@NSfmh;QS1aVst16K{vjY!JOZTMZXoUU0%>;uNV`{nlYj?-w0j*$yF)Z(k=z_W!hx|JrOl5XM?0_Jn$#Li9ot22A&7xlG4+!I_3h9 zth*{;29S$A)Y+%1MwWtM(=G=xJ(q^4q3x=MxYC2=A!XU3Ed$z;>q1!Gr7YKj@iegR zHi2Tx-l@&PKnA))BNrGf8&`|){gevv0Wd81E+G5RLqH~aSRpxhdRawq2^faE6v&h-fK0*lC{pZEDY!62ivOolYy_kG7O1(f4Fg&6T%vm3R7TH( zoC?wFK)U--<6nTx=BP$4_&mqdSaedGa7jUvG$146&O33u#$t^#HO|&}9+1x;*N?R{;$>8q_Iuo4H`FT+ybPZ9l-AacWU`#Ku@0veJ@C6eE=8*zNk%J z*YZDV`~b*``VdH$A8Y(l%Q>S}Mc0@Iq}^B`tH7(}Q?&dnjTdSC0Z18t#FZCKOMr~7 zT;o|9=V@G^u}b4oAe)do2}^)MAe*WiNPjytuQu|fR2%P@w{Y3B84uV6OgN1JSP~JxOh$=u5t0)!%RV*PJA4jgRXcv1aQ->iN|2Aojms$dL*>E zHBeVsv$n%8{^_zSz4N`ZeY1R(qGOHO-ng=*x+#7@gtIOlgnjGmr@6YLwgD$MtNj79 zsj<1S!#>_=u}`I#Z+*ajeJ7S-Yfpc=HUFIDSGMDnYWWWlN~5=G@uKAy`P;ASXl+}5 z(ZyFSzaUW8S=HFo+Ht9WWwp7gqI~(1*3Lk!fB6-G&X((z4_)Og*N1DO6U)4_%np4B zW!7}o*Q4YK=VSJ2kxFyTw-2Roj8s*1sX2eqWmha-bV>ZIZw}7lT2^}1508iv4iu}d zykyD6=G6-?x?+Jqu~xO}nrv66TTb5)Qro&Z%cS zojUvr_*d$j$`kks(YD$NqO*k^z+dOZSzmzi)_Uh?(N+FID^^6j)9@V6b#}+bsF;h^ zFijlz;p|gzv7fj+*R5e6lBynwv%x@zcSs&XjM#S>TC=~py))o%_P2DfUIRSOgrQko zSBL!ChcvrB*ZlSog<{4yy|cl&jlWs)0|mq zOZx!T?MD^Ue#|=C+6Eu9ctmqqTAm5BC$H7LuGTsJ{;m_>_RiW`){gu1TXns}lWz<+ z+)u&IdP77uM(hC53+|`^4*|caq%G4#dZ5p+%A_K-A3b1 z5p@yik4Vyaim!>t+K430<+h_IeU(e^xaMg`6t@(0gm)BKO*mvr30yx*QSGGR=zc_r zRyd-|FyLLYSxL_Q3f%N2)z2lOEqPx=GJ+v%rZ9^Qc4@#o~T z7&gn4on1_>!R~SK%;z;1AJ50Ocl_+c3*#HyQZy14Ac++9gepZg5||jpc6`9r2@_IA z%P1iwOjl$zflD4p2{+kN!X{hl1i*rXl>ZfsdB06ED>{~9g;N6{oitqhoP#%6inul+ zuZu|1A1eNuh%_UT^v5ne=`URRL_borI4-34v6C87Cj16Br0z5qDN$?Ou+`CWxpf40 zVh4}nJv$S#Mk%f)G+@Gnsv~fY1u5a6EhX@y7DyepA0RpBw^Gy`1pLJ&IUThPYaBxw zy^T9X*v(8!nmN2nVjV*qMt`8uM!ysda zY-+7_NS)1K#Q%^O2MZ2Lm|B4ds=tF6O9sg#+xg5LM5I8h-<3n$ErqHKF<6Ebsqz zea27_9$B-FbI{`a(t<%Zu`-LopV5_Dt$WgLmr-!1Jsep!7T<8t<|{8~mrb{tgb~E_?cW2z;e(?`-s^ zXIy%g@86g{8RI?*$n@`kcgOc}g8e`O??JkF{Krq_f_JB%1il>g!7Cf}Hv{~AinHtY z%mjWu_|fRKd9caBXv41x9e<4X8t@|r>l?F_g!DHg@Ea5OF!=nz^d50vg8u)4->l0h1?kE2 zD?N98Oasq(49CA5)Z*L({e|F9r9p{{@%|9Jv#&&dSAmb$pA~HTU*dI&azkSkgnoAa6{7xL}fQgUI19hg_hZuBRIotjwJ#n{S(4e9c32U$qg54kcYjg0b0@N;p zzoibmj&d$G!cOzy`_Pz%q?i>?TuD0aSnU8&f)DfC@%aa{VSNrsUO6Y8z5@GRiC?5+ z^Up-!*v(AbXB&*U!WBZrnJ#x{JG*-A&iCl-%S^MukIla! zWji}=Z+By-kK;?_K~I$xak_%Lu;^Q-`HKkmewNb<>^?E%Zy}sovEzV2?R7)`zQgph z3B=Gi&q4%#|LTDKEcn+ryWK: + 0: 0c 94 34 00 jmp 0x68 ; 0x68 <__ctors_end> + 4: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + 8: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + c: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + 10: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + 14: 0c 94 3f 02 jmp 0x47e ; 0x47e <__vector_5> + 18: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + 1c: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + 20: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + 24: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + 28: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + 2c: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + 30: 0c 94 14 02 jmp 0x428 ; 0x428 <__vector_12> + 34: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + 38: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + 3c: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + 40: 0c 94 03 02 jmp 0x406 ; 0x406 <__vector_16> + 44: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + 48: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + 4c: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + 50: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + 54: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + 58: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + 5c: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + 60: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + 64: 0c 94 51 00 jmp 0xa2 ; 0xa2 <__bad_interrupt> + +00000068 <__ctors_end>: + 68: 11 24 eor r1, r1 + 6a: 1f be out 0x3f, r1 ; 63 + 6c: cf ef ldi r28, 0xFF ; 255 + 6e: d8 e0 ldi r29, 0x08 ; 8 + 70: de bf out 0x3e, r29 ; 62 + 72: cd bf out 0x3d, r28 ; 61 + +00000074 <__do_copy_data>: + 74: 11 e0 ldi r17, 0x01 ; 1 + 76: a0 e0 ldi r26, 0x00 ; 0 + 78: b1 e0 ldi r27, 0x01 ; 1 + 7a: ee e1 ldi r30, 0x1E ; 30 + 7c: fc e0 ldi r31, 0x0C ; 12 + 7e: 02 c0 rjmp .+4 ; 0x84 <.do_copy_data_start> + +00000080 <.do_copy_data_loop>: + 80: 05 90 lpm r0, Z+ + 82: 0d 92 st X+, r0 + +00000084 <.do_copy_data_start>: + 84: a4 31 cpi r26, 0x14 ; 20 + 86: b1 07 cpc r27, r17 + 88: d9 f7 brne .-10 ; 0x80 <.do_copy_data_loop> + +0000008a <__do_clear_bss>: + 8a: 11 e0 ldi r17, 0x01 ; 1 + 8c: a4 e1 ldi r26, 0x14 ; 20 + 8e: b1 e0 ldi r27, 0x01 ; 1 + 90: 01 c0 rjmp .+2 ; 0x94 <.do_clear_bss_start> + +00000092 <.do_clear_bss_loop>: + 92: 1d 92 st X+, r1 + +00000094 <.do_clear_bss_start>: + 94: a0 33 cpi r26, 0x30 ; 48 + 96: b1 07 cpc r27, r17 + 98: e1 f7 brne .-8 ; 0x92 <.do_clear_bss_loop> + 9a: 0e 94 57 04 call 0x8ae ; 0x8ae
+ 9e: 0c 94 0d 06 jmp 0xc1a ; 0xc1a <_exit> + +000000a2 <__bad_interrupt>: + a2: 0c 94 00 00 jmp 0 ; 0x0 <__vectors> + +000000a6 : + +/** \ingroup avr_eeprom + Read one byte from EEPROM address \a __p. + */ +__ATTR_PURE__ static __inline__ uint8_t eeprom_read_byte (const uint8_t *__p) +{ + a6: 9c 01 movw r18, r24 + do {} while (!eeprom_is_ready ()); + a8: f9 99 sbic 0x1f, 1 ; 31 + aa: fe cf rjmp .-4 ; 0xa8 +#if E2END <= 0xFF + EEARL = (size_t)__p; +#else + EEAR = (size_t)__p; + ac: 32 bd out 0x22, r19 ; 34 + ae: 21 bd out 0x21, r18 ; 33 + /* Use inline assembly below as some AVRs have problems with accessing + EECR with STS instructions. For example, see errata for ATmega64. + The code below also assumes that EECR and EEDR are in the I/O space. + */ + uint8_t __result; + __asm__ __volatile__ + b0: f8 9a sbi 0x1f, 0 ; 31 + b2: 80 b5 in r24, 0x20 ; 32 + : "i" (_SFR_IO_ADDR(EECR)), + "i" (EERE), + "i" (_SFR_IO_ADDR(EEDR)) + ); + return __result; +} + b4: 08 95 ret + +000000b6 : + +/** \ingroup avr_eeprom + Write a byte \a __value to EEPROM address \a __p. + */ +static __inline__ void eeprom_write_byte (uint8_t *__p, uint8_t __value) +{ + b6: 9c 01 movw r18, r24 + do {} while (!eeprom_is_ready ()); + b8: f9 99 sbic 0x1f, 1 ; 31 + ba: fe cf rjmp .-4 ; 0xb8 + +#if defined(EEPM0) && defined(EEPM1) + EECR = 0; /* Set programming mode: erase and write. */ + bc: 1f ba out 0x1f, r1 ; 31 +#endif + +#if E2END <= 0xFF + EEARL = (size_t)__p; +#else + EEAR = (size_t)__p; + be: 32 bd out 0x22, r19 ; 34 + c0: 21 bd out 0x21, r18 ; 33 +#endif + EEDR = __value; + c2: 60 bd out 0x20, r22 ; 32 + + __asm__ __volatile__ ( + c4: 0f b6 in r0, 0x3f ; 63 + c6: f8 94 cli + c8: fa 9a sbi 0x1f, 2 ; 31 + ca: f9 9a sbi 0x1f, 1 ; 31 + cc: 0f be out 0x3f, r0 ; 63 + [__sreg] "i" (_SFR_IO_ADDR(SREG)), + [__eemwe] "i" (EEMWE), + [__eewe] "i" (EEWE) + : "r0" + ); +} + ce: 08 95 ret + +000000d0 : +void initialize_mcu(void) +{ +unsigned char x = 0; +unsigned int eep_address = 0; + +asm("cli"); + d0: f8 94 cli + +STOP_TIMER0(); + d2: 15 bc out 0x25, r1 ; 37 +RESET_TIMER0(); + d4: a8 9a sbi 0x15, 0 ; 21 + d6: 16 bc out 0x26, r1 ; 38 + d8: 10 92 1d 01 sts 0x011D, r1 + +/* Enable pwm mode 15 (fast pwm with top=OCR1A) and stop the timer. */ +/* The timer compare module must be configured before the DDR register. */ +// THE PPM GENERATOR IS CONFIGURED HERE ! +#if RC_PPM_OUTPUT_TYPE == 0 // NEGATIVE PULSES +RC_TIMER1_MODE_REG = (1< +// VERSION CONTROL +x=0; +eep_address = (E2END - (sizeof(version_info)-1)); +while(version_info[x]) + { + if( (eep_address) < E2END) + 150: 23 e0 ldi r18, 0x03 ; 3 + 152: 8f 3f cpi r24, 0xFF ; 255 + 154: 92 07 cpc r25, r18 + 156: 58 f4 brcc .+22 ; 0x16e +/** \ingroup avr_eeprom + Write a byte \a __value to EEPROM address \a __p. + */ +static __inline__ void eeprom_write_byte (uint8_t *__p, uint8_t __value) +{ + do {} while (!eeprom_is_ready ()); + 158: f9 99 sbic 0x1f, 1 ; 31 + 15a: fe cf rjmp .-4 ; 0x158 + +#if defined(EEPM0) && defined(EEPM1) + EECR = 0; /* Set programming mode: erase and write. */ + 15c: 1f ba out 0x1f, r1 ; 31 +#endif + +#if E2END <= 0xFF + EEARL = (size_t)__p; +#else + EEAR = (size_t)__p; + 15e: 92 bd out 0x22, r25 ; 34 + 160: 81 bd out 0x21, r24 ; 33 +#endif + EEDR = __value; + 162: e0 bd out 0x20, r30 ; 32 + + __asm__ __volatile__ ( + 164: 0f b6 in r0, 0x3f ; 63 + 166: f8 94 cli + 168: fa 9a sbi 0x1f, 2 ; 31 + 16a: f9 9a sbi 0x1f, 1 ; 31 + 16c: 0f be out 0x3f, r0 ; 63 + { + eeprom_write_byte( (unsigned char*)eep_address, version_info[x]); + } + eep_address++; + 16e: 01 96 adiw r24, 0x01 ; 1 +rc_lost_channel = (RC_LOST_CHANNEL-1); +ppm_off_threshold = RC_PPM_OFF_THRESHOLD_VAL; +// VERSION CONTROL +x=0; +eep_address = (E2END - (sizeof(version_info)-1)); +while(version_info[x]) + 170: e8 2f mov r30, r24 + 172: e0 5f subi r30, 0xF0 ; 240 + 174: f0 e0 ldi r31, 0x00 ; 0 + 176: e0 50 subi r30, 0x00 ; 0 + 178: ff 4f sbci r31, 0xFF ; 255 + 17a: e0 81 ld r30, Z + 17c: ee 23 and r30, r30 + 17e: 41 f7 brne .-48 ; 0x150 +/** \ingroup avr_eeprom + Write a byte \a __value to EEPROM address \a __p. + */ +static __inline__ void eeprom_write_byte (uint8_t *__p, uint8_t __value) +{ + do {} while (!eeprom_is_ready ()); + 180: f9 99 sbic 0x1f, 1 ; 31 + 182: fe cf rjmp .-4 ; 0x180 + +#if defined(EEPM0) && defined(EEPM1) + EECR = 0; /* Set programming mode: erase and write. */ + 184: 1f ba out 0x1f, r1 ; 31 +#endif + +#if E2END <= 0xFF + EEARL = (size_t)__p; +#else + EEAR = (size_t)__p; + 186: 8f ef ldi r24, 0xFF ; 255 + 188: 93 e0 ldi r25, 0x03 ; 3 + 18a: 92 bd out 0x22, r25 ; 34 + 18c: 81 bd out 0x21, r24 ; 33 +#endif + EEDR = __value; + 18e: 10 bc out 0x20, r1 ; 32 + + __asm__ __volatile__ ( + 190: 0f b6 in r0, 0x3f ; 63 + 192: f8 94 cli + 194: fa 9a sbi 0x1f, 2 ; 31 + 196: f9 9a sbi 0x1f, 1 ; 31 + 198: 0f be out 0x3f, r0 ; 63 + eep_address++; + x++; + } +eeprom_write_byte((unsigned char*)E2END, '\0'); //Terminate the version control string. + +asm("sei"); + 19a: 78 94 sei + +/* give some time for the pull up resistors to work. ~30 ms * 3 = 90 ms */ +LED_OFF(); + 19c: 28 98 cbi 0x05, 0 ; 5 +RESET_START_TIMER0(); + 19e: 15 bc out 0x25, r1 ; 37 + 1a0: a8 9a sbi 0x15, 0 ; 21 + 1a2: 16 bc out 0x26, r1 ; 38 + 1a4: 10 92 1d 01 sts 0x011D, r1 + 1a8: 82 e0 ldi r24, 0x02 ; 2 + 1aa: 85 bd out 0x25, r24 ; 37 + 1ac: 90 e0 ldi r25, 0x00 ; 0 +for(x=0; x<3; x++) + { + wdt_reset(); + 1ae: a8 95 wdr + RESET_TIMER0(); + 1b0: a8 9a sbi 0x15, 0 ; 21 + 1b2: 16 bc out 0x26, r1 ; 38 + 1b4: 10 92 1d 01 sts 0x011D, r1 + while(timer0.timer0[1] < RC_MAX_TIMEOUT_VAL ); + 1b8: 80 91 1d 01 lds r24, 0x011D + 1bc: 86 37 cpi r24, 0x76 ; 118 + 1be: e0 f3 brcs .-8 ; 0x1b8 +asm("sei"); + +/* give some time for the pull up resistors to work. ~30 ms * 3 = 90 ms */ +LED_OFF(); +RESET_START_TIMER0(); +for(x=0; x<3; x++) + 1c0: 9f 5f subi r25, 0xFF ; 255 + 1c2: 93 30 cpi r25, 0x03 ; 3 + 1c4: a1 f7 brne .-24 ; 0x1ae + wdt_reset(); + RESET_TIMER0(); + while(timer0.timer0[1] < RC_MAX_TIMEOUT_VAL ); + } +x = 0; +STOP_TIMER0(); + 1c6: 15 bc out 0x25, r1 ; 37 +RESET_TIMER0(); + 1c8: a8 9a sbi 0x15, 0 ; 21 + 1ca: 16 bc out 0x26, r1 ; 38 + 1cc: 10 92 1d 01 sts 0x011D, r1 +LED_ON(); + 1d0: 28 9a sbi 0x05, 0 ; 5 +to the timer1 compare module to initialize. +The timer1 compare module as the Mega 168 manual states must be initialized before setting the DDR register +of the OCR1X pins. +*/ +//RC_PPM_PORT_OUT_REG &= (~(1<: + +/*22222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222222*/ +/*33333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333*/ + +unsigned char detect_connected_channels(void) +{ + 1d6: cf 93 push r28 + 1d8: df 93 push r29 + +/* +There must be no error in which channels are connected to the encoder +because this will have devastating effects later on. +*/ +wdt_reset(); + 1da: a8 95 wdr +RESET_START_TIMER0(); + 1dc: 15 bc out 0x25, r1 ; 37 + 1de: a8 9a sbi 0x15, 0 ; 21 + 1e0: 16 bc out 0x26, r1 ; 38 + 1e2: 10 92 1d 01 sts 0x011D, r1 + 1e6: 82 e0 ldi r24, 0x02 ; 2 + 1e8: 85 bd out 0x25, r24 ; 37 + 1ea: f0 91 19 01 lds r31, 0x0119 + 1ee: a0 e0 ldi r26, 0x00 ; 0 + 1f0: 20 e0 ldi r18, 0x00 ; 0 + 1f2: 30 e0 ldi r19, 0x00 ; 0 + + for(channel=0; channel < RC_SERVO_INPUT_CHANNELS; channel++) + 1f4: c1 e0 ldi r28, 0x01 ; 1 + 1f6: d0 e0 ldi r29, 0x00 ; 0 + 1f8: 29 c0 rjmp .+82 ; 0x24c + { + servo_connected = 0; + for(y=0; y<5; y++) + { + wdt_reset(); + 1fa: a8 95 wdr + RESET_TIMER0(); + 1fc: a8 9a sbi 0x15, 0 ; 21 + 1fe: 16 bc out 0x26, r1 ; 38 + 200: 10 92 1d 01 sts 0x011D, r1 + 204: e0 e0 ldi r30, 0x00 ; 0 + 206: 11 c0 rjmp .+34 ; 0x22a + x=0; + while(timer0.timer0[1] <= RC_MAX_TIMEOUT_VAL ) + { + if(RC_SERVO_PORT_PIN_REG & (1< + 214: e0 91 1d 01 lds r30, 0x011D + if( (timer0.timer0[1] - x) >= RC_PULSE_TIMEOUT_VAL ){ servo_connected++; break; } + 218: 80 91 1d 01 lds r24, 0x011D + 21c: 90 e0 ldi r25, 0x00 ; 0 + 21e: 8e 1b sub r24, r30 + 220: 91 09 sbc r25, r1 + 222: 09 97 sbiw r24, 0x09 ; 9 + 224: 10 f0 brcs .+4 ; 0x22a + 226: 5f 5f subi r21, 0xFF ; 255 + 228: 04 c0 rjmp .+8 ; 0x232 + for(y=0; y<5; y++) + { + wdt_reset(); + RESET_TIMER0(); + x=0; + while(timer0.timer0[1] <= RC_MAX_TIMEOUT_VAL ) + 22a: 80 91 1d 01 lds r24, 0x011D + 22e: 87 37 cpi r24, 0x77 ; 119 + 230: 58 f3 brcs .-42 ; 0x208 + { + if(RC_SERVO_PORT_PIN_REG & (1<= RC_PULSE_TIMEOUT_VAL ){ servo_connected++; break; } + } + if(servo_connected >= 3){ channel_mask |= (1< + 236: f6 2b or r31, r22 + 238: af 5f subi r26, 0xFF ; 255 + 23a: 03 c0 rjmp .+6 ; 0x242 +RESET_START_TIMER0(); + + for(channel=0; channel < RC_SERVO_INPUT_CHANNELS; channel++) + { + servo_connected = 0; + for(y=0; y<5; y++) + 23c: 4f 5f subi r20, 0xFF ; 255 + 23e: 45 30 cpi r20, 0x05 ; 5 + 240: e1 f6 brne .-72 ; 0x1fa + 242: 2f 5f subi r18, 0xFF ; 255 + 244: 3f 4f sbci r19, 0xFF ; 255 +because this will have devastating effects later on. +*/ +wdt_reset(); +RESET_START_TIMER0(); + + for(channel=0; channel < RC_SERVO_INPUT_CHANNELS; channel++) + 246: 28 30 cpi r18, 0x08 ; 8 + 248: 31 05 cpc r19, r1 + 24a: 51 f0 breq .+20 ; 0x260 + 24c: be 01 movw r22, r28 + 24e: 02 2e mov r0, r18 + 250: 02 c0 rjmp .+4 ; 0x256 + 252: 66 0f add r22, r22 + 254: 77 1f adc r23, r23 + 256: 0a 94 dec r0 + 258: e2 f7 brpl .-8 ; 0x252 + 25a: 50 e0 ldi r21, 0x00 ; 0 + 25c: 40 e0 ldi r20, 0x00 ; 0 + 25e: cd cf rjmp .-102 ; 0x1fa + 260: f0 93 19 01 sts 0x0119, r31 + } + } +#endif + +return(connected_channels); +} + 264: 8a 2f mov r24, r26 + 266: df 91 pop r29 + 268: cf 91 pop r28 + 26a: 08 95 ret + +0000026c : +{ + +unsigned int pw = 0; +unsigned char pw_measurement_started = 0; + +wdt_reset(); + 26c: a8 95 wdr +/* The servo input pins are already configured as inputs with pullup resistors. */ +// First we must disable the pin interrupt. +RC_PIN_INT_EN_REG &= (~(1< + 27e: 22 0f add r18, r18 + 280: 33 1f adc r19, r19 + 282: 8a 95 dec r24 + 284: e2 f7 brpl .-8 ; 0x27e + 286: 20 93 6d 00 sts 0x006D, r18 +//Clear any pin interrupt flag set. +RC_PIN_INT_FLAG_REG |= (1<= RC_MAX_TIMEOUT_VAL ) + 2ae: 80 91 1d 01 lds r24, 0x011D + 2b2: 86 37 cpi r24, 0x76 ; 118 + 2b4: 18 f0 brcs .+6 ; 0x2bc + 2b6: 20 e0 ldi r18, 0x00 ; 0 + 2b8: 30 e0 ldi r19, 0x00 ; 0 + 2ba: 1f c0 rjmp .+62 ; 0x2fa + { + return(0); + } + + }while( pin_interrupt_detected == 0 ); + 2bc: 80 91 14 01 lds r24, 0x0114 + 2c0: 88 23 and r24, r24 + 2c2: a9 f3 breq .-22 ; 0x2ae + pin_interrupt_detected = 0; + 2c4: 10 92 14 01 sts 0x0114, r1 + if( RC_SERVO_PORT_PIN_REG & (1< + { + pw = isr_timer0_16; + 2d4: 40 91 16 01 lds r20, 0x0116 + 2d8: 50 91 17 01 lds r21, 0x0117 + 2dc: 61 e0 ldi r22, 0x01 ; 1 + 2de: e7 cf rjmp .-50 ; 0x2ae + pw_measurement_started = 1; /* signal that this channel got it's timer stamp.*/ + + }else{ + // If the pin is low and it already has a time stamp then we are done. + if( pw_measurement_started ) + 2e0: 66 23 and r22, r22 + 2e2: 29 f3 breq .-54 ; 0x2ae + { + pw = isr_timer0_16 - pw; + 2e4: 20 91 16 01 lds r18, 0x0116 + 2e8: 30 91 17 01 lds r19, 0x0117 + 2ec: 24 1b sub r18, r20 + 2ee: 35 0b sbc r19, r21 + } + } + } + +/*Stop the timer */ +STOP_TIMER0(); + 2f0: 15 bc out 0x25, r1 ; 37 +RESET_TIMER0(); + 2f2: a8 9a sbi 0x15, 0 ; 21 + 2f4: 16 bc out 0x26, r1 ; 38 + 2f6: 10 92 1d 01 sts 0x011D, r1 + +return((unsigned int)pw); +} + 2fa: c9 01 movw r24, r18 + 2fc: 08 95 ret + +000002fe : +} + +#else + +void wait_for_rx(void) +{ + 2fe: 0f 93 push r16 + 300: 1f 93 push r17 +unsigned int pw=0; +unsigned char x = 0; +unsigned char servo_connected = 0; +unsigned char channel = 0; + +RESET_START_TIMER0(); + 302: 15 bc out 0x25, r1 ; 37 + 304: a8 9a sbi 0x15, 0 ; 21 + 306: 16 bc out 0x26, r1 ; 38 + 308: 10 92 1d 01 sts 0x011D, r1 + 30c: 82 e0 ldi r24, 0x02 ; 2 + 30e: 85 bd out 0x25, r24 ; 37 + 310: 00 e0 ldi r16, 0x00 ; 0 +do{ + for(channel=0; channel < RC_SERVO_INPUT_CHANNELS; channel++) + { + wdt_reset(); + RESET_TIMER0(); + 312: 61 e0 ldi r22, 0x01 ; 1 + 314: 70 e0 ldi r23, 0x00 ; 0 + 316: 24 c0 rjmp .+72 ; 0x360 + +RESET_START_TIMER0(); +do{ + for(channel=0; channel < RC_SERVO_INPUT_CHANNELS; channel++) + { + wdt_reset(); + 318: a8 95 wdr + RESET_TIMER0(); + 31a: a8 9a sbi 0x15, 0 ; 21 + 31c: 16 bc out 0x26, r1 ; 38 + 31e: 10 92 1d 01 sts 0x011D, r1 + 322: 9b 01 movw r18, r22 + 324: 00 2e mov r0, r16 + 326: 02 c0 rjmp .+4 ; 0x32c + 328: 22 0f add r18, r18 + 32a: 33 1f adc r19, r19 + 32c: 0a 94 dec r0 + 32e: e2 f7 brpl .-8 ; 0x328 + 330: 40 e0 ldi r20, 0x00 ; 0 + 332: 0f c0 rjmp .+30 ; 0x352 + servo_connected = 0; + x=0; + while(timer0.timer0[1] <= RC_MAX_TIMEOUT_VAL ) + { + if(RC_SERVO_PORT_PIN_REG & (1< + 340: 40 91 1d 01 lds r20, 0x011D + if( (timer0.timer0[1] - x) >= RC_PULSE_TIMEOUT_VAL ){ servo_connected = 1; break; } + 344: 80 91 1d 01 lds r24, 0x011D + 348: 90 e0 ldi r25, 0x00 ; 0 + 34a: 84 1b sub r24, r20 + 34c: 91 09 sbc r25, r1 + 34e: 09 97 sbiw r24, 0x09 ; 9 + 350: 50 f4 brcc .+20 ; 0x366 + { + wdt_reset(); + RESET_TIMER0(); + servo_connected = 0; + x=0; + while(timer0.timer0[1] <= RC_MAX_TIMEOUT_VAL ) + 352: 80 91 1d 01 lds r24, 0x011D + 356: 87 37 cpi r24, 0x77 ; 119 + 358: 68 f3 brcs .-38 ; 0x334 + 35a: 12 c0 rjmp .+36 ; 0x380 + 35c: 00 e0 ldi r16, 0x00 ; 0 + 35e: dc cf rjmp .-72 ; 0x318 +unsigned char servo_connected = 0; +unsigned char channel = 0; + +RESET_START_TIMER0(); +do{ + for(channel=0; channel < RC_SERVO_INPUT_CHANNELS; channel++) + 360: 08 30 cpi r16, 0x08 ; 8 + 362: d0 f2 brcs .-76 ; 0x318 + 364: fb cf rjmp .-10 ; 0x35c + 366: 10 e0 ldi r17, 0x00 ; 0 + +//Now test the found channel for a proper servo pulse. +x = 0; +while(1) + { + pw=get_channel_pw(channel); + 368: 80 2f mov r24, r16 + 36a: 0e 94 36 01 call 0x26c ; 0x26c + if( (pw > RC_SERVO_MIN_PW_VAL) && (pw < RC_SERVO_MAX_PW_VAL) ) { x++; }else{ x=0; } + 36e: 81 52 subi r24, 0x21 ; 33 + 370: 93 40 sbci r25, 0x03 ; 3 + 372: 87 57 subi r24, 0x77 ; 119 + 374: 95 40 sbci r25, 0x05 ; 5 + 376: b8 f7 brcc .-18 ; 0x366 + 378: 1f 5f subi r17, 0xFF ; 255 + if(x >= 3) { break; } + 37a: 13 30 cpi r17, 0x03 ; 3 + 37c: a8 f3 brcs .-22 ; 0x368 + 37e: 02 c0 rjmp .+4 ; 0x384 +unsigned char servo_connected = 0; +unsigned char channel = 0; + +RESET_START_TIMER0(); +do{ + for(channel=0; channel < RC_SERVO_INPUT_CHANNELS; channel++) + 380: 0f 5f subi r16, 0xFF ; 255 + 382: ee cf rjmp .-36 ; 0x360 + if( (pw > RC_SERVO_MIN_PW_VAL) && (pw < RC_SERVO_MAX_PW_VAL) ) { x++; }else{ x=0; } + if(x >= 3) { break; } + } + +return; +} + 384: 1f 91 pop r17 + 386: 0f 91 pop r16 + 388: 08 95 ret + +0000038a : +/*77777777777777777777777777777777777777777777777777777777777777777777777777777777777777777777777777*/ +/*88888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888*/ + +void load_failsafe_values(void) +{ +wdt_reset(); + 38a: a8 95 wdr +isr_channel_pw[0] = RC_FS_CH_1_TIMER_VAL; + 38c: 8c ed ldi r24, 0xDC ; 220 + 38e: 95 e0 ldi r25, 0x05 ; 5 + 390: 90 93 1f 01 sts 0x011F, r25 + 394: 80 93 1e 01 sts 0x011E, r24 +#if RC_PPM_GEN_CHANNELS >= 2 +isr_channel_pw[1] = RC_FS_CH_2_TIMER_VAL; + 398: 90 93 21 01 sts 0x0121, r25 + 39c: 80 93 20 01 sts 0x0120, r24 +#endif +#if RC_PPM_GEN_CHANNELS >= 3 +isr_channel_pw[2] = RC_FS_CH_3_TIMER_VAL; + 3a0: 28 ee ldi r18, 0xE8 ; 232 + 3a2: 33 e0 ldi r19, 0x03 ; 3 + 3a4: 30 93 23 01 sts 0x0123, r19 + 3a8: 20 93 22 01 sts 0x0122, r18 +#endif +#if RC_PPM_GEN_CHANNELS >= 4 +isr_channel_pw[3] = RC_FS_CH_4_TIMER_VAL; + 3ac: 90 93 25 01 sts 0x0125, r25 + 3b0: 80 93 24 01 sts 0x0124, r24 +#endif +#if RC_PPM_GEN_CHANNELS >= 5 +isr_channel_pw[4] = RC_FS_CH_5_TIMER_VAL; + 3b4: 30 93 27 01 sts 0x0127, r19 + 3b8: 20 93 26 01 sts 0x0126, r18 +#endif +#if RC_PPM_GEN_CHANNELS >= 6 +isr_channel_pw[5] = RC_FS_CH_6_TIMER_VAL; + 3bc: 30 93 29 01 sts 0x0129, r19 + 3c0: 20 93 28 01 sts 0x0128, r18 +#endif +#if RC_PPM_GEN_CHANNELS >= 7 +isr_channel_pw[6] = RC_FS_CH_7_TIMER_VAL; + 3c4: 30 93 2b 01 sts 0x012B, r19 + 3c8: 20 93 2a 01 sts 0x012A, r18 +#endif +#if RC_PPM_GEN_CHANNELS >= 8 +isr_channel_pw[7] = RC_FS_CH_8_TIMER_VAL; + 3cc: 30 93 2d 01 sts 0x012D, r19 + 3d0: 20 93 2c 01 sts 0x012C, r18 +#endif +#if RC_PPM_GEN_CHANNELS >= 9 +isr_channel_pw[8] = RC_FS_CH_8_TIMER_VAL; +#endif +isr_channel_pw[RC_PPM_GEN_CHANNELS] = RC_RESET_PW_TIMER_VAL; + 3d4: 8c ee ldi r24, 0xEC ; 236 + 3d6: 9c e2 ldi r25, 0x2C ; 44 + 3d8: 90 93 2f 01 sts 0x012F, r25 + 3dc: 80 93 2e 01 sts 0x012E, r24 + + +return; +} + 3e0: 08 95 ret + +000003e2 : +/*12121212121212121212121212121212121212121212121212121212121212121212121212121212121212121212121212*/ + + +void mux_control(void) +{ +long PULSE_WIDTH = isr_channel_pw[RC_MUX_CHANNEL-1]; + 3e2: 80 91 2c 01 lds r24, 0x012C + 3e6: 90 91 2d 01 lds r25, 0x012D + +#if RC_MUX_REVERSE == 0 + + if((PULSE_WIDTH>RC_MUX_MIN)&&(PULSE_WIDTH + { + MUX_ON(); + 3fe: 29 9a sbi 0x05, 1 ; 5 + 400: 08 95 ret + } + else + { + MUX_OFF(); + 402: 29 98 cbi 0x05, 1 ; 5 + 404: 08 95 ret + +00000406 <__vector_16>: +/* INTERRUPT SERVICE ROUTINES */ +/********************************************************************************************************/ + +//ISR(TIMER0_OVF_vect, ISR_NAKED) +ISR(TIMER0_OVF_vect) +{ + 406: 1f 92 push r1 + 408: 0f 92 push r0 + 40a: 0f b6 in r0, 0x3f ; 63 + 40c: 0f 92 push r0 + 40e: 11 24 eor r1, r1 + 410: 8f 93 push r24 +timer0.timer0[1]++; + 412: 80 91 1d 01 lds r24, 0x011D + 416: 8f 5f subi r24, 0xFF ; 255 + 418: 80 93 1d 01 sts 0x011D, r24 + +return; +} + 41c: 8f 91 pop r24 + 41e: 0f 90 pop r0 + 420: 0f be out 0x3f, r0 ; 63 + 422: 0f 90 pop r0 + 424: 1f 90 pop r1 + 426: 18 95 reti + +00000428 <__vector_12>: +modifying their buffers and not the actual registers. +Both actual registers are updated when the timer reaches the OCR1A (TOP) value automatically. +This way the OCR1B interrupt can be delayed as needed without any loss of timing accuracy. +*/ +ISR(TIMER1_COMPB_vect) +{ + 428: 1f 92 push r1 + 42a: 0f 92 push r0 + 42c: 0f b6 in r0, 0x3f ; 63 + 42e: 0f 92 push r0 + 430: 11 24 eor r1, r1 + 432: 8f 93 push r24 + 434: 9f 93 push r25 + 436: ef 93 push r30 + 438: ff 93 push r31 +asm("sei"); + 43a: 78 94 sei + } + +#endif +#if RC_CONSTANT_PPM_FRAME_TIME == 0 + +isr_channel_number++; + 43c: 80 91 15 01 lds r24, 0x0115 + 440: 8f 5f subi r24, 0xFF ; 255 + 442: 80 93 15 01 sts 0x0115, r24 +if( isr_channel_number >= (RC_PPM_GEN_CHANNELS + 1) ) {isr_channel_number = 0; } + 446: 80 91 15 01 lds r24, 0x0115 + 44a: 89 30 cpi r24, 0x09 ; 9 + 44c: 10 f0 brcs .+4 ; 0x452 <__vector_12+0x2a> + 44e: 10 92 15 01 sts 0x0115, r1 + RC_TIMER1_COMP1_REG = isr_channel_pw[isr_channel_number]; + 452: e0 91 15 01 lds r30, 0x0115 + 456: f0 e0 ldi r31, 0x00 ; 0 + 458: ee 0f add r30, r30 + 45a: ff 1f adc r31, r31 + 45c: e2 5e subi r30, 0xE2 ; 226 + 45e: fe 4f sbci r31, 0xFE ; 254 + 460: 80 81 ld r24, Z + 462: 91 81 ldd r25, Z+1 ; 0x01 + 464: 90 93 89 00 sts 0x0089, r25 + 468: 80 93 88 00 sts 0x0088, r24 + +#endif + +return; +} + 46c: ff 91 pop r31 + 46e: ef 91 pop r30 + 470: 9f 91 pop r25 + 472: 8f 91 pop r24 + 474: 0f 90 pop r0 + 476: 0f be out 0x3f, r0 ; 63 + 478: 0f 90 pop r0 + 47a: 1f 90 pop r1 + 47c: 18 95 reti + +0000047e <__vector_5>: + +/********************************************************************************************************/ + +ISR(PCINT2_vect) +{ + 47e: 1f 92 push r1 + 480: 0f 92 push r0 + 482: 0f b6 in r0, 0x3f ; 63 + 484: 0f 92 push r0 + 486: 11 24 eor r1, r1 + 488: 8f 93 push r24 + 48a: 9f 93 push r25 + +timer0.timer0[0]= TCNT0; + 48c: 86 b5 in r24, 0x26 ; 38 + 48e: 80 93 1c 01 sts 0x011C, r24 +if( RC_TIMER0_TIFR & (1< + 496: a8 9a sbi 0x15, 0 ; 21 + 498: 80 91 1d 01 lds r24, 0x011D + 49c: 8f 5f subi r24, 0xFF ; 255 + 49e: 80 93 1d 01 sts 0x011D, r24 + 4a2: 10 92 1c 01 sts 0x011C, r1 +isr_timer0_16 = timer0.timer0_16; + 4a6: 80 91 1c 01 lds r24, 0x011C + 4aa: 90 91 1d 01 lds r25, 0x011D + 4ae: 90 93 17 01 sts 0x0117, r25 + 4b2: 80 93 16 01 sts 0x0116, r24 +pin_interrupt_detected = 1; + 4b6: 81 e0 ldi r24, 0x01 ; 1 + 4b8: 80 93 14 01 sts 0x0114, r24 + + +return; +} + 4bc: 9f 91 pop r25 + 4be: 8f 91 pop r24 + 4c0: 0f 90 pop r0 + 4c2: 0f be out 0x3f, r0 ; 63 + 4c4: 0f 90 pop r0 + 4c6: 1f 90 pop r1 + 4c8: 18 95 reti + +000004ca : + +/*33333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333333*/ +/*44444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444444*/ + +void write_default_values_to_eeprom(void) +{ + 4ca: cf 93 push r28 + 4cc: df 93 push r29 + 4ce: c4 e1 ldi r28, 0x14 ; 20 + 4d0: d0 e0 ldi r29, 0x00 ; 0 + Write a word \a __value to EEPROM address \a __p. + */ +static __inline__ void eeprom_write_word (uint16_t *__p, uint16_t __value) +{ +#if (! (defined(__AVR_ATmega2560__) || defined(__AVR_ATmega2561__)) ) + __eewr_word (__p, __value, eeprom_write_byte); + 4d2: ce 01 movw r24, r28 + 4d4: 69 ee ldi r22, 0xE9 ; 233 + 4d6: 77 e0 ldi r23, 0x07 ; 7 + 4d8: 4b e5 ldi r20, 0x5B ; 91 + 4da: 50 e0 ldi r21, 0x00 ; 0 + 4dc: 0e 94 e4 05 call 0xbc8 ; 0xbc8 <__eewr_word> + 4e0: 22 96 adiw r28, 0x02 ; 2 + +unsigned char x = 0; + +for(x=0; x<(sizeof(ppm_off_threshold_e)/sizeof(int)); x++) + 4e2: 30 e0 ldi r19, 0x00 ; 0 + 4e4: ca 32 cpi r28, 0x2A ; 42 + 4e6: d3 07 cpc r29, r19 + 4e8: a1 f7 brne .-24 ; 0x4d2 + 4ea: 8a e2 ldi r24, 0x2A ; 42 + 4ec: 90 e0 ldi r25, 0x00 ; 0 +#if E2END <= 0xFF + EEARL = (size_t)__p; +#else + EEAR = (size_t)__p; +#endif + EEDR = __value; + 4ee: 22 e0 ldi r18, 0x02 ; 2 +/** \ingroup avr_eeprom + Write a byte \a __value to EEPROM address \a __p. + */ +static __inline__ void eeprom_write_byte (uint8_t *__p, uint8_t __value) +{ + do {} while (!eeprom_is_ready ()); + 4f0: f9 99 sbic 0x1f, 1 ; 31 + 4f2: fe cf rjmp .-4 ; 0x4f0 + +#if defined(EEPM0) && defined(EEPM1) + EECR = 0; /* Set programming mode: erase and write. */ + 4f4: 1f ba out 0x1f, r1 ; 31 +#endif + +#if E2END <= 0xFF + EEARL = (size_t)__p; +#else + EEAR = (size_t)__p; + 4f6: 92 bd out 0x22, r25 ; 34 + 4f8: 81 bd out 0x21, r24 ; 33 +#endif + EEDR = __value; + 4fa: 20 bd out 0x20, r18 ; 32 + + __asm__ __volatile__ ( + 4fc: 0f b6 in r0, 0x3f ; 63 + 4fe: f8 94 cli + 500: fa 9a sbi 0x1f, 2 ; 31 + 502: f9 9a sbi 0x1f, 1 ; 31 + 504: 0f be out 0x3f, r0 ; 63 + 506: 01 96 adiw r24, 0x01 ; 1 + { + eeprom_write_word(&ppm_off_threshold_e[x], RC_PPM_OFF_THRESHOLD_VAL); + } +for(x=0; x < (sizeof(rc_lost_channel_e)/sizeof(char)); x++) + 508: 30 e0 ldi r19, 0x00 ; 0 + 50a: 85 33 cpi r24, 0x35 ; 53 + 50c: 93 07 cpc r25, r19 + 50e: 81 f7 brne .-32 ; 0x4f0 + { + eeprom_write_byte(&rc_lost_channel_e[x], (RC_LOST_CHANNEL - 1)); + } +rc_lost_channel = (RC_LOST_CHANNEL - 1); + 510: 82 e0 ldi r24, 0x02 ; 2 + 512: 80 93 12 01 sts 0x0112, r24 +ppm_off_threshold = RC_PPM_OFF_THRESHOLD_VAL; + 516: 89 ee ldi r24, 0xE9 ; 233 + 518: 97 e0 ldi r25, 0x07 ; 7 + 51a: 90 93 11 01 sts 0x0111, r25 + 51e: 80 93 10 01 sts 0x0110, r24 + +return; +} + 522: df 91 pop r29 + 524: cf 91 pop r28 + 526: 08 95 ret + +00000528 : + +/*66666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666666*/ +/*77777777777777777777777777777777777777777777777777777777777777777777777777777777777777777777777777*/ + +void check_for_setup_mode(void) +{ + 528: 0f 93 push r16 + 52a: 1f 93 push r17 + 52c: cf 93 push r28 + 52e: df 93 push r29 +unsigned char x = 0; +unsigned char y = 0; + + +//We have to make sure that the setup mode is requested. +wdt_reset(); + 530: a8 95 wdr +x=0; +y=0; +setup_mode = 1; +RESET_START_TIMER0(); + 532: 15 bc out 0x25, r1 ; 37 + 534: a8 9a sbi 0x15, 0 ; 21 + 536: 16 bc out 0x26, r1 ; 38 + 538: 10 92 1d 01 sts 0x011D, r1 + 53c: 82 e0 ldi r24, 0x02 ; 2 + 53e: 85 bd out 0x25, r24 ; 37 + 540: 90 e0 ldi r25, 0x00 ; 0 +do{ + if( (RC_SETUP_PIN_REG & (1< + 546: 90 e0 ldi r25, 0x00 ; 0 + 548: 01 c0 rjmp .+2 ; 0x54c + 54a: 9f 5f subi r25, 0xFF ; 255 + { + if(timer0.timer0[1] > RC_MAX_TIMEOUT_VAL ){setup_mode = 0; break; } + 54c: 80 91 1d 01 lds r24, 0x011D + 550: 87 37 cpi r24, 0x77 ; 119 + 552: 08 f0 brcs .+2 ; 0x556 + 554: e7 c0 rjmp .+462 ; 0x724 + } + + }while(x < 100); + 556: 94 36 cpi r25, 0x64 ; 100 + 558: a0 f3 brcs .-24 ; 0x542 + 55a: d9 c0 rjmp .+434 ; 0x70e +/****************************************************************************************************/ + wdt_reset(); + success = 0; + if(channels_in_use > 1 ) + { + if( channel_mask & (1<<(RC_LOST_CHANNEL - 1)) ) + 55c: 80 91 19 01 lds r24, 0x0119 + 560: 82 ff sbrs r24, 2 + 562: 8e c0 rjmp .+284 ; 0x680 + { + rc_lost_channel = (RC_LOST_CHANNEL - 1); + 564: 82 e0 ldi r24, 0x02 ; 2 + 566: 80 93 12 01 sts 0x0112, r24 + 56a: 8a e2 ldi r24, 0x2A ; 42 + 56c: 90 e0 ldi r25, 0x00 ; 0 +#if E2END <= 0xFF + EEARL = (size_t)__p; +#else + EEAR = (size_t)__p; +#endif + EEDR = __value; + 56e: 22 e0 ldi r18, 0x02 ; 2 +/** \ingroup avr_eeprom + Write a byte \a __value to EEPROM address \a __p. + */ +static __inline__ void eeprom_write_byte (uint8_t *__p, uint8_t __value) +{ + do {} while (!eeprom_is_ready ()); + 570: f9 99 sbic 0x1f, 1 ; 31 + 572: fe cf rjmp .-4 ; 0x570 + +#if defined(EEPM0) && defined(EEPM1) + EECR = 0; /* Set programming mode: erase and write. */ + 574: 1f ba out 0x1f, r1 ; 31 +#endif + +#if E2END <= 0xFF + EEARL = (size_t)__p; +#else + EEAR = (size_t)__p; + 576: 92 bd out 0x22, r25 ; 34 + 578: 81 bd out 0x21, r24 ; 33 +#endif + EEDR = __value; + 57a: 20 bd out 0x20, r18 ; 32 + + __asm__ __volatile__ ( + 57c: 0f b6 in r0, 0x3f ; 63 + 57e: f8 94 cli + 580: fa 9a sbi 0x1f, 2 ; 31 + 582: f9 9a sbi 0x1f, 1 ; 31 + 584: 0f be out 0x3f, r0 ; 63 + 586: 01 96 adiw r24, 0x01 ; 1 + for(x=0; x < (sizeof(rc_lost_channel_e)/sizeof(char)); x++) + 588: 30 e0 ldi r19, 0x00 ; 0 + 58a: 85 33 cpi r24, 0x35 ; 53 + 58c: 93 07 cpc r25, r19 + 58e: 81 f7 brne .-32 ; 0x570 + 590: 2e c0 rjmp .+92 ; 0x5ee + eeprom_write_byte(&rc_lost_channel_e[x], rc_lost_channel); + } + success += 1; + } + + }else if(channels_in_use == 1) + 592: 81 30 cpi r24, 0x01 ; 1 + 594: 09 f0 breq .+2 ; 0x598 + 596: 74 c0 rjmp .+232 ; 0x680 + { + for(x=0; x < RC_SERVO_INPUT_CHANNELS; x++) + { + if(channel_mask & (1< + 5ac: 95 95 asr r25 + 5ae: 87 95 ror r24 + 5b0: 0a 94 dec r0 + 5b2: e2 f7 brpl .-8 ; 0x5ac + 5b4: 80 ff sbrs r24, 0 + 5b6: 15 c0 rjmp .+42 ; 0x5e2 + { + rc_lost_channel = x; + 5b8: 20 93 12 01 sts 0x0112, r18 + 5bc: 8a e2 ldi r24, 0x2A ; 42 + 5be: 90 e0 ldi r25, 0x00 ; 0 +/** \ingroup avr_eeprom + Write a byte \a __value to EEPROM address \a __p. + */ +static __inline__ void eeprom_write_byte (uint8_t *__p, uint8_t __value) +{ + do {} while (!eeprom_is_ready ()); + 5c0: f9 99 sbic 0x1f, 1 ; 31 + 5c2: fe cf rjmp .-4 ; 0x5c0 + +#if defined(EEPM0) && defined(EEPM1) + EECR = 0; /* Set programming mode: erase and write. */ + 5c4: 1f ba out 0x1f, r1 ; 31 +#endif + +#if E2END <= 0xFF + EEARL = (size_t)__p; +#else + EEAR = (size_t)__p; + 5c6: 92 bd out 0x22, r25 ; 34 + 5c8: 81 bd out 0x21, r24 ; 33 +#endif + EEDR = __value; + 5ca: 60 bd out 0x20, r22 ; 32 + + __asm__ __volatile__ ( + 5cc: 0f b6 in r0, 0x3f ; 63 + 5ce: f8 94 cli + 5d0: fa 9a sbi 0x1f, 2 ; 31 + 5d2: f9 9a sbi 0x1f, 1 ; 31 + 5d4: 0f be out 0x3f, r0 ; 63 + 5d6: 01 96 adiw r24, 0x01 ; 1 + for(x=0; x < (sizeof(rc_lost_channel_e)/sizeof(char)); x++) + 5d8: 40 e0 ldi r20, 0x00 ; 0 + 5da: 85 33 cpi r24, 0x35 ; 53 + 5dc: 94 07 cpc r25, r20 + 5de: 81 f7 brne .-32 ; 0x5c0 + 5e0: 06 c0 rjmp .+12 ; 0x5ee + 5e2: 2f 5f subi r18, 0xFF ; 255 + 5e4: 3f 4f sbci r19, 0xFF ; 255 + success += 1; + } + + }else if(channels_in_use == 1) + { + for(x=0; x < RC_SERVO_INPUT_CHANNELS; x++) + 5e6: 28 30 cpi r18, 0x08 ; 8 + 5e8: 31 05 cpc r19, r1 + 5ea: e1 f6 brne .-72 ; 0x5a4 + 5ec: 49 c0 rjmp .+146 ; 0x680 +/****************************************************************************************************/ +/* NOW WE NEED TO FIND THE THRESHOLD PULSE WIDTH THAT WILL BE USED AN A TX SIGNAL LOST TRIGGER */ +/****************************************************************************************************/ + if(success == 1) + { + wdt_reset(); + 5ee: a8 95 wdr + 5f0: c0 e0 ldi r28, 0x00 ; 0 + 5f2: d0 e0 ldi r29, 0x00 ; 0 + 5f4: 10 e0 ldi r17, 0x00 ; 0 + 5f6: 00 e0 ldi r16, 0x00 ; 0 + y = 0; + pw_buffer = 0; + for(x=0; x < 10; x++) + { + pw=get_channel_pw(rc_lost_channel); + 5f8: 80 91 12 01 lds r24, 0x0112 + 5fc: 0e 94 36 01 call 0x26c ; 0x26c + 600: 9c 01 movw r18, r24 + if(pw >= RC_SERVO_MIN_PW_VAL && pw <= RC_SERVO_MAX_PW_VAL) + 602: 80 52 subi r24, 0x20 ; 32 + 604: 93 40 sbci r25, 0x03 ; 3 + 606: 89 57 subi r24, 0x79 ; 121 + 608: 95 40 sbci r25, 0x05 ; 5 + 60a: 18 f4 brcc .+6 ; 0x612 + { + pw_buffer += pw; + 60c: c2 0f add r28, r18 + 60e: d3 1f adc r29, r19 + y++; + 610: 0f 5f subi r16, 0xFF ; 255 + if(success == 1) + { + wdt_reset(); + y = 0; + pw_buffer = 0; + for(x=0; x < 10; x++) + 612: 1f 5f subi r17, 0xFF ; 255 + 614: 1a 30 cpi r17, 0x0A ; 10 + 616: 81 f7 brne .-32 ; 0x5f8 + { + pw_buffer += pw; + y++; + } + } + pw_buffer /= y; + 618: ce 01 movw r24, r28 + 61a: 60 2f mov r22, r16 + 61c: 70 e0 ldi r23, 0x00 ; 0 + 61e: 0e 94 f9 05 call 0xbf2 ; 0xbf2 <__udivmodhi4> + wdt_reset(); + 622: a8 95 wdr + if( (pw_buffer >= RC_PPM_OFF_UPPER_WINDOW_VAL) && (pw_buffer <= RC_SERVO_MAX_PW_VAL) ) + 624: cb 01 movw r24, r22 + 626: 84 5a subi r24, 0xA4 ; 164 + 628: 96 40 sbci r25, 0x06 ; 6 + 62a: 85 5f subi r24, 0xF5 ; 245 + 62c: 91 40 sbci r25, 0x01 ; 1 + 62e: 88 f4 brcc .+34 ; 0x652 + 630: c4 e1 ldi r28, 0x14 ; 20 + 632: d0 e0 ldi r29, 0x00 ; 0 + Write a word \a __value to EEPROM address \a __p. + */ +static __inline__ void eeprom_write_word (uint16_t *__p, uint16_t __value) +{ +#if (! (defined(__AVR_ATmega2560__) || defined(__AVR_ATmega2561__)) ) + __eewr_word (__p, __value, eeprom_write_byte); + 634: 8b 01 movw r16, r22 + 636: 07 5e subi r16, 0xE7 ; 231 + 638: 1f 4f sbci r17, 0xFF ; 255 + 63a: ce 01 movw r24, r28 + 63c: b8 01 movw r22, r16 + 63e: 4b e5 ldi r20, 0x5B ; 91 + 640: 50 e0 ldi r21, 0x00 ; 0 + 642: 0e 94 e4 05 call 0xbc8 ; 0xbc8 <__eewr_word> + 646: 22 96 adiw r28, 0x02 ; 2 + { + for(x=0; x<(sizeof(ppm_off_threshold_e)/sizeof(int)); x++) + 648: 80 e0 ldi r24, 0x00 ; 0 + 64a: ca 32 cpi r28, 0x2A ; 42 + 64c: d8 07 cpc r29, r24 + 64e: a9 f7 brne .-22 ; 0x63a + 650: 65 c0 rjmp .+202 ; 0x71c + { + eeprom_write_word(&ppm_off_threshold_e[x], (pw_buffer+RC_PPM_OFF_OFFSET_VAL)); + } + success += 1; + + }else if( (pw_buffer <= RC_PPM_OFF_LOWER_WINDOW_VAL) && (pw_buffer >= RC_SERVO_MIN_PW_VAL) ) + 652: cb 01 movw r24, r22 + 654: 80 52 subi r24, 0x20 ; 32 + 656: 93 40 sbci r25, 0x03 ; 3 + 658: 85 5f subi r24, 0xF5 ; 245 + 65a: 91 40 sbci r25, 0x01 ; 1 + 65c: 88 f4 brcc .+34 ; 0x680 + 65e: c4 e1 ldi r28, 0x14 ; 20 + 660: d0 e0 ldi r29, 0x00 ; 0 + 662: 8b 01 movw r16, r22 + 664: 09 51 subi r16, 0x19 ; 25 + 666: 10 40 sbci r17, 0x00 ; 0 + 668: ce 01 movw r24, r28 + 66a: b8 01 movw r22, r16 + 66c: 4b e5 ldi r20, 0x5B ; 91 + 66e: 50 e0 ldi r21, 0x00 ; 0 + 670: 0e 94 e4 05 call 0xbc8 ; 0xbc8 <__eewr_word> + 674: 22 96 adiw r28, 0x02 ; 2 + { + for(x=0; x<(sizeof(ppm_off_threshold_e)/sizeof(int)); x++) + 676: 80 e0 ldi r24, 0x00 ; 0 + 678: ca 32 cpi r28, 0x2A ; 42 + 67a: d8 07 cpc r29, r24 + 67c: a9 f7 brne .-22 ; 0x668 + 67e: 4e c0 rjmp .+156 ; 0x71c + while(timer0.timer0[1] < RC_MAX_TIMEOUT_VAL ); + } + } + + }else{ + write_default_values_to_eeprom(); + 680: 0e 94 65 02 call 0x4ca ; 0x4ca + { + LED_ON(); + for(x=0; x<30; x++) + { + wdt_reset(); + RESET_START_TIMER0(); + 684: 32 e0 ldi r19, 0x02 ; 2 + 686: 22 e0 ldi r18, 0x02 ; 2 + 688: 21 c0 rjmp .+66 ; 0x6cc + if(success == 2) + { + RC_SETUP_PORT_OUT_REG &= (~(1< + { + RC_SETUP_PORT_OUT_REG &= (~(1< + wdt_reset(); + RESET_START_TIMER0(); + /* delay ~30 ms * 3 = 100 milliseconds */ + while(timer0.timer0[1] < RC_MAX_TIMEOUT_VAL ); + } + LED_OFF(); + 6aa: 28 98 cbi 0x05, 0 ; 5 + 6ac: 90 e0 ldi r25, 0x00 ; 0 + for(x=0; x<30; x++) + { + wdt_reset(); + 6ae: a8 95 wdr + RESET_START_TIMER0(); + 6b0: 15 bc out 0x25, r1 ; 37 + 6b2: a8 9a sbi 0x15, 0 ; 21 + 6b4: 16 bc out 0x26, r1 ; 38 + 6b6: 10 92 1d 01 sts 0x011D, r1 + 6ba: 35 bd out 0x25, r19 ; 37 + /* delay ~30 ms * 30 = 900 milliseconds */ + while(timer0.timer0[1] < RC_MAX_TIMEOUT_VAL ); + 6bc: 80 91 1d 01 lds r24, 0x011D + 6c0: 86 37 cpi r24, 0x76 ; 118 + 6c2: e0 f3 brcs .-8 ; 0x6bc + RESET_START_TIMER0(); + /* delay ~30 ms * 3 = 100 milliseconds */ + while(timer0.timer0[1] < RC_MAX_TIMEOUT_VAL ); + } + LED_OFF(); + for(x=0; x<30; x++) + 6c4: 9f 5f subi r25, 0xFF ; 255 + 6c6: 9e 31 cpi r25, 0x1E ; 30 + 6c8: 91 f7 brne .-28 ; 0x6ae + 6ca: df cf rjmp .-66 ; 0x68a + + }else{ + write_default_values_to_eeprom(); + while(1) + { + LED_ON(); + 6cc: 28 9a sbi 0x05, 0 ; 5 + 6ce: 90 e0 ldi r25, 0x00 ; 0 + for(x=0; x<30; x++) + { + wdt_reset(); + 6d0: a8 95 wdr + RESET_START_TIMER0(); + 6d2: 15 bc out 0x25, r1 ; 37 + 6d4: a8 9a sbi 0x15, 0 ; 21 + 6d6: 16 bc out 0x26, r1 ; 38 + 6d8: 10 92 1d 01 sts 0x011D, r1 + 6dc: 25 bd out 0x25, r18 ; 37 + /* delay ~30 ms * 30 = 900 milliseconds */ + while(timer0.timer0[1] < RC_MAX_TIMEOUT_VAL ); + 6de: 80 91 1d 01 lds r24, 0x011D + 6e2: 86 37 cpi r24, 0x76 ; 118 + 6e4: e0 f3 brcs .-8 ; 0x6de + }else{ + write_default_values_to_eeprom(); + while(1) + { + LED_ON(); + for(x=0; x<30; x++) + 6e6: 9f 5f subi r25, 0xFF ; 255 + 6e8: 9e 31 cpi r25, 0x1E ; 30 + 6ea: 91 f7 brne .-28 ; 0x6d0 + wdt_reset(); + RESET_START_TIMER0(); + /* delay ~30 ms * 30 = 900 milliseconds */ + while(timer0.timer0[1] < RC_MAX_TIMEOUT_VAL ); + } + LED_OFF(); + 6ec: 28 98 cbi 0x05, 0 ; 5 + 6ee: 90 e0 ldi r25, 0x00 ; 0 + for(x=0; x<3; x++) + { + wdt_reset(); + 6f0: a8 95 wdr + RESET_START_TIMER0(); + 6f2: 15 bc out 0x25, r1 ; 37 + 6f4: a8 9a sbi 0x15, 0 ; 21 + 6f6: 16 bc out 0x26, r1 ; 38 + 6f8: 10 92 1d 01 sts 0x011D, r1 + 6fc: 35 bd out 0x25, r19 ; 37 + /* delay ~30 ms * 3 = 100 milliseconds */ + while(timer0.timer0[1] < RC_MAX_TIMEOUT_VAL ); + 6fe: 80 91 1d 01 lds r24, 0x011D + 702: 86 37 cpi r24, 0x76 ; 118 + 704: e0 f3 brcs .-8 ; 0x6fe + RESET_START_TIMER0(); + /* delay ~30 ms * 30 = 900 milliseconds */ + while(timer0.timer0[1] < RC_MAX_TIMEOUT_VAL ); + } + LED_OFF(); + for(x=0; x<3; x++) + 706: 9f 5f subi r25, 0xFF ; 255 + 708: 93 30 cpi r25, 0x03 ; 3 + 70a: 91 f7 brne .-28 ; 0x6f0 + 70c: df cf rjmp .-66 ; 0x6cc +if(setup_mode) + { +/****************************************************************************************************/ +/* FIRST WE MUST FIND WHICH CHANNEL WILL BE USED AS A TX SIGNAL LOST INDICATOR */ +/****************************************************************************************************/ + wdt_reset(); + 70e: a8 95 wdr + success = 0; + if(channels_in_use > 1 ) + 710: 80 91 18 01 lds r24, 0x0118 + 714: 82 30 cpi r24, 0x02 ; 2 + 716: 08 f0 brcs .+2 ; 0x71a + 718: 21 cf rjmp .-446 ; 0x55c + 71a: 3b cf rjmp .-394 ; 0x592 +/****************************************************************************************************/ +/* LASTLY WE MUST INDICATE TO THE USER IF THE SETUP PROCEDURE WAS SUCCESSFUL */ +/****************************************************************************************************/ + if(success == 2) + { + RC_SETUP_PORT_OUT_REG &= (~(1< + } + + } // End of "if(setup_mode)" statement. + +return; +} + 724: df 91 pop r29 + 726: cf 91 pop r28 + 728: 1f 91 pop r17 + 72a: 0f 91 pop r16 + 72c: 08 95 ret + +0000072e : +11 copies of each variable are read and if more than 50% + 1 values are the same and within limits +this value is taken to be valid. +If not all 11 values are the same then the array is written again with this 50% +1 value. +*/ +void load_values_from_eeprom(void) +{ + 72e: 6f 92 push r6 + 730: 7f 92 push r7 + 732: 8f 92 push r8 + 734: 9f 92 push r9 + 736: bf 92 push r11 + 738: cf 92 push r12 + 73a: df 92 push r13 + 73c: ef 92 push r14 + 73e: ff 92 push r15 + 740: 0f 93 push r16 + 742: 1f 93 push r17 + 744: cf 93 push r28 + 746: df 93 push r29 +unsigned char match_upper_limit = 0; +unsigned char match_lower_limit = 0; +unsigned char x = 0; +unsigned char y = 0; + +wdt_reset(); + 748: a8 95 wdr + 74a: 6a e2 ldi r22, 0x2A ; 42 + 74c: 70 e0 ldi r23, 0x00 ; 0 + 74e: ab 01 movw r20, r22 +11 copies of each variable are read and if more than 50% + 1 values are the same and within limits +this value is taken to be valid. +If not all 11 values are the same then the array is written again with this 50% +1 value. +*/ +void load_values_from_eeprom(void) +{ + 750: eb 01 movw r28, r22 + 752: 2b 96 adiw r28, 0x0b ; 11 +/** \ingroup avr_eeprom + Read one byte from EEPROM address \a __p. + */ +__ATTR_PURE__ static __inline__ uint8_t eeprom_read_byte (const uint8_t *__p) +{ + do {} while (!eeprom_is_ready ()); + 754: f9 99 sbic 0x1f, 1 ; 31 + 756: fe cf rjmp .-4 ; 0x754 +#if E2END <= 0xFF + EEARL = (size_t)__p; +#else + EEAR = (size_t)__p; + 758: 52 bd out 0x22, r21 ; 34 + 75a: 41 bd out 0x21, r20 ; 33 + /* Use inline assembly below as some AVRs have problems with accessing + EECR with STS instructions. For example, see errata for ATmega64. + The code below also assumes that EECR and EEDR are in the I/O space. + */ + uint8_t __result; + __asm__ __volatile__ + 75c: f8 9a sbi 0x1f, 0 ; 31 + 75e: f0 b5 in r31, 0x20 ; 32 +match_lower_limit = (((sizeof(rc_lost_channel_e)/sizeof(char))/2)+1); + +for(x=0; x +#if E2END <= 0xFF + EEARL = (size_t)__p; +#else + EEAR = (size_t)__p; + 76e: 32 bd out 0x22, r19 ; 34 + 770: 21 bd out 0x21, r18 ; 33 + /* Use inline assembly below as some AVRs have problems with accessing + EECR with STS instructions. For example, see errata for ATmega64. + The code below also assumes that EECR and EEDR are in the I/O space. + */ + uint8_t __result; + __asm__ __volatile__ + 772: f8 9a sbi 0x1f, 0 ; 31 + 774: 80 b5 in r24, 0x20 ; 32 + for(y=0; y + 77e: ef 5f subi r30, 0xFF ; 255 + 780: 2f 5f subi r18, 0xFF ; 255 + 782: 3f 4f sbci r19, 0xFF ; 255 +11 copies of each variable are read and if more than 50% + 1 values are the same and within limits +this value is taken to be valid. +If not all 11 values are the same then the array is written again with this 50% +1 value. +*/ +void load_values_from_eeprom(void) +{ + 784: 85 e3 ldi r24, 0x35 ; 53 + 786: 90 e0 ldi r25, 0x00 ; 0 + +for(x=0; x + eeprom_buf_y = eeprom_read_byte(&rc_lost_channel_e[y]); + if(eeprom_buf_y == eeprom_buf_x){ match++; } + } + // If 50% +1 or more char values in the array are the same a match has been found. + // Now test them to see if they are within limits. + if(match >= match_lower_limit ) + 78e: e6 30 cpi r30, 0x06 ; 6 + 790: c0 f0 brcs .+48 ; 0x7c2 + { + if( eeprom_buf_x < RC_SERVO_INPUT_CHANNELS ) + 792: 18 97 sbiw r26, 0x08 ; 8 + 794: 08 f0 brcs .+2 ; 0x798 + 796: 7b c0 rjmp .+246 ; 0x88e + { + rc_lost_channel = eeprom_buf_x; //Load the stored value to throttle_thershold. + 798: f0 93 12 01 sts 0x0112, r31 + if(match < match_upper_limit) + 79c: eb 30 cpi r30, 0x0B ; 11 + 79e: c0 f4 brcc .+48 ; 0x7d0 +/** \ingroup avr_eeprom + Write a byte \a __value to EEPROM address \a __p. + */ +static __inline__ void eeprom_write_byte (uint8_t *__p, uint8_t __value) +{ + do {} while (!eeprom_is_ready ()); + 7a0: f9 99 sbic 0x1f, 1 ; 31 + 7a2: fe cf rjmp .-4 ; 0x7a0 + +#if defined(EEPM0) && defined(EEPM1) + EECR = 0; /* Set programming mode: erase and write. */ + 7a4: 1f ba out 0x1f, r1 ; 31 +#endif + +#if E2END <= 0xFF + EEARL = (size_t)__p; +#else + EEAR = (size_t)__p; + 7a6: 72 bd out 0x22, r23 ; 34 + 7a8: 61 bd out 0x21, r22 ; 33 +#endif + EEDR = __value; + 7aa: f0 bd out 0x20, r31 ; 32 + + __asm__ __volatile__ ( + 7ac: 0f b6 in r0, 0x3f ; 63 + 7ae: f8 94 cli + 7b0: fa 9a sbi 0x1f, 2 ; 31 + 7b2: f9 9a sbi 0x1f, 1 ; 31 + 7b4: 0f be out 0x3f, r0 ; 63 + 7b6: 6f 5f subi r22, 0xFF ; 255 + 7b8: 7f 4f sbci r23, 0xFF ; 255 + { + for(x=0; x + 7c0: 07 c0 rjmp .+14 ; 0x7d0 + 7c2: 4f 5f subi r20, 0xFF ; 255 + 7c4: 5f 4f sbci r21, 0xFF ; 255 +/* READ WHICH CHANNEL WILL BE USED AS A RX LOST INDICATOR */ +/****************************************************************************************************/ +match_upper_limit = (sizeof(rc_lost_channel_e)/sizeof(char)); +match_lower_limit = (((sizeof(rc_lost_channel_e)/sizeof(char))/2)+1); + +for(x=0; x + 7cc: c3 cf rjmp .-122 ; 0x754 + 7ce: 5f c0 rjmp .+190 ; 0x88e + }else{ match = 0; } + break; + } + } + +if( match < match_lower_limit ){ write_default_values_to_eeprom(); return; } + 7d0: 04 e1 ldi r16, 0x14 ; 20 + 7d2: 10 e0 ldi r17, 0x00 ; 0 + 7d4: 68 01 movw r12, r16 +11 copies of each variable are read and if more than 50% + 1 values are the same and within limits +this value is taken to be valid. +If not all 11 values are the same then the array is written again with this 50% +1 value. +*/ +void load_values_from_eeprom(void) +{ + 7d6: 26 e1 ldi r18, 0x16 ; 22 + 7d8: 82 2e mov r8, r18 + 7da: 91 2c mov r9, r1 + 7dc: 80 0e add r8, r16 + 7de: 91 1e adc r9, r17 + Read one 16-bit word (little endian) from EEPROM address \a __p. + */ +__ATTR_PURE__ static __inline__ uint16_t eeprom_read_word (const uint16_t *__p) +{ +#if (! (defined(__AVR_ATmega2560__) || defined(__AVR_ATmega2561__)) ) + return __eerd_word (__p, eeprom_read_byte); + 7e0: c6 01 movw r24, r12 + 7e2: 63 e5 ldi r22, 0x53 ; 83 + 7e4: 70 e0 ldi r23, 0x00 ; 0 + 7e6: 0e 94 ce 05 call 0xb9c ; 0xb9c <__eerd_word> + 7ea: 7c 01 movw r14, r24 + 7ec: c4 e1 ldi r28, 0x14 ; 20 + 7ee: d0 e0 ldi r29, 0x00 ; 0 + 7f0: bb 24 eor r11, r11 + match = 0; + eeprom_buf_x = eeprom_read_word(&ppm_off_threshold_e[x]); + for(y=0; y < match_upper_limit; y++) + { + eeprom_buf_y = eeprom_read_word(&ppm_off_threshold_e[y]); + if(eeprom_buf_y == eeprom_buf_x){ match++; } + 7f2: ce 01 movw r24, r28 + 7f4: 63 e5 ldi r22, 0x53 ; 83 + 7f6: 70 e0 ldi r23, 0x00 ; 0 + 7f8: 0e 94 ce 05 call 0xb9c ; 0xb9c <__eerd_word> + 7fc: 8e 15 cp r24, r14 + 7fe: 9f 05 cpc r25, r15 + 800: 09 f4 brne .+2 ; 0x804 + 802: b3 94 inc r11 + 804: 22 96 adiw r28, 0x02 ; 2 +11 copies of each variable are read and if more than 50% + 1 values are the same and within limits +this value is taken to be valid. +If not all 11 values are the same then the array is written again with this 50% +1 value. +*/ +void load_values_from_eeprom(void) +{ + 806: 9a e2 ldi r25, 0x2A ; 42 + 808: 69 2e mov r6, r25 + 80a: 90 e0 ldi r25, 0x00 ; 0 + 80c: 79 2e mov r7, r25 + +for(x=0; x < match_upper_limit; x++) + { + match = 0; + eeprom_buf_x = eeprom_read_word(&ppm_off_threshold_e[x]); + for(y=0; y < match_upper_limit; y++) + 80e: 8c 16 cp r8, r28 + 810: 9d 06 cpc r9, r29 + 812: 79 f7 brne .-34 ; 0x7f2 + eeprom_buf_y = eeprom_read_word(&ppm_off_threshold_e[y]); + if(eeprom_buf_y == eeprom_buf_x){ match++; } + } + // If 50% +1 or more integer values in the array are the same a match has been found. + // Now test them to see if they are within limits. + if(match >= match_lower_limit ) + 814: 25 e0 ldi r18, 0x05 ; 5 + 816: 2b 15 cp r18, r11 + 818: 90 f5 brcc .+100 ; 0x87e + { + if( (eeprom_buf_x >= RC_PPM_OFF_UPPER_WINDOW_VAL) && (eeprom_buf_x <= RC_SERVO_MAX_PW_VAL) ) + 81a: c7 01 movw r24, r14 + 81c: 84 5a subi r24, 0xA4 ; 164 + 81e: 96 40 sbci r25, 0x06 ; 6 + 820: 85 5f subi r24, 0xF5 ; 245 + 822: 91 40 sbci r25, 0x01 ; 1 + 824: 98 f4 brcc .+38 ; 0x84c + { + ppm_off_threshold = eeprom_buf_x; //Load the stored value to throttle_thershold. + 826: f0 92 11 01 sts 0x0111, r15 + 82a: e0 92 10 01 sts 0x0110, r14 + if(match < match_upper_limit) + 82e: 8a e0 ldi r24, 0x0A ; 10 + 830: 8b 15 cp r24, r11 + 832: 78 f1 brcs .+94 ; 0x892 + Write a word \a __value to EEPROM address \a __p. + */ +static __inline__ void eeprom_write_word (uint16_t *__p, uint16_t __value) +{ +#if (! (defined(__AVR_ATmega2560__) || defined(__AVR_ATmega2561__)) ) + __eewr_word (__p, __value, eeprom_write_byte); + 834: c8 01 movw r24, r16 + 836: b7 01 movw r22, r14 + 838: 4b e5 ldi r20, 0x5B ; 91 + 83a: 50 e0 ldi r21, 0x00 ; 0 + 83c: 0e 94 e4 05 call 0xbc8 ; 0xbc8 <__eewr_word> + 840: 0e 5f subi r16, 0xFE ; 254 + 842: 1f 4f sbci r17, 0xFF ; 255 + { + for(x=0; x < match_upper_limit; x++){ eeprom_write_word(&ppm_off_threshold_e[x], eeprom_buf_x); } + 844: 60 16 cp r6, r16 + 846: 71 06 cpc r7, r17 + 848: a9 f7 brne .-22 ; 0x834 + 84a: 23 c0 rjmp .+70 ; 0x892 + } + + }else if( (eeprom_buf_x <= RC_PPM_OFF_LOWER_WINDOW_VAL) && (eeprom_buf_x >= RC_SERVO_MIN_PW_VAL) ) + 84c: c7 01 movw r24, r14 + 84e: 80 52 subi r24, 0x20 ; 32 + 850: 93 40 sbci r25, 0x03 ; 3 + 852: 85 5f subi r24, 0xF5 ; 245 + 854: 91 40 sbci r25, 0x01 ; 1 + 856: d8 f4 brcc .+54 ; 0x88e + { + ppm_off_threshold = eeprom_buf_x; //Load the stored value to throttle_thershold. + 858: f0 92 11 01 sts 0x0111, r15 + 85c: e0 92 10 01 sts 0x0110, r14 + if(match < match_upper_limit) + 860: 8a e0 ldi r24, 0x0A ; 10 + 862: 8b 15 cp r24, r11 + 864: b0 f0 brcs .+44 ; 0x892 + 866: c8 01 movw r24, r16 + 868: b7 01 movw r22, r14 + 86a: 4b e5 ldi r20, 0x5B ; 91 + 86c: 50 e0 ldi r21, 0x00 ; 0 + 86e: 0e 94 e4 05 call 0xbc8 ; 0xbc8 <__eewr_word> + 872: 0e 5f subi r16, 0xFE ; 254 + 874: 1f 4f sbci r17, 0xFF ; 255 + { + for(x=0; x < match_upper_limit; x++){ eeprom_write_word(&ppm_off_threshold_e[x], eeprom_buf_x); } + 876: 60 16 cp r6, r16 + 878: 71 06 cpc r7, r17 + 87a: a9 f7 brne .-22 ; 0x866 + 87c: 0a c0 rjmp .+20 ; 0x892 + 87e: 82 e0 ldi r24, 0x02 ; 2 + 880: 90 e0 ldi r25, 0x00 ; 0 + 882: c8 0e add r12, r24 + 884: d9 1e adc r13, r25 +/* NOW READ THE CHANNEL'S PULSE WIDTH SO IT CAN BE USED AS A TRIGGER */ +/****************************************************************************************************/ +match_upper_limit = (sizeof(ppm_off_threshold_e)/sizeof(int)); +match_lower_limit = (((sizeof(ppm_off_threshold_e)/sizeof(int))/2)+1); + +for(x=0; x < match_upper_limit; x++) + 886: 8c 14 cp r8, r12 + 888: 9d 04 cpc r9, r13 + 88a: 09 f0 breq .+2 ; 0x88e + 88c: a9 cf rjmp .-174 ; 0x7e0 + }else{ match = 0; } + break; + } + } + +if( match < match_lower_limit ){ write_default_values_to_eeprom(); return; } + 88e: 0e 94 65 02 call 0x4ca ; 0x4ca + + +return; +} + 892: df 91 pop r29 + 894: cf 91 pop r28 + 896: 1f 91 pop r17 + 898: 0f 91 pop r16 + 89a: ff 90 pop r15 + 89c: ef 90 pop r14 + 89e: df 90 pop r13 + 8a0: cf 90 pop r12 + 8a2: bf 90 pop r11 + 8a4: 9f 90 pop r9 + 8a6: 8f 90 pop r8 + 8a8: 7f 90 pop r7 + 8aa: 6f 90 pop r6 + 8ac: 08 95 ret + +000008ae
: + +/********************************************************************************************************/ +/* MAIN FUNCTION */ +/********************************************************************************************************/ +__attribute__((noreturn)) void main(void) +{ + 8ae: df 93 push r29 + 8b0: cf 93 push r28 + 8b2: cd b7 in r28, 0x3d ; 61 + 8b4: de b7 in r29, 0x3e ; 62 + 8b6: 60 97 sbiw r28, 0x10 ; 16 + 8b8: 0f b6 in r0, 0x3f ; 63 + 8ba: f8 94 cli + 8bc: de bf out 0x3e, r29 ; 62 + 8be: 0f be out 0x3f, r0 ; 63 + 8c0: cd bf out 0x3d, r28 ; 61 +unsigned char tx_signal_detected = 0; +unsigned char servo_signals_lost = 0; +unsigned char led_frequency = 0; +unsigned char led_counter = 0; + +wdt_disable(); + 8c2: 88 e1 ldi r24, 0x18 ; 24 + 8c4: 0f b6 in r0, 0x3f ; 63 + 8c6: f8 94 cli + 8c8: 80 93 60 00 sts 0x0060, r24 + 8cc: 10 92 60 00 sts 0x0060, r1 + 8d0: 0f be out 0x3f, r0 ; 63 +wdt_enable(WDTO_120MS); + 8d2: 2b e0 ldi r18, 0x0B ; 11 + 8d4: 88 e1 ldi r24, 0x18 ; 24 + 8d6: 90 e0 ldi r25, 0x00 ; 0 + 8d8: 0f b6 in r0, 0x3f ; 63 + 8da: f8 94 cli + 8dc: a8 95 wdr + 8de: 80 93 60 00 sts 0x0060, r24 + 8e2: 0f be out 0x3f, r0 ; 63 + 8e4: 20 93 60 00 sts 0x0060, r18 +wdt_reset(); + 8e8: a8 95 wdr + +initialize_mcu(); + 8ea: 0e 94 68 00 call 0xd0 ; 0xd0 +/*Load the values stored in the eeprom like the throttle channel threshold etc. */ +load_values_from_eeprom(); + 8ee: 0e 94 97 03 call 0x72e ; 0x72e +/*77777777777777777777777777777777777777777777777777777777777777777777777777777777777777777777777777*/ +/*88888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888*/ + +void load_failsafe_values(void) +{ +wdt_reset(); + 8f2: a8 95 wdr +isr_channel_pw[0] = RC_FS_CH_1_TIMER_VAL; + 8f4: 2c ed ldi r18, 0xDC ; 220 + 8f6: 35 e0 ldi r19, 0x05 ; 5 + 8f8: 30 93 1f 01 sts 0x011F, r19 + 8fc: 20 93 1e 01 sts 0x011E, r18 +#if RC_PPM_GEN_CHANNELS >= 2 +isr_channel_pw[1] = RC_FS_CH_2_TIMER_VAL; + 900: 30 93 21 01 sts 0x0121, r19 + 904: 20 93 20 01 sts 0x0120, r18 +#endif +#if RC_PPM_GEN_CHANNELS >= 3 +isr_channel_pw[2] = RC_FS_CH_3_TIMER_VAL; + 908: 88 ee ldi r24, 0xE8 ; 232 + 90a: 93 e0 ldi r25, 0x03 ; 3 + 90c: 90 93 23 01 sts 0x0123, r25 + 910: 80 93 22 01 sts 0x0122, r24 +#endif +#if RC_PPM_GEN_CHANNELS >= 4 +isr_channel_pw[3] = RC_FS_CH_4_TIMER_VAL; + 914: 30 93 25 01 sts 0x0125, r19 + 918: 20 93 24 01 sts 0x0124, r18 +#endif +#if RC_PPM_GEN_CHANNELS >= 5 +isr_channel_pw[4] = RC_FS_CH_5_TIMER_VAL; + 91c: 90 93 27 01 sts 0x0127, r25 + 920: 80 93 26 01 sts 0x0126, r24 +#endif +#if RC_PPM_GEN_CHANNELS >= 6 +isr_channel_pw[5] = RC_FS_CH_6_TIMER_VAL; + 924: 90 93 29 01 sts 0x0129, r25 + 928: 80 93 28 01 sts 0x0128, r24 +#endif +#if RC_PPM_GEN_CHANNELS >= 7 +isr_channel_pw[6] = RC_FS_CH_7_TIMER_VAL; + 92c: 90 93 2b 01 sts 0x012B, r25 + 930: 80 93 2a 01 sts 0x012A, r24 +#endif +#if RC_PPM_GEN_CHANNELS >= 8 +isr_channel_pw[7] = RC_FS_CH_8_TIMER_VAL; + 934: 90 93 2d 01 sts 0x012D, r25 + 938: 80 93 2c 01 sts 0x012C, r24 +#endif +#if RC_PPM_GEN_CHANNELS >= 9 +isr_channel_pw[8] = RC_FS_CH_8_TIMER_VAL; +#endif +isr_channel_pw[RC_PPM_GEN_CHANNELS] = RC_RESET_PW_TIMER_VAL; + 93c: 8c ee ldi r24, 0xEC ; 236 + 93e: 9c e2 ldi r25, 0x2C ; 44 + 940: 90 93 2f 01 sts 0x012F, r25 + 944: 80 93 2e 01 sts 0x012E, r24 +load_failsafe_values(); +/* +The "wait_for_rx(): function waits untill the receiver has been powered up and running +so we can then detect the connected channels with certainty. +*/ +wait_for_rx(); + 948: 0e 94 7f 01 call 0x2fe ; 0x2fe +channels_in_use = detect_connected_channels(); + 94c: 0e 94 eb 00 call 0x1d6 ; 0x1d6 + 950: 80 93 18 01 sts 0x0118, r24 +check_for_setup_mode(); + 954: 0e 94 94 02 call 0x528 ; 0x528 +led_frequency = RC_LED_FREQUENCY_VAL_1HZ; //load the defined led frequency. + +/**************************** SETUP THE PIN INTERRUPT *************************************************/ + +// Now we must disable the pin interrupt. +RC_PIN_INT_EN_REG &= (~(1< RC_SERVO_MIN_PW_VAL) && (timer0_buffer < RC_SERVO_MAX_PW_VAL) ) + { +#if defined(RC_LOST_CHANNEL) && RC_LOST_CHANNEL > 0 + if(x == rc_lost_channel) + 998: 20 90 12 01 lds r2, 0x0112 + { + if(ppm_off_threshold > RC_SERVO_CENTER_PW_VAL) + 99c: e0 90 10 01 lds r14, 0x0110 + 9a0: f0 90 11 01 lds r15, 0x0111 + 9a4: 66 24 eor r6, r6 + 9a6: cc 24 eor r12, r12 + 9a8: bb 24 eor r11, r11 + 9aa: dd 24 eor r13, r13 + 9ac: d3 94 inc r13 + 9ae: 40 e1 ldi r20, 0x10 ; 16 + 9b0: a4 2e mov r10, r20 + 9b2: 00 e0 ldi r16, 0x00 ; 0 +/*88888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888*/ + +void load_failsafe_values(void) +{ +wdt_reset(); +isr_channel_pw[0] = RC_FS_CH_1_TIMER_VAL; + 9b4: 3c ed ldi r19, 0xDC ; 220 + 9b6: 43 2e mov r4, r19 + 9b8: 35 e0 ldi r19, 0x05 ; 5 + 9ba: 53 2e mov r5, r19 +#if RC_PPM_GEN_CHANNELS >= 2 +isr_channel_pw[1] = RC_FS_CH_2_TIMER_VAL; +#endif +#if RC_PPM_GEN_CHANNELS >= 3 +isr_channel_pw[2] = RC_FS_CH_3_TIMER_VAL; + 9bc: 98 ee ldi r25, 0xE8 ; 232 + 9be: 89 2e mov r8, r25 + 9c0: 93 e0 ldi r25, 0x03 ; 3 + 9c2: 99 2e mov r9, r25 +pin_reg_buffer1 = (RC_SERVO_PORT_PIN_REG & channel_mask); +RESET_START_TIMER0(); +//Main endless loop. +while(1) + { + wdt_reset(); + 9c4: a8 95 wdr + channel_mask_buffer = channel_mask; + RESET_TIMER0(); + 9c6: a8 9a sbi 0x15, 0 ; 21 + 9c8: 16 bc out 0x26, r1 ; 38 + 9ca: 10 92 1d 01 sts 0x011D, r1 + 9ce: 17 2d mov r17, r7 + 9d0: 5b c0 rjmp .+182 ; 0xa88 <__stack+0x189> + while(channel_mask_buffer) + { + /* Wait until a pin change state. */ + do{ + if( timer0.timer0[1] >= RC_MAX_TIMEOUT_VAL ) + 9d2: 80 91 1d 01 lds r24, 0x011D + 9d6: 86 37 cpi r24, 0x76 ; 118 + 9d8: 08 f0 brcs .+2 ; 0x9dc <__stack+0xdd> + 9da: 5c c0 rjmp .+184 ; 0xa94 <__stack+0x195> + { + goto PPM_CONTROL; + } + + }while( pin_interrupt_detected == 0 ); + 9dc: 80 91 14 01 lds r24, 0x0114 + 9e0: 88 23 and r24, r24 + 9e2: b9 f3 breq .-18 ; 0x9d2 <__stack+0xd3> + x=0; + y=1; + //Only pins that changed their state will be tested for a high or low level + pin_reg_buffer0 = (RC_SERVO_PORT_PIN_REG & channel_mask_buffer); + 9e4: 79 b1 in r23, 0x09 ; 9 + 9e6: 71 23 and r23, r17 + pin_interrupt_detected = 0; + 9e8: 10 92 14 01 sts 0x0114, r1 + channels_to_check = pin_reg_buffer1 ^ pin_reg_buffer0; + 9ec: 37 2e mov r3, r23 + 9ee: 32 26 eor r3, r18 + 9f0: de 01 movw r26, r28 + 9f2: 11 96 adiw r26, 0x01 ; 1 + 9f4: 61 e0 ldi r22, 0x01 ; 1 + 9f6: 40 e0 ldi r20, 0x00 ; 0 + 9f8: 50 e0 ldi r21, 0x00 ; 0 + pin_reg_buffer1 = pin_reg_buffer0; + while(x + a00: 3a c0 rjmp .+116 ; 0xa76 <__stack+0x177> + { + if( (pin_reg_buffer0 & y) ) /* if the pin is high then... */ + a02: 86 2f mov r24, r22 + a04: 87 23 and r24, r23 + a06: 49 f0 breq .+18 ; 0xa1a <__stack+0x11b> + { + pw_of_channel[x] = isr_timer0_16; + a08: 80 91 16 01 lds r24, 0x0116 + a0c: 90 91 17 01 lds r25, 0x0117 + a10: 11 96 adiw r26, 0x01 ; 1 + a12: 9c 93 st X, r25 + a14: 8e 93 st -X, r24 + channel_status |= y; /* signal that this channel got it's timer stamp. */ + a16: 66 2a or r6, r22 + a18: 2e c0 rjmp .+92 ; 0xa76 <__stack+0x177> + + }else{ + if( channel_status & y ) + a1a: 86 2d mov r24, r6 + a1c: 86 23 and r24, r22 + a1e: 59 f1 breq .+86 ; 0xa76 <__stack+0x177> + { + channel_mask_buffer &= (~y); + a20: 86 2f mov r24, r22 + a22: 80 95 com r24 + a24: 18 23 and r17, r24 + timer0_buffer = isr_timer0_16 - pw_of_channel[x]; + a26: 20 91 16 01 lds r18, 0x0116 + a2a: 30 91 17 01 lds r19, 0x0117 + a2e: 8d 91 ld r24, X+ + a30: 9c 91 ld r25, X + a32: 11 97 sbiw r26, 0x01 ; 1 + a34: 28 1b sub r18, r24 + a36: 39 0b sbc r19, r25 + if( (timer0_buffer > RC_SERVO_MIN_PW_VAL) && (timer0_buffer < RC_SERVO_MAX_PW_VAL) ) + a38: c9 01 movw r24, r18 + a3a: 81 52 subi r24, 0x21 ; 33 + a3c: 93 40 sbci r25, 0x03 ; 3 + a3e: 87 57 subi r24, 0x77 ; 119 + a40: 95 40 sbci r25, 0x05 ; 5 + a42: c8 f4 brcc .+50 ; 0xa76 <__stack+0x177> + { +#if defined(RC_LOST_CHANNEL) && RC_LOST_CHANNEL > 0 + if(x == rc_lost_channel) + a44: 24 16 cp r2, r20 + a46: 61 f4 brne .+24 ; 0xa60 <__stack+0x161> + { + if(ppm_off_threshold > RC_SERVO_CENTER_PW_VAL) + a48: 8d ed ldi r24, 0xDD ; 221 + a4a: e8 16 cp r14, r24 + a4c: 85 e0 ldi r24, 0x05 ; 5 + a4e: f8 06 cpc r15, r24 + a50: 20 f0 brcs .+8 ; 0xa5a <__stack+0x15b> + { + if(timer0_buffer >= ppm_off_threshold) + a52: 2e 15 cp r18, r14 + a54: 3f 05 cpc r19, r15 + a56: 20 f0 brcs .+8 ; 0xa60 <__stack+0x161> + a58: 1b c0 rjmp .+54 ; 0xa90 <__stack+0x191> + channel_mask_buffer = 0xFF; + goto PPM_CONTROL; + } + + }else{ + if(timer0_buffer <= ppm_off_threshold) + a5a: e2 16 cp r14, r18 + a5c: f3 06 cpc r15, r19 + a5e: c0 f4 brcc .+48 ; 0xa90 <__stack+0x191> + goto PPM_CONTROL; + } + } + } +#endif + if(servo_signals_lost == 0) + a60: dd 20 and r13, r13 + a62: 49 f4 brne .+18 ; 0xa76 <__stack+0x177> + { + asm("cli"); //Atomic operation needed here. + a64: f8 94 cli + isr_channel_pw[x] = timer0_buffer; + a66: fa 01 movw r30, r20 + a68: ee 0f add r30, r30 + a6a: ff 1f adc r31, r31 + a6c: e2 5e subi r30, 0xE2 ; 226 + a6e: fe 4f sbci r31, 0xFE ; 254 + a70: 31 83 std Z+1, r19 ; 0x01 + a72: 20 83 st Z, r18 + asm("sei"); + a74: 78 94 sei + a76: 4f 5f subi r20, 0xFF ; 255 + a78: 5f 4f sbci r21, 0xFF ; 255 + a7a: 12 96 adiw r26, 0x02 ; 2 + a7c: 27 2f mov r18, r23 + //Only pins that changed their state will be tested for a high or low level + pin_reg_buffer0 = (RC_SERVO_PORT_PIN_REG & channel_mask_buffer); + pin_interrupt_detected = 0; + channels_to_check = pin_reg_buffer1 ^ pin_reg_buffer0; + pin_reg_buffer1 = pin_reg_buffer0; + while(x + } // End of "if( channel_status & y )" statement. + + } // End of "if( (pin_reg_buffer0 & y) )...else..." statement + } // End of "if(channels_to_check & y)" statement. + x++; + y=(y<<1); + a84: 66 0f add r22, r22 + a86: b9 cf rjmp .-142 ; 0x9fa <__stack+0xfb> +while(1) + { + wdt_reset(); + channel_mask_buffer = channel_mask; + RESET_TIMER0(); + while(channel_mask_buffer) + a88: 11 23 and r17, r17 + a8a: 09 f0 breq .+2 ; 0xa8e <__stack+0x18f> + a8c: a2 cf rjmp .-188 ; 0x9d2 <__stack+0xd3> + a8e: 02 c0 rjmp .+4 ; 0xa94 <__stack+0x195> + x++; + y=(y<<1); + } + + } // End of "while(channel_mask_buffer)" loop. +PPM_CONTROL: + a90: 27 2f mov r18, r23 + a92: 1f ef ldi r17, 0xFF ; 255 + + led_counter++; + a94: 0f 5f subi r16, 0xFF ; 255 + if( led_counter >= led_frequency ){ led_counter = 0; TOGGLE_LED(); } + a96: 0a 15 cp r16, r10 + a98: 28 f0 brcs .+10 ; 0xaa4 <__stack+0x1a5> + a9a: 85 b1 in r24, 0x05 ; 5 + a9c: 91 e0 ldi r25, 0x01 ; 1 + a9e: 89 27 eor r24, r25 + aa0: 85 b9 out 0x05, r24 ; 5 + aa2: 00 e0 ldi r16, 0x00 ; 0 + +//We need 'RC_MAX_BAD_PPM_FRAMES" consecutive readings in order to change the PPM generator's status. + if( channel_mask_buffer == 0 ) //IF ALL CHANNELS HAVE BEEN MEASURED... + aa4: 11 23 and r17, r17 + aa6: 79 f5 brne .+94 ; 0xb06 <__stack+0x207> + { + tx_signal_lost = 0; + if(servo_signals_lost == 1) //IF PREVIOUSLY THE SERVO SIGNAL WAS LOST... + aa8: e1 e0 ldi r30, 0x01 ; 1 + aaa: de 16 cp r13, r30 + aac: 09 f0 breq .+2 ; 0xab0 <__stack+0x1b1> + aae: 61 c0 rjmp .+194 ; 0xb72 <__stack+0x273> + { + tx_signal_detected++; + ab0: b3 94 inc r11 + if(tx_signal_detected > RC_MAX_BAD_PPM_FRAMES) + ab2: 34 e0 ldi r19, 0x04 ; 4 + ab4: 3b 15 cp r19, r11 + ab6: 08 f0 brcs .+2 ; 0xaba <__stack+0x1bb> + ab8: 5c c0 rjmp .+184 ; 0xb72 <__stack+0x273> + + +static inline void ppm_on(void) +{ + + RC_TIMER1_PRESCALER_REG &= (~(TIMER1_PRESCALER_BITS)); + aba: 80 91 81 00 lds r24, 0x0081 + abe: 8d 7f andi r24, 0xFD ; 253 + ac0: 80 93 81 00 sts 0x0081, r24 + TCNT1 = 0; + ac4: 10 92 85 00 sts 0x0085, r1 + ac8: 10 92 84 00 sts 0x0084, r1 + isr_channel_number = RC_PPM_GEN_CHANNELS; + acc: 88 e0 ldi r24, 0x08 ; 8 + ace: 80 93 15 01 sts 0x0115, r24 + RC_TIMER1_COMP1_REG = RC_RESET_PW_TIMER_VAL; + ad2: 8c ee ldi r24, 0xEC ; 236 + ad4: 9c e2 ldi r25, 0x2C ; 44 + ad6: 90 93 89 00 sts 0x0089, r25 + ada: 80 93 88 00 sts 0x0088, r24 + RC_TIMER1_COMP2_REG = RC_PPM_SYNC_PW_VAL; + ade: 8c e2 ldi r24, 0x2C ; 44 + ae0: 91 e0 ldi r25, 0x01 ; 1 + ae2: 90 93 8b 00 sts 0x008B, r25 + ae6: 80 93 8a 00 sts 0x008A, r24 + RC_TIMER1_TIFR |= ( (1< RC_MAX_BAD_PPM_FRAMES) + { + ppm_on(); + servo_signals_lost = 0; + LED_ON(); + afa: 28 9a sbi 0x05, 0 ; 5 + afc: cc 24 eor r12, r12 + afe: dd 24 eor r13, r13 + b00: 44 e0 ldi r20, 0x04 ; 4 + b02: a4 2e mov r10, r20 + b04: 34 c0 rjmp .+104 ; 0xb6e <__stack+0x26f> + led_frequency = RC_LED_FREQUENCY_VAL; + } + } + } + else{ //IF NOT ALL CHANNELS HAVE BEEN MEASURED... + pin_reg_buffer1 = (RC_SERVO_PORT_PIN_REG & channel_mask); + b06: 29 b1 in r18, 0x09 ; 9 + b08: 27 21 and r18, r7 + tx_signal_detected = 0; + if(servo_signals_lost == 0) + b0a: dd 20 and r13, r13 + b0c: a1 f5 brne .+104 ; 0xb76 <__stack+0x277> + { + tx_signal_lost++; + b0e: c3 94 inc r12 + if(tx_signal_lost > RC_MAX_BAD_PPM_FRAMES) + b10: 94 e0 ldi r25, 0x04 ; 4 + b12: 9c 15 cp r25, r12 + b14: 80 f5 brcc .+96 ; 0xb76 <__stack+0x277> +/*77777777777777777777777777777777777777777777777777777777777777777777777777777777777777777777777777*/ +/*88888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888888*/ + +void load_failsafe_values(void) +{ +wdt_reset(); + b16: a8 95 wdr +isr_channel_pw[0] = RC_FS_CH_1_TIMER_VAL; + b18: 50 92 1f 01 sts 0x011F, r5 + b1c: 40 92 1e 01 sts 0x011E, r4 +#if RC_PPM_GEN_CHANNELS >= 2 +isr_channel_pw[1] = RC_FS_CH_2_TIMER_VAL; + b20: 50 92 21 01 sts 0x0121, r5 + b24: 40 92 20 01 sts 0x0120, r4 +#endif +#if RC_PPM_GEN_CHANNELS >= 3 +isr_channel_pw[2] = RC_FS_CH_3_TIMER_VAL; + b28: 90 92 23 01 sts 0x0123, r9 + b2c: 80 92 22 01 sts 0x0122, r8 +#endif +#if RC_PPM_GEN_CHANNELS >= 4 +isr_channel_pw[3] = RC_FS_CH_4_TIMER_VAL; + b30: 50 92 25 01 sts 0x0125, r5 + b34: 40 92 24 01 sts 0x0124, r4 +#endif +#if RC_PPM_GEN_CHANNELS >= 5 +isr_channel_pw[4] = RC_FS_CH_5_TIMER_VAL; + b38: 90 92 27 01 sts 0x0127, r9 + b3c: 80 92 26 01 sts 0x0126, r8 +#endif +#if RC_PPM_GEN_CHANNELS >= 6 +isr_channel_pw[5] = RC_FS_CH_6_TIMER_VAL; + b40: 90 92 29 01 sts 0x0129, r9 + b44: 80 92 28 01 sts 0x0128, r8 +#endif +#if RC_PPM_GEN_CHANNELS >= 7 +isr_channel_pw[6] = RC_FS_CH_7_TIMER_VAL; + b48: 90 92 2b 01 sts 0x012B, r9 + b4c: 80 92 2a 01 sts 0x012A, r8 +#endif +#if RC_PPM_GEN_CHANNELS >= 8 +isr_channel_pw[7] = RC_FS_CH_8_TIMER_VAL; + b50: 90 92 2d 01 sts 0x012D, r9 + b54: 80 92 2c 01 sts 0x012C, r8 +#endif +#if RC_PPM_GEN_CHANNELS >= 9 +isr_channel_pw[8] = RC_FS_CH_8_TIMER_VAL; +#endif +isr_channel_pw[RC_PPM_GEN_CHANNELS] = RC_RESET_PW_TIMER_VAL; + b58: 8c ee ldi r24, 0xEC ; 236 + b5a: 9c e2 ldi r25, 0x2C ; 44 + b5c: 90 93 2f 01 sts 0x012F, r25 + b60: 80 93 2e 01 sts 0x012E, r24 + b64: bb 24 eor r11, r11 + b66: dd 24 eor r13, r13 + b68: d3 94 inc r13 + b6a: 30 e1 ldi r19, 0x10 ; 16 + b6c: a3 2e mov r10, r19 + b6e: 00 e0 ldi r16, 0x00 ; 0 + b70: 03 c0 rjmp .+6 ; 0xb78 <__stack+0x279> + b72: cc 24 eor r12, r12 + b74: 01 c0 rjmp .+2 ; 0xb78 <__stack+0x279> + b76: bb 24 eor r11, r11 +/*12121212121212121212121212121212121212121212121212121212121212121212121212121212121212121212121212*/ + + +void mux_control(void) +{ +long PULSE_WIDTH = isr_channel_pw[RC_MUX_CHANNEL-1]; + b78: 80 91 2c 01 lds r24, 0x012C + b7c: 90 91 2d 01 lds r25, 0x012D + +#if RC_MUX_REVERSE == 0 + + if((PULSE_WIDTH>RC_MUX_MIN)&&(PULSE_WIDTH + { + MUX_ON(); + b94: 29 9a sbi 0x05, 1 ; 5 + b96: 16 cf rjmp .-468 ; 0x9c4 <__stack+0xc5> + } + else + { + MUX_OFF(); + b98: 29 98 cbi 0x05, 1 ; 5 + b9a: 14 cf rjmp .-472 ; 0x9c4 <__stack+0xc5> + +00000b9c <__eerd_word>: + b9c: df 92 push r13 + b9e: ef 92 push r14 + ba0: ff 92 push r15 + ba2: 0f 93 push r16 + ba4: 1f 93 push r17 + ba6: 7b 01 movw r14, r22 + ba8: 8c 01 movw r16, r24 + baa: fb 01 movw r30, r22 + bac: 09 95 icall + bae: d8 2e mov r13, r24 + bb0: c8 01 movw r24, r16 + bb2: 01 96 adiw r24, 0x01 ; 1 + bb4: f7 01 movw r30, r14 + bb6: 09 95 icall + bb8: 98 2f mov r25, r24 + bba: 8d 2d mov r24, r13 + bbc: 1f 91 pop r17 + bbe: 0f 91 pop r16 + bc0: ff 90 pop r15 + bc2: ef 90 pop r14 + bc4: df 90 pop r13 + bc6: 08 95 ret + +00000bc8 <__eewr_word>: + bc8: df 92 push r13 + bca: ef 92 push r14 + bcc: ff 92 push r15 + bce: 0f 93 push r16 + bd0: 1f 93 push r17 + bd2: d7 2e mov r13, r23 + bd4: 7a 01 movw r14, r20 + bd6: 8c 01 movw r16, r24 + bd8: fa 01 movw r30, r20 + bda: 09 95 icall + bdc: c8 01 movw r24, r16 + bde: 01 96 adiw r24, 0x01 ; 1 + be0: 6d 2d mov r22, r13 + be2: f7 01 movw r30, r14 + be4: 09 95 icall + be6: 1f 91 pop r17 + be8: 0f 91 pop r16 + bea: ff 90 pop r15 + bec: ef 90 pop r14 + bee: df 90 pop r13 + bf0: 08 95 ret + +00000bf2 <__udivmodhi4>: + bf2: aa 1b sub r26, r26 + bf4: bb 1b sub r27, r27 + bf6: 51 e1 ldi r21, 0x11 ; 17 + bf8: 07 c0 rjmp .+14 ; 0xc08 <__udivmodhi4_ep> + +00000bfa <__udivmodhi4_loop>: + bfa: aa 1f adc r26, r26 + bfc: bb 1f adc r27, r27 + bfe: a6 17 cp r26, r22 + c00: b7 07 cpc r27, r23 + c02: 10 f0 brcs .+4 ; 0xc08 <__udivmodhi4_ep> + c04: a6 1b sub r26, r22 + c06: b7 0b sbc r27, r23 + +00000c08 <__udivmodhi4_ep>: + c08: 88 1f adc r24, r24 + c0a: 99 1f adc r25, r25 + c0c: 5a 95 dec r21 + c0e: a9 f7 brne .-22 ; 0xbfa <__udivmodhi4_loop> + c10: 80 95 com r24 + c12: 90 95 com r25 + c14: bc 01 movw r22, r24 + c16: cd 01 movw r24, r26 + c18: 08 95 ret + +00000c1a <_exit>: + c1a: f8 94 cli + +00000c1c <__stop_program>: + c1c: ff cf rjmp .-2 ; 0xc1c <__stop_program> diff --git a/Tools/PPMEncoder/default/ap_ppm_encoder.map b/Tools/PPMEncoder/default/ap_ppm_encoder.map new file mode 100644 index 0000000000..c7c0a8decb --- /dev/null +++ b/Tools/PPMEncoder/default/ap_ppm_encoder.map @@ -0,0 +1,419 @@ +Archive member included because of file (symbol) + +c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_udivmodhi4.o) + ap_ppm_encoder.o (__udivmodhi4) +c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_exit.o) + c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/../../../../avr/lib/avr5/crtm328p.o (exit) +c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_copy_data.o) + ap_ppm_encoder.o (__do_copy_data) +c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_clear_bss.o) + ap_ppm_encoder.o (__do_clear_bss) +c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/../../../../avr/lib/avr5\libc.a(eerd_word.o) + ap_ppm_encoder.o (__eerd_word) +c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/../../../../avr/lib/avr5\libc.a(eewr_word.o) + ap_ppm_encoder.o (__eewr_word) + +Allocating common symbols +Common symbol size file + +timer0 0x2 ap_ppm_encoder.o +isr_channel_pw 0x12 ap_ppm_encoder.o + +Memory Configuration + +Name Origin Length Attributes +text 0x00000000 0x00020000 xr +data 0x00800060 0x0000ffa0 rw !x +eeprom 0x00810000 0x00010000 rw !x +fuse 0x00820000 0x00000400 rw !x +lock 0x00830000 0x00000400 rw !x +signature 0x00840000 0x00000400 rw !x +*default* 0x00000000 0xffffffff + +Linker script and memory map + +Address of section .data set to 0x800100 +LOAD c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/../../../../avr/lib/avr5/crtm328p.o +LOAD ap_ppm_encoder.o +LOAD c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a +LOAD c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/../../../../avr/lib/avr5\libc.a +LOAD c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a + +.hash + *(.hash) + +.dynsym + *(.dynsym) + +.dynstr + *(.dynstr) + +.gnu.version + *(.gnu.version) + +.gnu.version_d + *(.gnu.version_d) + +.gnu.version_r + *(.gnu.version_r) + +.rel.init + *(.rel.init) + +.rela.init + *(.rela.init) + +.rel.text + *(.rel.text) + *(.rel.text.*) + *(.rel.gnu.linkonce.t*) + +.rela.text + *(.rela.text) + *(.rela.text.*) + *(.rela.gnu.linkonce.t*) + +.rel.fini + *(.rel.fini) + +.rela.fini + *(.rela.fini) + +.rel.rodata + *(.rel.rodata) + *(.rel.rodata.*) + *(.rel.gnu.linkonce.r*) + +.rela.rodata + *(.rela.rodata) + *(.rela.rodata.*) + *(.rela.gnu.linkonce.r*) + +.rel.data + *(.rel.data) + *(.rel.data.*) + *(.rel.gnu.linkonce.d*) + +.rela.data + *(.rela.data) + *(.rela.data.*) + *(.rela.gnu.linkonce.d*) + +.rel.ctors + *(.rel.ctors) + +.rela.ctors + *(.rela.ctors) + +.rel.dtors + *(.rel.dtors) + +.rela.dtors + *(.rela.dtors) + +.rel.got + *(.rel.got) + +.rela.got + *(.rela.got) + +.rel.bss + *(.rel.bss) + +.rela.bss + *(.rela.bss) + +.rel.plt + *(.rel.plt) + +.rela.plt + *(.rela.plt) + +.text 0x00000000 0xc1e + *(.vectors) + .vectors 0x00000000 0x68 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/../../../../avr/lib/avr5/crtm328p.o + 0x00000000 __vectors + 0x00000000 __vector_default + *(.vectors) + *(.progmem.gcc*) + *(.progmem*) + 0x00000068 . = ALIGN (0x2) + 0x00000068 __trampolines_start = . + *(.trampolines) + .trampolines 0x00000068 0x0 linker stubs + *(.trampolines*) + 0x00000068 __trampolines_end = . + *(.jumptables) + *(.jumptables*) + *(.lowtext) + *(.lowtext*) + 0x00000068 __ctors_start = . + *(.ctors) + 0x00000068 __ctors_end = . + 0x00000068 __dtors_start = . + *(.dtors) + 0x00000068 __dtors_end = . + SORT(*)(.ctors) + SORT(*)(.dtors) + *(.init0) + .init0 0x00000068 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/../../../../avr/lib/avr5/crtm328p.o + 0x00000068 __init + *(.init0) + *(.init1) + *(.init1) + *(.init2) + .init2 0x00000068 0xc c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/../../../../avr/lib/avr5/crtm328p.o + *(.init2) + *(.init3) + *(.init3) + *(.init4) + .init4 0x00000074 0x16 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_copy_data.o) + 0x00000074 __do_copy_data + .init4 0x0000008a 0x10 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_clear_bss.o) + 0x0000008a __do_clear_bss + *(.init4) + *(.init5) + *(.init5) + *(.init6) + *(.init6) + *(.init7) + *(.init7) + *(.init8) + *(.init8) + *(.init9) + .init9 0x0000009a 0x8 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/../../../../avr/lib/avr5/crtm328p.o + *(.init9) + *(.text) + .text 0x000000a2 0x4 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/../../../../avr/lib/avr5/crtm328p.o + 0x000000a2 __vector_22 + 0x000000a2 __vector_1 + 0x000000a2 __vector_24 + 0x000000a2 __bad_interrupt + 0x000000a2 __vector_6 + 0x000000a2 __vector_3 + 0x000000a2 __vector_23 + 0x000000a2 __vector_25 + 0x000000a2 __vector_11 + 0x000000a2 __vector_13 + 0x000000a2 __vector_17 + 0x000000a2 __vector_19 + 0x000000a2 __vector_7 + 0x000000a2 __vector_4 + 0x000000a2 __vector_9 + 0x000000a2 __vector_2 + 0x000000a2 __vector_21 + 0x000000a2 __vector_15 + 0x000000a2 __vector_8 + 0x000000a2 __vector_14 + 0x000000a2 __vector_10 + 0x000000a2 __vector_18 + 0x000000a2 __vector_20 + .text 0x000000a6 0xaf6 ap_ppm_encoder.o + 0x0000038a load_failsafe_values + 0x00000428 __vector_12 + 0x00000528 check_for_setup_mode + 0x0000047e __vector_5 + 0x0000026c get_channel_pw + 0x000004ca write_default_values_to_eeprom + 0x000008ae main + 0x000001d6 detect_connected_channels + 0x000000d0 initialize_mcu + 0x0000072e load_values_from_eeprom + 0x000002fe wait_for_rx + 0x000003e2 mux_control + 0x00000406 __vector_16 + .text 0x00000b9c 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_udivmodhi4.o) + .text 0x00000b9c 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_exit.o) + .text 0x00000b9c 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_copy_data.o) + .text 0x00000b9c 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_clear_bss.o) + .text 0x00000b9c 0x2c c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/../../../../avr/lib/avr5\libc.a(eerd_word.o) + 0x00000b9c __eerd_word + .text 0x00000bc8 0x2a c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/../../../../avr/lib/avr5\libc.a(eewr_word.o) + 0x00000bc8 __eewr_word + 0x00000bf2 . = ALIGN (0x2) + *(.text.*) + .text.libgcc 0x00000bf2 0x28 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_udivmodhi4.o) + 0x00000bf2 __udivmodhi4 + .text.libgcc 0x00000c1a 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_exit.o) + .text.libgcc 0x00000c1a 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_copy_data.o) + .text.libgcc 0x00000c1a 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_clear_bss.o) + 0x00000c1a . = ALIGN (0x2) + *(.fini9) + .fini9 0x00000c1a 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_exit.o) + 0x00000c1a exit + 0x00000c1a _exit + *(.fini9) + *(.fini8) + *(.fini8) + *(.fini7) + *(.fini7) + *(.fini6) + *(.fini6) + *(.fini5) + *(.fini5) + *(.fini4) + *(.fini4) + *(.fini3) + *(.fini3) + *(.fini2) + *(.fini2) + *(.fini1) + *(.fini1) + *(.fini0) + .fini0 0x00000c1a 0x4 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_exit.o) + *(.fini0) + 0x00000c1e _etext = . + +.data 0x00800100 0x14 load address 0x00000c1e + 0x00800100 PROVIDE (__data_start, .) + *(.data) + .data 0x00800100 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/../../../../avr/lib/avr5/crtm328p.o + .data 0x00800100 0x13 ap_ppm_encoder.o + 0x00800112 rc_lost_channel + 0x00800100 version_info + 0x00800110 ppm_off_threshold + .data 0x00800113 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_udivmodhi4.o) + .data 0x00800113 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_exit.o) + .data 0x00800113 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_copy_data.o) + .data 0x00800113 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_clear_bss.o) + .data 0x00800113 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/../../../../avr/lib/avr5\libc.a(eerd_word.o) + .data 0x00800113 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/../../../../avr/lib/avr5\libc.a(eewr_word.o) + *(.data*) + *(.rodata) + *(.rodata*) + *(.gnu.linkonce.d*) + 0x00800114 . = ALIGN (0x2) + *fill* 0x00800113 0x1 00 + 0x00800114 _edata = . + 0x00800114 PROVIDE (__data_end, .) + +.bss 0x00800114 0x1c + 0x00800114 PROVIDE (__bss_start, .) + *(.bss) + .bss 0x00800114 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/../../../../avr/lib/avr5/crtm328p.o + .bss 0x00800114 0x8 ap_ppm_encoder.o + 0x00800119 channel_mask + 0x0080011a isr_timer0 + 0x00800115 isr_channel_number + 0x00800118 channels_in_use + 0x00800114 pin_interrupt_detected + 0x00800116 isr_timer0_16 + .bss 0x0080011c 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_udivmodhi4.o) + .bss 0x0080011c 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_exit.o) + .bss 0x0080011c 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_copy_data.o) + .bss 0x0080011c 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/avr5\libgcc.a(_clear_bss.o) + .bss 0x0080011c 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/../../../../avr/lib/avr5\libc.a(eerd_word.o) + .bss 0x0080011c 0x0 c:/winavr-20090313/bin/../lib/gcc/avr/4.3.2/../../../../avr/lib/avr5\libc.a(eewr_word.o) + *(.bss*) + *(COMMON) + COMMON 0x0080011c 0x14 ap_ppm_encoder.o + 0x0080011c timer0 + 0x0080011e isr_channel_pw + 0x00800130 PROVIDE (__bss_end, .) + 0x00000c1e __data_load_start = LOADADDR (.data) + 0x00000c32 __data_load_end = (__data_load_start + SIZEOF (.data)) + +.noinit 0x00800130 0x0 + 0x00800130 PROVIDE (__noinit_start, .) + *(.noinit*) + 0x00800130 PROVIDE (__noinit_end, .) + 0x00800130 _end = . + 0x00800130 PROVIDE (__heap_start, .) + +.eeprom 0x00810000 0x35 + *(.eeprom*) + .eeprom 0x00810000 0x35 ap_ppm_encoder.o + 0x00810014 ppm_off_threshold_e + 0x00810000 dummy_int + 0x0081002a rc_lost_channel_e + 0x00810035 __eeprom_end = . + +.fuse + *(.fuse) + *(.lfuse) + *(.hfuse) + *(.efuse) + +.lock + *(.lock*) + +.signature + *(.signature*) + +.stab + *(.stab) + +.stabstr + *(.stabstr) + +.stab.excl + *(.stab.excl) + +.stab.exclstr + *(.stab.exclstr) + +.stab.index + *(.stab.index) + +.stab.indexstr + *(.stab.indexstr) + +.comment + *(.comment) + +.debug + *(.debug) + +.line + *(.line) + +.debug_srcinfo + *(.debug_srcinfo) + +.debug_sfnames + *(.debug_sfnames) + +.debug_aranges 0x00000000 0x20 + *(.debug_aranges) + .debug_aranges + 0x00000000 0x20 ap_ppm_encoder.o + +.debug_pubnames + 0x00000000 0x22c + *(.debug_pubnames) + .debug_pubnames + 0x00000000 0x22c ap_ppm_encoder.o + +.debug_info 0x00000000 0x914 + *(.debug_info) + .debug_info 0x00000000 0x914 ap_ppm_encoder.o + *(.gnu.linkonce.wi.*) + +.debug_abbrev 0x00000000 0x262 + *(.debug_abbrev) + .debug_abbrev 0x00000000 0x262 ap_ppm_encoder.o + +.debug_line 0x00000000 0xc94 + *(.debug_line) + .debug_line 0x00000000 0xc94 ap_ppm_encoder.o + +.debug_frame 0x00000000 0x100 + *(.debug_frame) + .debug_frame 0x00000000 0x100 ap_ppm_encoder.o + +.debug_str 0x00000000 0x43c + *(.debug_str) + .debug_str 0x00000000 0x43c ap_ppm_encoder.o + 0x4ac (size before relaxing) + +.debug_loc 0x00000000 0x3b4 + *(.debug_loc) + .debug_loc 0x00000000 0x3b4 ap_ppm_encoder.o + +.debug_macinfo + *(.debug_macinfo) +OUTPUT(ap_ppm_encoder.elf elf32-avr) +LOAD linker stubs + +.debug_ranges 0x00000000 0x18 + .debug_ranges 0x00000000 0x18 ap_ppm_encoder.o diff --git a/Tools/PPMEncoder/default/dep/ap_ppm_encoder.o.d b/Tools/PPMEncoder/default/dep/ap_ppm_encoder.o.d new file mode 100644 index 0000000000..e48e02fc8e --- /dev/null +++ b/Tools/PPMEncoder/default/dep/ap_ppm_encoder.o.d @@ -0,0 +1,46 @@ +ap_ppm_encoder.o: ../ap_ppm_encoder.c \ + c:/winavr-20090313/lib/gcc/../../avr/include/avr/io.h \ + c:/winavr-20090313/lib/gcc/../../avr/include/avr/sfr_defs.h \ + c:/winavr-20090313/lib/gcc/../../avr/include/inttypes.h \ + c:/winavr-20090313/lib/gcc/../../avr/include/stdint.h \ + c:/winavr-20090313/lib/gcc/../../avr/include/avr/iom328p.h \ + c:/winavr-20090313/lib/gcc/../../avr/include/avr/portpins.h \ + c:/winavr-20090313/lib/gcc/../../avr/include/avr/common.h \ + c:/winavr-20090313/lib/gcc/../../avr/include/avr/version.h \ + c:/winavr-20090313/lib/gcc/../../avr/include/avr/fuse.h \ + c:/winavr-20090313/lib/gcc/../../avr/include/avr/lock.h \ + c:/winavr-20090313/lib/gcc/../../avr/include/avr/interrupt.h \ + c:/winavr-20090313/lib/gcc/../../avr/include/avr/wdt.h \ + c:/winavr-20090313/lib/gcc/../../avr/include/avr/eeprom.h \ + c:\winavr-20090313\bin\../lib/gcc/avr/4.3.2/include/stddef.h \ + ../servo2ppm_settings.h + +c:/winavr-20090313/lib/gcc/../../avr/include/avr/io.h: + +c:/winavr-20090313/lib/gcc/../../avr/include/avr/sfr_defs.h: + +c:/winavr-20090313/lib/gcc/../../avr/include/inttypes.h: + +c:/winavr-20090313/lib/gcc/../../avr/include/stdint.h: + +c:/winavr-20090313/lib/gcc/../../avr/include/avr/iom328p.h: + +c:/winavr-20090313/lib/gcc/../../avr/include/avr/portpins.h: + +c:/winavr-20090313/lib/gcc/../../avr/include/avr/common.h: + +c:/winavr-20090313/lib/gcc/../../avr/include/avr/version.h: + +c:/winavr-20090313/lib/gcc/../../avr/include/avr/fuse.h: + +c:/winavr-20090313/lib/gcc/../../avr/include/avr/lock.h: + +c:/winavr-20090313/lib/gcc/../../avr/include/avr/interrupt.h: + +c:/winavr-20090313/lib/gcc/../../avr/include/avr/wdt.h: + +c:/winavr-20090313/lib/gcc/../../avr/include/avr/eeprom.h: + +c:\winavr-20090313\bin\../lib/gcc/avr/4.3.2/include/stddef.h: + +../servo2ppm_settings.h: diff --git a/Tools/PPMEncoder/default/ppm_enconder_at328.bat b/Tools/PPMEncoder/default/ppm_enconder_at328.bat new file mode 100644 index 0000000000..3100826ada --- /dev/null +++ b/Tools/PPMEncoder/default/ppm_enconder_at328.bat @@ -0,0 +1,17 @@ +@echo ********************************* +@echo Visit DIYdrones.com! +@echo This Bat is for PPM_encoder! +@echo ********************************* +@echo Press enter to do some Magic! +pause +:A +@echo Programming Arduino... + +"C:\Program Files\Atmel\AVR Tools\STK500\Stk500.exe" -cUSB -dATmega328P -fD9E2 -EFD -FD9E2 -GFD -e -ifap_ppm_encoder.hex -pf -lCF -LCF + +@echo **************************************************************** +@echo Jordi: CHECK FOR PROBLEMS ABOVE before closing this window! +@echo Press enter to do the Magic again! +@echo **************************************************************** +pause +Goto A diff --git a/Tools/PPMEncoder/servo2ppm_settings.h b/Tools/PPMEncoder/servo2ppm_settings.h new file mode 100644 index 0000000000..9431b7cde4 --- /dev/null +++ b/Tools/PPMEncoder/servo2ppm_settings.h @@ -0,0 +1,128 @@ + +/********************************************************************************************************* + Title : header file for the rc ppm encoder (servo2ppm_settings.h) + Author: Chris Efstathiou + E-mail: hendrix at vivodinet dot gr + Homepage: ........................ + Date: 03/Aug/2009 + Compiler: AVR-GCC with AVR-AS + MCU type: ATmega168 + Comments: This software is FREE. Use it at your own risk. +*********************************************************************************************************/ + +#ifndef SERVO2PPM_SETTINGS_H +#define SERVO2PPM_SETTINGS_H + +/********************************************************************************************************/ +/* USER CONFIGURATION BLOCK START */ +/********************************************************************************************************/ +/* +The cpu frequency is defined in the makefile, the below definition is used only if the cpu frequency +is not defined in the makefile. +*/ + +#ifndef F_CPU +#define F_CPU 8000000UL /* CPU CLOCK FREQUENCY */ +#endif +/* +Those values are the failsafe servo values and the values for the non used channels +If for example you leave unconnected channel 7, the servo pulse of channel 7 in the PPM train +will be the failsafe value set below as "RC_FAILSAFE_CHANNEL_7" thus 1000 microseconds. +*/ + +#define RC_FAILSAFE_CHANNEL_1 1500UL +#define RC_FAILSAFE_CHANNEL_2 1500UL +#define RC_FAILSAFE_CHANNEL_3 1000UL +#define RC_FAILSAFE_CHANNEL_4 1500UL +#define RC_FAILSAFE_CHANNEL_5 1000UL +#define RC_FAILSAFE_CHANNEL_6 1000UL +#define RC_FAILSAFE_CHANNEL_7 1000UL +#define RC_FAILSAFE_CHANNEL_8 1000UL + + +/* +When signal is lost 1= ppm waveform remain on with failsafe values, 0= ppm waveform is off +For use with Paparazzi use "0", the autopilot has it's own failsafe values when PPM signal is lost. +The above failsafe values will kick in only if the servo inputs are lost which cannot happen with +a receiver with dsp processing that provides "hold" or "failsafe" features. +If the receiver only provides "hold" on the last good servo signals received it is not suitable +for use with the Paparazzi autopilot as it will prohibit the autopilot entering the "AUTO2" or "HOME" mode. +If you use a receiver with failsafe ability then remember to set the failsafe value of the receiver +to 2000 microseconds for the "MODE" channel so the autopilot can go to "AUTO2" or "HOME" mode when the receiver +loose the tx signal. The servo signals on receivers like PCM, IPD and any receiver with servo hold AND failsafe +will not stop outputing servo pulses thus the encoder will never stop producing a PPM pulse train +except if the throttle channel is used as an indication by setting the "RC_LOST_CHANNEL" to a value above 0. +If you use the throttle channel as an indication that the TX signal is lost then: +RC_USE_FAILSAFE set to 0 means that the ppm output will be shut down and if you set +RC_USE_FAILSAFE to 1 the ppm output will NOT shut down but it will now output the failsafe values +defined above. +*/ +#define RC_USE_FAILSAFE 1 + +/* +The channel number (1,2,3...7,8) that will be used as a receiver ready indicator. +If set above 0 then this channel should be always connected otherwise +the PPM output will not be enabled as the ppm encoder will wait for ever for this channel +to produce a valid servo pulse. +If 0 then the first connected channel going from channel 1 to 8 will be used, in other words +which ever connected channel comes on first it will indicate that the receiver is operational. +The detection of all connected channels is independent of this setting, this channel +is used as an indication that the receiver is up and running only. +Valid only if greater than 0. +*/ +#define RC_RX_READY_CHANNEL 0 /* 0 = No channel will be exclusively checked. */ + +/* +The default value for the rc signal lost indicator channel and it's threshold value in microseconds . +If the value is above 1500 microseconds then when the signal lost indicator channel servo pulse exceeds +RC_LOST_THRESHOLD then the ppm output will be shut down. +If the value is below 1500 microseconds then when the signal lost indicator channel servo pulse gets lower than +RC_LOST_THRESHOLD then the ppm output will be shut down. +If the RC_USE_FAILSAFE is set to 1 then the ppm output will not stop producing pulses but now +the ppm wavetrain will contain the failsafe values defined in the beginning of this file. +Valid only if RC_LOST_CHANNEL > 0 +*/ +#define RC_LOST_CHANNEL 3 /* Defaul is the throttle channel. You can use any channel. */ +#define RC_LOST_THRESHOLD 2025 /* Any value below 1300 or above 1700 microseconds. */ + + +#define RC_PPM_GEN_CHANNELS 8 /* How many channels the PPM output frame will have. */ + +#define RC_PPM_OUTPUT_TYPE 0 /* 1 = POSITIVE PULSE, 0= NEGATIVE PULSE */ + + +#define RC_MUX_CHANNEL 8 /*Jordi: Channel that will control the MUX */ +#define RC_MUX_REVERSE 0 /*Jordi: Inverted the MUX output (NOT), 0 = normal, 1 = Rev*/ +#define RC_MUX_MIN 0 /*Jordi: the min point */ +#define RC_MUX_MAX 1250 /*Jordi: the max point */ + +/* +0 = the reset period is constant but the total PPM frame period is varying. +1 = the reset period is varying but the total PPM frame period is constant (always 23,5 ms for example). +*/ +#define RC_CONSTANT_PPM_FRAME_TIME 0 + + +/********************************************************************************************************/ +/* + D A N G E R +The below settings are good for almost every situation and probably you don't need to change them. +Be carefull because you can cause a lot of problems if you change anything without knowing +exactly what you are doing. +*/ +#define RC_SERVO_MIN_PW 800UL /* in microseconds */ +#define RC_SERVO_CENTER_PW 1500UL /* in microseconds */ +#define RC_SERVO_MAX_PW 2200UL /* in microseconds */ +#define RC_PPM_CHANNEL_SYNC_PW 300UL /* the width of the PPM channel sync pulse in microseconds. */ +#define RC_PPM_RESET_PW 0UL /* Min reset time in microseconds(ie. 7500UL), 0=AUTO */ +#define RC_PPM_FRAME_LENGTH_MS 0UL /* Max PPM frame period in microseconds(ie. 23500UL), 0=AUTO */ +#define RC_MAX_TIMEOUT 30000UL /* in microseconds, max ~65000 @ 8 mHZ, ~32000 @ 16 Mhz */ +#define RC_MAX_BAD_PPM_FRAMES 4 /* Max consecutive bad servo pulse samples before failsafe */ +#define RC_LED_FREQUENCY 5UL /* In Hertz, not accurate, min=1, max=you will get a warning */ +#define RC_THROTTLE_CH_OFFSET_PW 25 /* in microseconds. */ + +/********************************************************************************************************/ +/* USER CONFIGURATION BLOCK END */ +/********************************************************************************************************/ + +#endif //#ifndef SERVO2PPM_SETTINGS_H