;*************************************************************************** ;* E S C 2 D I R 2 A M P S F I R M W A R E F O R A T T I N Y 1 3 ;* ;* File Name :"ESC2AR.asm" ;* Title : Electronic speed controller, max 2A, reverse ;* Date : 07.03.2008 ;* Version : 1.0 ;* Support email : support@gadgetparadise.com ;* Target MCU : ATtiny13 ;* Clock source : Internal R/C 4.8MHz ;* Compiler : AVRAssembler2 ;* Source TAB size : 2 ;* ;* DESCRIPTION ;* ;* This program is a bidirectional speed controller software. It receives ;* standard servo pulses (1-2ms) with 1200 steps resolution and ;* it generates PWM signal with 256 points resolution ;* to drive a H-bridge. It has arming function, signal loss protection, ;* overtemperature protection and speed change ramp. ;* ;* WARRANTY DISCLAIMER: ;* THIS PROGRAM IS PROVIDED AS IS WITHOUT ANY WARRANTY, EXPRESSED OR ;* IMPLIED, INCLUDING BUT NOT LIMITED TO FITNESS FOR A PARTICULAR ;* PURPOSE. THE AUTHOR ASSUME NO LIABILITY FOR DAMAGES, DIRECT OR ;* CONSEQUENTIAL, WHICH MAY RESULT FROM THE USE OF TTF2FNA SOFTWARE. ;* ;* SOFTWARE LICENSE AND DISTRIBUTION: ;* ;* The ESC2AR software is exclusively owned by Laszlo Arvai. ;* This software is released as a freeware. You may freely use or ;* distribute this program for any non-commercial purposes. ;* Distribution is permitted only for the whole software package ;* without any modifications and without any fee of charge. All ;* accompanying files must be included. ;* The source code is published under the GNU General Public ;* License. You can freely modify it or integrate it into your ;* projects even if your project is commercial. ;* But you can't use the hex files or the ;* source files in their original form for commercial ;* purpose, without a written permission from the ;* software author. ;* ;*************************************************************************** .include "tn13def.inc" ;***** Macros #define RAMINDEX( x ) Z + (x - SRAM_START) #define RAMINDEX_LOW( x ) Z + (x - SRAM_START) #define RAMINDEX_HIGH( x ) Z + (x - SRAM_START + 1) #define BV(x) (1<<(x)) ;***** We don't need index registers (except Z) .undef XH .undef XL .undef YH .undef YL ;***** Constants ;-- Settings -- .equ DEAD_STICK_LENGTH = 50 ; Pulse length around middle position which considered as zero throttle .equ FULL_SPEED_LENGTH = 240 ; Full speed limit value (above this PWM value the motor is runnin in full speed) .equ MOTOR_STALL_PWM = 35 ; Maximum Motor stall PWM value (above this value the motor is rotating) .equ MAX_PWM = 255 ; Maximum PWM value .equ MAX_MISSING_PULSES = 50 ; maximum mumber of missing pulses before shutting dowm the motor .equ MAX_TEMPERATURE = 215 ; Maximum H Bridge temperature .equ RESET_TEMPERATURE = 300 ; Temperature limit to exit from overtemperature status .equ SPEED_RAMP_DELAY = 20 ; speed change ramp delay .equ NOT_ARMED_TIMEOUT = 10 ; not armed time out constants ;-- Other constants -- .equ STATUS_STOP = 1 ; stopped status .equ STATUS_FORWARD = 2 ; forward direction .equ STATUS_FULL_FORWARD = 3 ; full forward speed status .equ STATUS_BACKWARD = 4 ; backward status .equ STATUS_FULL_BACKWARD = 5 ; full backward status .equ STATUS_NOT_ARMED = 6 ; not armed status .equ STATUS_OVERTEMPERATURE = 7 ; H Bridge overheated status .equ CHANNEL_RESOLUTION = 1200 ; resolution of channels .equ MIDDLE_VALUE = CHANNEL_RESOLUTION / 2 ; middle position value .equ CHANNEL_MIN_PULSE_LENGTH = 1200; min pulse length measured in timer increments .equ GLITCH_TOLERANCE = 600 ; If the pulse length is lower or higher by this value than expected the pulse considered as glitch .equ MAX_PULSE_GAP_LENGTH = 200 ; Servo pulse max. repeat time value (*256/1.2us) .equ MIN_PULSE_GAP_LENGTH = 50 ; Servo pulse min. repeat time value (*256/1.2us) .equ TEMPERATURE_AVERAGE = 16 ; Temperature A/D average value ; default values .equ DEFAULT_CHANNEL_IN_VALUE = MIDDLE_VALUE; ;***** Pin definitions ; input pins .equ SERVO_INPUT_PIN = PB4 .equ BRIDGE1_LOW_PIN = PB0 .equ BRIDGE2_LOW_PIN = PB1 .equ BRIDGE1_HIGH_PIN = PB3 .equ BRIDGE2_HIGH_PIN = PB2 .equ THERMISTOR_INPUT_PIN = PB5 ;***** Reserved memory address .dseg si_rising_timestamp: .byte 2 ; Servo input signal rising edge timestamp si_value: .byte 2 ; Input Channel pulse length si_prev_value: .byte 2 ; Previous servo in pulse length si_missing_pulse_counter: .byte 1 ; counter for missing servo pulses prev_timer_value: .byte 1 ; Previous timer value for speed ramp function temperature_sum: .byte 2 ; H bridge temperature average sum temperature_avg: .byte 1 ; H bridge temperature average counter ;***** Global Register Variables .def g_timer_high = R15 ; Timer counter high byte .def g_pulse_gap = r1 .def g_not_armed_timeout = r2 .def g_si_status = r3 ; Servo input status ;***** Interrupt table .cseg .org $0000 rjmp RESET_vect ; Reset handle .org INT0addr reti .org PCI0addr ; Pin change interrupt rjmp PCINT0_vect .org OVF0addr ; Timer0 overflow interrupt rjmp TIM0_OVF_vect .org ERDYaddr reti .org ACIaddr reti .org OC0Aaddr reti .org OC0Baddr reti .org WDTaddr reti .org ADCCaddr reti ;***** Code ;**************************************************************************** ;* ;* Main Program ;* ;* Folowing functions are implemented: ;* - system init ;* - check for arming conditon ;* - check for servo signal presence ;* - handle speed change ramp ;* - determine direction ;* - calculate PWM value ;* - set H-bridge and pwm ;* ;* All other functions are implementend in irq functions ;* ;*************************************************************************** ;***** Main Program Register Variables .def m_temp = r16 ; Main program temp variable .def m_low = r17 .def m_high = r18 .def m_low2 = r20 .def m_high2 = r21 .def m_high3 = r8 .def m_high4 = r9 .def m_status = r19 .def m_rem_low = r10 .def m_rem_high = r11 .def m_rem_high2 = r12 .def m_rem_high3 = r13 .def m_zero = r14 ;***** Code ;** System initialization RESET_vect: ; Init stack ldi m_temp,low(RAMEND) ; Load low byte address of end of RAM into register R16 out SPL,m_temp ; Initialize stack pointer to end of internal RAM ; Init OSCCAL ldi m_temp, 0 ; Setup EEPROM address out EEARL, m_temp sbi EECR, EERE ; Read data in m_temp, EEDR cpi m_temp, 0xff ; Check if data is valid breq no_osccal out OSCCAL, m_temp ; set OSCCAL no_osccal: ; Init RAM address pointer (indirect memory access is used to avoid 32 bit length of lds and sts instruction) ldi ZL, low( SRAM_START ) ldi ZH, high( SRAM_START ) ; init SRAM movw Y, Z clr m_low ldi m_temp, SRAM_SIZE clr_sram: st Y+, m_low dec m_temp brne clr_sram ; init status ldi m_status, STATUS_NOT_ARMED ; init pulse timeout clr g_pulse_gap ; init si_value clr m_temp std RAMINDEX_LOW(si_value), m_temp std RAMINDEX_HIGH(si_value), m_temp ; init temperature average ldi m_temp, TEMPERATURE_AVERAGE std RAMINDEX(temperature_avg), m_temp ; Setup port clr m_temp out PORTB, m_temp ldi m_temp, BV(BRIDGE1_LOW_PIN) + BV(BRIDGE2_LOW_PIN) + BV(BRIDGE1_HIGH_PIN) + BV(BRIDGE2_HIGH_PIN) out DDRB, m_temp ; Enable timer inerrupts ldi m_temp, BV(TOIE0) out TIMSK0, m_temp ; Setup timer0 and PWM ldi m_temp, BV(WGM01) + BV(WGM00) out TCCR0A, m_temp ldi m_temp, BV(CS00) out TCCR0B, m_temp ; Setup pin change interrupt ldi m_temp, BV(PCIE) out GIMSK, m_temp ldi m_temp, BV(SERVO_INPUT_PIN) out PCMSK, m_temp ; Setup A/D converter ldi m_temp, 0 ; free runing mode out ADCSRB, m_temp ldi m_temp, BV(ADC0D) ; disable digital input out DIDR0, m_temp ldi m_temp, 0 ; select ADC0 and vcc as reference out ADMUX, m_temp ldi m_temp, BV(ADEN) + BV(ADSC) + BV(ADPS2) + BV(ADPS1) +BV(ADPS0) out ADCSRA, m_temp ; Enable interrupts sei ; ************* ; * Main loop * ; ************* forever: ; check for pulse timeout ldd m_temp, RAMINDEX(si_missing_pulse_counter) tst m_temp breq not_armed ; go to not armed if enough missing pulses detected ; check status cpi m_status, STATUS_NOT_ARMED brne check_temperature ; go to armed mode ; ****************** ; * Not armed mode * ; ****************** not_armed: cpi m_status, STATUS_NOT_ARMED breq check_for_leave_not_armed set_not_armed_status: ; set not armed condition ldi m_status, STATUS_NOT_ARMED ; disable pwm on both side of the bridge in m_temp, TCCR0A andi m_temp, ~( BV(COM0A1) + BV(COM0B1) ) out TCCR0A, m_temp ; set all output pin to low clr m_temp out PORTB, m_temp ;reload not armed timeout reload_not_armed_timeout: ldi m_temp, NOT_ARMED_TIMEOUT mov g_not_armed_timeout, m_temp rjmp forever check_for_leave_not_armed: ldd m_temp, RAMINDEX(si_missing_pulse_counter) tst m_temp ; if there is no pulse stay in breq reload_not_armed_timeout ; not armed status ; load pulse length cli ldd m_low, RAMINDEX_LOW(si_value) ldd m_high, RAMINDEX_HIGH(si_value) sei ; check for middle low value cpi m_low, low( MIDDLE_VALUE - DEAD_STICK_LENGTH ) ldi m_temp, high( MIDDLE_VALUE - DEAD_STICK_LENGTH ) cpc m_high, m_temp brlo reload_not_armed_timeout ; check for middle high value cpi m_low, low( MIDDLE_VALUE + DEAD_STICK_LENGTH + 1 ) ldi m_temp, high( MIDDLE_VALUE + DEAD_STICK_LENGTH + 1 ) cpc m_high, m_temp brsh reload_not_armed_timeout ; arm speed controller if timeout ellapsed tst g_not_armed_timeout brne forever ; **************************** ; * normal (armed operation) * ; **************************** ; set ramp delay std RAMINDEX(prev_timer_value), g_timer_high ; set position to middle ldi m_low, low(MIDDLE_VALUE) ldi m_high, high(MIDDLE_VALUE) std RAMINDEX_LOW(si_prev_value), m_low std RAMINDEX_HIGH(si_prev_value), m_high ; activate stopped mode rjmp ci_stopped_mode check_temperature: ; ********************************** ; * check H bridge temperature * ; ********************************** ; check A/D is ready sbic ADCSRA, ADIF rjmp ct_read_temperature ; check status check_overtemperature: cpi m_status, STATUS_OVERTEMPERATURE breq check_temperature rjmp check_input ; conversion is ready, read data and add it to the sum ct_read_temperature: in m_low, ADCL in m_high, ADCH ldd m_low2, RAMINDEX_LOW(temperature_sum) ldd m_high2, RAMINDEX_HIGH(temperature_sum) add m_low, m_low2 adc m_high, m_high2 std RAMINDEX_LOW(temperature_sum), m_low std RAMINDEX_HIGH(temperature_sum), m_high ; restart A/D sbi ADCSRA, ADIF sbi ADCSRA, ADSC ; check counter ldd m_temp, RAMINDEX(temperature_avg) dec m_temp breq ci_l7 std RAMINDEX(temperature_avg), m_temp rjmp check_overtemperature ; counter reached zero, make average ci_l7: ldi m_temp, LOG2(TEMPERATURE_AVERAGE) ldd m_low, RAMINDEX_LOW(temperature_sum) ldd m_high, RAMINDEX_HIGH(temperature_sum) ci_l9: lsr m_high ; divide by 2 ror m_low dec m_temp brne ci_l9 ; reset sum clr m_temp std RAMINDEX_LOW(temperature_sum), m_temp std RAMINDEX_HIGH(temperature_sum), m_temp ; reload counter ldi m_temp, TEMPERATURE_AVERAGE std RAMINDEX(temperature_avg), m_temp ; check status cpi m_status, STATUS_OVERTEMPERATURE breq ci_overtemperature ; check temperature result cpi m_low, low( MAX_TEMPERATURE ) ldi m_temp, high( MAX_TEMPERATURE ) cpc m_high, m_temp brsh check_input ; temperature is ok, continue ; overtemperature dectected ; disable pwm on both sides of the bridge in m_temp, TCCR0A andi m_temp, ~( BV(COM0A1) + BV(COM0B1) ) out TCCR0A, m_temp ; set all output pin to low clr m_temp out PORTB, m_temp ; set status to overtemperature ldi m_status, STATUS_OVERTEMPERATURE rjmp check_temperature ci_overtemperature: ; check temperature result cpi m_low, low( RESET_TEMPERATURE ) ldi m_temp, high( RESET_TEMPERATURE ) cpc m_high, m_temp brlo check_temperature ; temperature is still high ; temperature low enough to restart operation rjmp set_not_armed_status ; ********************************** ; * apply pulse length change ramp * ; ********************************** check_input: ; check elapsed time since last value update mov m_temp, g_timer_high ldd m_low, RAMINDEX(prev_timer_value) sub m_temp, m_low cpi m_temp, SPEED_RAMP_DELAY brlo jump_to_forever ; if ramp delay expired -> change servo in value ; reload ramp delay counter std RAMINDEX(prev_timer_value), g_timer_high ; calculate pulse change ; load input value cli ldd m_low2, RAMINDEX_LOW(si_value) ldd m_high2, RAMINDEX_HIGH(si_value) sei ; load previous input value ldd m_low, RAMINDEX_LOW(si_prev_value) ldd m_high, RAMINDEX_HIGH(si_prev_value) ; substact previous value from current value sub m_low2, m_low sbc m_high2, m_high breq jump_to_forever brcc ci_l5 ; changes was negative -> decrement previous value subi m_low, low(1) sbci m_high, high(1) rjmp ci_update_prev_value ci_l5: ; changes was positive -> increment previous value subi m_low, low(-1) sbci m_high, high(-1) ; update previous value ci_update_prev_value: std RAMINDEX_LOW(si_prev_value), m_low std RAMINDEX_HIGH(si_prev_value), m_high ; check for full backward value cpi m_low, low( FULL_SPEED_LENGTH + 1) ldi m_temp, high( FULL_SPEED_LENGTH + 1) cpc m_high, m_temp brsh ci_l1 ; *************************** ; * full backward direction * ; *************************** ; check status cpi m_status, STATUS_FULL_BACKWARD breq jump_to_forever ; disable high side of this half of the bridge cbi PORTB, BRIDGE2_HIGH_PIN cbi PORTB, BRIDGE1_LOW_PIN ; disable pwm and set output to high in m_temp, TCCR0A andi m_temp, ~BV(COM0B1) out TCCR0A, m_temp sbi PORTB, BRIDGE2_LOW_PIN ; enable high side of the other side of the bridge sbi PORTB, BRIDGE1_HIGH_PIN ; update status ldi m_status, STATUS_FULL_BACKWARD jump_to_forever: rjmp forever ci_l1: ; check for backward values cpi m_low, low( MIDDLE_VALUE - DEAD_STICK_LENGTH ) ldi m_temp, high( MIDDLE_VALUE - DEAD_STICK_LENGTH ) cpc m_high, m_temp brsh ci_l2 ; ********************** ; * backward direction * ; ********************** ; check status cpi m_status, STATUS_BACKWARD breq update_backward_value ; disable high side of this half of the bridge cbi PORTB, BRIDGE2_HIGH_PIN ; disable pwm on other side of the bridge in m_temp, TCCR0A andi m_temp, ~BV(COM0A1) out TCCR0A, m_temp cbi PORTB, BRIDGE1_LOW_PIN ; enable pwm (low side) of this bridge in m_temp, TCCR0A ori m_temp, BV(COM0B1) out TCCR0A, m_temp ; enable high side of the other side of the bridge sbi PORTB, BRIDGE1_HIGH_PIN ; update status ldi m_status, STATUS_BACKWARD update_backward_value: ; substact from medium value ldi m_low2, low(MIDDLE_VALUE - DEAD_STICK_LENGTH) ldi m_high2, high(MIDDLE_VALUE - DEAD_STICK_LENGTH) sub m_low2, m_low sbc m_high2, m_high ; calculate pwm value mov m_low, m_low2 mov m_high, m_high2 rcall calculate_pwm_value ; set pwm value out OCR0B, m_low2 rjmp forever ci_l2: ; check for stop values cpi m_low, low( MIDDLE_VALUE + DEAD_STICK_LENGTH + 1 ) ldi m_temp, high( MIDDLE_VALUE + DEAD_STICK_LENGTH + 1 ) cpc m_high, m_temp brsh ci_l3 ; **************** ; * stopped mode * ; **************** ci_stopped_mode: ; check status cpi m_status, STATUS_STOP breq end_stop ; disable high sides cbi PORTB, BRIDGE1_HIGH_PIN cbi PORTB, BRIDGE2_HIGH_PIN ; disable pwm on both side in m_temp, TCCR0A andi m_temp, ~( BV(COM0A1) + BV(COM0B1) ) out TCCR0A, m_temp ; enable break sbi PORTB, BRIDGE1_LOW_PIN sbi PORTB, BRIDGE2_LOW_PIN ; set stopped status ldi m_status, STATUS_STOP end_stop: rjmp forever ci_l3: ; check for forward value cpi m_low, low( CHANNEL_RESOLUTION - FULL_SPEED_LENGTH ) ldi m_temp, high( CHANNEL_RESOLUTION - FULL_SPEED_LENGTH ) cpc m_high, m_temp brsh ci_l4 ; ********************* ; * forward direction * ; ********************* ; check status cpi m_status, STATUS_FORWARD breq update_forward_value ; disable high side of this half of the bridge cbi PORTB, BRIDGE1_HIGH_PIN ; disable pwm on other side of the bridge in m_temp, TCCR0A andi m_temp, ~BV(COM0B1) out TCCR0A, m_temp cbi PORTB, BRIDGE2_LOW_PIN ; enable pwm (low side) of this bridge in m_temp, TCCR0A ori m_temp, BV(COM0A1) out TCCR0A, m_temp ; enable high side of the other side of the bridge sbi PORTB, BRIDGE2_HIGH_PIN ; update status ldi m_status, STATUS_FORWARD update_forward_value: ; substact min forward value subi m_low, low(MIDDLE_VALUE + DEAD_STICK_LENGTH) ldi m_temp, high(MIDDLE_VALUE + DEAD_STICK_LENGTH) sbc m_high, m_temp ; calculate pwm value rcall calculate_pwm_value ; set pwm value out OCR0A, m_low2 rjmp forever ; ********************** ; * full forward value * ; ********************** ci_l4: ; disable high side of this half of the bridge cbi PORTB, BRIDGE1_HIGH_PIN cbi PORTB, BRIDGE2_LOW_PIN ; disable pwm and set output to high in m_temp, TCCR0A andi m_temp, ~BV(COM0A1) out TCCR0A, m_temp sbi PORTB, BRIDGE1_LOW_PIN ; enable high side of the other side of the bridge sbi PORTB, BRIDGE2_HIGH_PIN ; update status ldi m_status, STATUS_FULL_BACKWARD rjmp forever ;****************************************************************************** ;* Calculate PWM Value ;* ;* Calculates PWM value proportional between stop and full speed value ;* and omitting low PWM values where the motor stalls ;* Input value must be in m_low and m_high register the output PWM ;* value will be in m_low register ;****************************************************************************** calculate_pwm_value: ; load PWM range ldi m_low2, low( MAX_PWM - MOTOR_STALL_PWM ) ldi m_high2, high( MAX_PWM - MOTOR_STALL_PWM ) ; multiply value with the PWM range clr m_high4 ; clear 2 highest bytes of result clr m_high3 ldi m_temp,16 ; init loop counter lsr m_high2 ror m_low2 m16u_1: brcc noad8 ; if bit 0 of multiplier set add m_high3,m_low ; add multiplicand Low to byte 2 of res adc m_high4,m_high ; add multiplicand high to byte 3 of res noad8: ror m_high4 ; shift right result byte 3 ror m_high3 ; rotate right result byte 2 ror m_high2 ; rotate result byte 1 and multiplier High ror m_low2 ; rotate result byte 0 and multiplier Low dec m_temp ; decrement loop counter brne m16u_1 ; if not done, loop more ; load divisor ldi m_low, low( MIDDLE_VALUE - DEAD_STICK_LENGTH - FULL_SPEED_LENGTH ) ldi m_high, high( MIDDLE_VALUE - DEAD_STICK_LENGTH - FULL_SPEED_LENGTH ) clr m_zero div16u: clr m_rem_low ; clear remainder Low byte clr m_rem_high2 clr m_rem_high3 sub m_rem_high,m_rem_high;clear remainder High byte and carry ldi m_temp,33 ; init loop counter d16u_1: rol m_low2 ; shift left dividend rol m_high2 rol m_high3 rol m_high4 dec m_temp ; decrement counter breq cpv_return ; if done d16u_2: rol m_rem_low ; shift dividend into remainder rol m_rem_high rol m_rem_high2 rol m_rem_high3 sub m_rem_low,m_low ; remainder = remainder - divisor sbc m_rem_high,m_high sbc m_rem_high2, m_zero sbc m_rem_high3, m_zero brcc d16u_3 ; if result negative add m_rem_low,m_low ; restore remainder adc m_rem_high,m_high adc m_rem_high2, m_zero adc m_rem_high3, m_zero clc ; clear carry to be shifted into result rjmp d16u_1 ; else d16u_3: sec ; set carry to be shifted into result rjmp d16u_1 cpv_return: ; add minimum pwm value subi m_low2, -MOTOR_STALL_PWM ret ;****************************************************************************** ;* Timer0 Overflow interupt ;* ;* This interrupt increases timer counter high byte and handles ;* pulse time out counter ;****************************************************************************** ;***** Timer0 Overflow Register Variables .def t0owf_temp = r29 ; Timer0 overflow temp variable TIM0_OVF_vect: ; store SREG in t0owf_temp, SREG push t0owf_temp ; increment counter high inc g_timer_high ; avoid to pulse length calculation turnover tst g_si_status breq to_four ; load timestamp high byte ldd t0owf_temp, RAMINDEX_HIGH(si_rising_timestamp) cp t0owf_temp, g_timer_high brne to_four ; reset servo in status clr g_si_status ; divide this timing by 4 to_four: mov t0owf_temp, g_timer_high andi t0owf_temp, 0b00000011 brne to_armed_timeout ; increment pulse gap timer inc g_pulse_gap mov t0owf_temp, g_pulse_gap cpi t0owf_temp, MAX_PULSE_GAP_LENGTH brlo to_armed_timeout ; timer is not expired ; the time expired now -> reset timer clr g_pulse_gap ; reset servo in status clr g_si_status ; decrement missing pulse ldd t0owf_temp, RAMINDEX(si_missing_pulse_counter) tst t0owf_temp breq to_armed_timeout dec t0owf_temp std RAMINDEX(si_missing_pulse_counter), t0owf_temp to_armed_timeout: ; decrement armed timeout if it's not zero tst g_not_armed_timeout breq to_exit dec g_not_armed_timeout ; exit from the it routine to_exit: pop t0owf_temp out SREG, t0owf_temp reti ;**************************************************************************** ;* PIN Change interrupt ;* ;* This interrupt measures input pulse length ;*************************************************************************** ;***** PIN Change Register Variables .def pc_low = r23 ; PIN Change low temp variable .def pc_high = r24 ; PIN Change high temp variable .def pc_temp = r27 ; PIN Change temp variable .def pc_low1 = r28 .def pc_high1 = r25 ;***** Code PCINT0_vect: ; save SREG in pc_temp, SREG push pc_temp ; disable pin change interrupt in pc_temp, GIMSK andi pc_temp, ~BV(PCIE) out GIMSK, pc_temp ; get timestamp mov pc_high, g_timer_high in pc_low, TCNT0 ; handle the case when owerflow occurs ; between reading low and high bytes in pc_temp, TIFR0 sbrs pc_temp, TOV0 rjmp pc_l1 in pc_low, TCNT0 inc pc_high pc_l1: ; enable other interrupts sei ;** check input channel low to high transition ; check for rising edge sbis PINB, SERVO_INPUT_PIN rjmp pc_check_for_falling_edge ; status must be zero tst g_si_status brne pc_reenable_pin_change_interrupt ; ** rising edge detected ; skip pulse repeat ratio check in the case of missing pulse ldd pc_temp, RAMINDEX(si_missing_pulse_counter) cpi pc_temp, MAX_MISSING_PULSES brne pc_store_time_stamp ; check pulse repeat ratio (pulse gap) mov pc_temp, g_pulse_gap cpi pc_temp, MIN_PULSE_GAP_LENGTH brlo pc_reenable_pin_change_interrupt ; repeat ratio is too short ignore pulse pc_store_time_stamp: ; store time stamp std RAMINDEX_LOW(si_rising_timestamp), pc_low ; store low byte std RAMINDEX_HIGH(si_rising_timestamp), pc_high ; store high byte ; change status ldi pc_temp, 1 mov g_si_status, pc_temp rjmp pc_reenable_pin_change_interrupt ;** check input channel0 high to low transition pc_check_for_falling_edge: sbic PINB, SERVO_INPUT_PIN rjmp pc_reenable_pin_change_interrupt ; reset status clr g_si_status ; prepare for pulse length calculation ldd pc_low1, RAMINDEX_LOW(si_rising_timestamp) ; load rising edge time stamp ldd pc_high1, RAMINDEX_HIGH(si_rising_timestamp) ; calculate length sub pc_low, pc_low1 sbc pc_high, pc_high1 ; divide by 4 lsr pc_high ; divide by 2 ror pc_low lsr pc_high ; divide by 2 ror pc_low ;** limit pulse length ; check for glitch length cpi pc_low, low( CHANNEL_MIN_PULSE_LENGTH - GLITCH_TOLERANCE ) ldi pc_temp, high( CHANNEL_MIN_PULSE_LENGTH - GLITCH_TOLERANCE ) cpc pc_high, pc_temp brlo pc_reenable_pin_change_interrupt ; if this is a glitch use previous value ; limit low pulse length cpi pc_low, low(CHANNEL_MIN_PULSE_LENGTH) ldi pc_temp, high(CHANNEL_MIN_PULSE_LENGTH) cpc pc_high, pc_temp brlo pc_low_value ; substact min pulse length subi pc_low, low(CHANNEL_MIN_PULSE_LENGTH) ldi pc_temp, high(CHANNEL_MIN_PULSE_LENGTH) sbc pc_high, pc_temp ; check value for glitch limit cpi pc_low, low(CHANNEL_RESOLUTION + GLITCH_TOLERANCE ) ldi pc_temp, high(CHANNEL_RESOLUTION + GLITCH_TOLERANCE ) cpc pc_high, pc_temp brsh pc_reenable_pin_change_interrupt ; check for max value cpi pc_low, low(CHANNEL_RESOLUTION ) ldi pc_temp, high(CHANNEL_RESOLUTION ) cpc pc_high, pc_temp brsh pc_high_value ; make average of this value pc_calculate_average: ldd pc_low1, RAMINDEX_LOW(si_value) ; load previous pulse length ldd pc_high1, RAMINDEX_HIGH(si_value) add pc_low, pc_low1 adc pc_high, pc_high1 lsr pc_high ; divide by 2 ror pc_low ; store result std RAMINDEX_LOW(si_value), pc_low ; store result std RAMINDEX_HIGH(si_value), pc_high ; restart pulse timeout clr g_pulse_gap ; restart missing pulse counter ldi pc_temp, MAX_MISSING_PULSES std RAMINDEX(si_missing_pulse_counter), pc_temp pc_reenable_pin_change_interrupt: in pc_temp, GIMSK ori pc_temp, BV(PCIE) out GIMSK, pc_temp ; restore status register pop pc_temp out SREG, pc_temp reti pc_high_value: ; set output value to maximum value ldi pc_low, low( CHANNEL_RESOLUTION ) ldi pc_high, high( CHANNEL_RESOLUTION ) rjmp pc_calculate_average pc_low_value: ; set output value to minimum value ldi pc_low, low( CHANNEL_MIN_PULSE_LENGTH ) ldi pc_high, high( CHANNEL_MIN_PULSE_LENGTH ) rjmp pc_calculate_average