diff --git a/Tools/PPMEncoder/ap_ppm_encoder.aps b/Tools/PPMEncoder/ap_ppm_encoder.aps deleted file mode 100644 index f324a0d4a9..0000000000 --- a/Tools/PPMEncoder/ap_ppm_encoder.aps +++ /dev/null @@ -1 +0,0 @@ -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 deleted file mode 100644 index ac2139b58e..0000000000 --- a/Tools/PPMEncoder/ap_ppm_encoder.aws +++ /dev/null @@ -1 +0,0 @@ - diff --git a/Tools/PPMEncoder/ap_ppm_encoder.c b/Tools/PPMEncoder/ap_ppm_encoder.c deleted file mode 100644 index ea707c6d91..0000000000 --- a/Tools/PPMEncoder/ap_ppm_encoder.c +++ /dev/null @@ -1,1286 +0,0 @@ - -/********************************************************************************************************* - 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 deleted file mode 100644 index cd488d1e5f..0000000000 --- a/Tools/PPMEncoder/default/ap_ppm_encoder.eep +++ /dev/null @@ -1,5 +0,0 @@ -:1000000001000100010001000100010001000100E8 -:1000100001000100E907E907E907E907E907E9073E -:10002000E907E907E907E907E90702020202020214 -:050030000202020202C1 -:00000001FF diff --git a/Tools/PPMEncoder/default/ap_ppm_encoder.elf b/Tools/PPMEncoder/default/ap_ppm_encoder.elf deleted file mode 100644 index 4ee1ca8525..0000000000 Binary files a/Tools/PPMEncoder/default/ap_ppm_encoder.elf and /dev/null differ diff --git a/Tools/PPMEncoder/default/ap_ppm_encoder.lss b/Tools/PPMEncoder/default/ap_ppm_encoder.lss deleted file mode 100644 index e67f4bdffe..0000000000 --- a/Tools/PPMEncoder/default/ap_ppm_encoder.lss +++ /dev/null @@ -1,2636 +0,0 @@ - -ap_ppm_encoder.elf: file format elf32-avr - -Sections: -Idx Name Size VMA LMA File off Algn - 0 .data 00000014 00800100 00000c1e 00000cd2 2**0 - CONTENTS, ALLOC, LOAD, DATA - 1 .text 00000c1e 00000000 00000000 000000b4 2**1 - CONTENTS, ALLOC, LOAD, READONLY, CODE - 2 .bss 0000001c 00800114 00800114 00000ce6 2**0 - ALLOC - 3 .eeprom 00000035 00810000 00810000 00000ce6 2**0 - CONTENTS, ALLOC, LOAD, DATA - 4 .debug_aranges 00000020 00000000 00000000 00000d1b 2**0 - CONTENTS, READONLY, DEBUGGING - 5 .debug_pubnames 0000022c 00000000 00000000 00000d3b 2**0 - CONTENTS, READONLY, DEBUGGING - 6 .debug_info 00000914 00000000 00000000 00000f67 2**0 - CONTENTS, READONLY, DEBUGGING - 7 .debug_abbrev 00000262 00000000 00000000 0000187b 2**0 - CONTENTS, READONLY, DEBUGGING - 8 .debug_line 00000c94 00000000 00000000 00001add 2**0 - CONTENTS, READONLY, DEBUGGING - 9 .debug_frame 00000100 00000000 00000000 00002774 2**2 - CONTENTS, READONLY, DEBUGGING - 10 .debug_str 0000043c 00000000 00000000 00002874 2**0 - CONTENTS, READONLY, DEBUGGING - 11 .debug_loc 000003b4 00000000 00000000 00002cb0 2**0 - CONTENTS, READONLY, DEBUGGING - 12 .debug_ranges 00000018 00000000 00000000 00003064 2**0 - CONTENTS, READONLY, DEBUGGING - -Disassembly of section .text: - -00000000 <__vectors>: - 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 deleted file mode 100644 index c7c0a8decb..0000000000 --- a/Tools/PPMEncoder/default/ap_ppm_encoder.map +++ /dev/null @@ -1,419 +0,0 @@ -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 deleted file mode 100644 index e48e02fc8e..0000000000 --- a/Tools/PPMEncoder/default/dep/ap_ppm_encoder.o.d +++ /dev/null @@ -1,46 +0,0 @@ -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 deleted file mode 100644 index 3100826ada..0000000000 --- a/Tools/PPMEncoder/default/ppm_enconder_at328.bat +++ /dev/null @@ -1,17 +0,0 @@ -@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 deleted file mode 100644 index 9431b7cde4..0000000000 --- a/Tools/PPMEncoder/servo2ppm_settings.h +++ /dev/null @@ -1,128 +0,0 @@ - -/********************************************************************************************************* - 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