You can not select more than 25 topics
Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.
2963 lines
90 KiB
2963 lines
90 KiB
;**** **** **** **** ****
|
|
;
|
|
; BLHeli program for controlling brushless motors in helicopters
|
|
;
|
|
; Copyright 2011, 2012 Steffen Skaug
|
|
; This program is distributed under the terms of the GNU General Public License
|
|
;
|
|
; This file is part of BLHeli.
|
|
;
|
|
; BLHeli is free software: you can redistribute it and/or modify
|
|
; it under the terms of the GNU General Public License as published by
|
|
; the Free Software Foundation, either version 3 of the License, or
|
|
; (at your option) any later version.
|
|
;
|
|
; BLHeli is distributed in the hope that it will be useful,
|
|
; but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
; GNU General Public License for more details.
|
|
;
|
|
; You should have received a copy of the GNU General Public License
|
|
; along with BLHeli. If not, see <http://www.gnu.org/licenses/>.
|
|
;
|
|
;**** **** **** **** ****
|
|
;
|
|
; This software is intended for AVR 8bit controllers in a micro heli environment.
|
|
;
|
|
; The software was inspired by and started from from Bernard Konze's BLMC: http://home.versanet.de/~b-konze/blc_6a/blc_6a.htm
|
|
; And also Simon Kirby's TGY: https://github.com/sim-/tgy
|
|
;
|
|
; This file is best viewed with tab width set to 5
|
|
;
|
|
; The input signal can be positive 1kHz, 2kHz, 4kHz or 8kHz PWM (taken from the "resistor tap" on mCPx)
|
|
; The code adapts itself to one of the three pwm frequencies
|
|
;
|
|
; The first lines of the software must be modified according to the chosen environment:
|
|
; - .include "ESC".inc ; Select ESC pinout
|
|
; - .equ TAIL = 0 ; Choose main or tail mode
|
|
; - .equ ICP = 0 ; Choose INT0 or ICP1 as input pin (ESC boards are wired for INT0). Feigao does not support ICP1
|
|
;
|
|
;**** **** **** **** ****
|
|
; Revision history:
|
|
; - Rev0.0: Initial revision
|
|
; - Rev1.0: Governor functionality added
|
|
; - Rev1.1: Increased tail gain to 1.0625. Implemented for tail only
|
|
; Decreased governor proportional and integral gain by 4
|
|
; Fixed bug that caused tail power not always to be max
|
|
; - Rev1.2: Governor integral gain should be higher in order to achieve full PWM range
|
|
; Integral gain can be higher, and is increased by 2x. An integral of +-128 can now be added to requested PWM
|
|
; - Rev1.3: Governor integral extended to 24bit
|
|
; Governor proportional gain increased by 2x
|
|
; Added slow spoolup/down for governor
|
|
; Set pwm to 100% (do not turn off nFET) for high values of current pwm
|
|
; Added support for PPM input (1us to 2us pulse)
|
|
; Removed USE_COMP_STORED as it was never used
|
|
; - Rev2.0 Added measurement of pwm frequency and support for 1kHz, 2kHz, 4kHz and 8kHz
|
|
; Optimized pwm on and off routines
|
|
; Improved mosfet switching in beep routines, to reduce current draw
|
|
; Added support for ICP1 interrupt pin input
|
|
; Added ADC measurement of supply voltage, with limiting of main motor power for low voltage
|
|
; Miscellaneous other changes
|
|
; - Rev2.1 Rewritten INT0 routine to be similar to ICP
|
|
; Reduced validation threshold (RCP_VALIDATE)
|
|
; Removed requirement for RCP to go to zero again in tail arming sequence
|
|
; Removed PPM support
|
|
; - Rev2.2 Added support for HC 5A 1S ESC with Atmega48V MPU
|
|
; Increased governor proportional gain by 2x
|
|
; - Rev3.0 Added functionality for programming from TX
|
|
; Added low voltage limit scaling for 2S and 3S
|
|
;
|
|
;**** **** **** **** ****
|
|
; 4/8K Bytes of In-System Self-Programmable Flash
|
|
; 256/512 Bytes EEPROM
|
|
; 512/1K Bytes Internal SRAM
|
|
;
|
|
;**** **** **** **** ****
|
|
; Timer 0 (1us counts) always counts up and is used for
|
|
; - RC pulse timeout and skip counts
|
|
; Timer 1 (1us counts) always counts up and is used for
|
|
; - RC pulse measurement (via external interrupt 0 or input capture pin)
|
|
; - Commutation timing (via output compare register A interrupt)
|
|
; Timer 2 (125ns counts) always counts up and is used for
|
|
; - PWM generation
|
|
;
|
|
;**** **** **** **** ****
|
|
; Interrupt handling
|
|
; The Atmega8 disables all interrupts when entering an interrupt routine,
|
|
; and enables them again when exiting. Thereby disabling nested interrupts.
|
|
; - Interrupts are disabled during beeps, to avoid interference from interrupts
|
|
; - RC pulse interrupts are periodically disabled in order to reduce interference with pwm interrupts.
|
|
;
|
|
;**** **** **** **** ****
|
|
; Motor control:
|
|
; - Brushless motor control with 6 states for each electrical 60 degrees
|
|
; - An advance timing of 0deg has zero cross 30deg after one commutation and 30deg before the next
|
|
; - Timing advance in this implementation is set to 15deg
|
|
; - A "damped" commutation scheme is used, where all pfets are on when pwm is off. This will absorb energy from bemf and make step settling more damped.
|
|
; Motor sequence starting from zero crossing:
|
|
; - Timer wait: Wt_Comm 15deg ; Time to wait from zero cross to actual commutation
|
|
; - Timer wait: Wt_Advance 15deg ; Time to wait for timing advance. Nominal commutation point is after this
|
|
; - Timer wait: Wt_Zc_Scan 7.5deg ; Time to wait before looking for zero cross
|
|
; - Scan for zero cross 22.5deg , Nominal, with some motor variations
|
|
;
|
|
; Motor startup:
|
|
; Initial motor rotations are done with the motor controlled as a stepper motor.
|
|
; In this stepper motor mode comparator information is not used.
|
|
; Settle phase is the first, where there are a few commutations with increasing step length, in order to settle the motor in a predefined position.
|
|
; Stepper phase comes next, where there is a step length decrease sequence.
|
|
; Aquisition phase is the final phase, for stabilisation before normal bemf commutation run begins.
|
|
;
|
|
;**** **** **** **** ****
|
|
|
|
.include "WalkeraWST10LT.inc" ; Select Walkera LT pinout (INT0 or ICP1 input)
|
|
;.include "Feigao6.inc" ; Select Feigao 6A pinout (INT0 input only!!!)
|
|
;.include "HC5A1SA8.inc" ; Select HobbyCity 5A 1S pinout with Atmega8 (INT0 or ICP1 input)
|
|
;.include "HC5A1SA48V.inc" ; Select HobbyCity 5A 1S pinout with Atmega48V (INT0 input only!!!)
|
|
.equ TAIL = 1 ; Choose mode. Set to 0 for main motor and 1 for tail motor
|
|
.equ ICP = 1 ; Choose input pin. Set to 0 for INT0 (pin32) and 1 for ICP1 (pin12)
|
|
|
|
;**** **** **** **** ****
|
|
|
|
.equ TX_PGM = 1 ; Set to 0 to disable tx programming (reduces code size)
|
|
|
|
;**** **** **** **** ****
|
|
; Constant definitions for main
|
|
.if TAIL == 0
|
|
|
|
.equ GOV_SPOOLRATE = 2 ; Number of steps for governor requested pwm per 65ms
|
|
|
|
.equ RCP_TIMEOUT = 32 ; Number of timer0 overflows (about 256us) before considering rc pulse lost
|
|
.equ RCP_SKIP_RATE = 16 ; Number of timer0 overflows (about 256us) before reenabling rc pulse detection
|
|
.equ RCP_MIN = 0 ; This is minimum RC pulse length
|
|
.equ RCP_MAX = 250 ; This is maximum RC pulse length
|
|
.equ RCP_VALIDATE = 2 ; Require minimum this pulse length to validate RC pulse
|
|
.equ RCP_STOP = 1 ; Stop motor at or below this pulse length
|
|
.equ RCP_STOP_LIMIT = 1 ; Stop motor if this many timer1 overflows (~65ms) are below stop limit
|
|
|
|
.equ PWM_SETTLE = 50 ; PWM used when in start settling mode
|
|
.equ PWM_STEPPER = 80 ; PWM used when in start stepper mode
|
|
.equ PWM_AQUISITION = 80 ; PWM used when in start aquisition mode
|
|
.equ PWM_INITIAL_RUN = 40 ; PWM used when in initial run mode
|
|
|
|
.equ COMM_TIME_RED = 5 ; Fixed reduction (in us) for commutation wait (to account for fixed delays)
|
|
.equ COMM_TIME_MIN = 5 ; Minimum time (in us) for commutation wait
|
|
|
|
.equ STEPPER_STEP_BEG = 3000 ; ~3300 eRPM
|
|
.equ STEPPER_STEP_END = 1000 ; ~10000 eRPM
|
|
.equ STEPPER_STEP_DECREMENT = 5 ; Amount to decrease stepper step by per commutation
|
|
|
|
.equ AQUISITION_ROTATIONS = 2 ; Number of rotations to do in the aquisition phase
|
|
.equ DAMPED_RUN_ROTATIONS = 1 ; Number of rotations to do in the damped run phase
|
|
|
|
; Constant definitions for tail
|
|
.else
|
|
|
|
.equ GOV_SPOOLRATE = 1 ; Number of steps for governor requested pwm per 65ms
|
|
|
|
.equ RCP_TIMEOUT = 12 ; Number of timer0 overflows (about 256us) before considering rc pulse lost
|
|
.equ RCP_SKIP_RATE = 3 ; Number of timer0 overflows (about 256us) before reenabling rc pulse detection
|
|
.equ RCP_MIN = 0 ; This is minimum RC pulse length
|
|
.equ RCP_MAX = 250 ; This is maximum RC pulse length
|
|
.equ RCP_VALIDATE = 2 ; Require minimum this pulse length to validate RC pulse
|
|
.equ RCP_STOP = 1 ; Stop motor at or below this pulse length
|
|
.equ RCP_STOP_LIMIT = 50 ; Stop motor if this many timer1 overflows (~65ms) are below stop limit
|
|
|
|
.equ PWM_SETTLE = 50 ; PWM used when in start settling mode
|
|
.equ PWM_STEPPER = 120 ; PWM used when in start stepper mode
|
|
.equ PWM_AQUISITION = 80 ; PWM used when in start aquisition mode
|
|
.equ PWM_INITIAL_RUN = 40 ; PWM used when in initial run mode
|
|
|
|
.equ COMM_TIME_RED = 5 ; Fixed reduction (in us) for commutation wait (to account for fixed delays)
|
|
.equ COMM_TIME_MIN = 5 ; Minimum time (in us) for commutation wait
|
|
|
|
.equ STEPPER_STEP_BEG = 3000 ; ~3300 eRPM
|
|
.equ STEPPER_STEP_END = 1000 ; ~10000 eRPM
|
|
.equ STEPPER_STEP_DECREMENT = 30 ; Amount to decrease stepper step by per commutation
|
|
|
|
.equ AQUISITION_ROTATIONS = 2 ; Number of rotations to do in the aquisition phase
|
|
.equ DAMPED_RUN_ROTATIONS = 1 ; Number of rotations to do in the damped run phase
|
|
|
|
.endif
|
|
|
|
;**** **** **** **** ****
|
|
; Register definitions
|
|
.def Zero = r0 ; Register variable initialized to 0, always at 0
|
|
.def I_Sreg = r1 ; Status register saved in interrupts
|
|
.def Requested_Pwm = r2 ; Requested pwm (from RC pulse value)
|
|
.def Governor_Req_Pwm = r3 ; Governor requested pwm (sets governor target)
|
|
.def Current_Pwm = r4 ; Current pwm
|
|
.def Current_Pwm_Limited = r5 ; Current pwm that is limited (applied to the motor output)
|
|
.def Pwm_Timer_Second = r6 ; Second timer wait for pwm timer (if exceeding 256)
|
|
.def Rcp_Prev_Edge_L = r7 ; RC pulse previous edge timer1 timestamp (lo byte)
|
|
.def Rcp_Prev_Edge_H = r8 ; RC pulse previous edge timer1 timestamp (hi byte)
|
|
;.def Temp5 = r9 ; (Used by Temp5)
|
|
;.def Temp6 = r10 ; (Used by Temp6)
|
|
;.def I_Temp4 = r11 ; (Used by I_Temp4)
|
|
;.def I_Temp5 = r12 ; (Used by I_Temp5)
|
|
.def Rcp_Timeout_Cnt = r13 ; RC pulse timeout counter (decrementing)
|
|
.def Rcp_Skip_Cnt = r14 ; RC pulse skip counter (decrementing)
|
|
.def Rcp_Edge_Cnt = r15 ; RC pulse edge counter
|
|
|
|
.def Temp1 = r16 ; Main temporary
|
|
.def Temp2 = r17 ; Main temporary
|
|
.def Temp3 = r18 ; Main temporary
|
|
.def Temp4 = r19 ; Main temporary
|
|
.def Temp5 = r9 ; Aux temporary (limited operations)
|
|
.def Temp6 = r10 ; Aux temporary (limited operations)
|
|
|
|
.def I_Temp1 = r20 ; Interrupt temporary
|
|
.def I_Temp2 = r21 ; Interrupt temporary
|
|
.def I_Temp3 = r22 ; Interrupt temporary
|
|
.def I_Temp4 = r11 ; Interrupt temporary (limited operations)
|
|
.def I_Temp5 = r12 ; Interrupt temporary (limited operations)
|
|
|
|
.def Flags0 = r23 ; State flags
|
|
.equ OCA_PENDING = 0 ; If set, timer1 output compare interrunpt A is pending
|
|
;.equ = 1
|
|
;.equ = 2
|
|
;.equ = 3
|
|
.equ SETTLE_MODE = 4 ; Set when in motor start settling mode
|
|
.equ STEPPER_MODE = 5 ; Set when in motor start stepper motor mode
|
|
.equ AQUISITION_MODE = 6 ; Set when in motor start aquisition mode
|
|
.equ INITIAL_RUN_MODE = 7 ; Set when in initial rotations of run mode
|
|
|
|
.def Flags1 = r24 ; State flags
|
|
.equ COMP_STORED = 0 ; If set, comparator output was high in the last PWM cycle (evaluated in the PWM on period)
|
|
.equ PWM_ON = 1 ; Set in on part of pwm cycle
|
|
.equ PWM_OFF_DAMPED = 2 ; Set when pfets shall be on in pwm_off period
|
|
.equ RCP_MEAS_PWM_FREQ = 3 ; Measure RC pulse pwm frequency
|
|
;.equ = 4
|
|
;.equ = 5
|
|
;.equ = 6
|
|
;.equ = 7
|
|
|
|
.def Flags2 = r25 ; State flags
|
|
.equ RCP_UPDATED = 0 ; New RC pulse length value available
|
|
.equ RCP_EDGE_NO = 1 ; RC pulse edge no. 0=first, 1=second
|
|
.equ RCP_PWM_FREQ_1KHZ = 2 ; RC pulse pwm frequency is 1kHz
|
|
.equ RCP_PWM_FREQ_2KHZ = 3 ; RC pulse pwm frequency is 2kHz
|
|
.equ RCP_PWM_FREQ_4KHZ = 4 ; RC pulse pwm frequency is 4kHz
|
|
.equ RCP_PWM_FREQ_8KHZ = 5 ; RC pulse pwm frequency is 8kHz
|
|
.equ PGM_PWM_HIGH_FREQ = 6 ; Programmed pwm frequency. 0=low, 1=high
|
|
.equ PGM_RCP_PWM_POL = 7 ; Programmed RC pulse pwm polarity. 0=positive, 1=negative
|
|
|
|
; Here the general temporary register XYZ are placed (r26-r31)
|
|
|
|
; X: General temporary
|
|
; Y: General temporary
|
|
; Z: Interrupt-accessed address of current PWM FET ON routine (eg: pwm_afet_on)
|
|
|
|
|
|
;**** **** **** **** ****
|
|
; RAM definitions
|
|
.dseg ; Data segment
|
|
.org SRAM_START
|
|
|
|
Startup_Rot_Cnt: .byte 1 ; Startup mode rotations counter (decrementing)
|
|
|
|
Prev_Comm_L: .byte 1 ; Previous commutation timer1 timestamp (lo byte)
|
|
Prev_Comm_H: .byte 1 ; Previous commutation timer1 timestamp (hi byte)
|
|
Comm_Period4x_L: .byte 1 ; Timer1 counts between the last 4 commutations (lo byte)
|
|
Comm_Period4x_H: .byte 1 ; Timer1 counts between the last 4 commutations (hi byte)
|
|
|
|
Gov_Target_L: .byte 1 ; Governor target (lo byte)
|
|
Gov_Target_H: .byte 1 ; Governor target (hi byte)
|
|
Gov_Integral_L: .byte 1 ; Governor integral error (lo byte)
|
|
Gov_Integral_H: .byte 1 ; Governor integral error (hi byte)
|
|
Gov_Integral_X: .byte 1 ; Governor integral error (ex byte)
|
|
Gov_Proportional_L: .byte 1 ; Governor proportional error (lo byte)
|
|
Gov_Proportional_H: .byte 1 ; Governor proportional error (hi byte)
|
|
Gov_Prop_Pwm: .byte 1 ; Governor calculated new pwm based upon proportional error
|
|
Gov_Arm_Target: .byte 1 ; Governor arm target value
|
|
Gov_Active: .byte 1 ; Governor active (enabled and speed above minimum)
|
|
|
|
Wt_Advance_L: .byte 1 ; Timer1 counts for commutation advance timing (lo byte)
|
|
Wt_Advance_H: .byte 1 ; Timer1 counts for commutation advance timing (hi byte)
|
|
Wt_Zc_Scan_L: .byte 1 ; Timer1 counts from commutation to zero cross scan (lo byte)
|
|
Wt_Zc_Scan_H: .byte 1 ; Timer1 counts from commutation to zero cross scan (hi byte)
|
|
Wt_Comm_L: .byte 1 ; Timer1 counts from zero cross to commutation (lo byte)
|
|
Wt_Comm_H: .byte 1 ; Timer1 counts from zero cross to commutation (hi byte)
|
|
Wt_Stepper_Step_L: .byte 1 ; Timer1 counts for stepper step (lo byte)
|
|
Wt_Stepper_Step_H: .byte 1 ; Timer1 counts for stepper step (hi byte)
|
|
|
|
Rcp_PrePrev_Edge_L: .byte 1 ; RC pulse pre previous edge timer1 timestamp (lo byte)
|
|
Rcp_PrePrev_Edge_H: .byte 1 ; RC pulse pre previous edge timer1 timestamp (hi byte)
|
|
Rcp_Edge_L: .byte 1 ; RC pulse edge timer1 timestamp (lo byte)
|
|
Rcp_Edge_H: .byte 1 ; RC pulse edge timer1 timestamp (hi byte)
|
|
New_Rcp: .byte 1 ; New RC pulse value in timer1 counts
|
|
Prev_Rcp: .byte 1 ; Previous RC pulse value in timer1 counts
|
|
Prev_Rcp_Pwm_Freq: .byte 1 ; Previous RC pulse pwm frequency (used during pwm frequency measurement)
|
|
Rcp_Stop_Cnt: .byte 1 ; Counter for RC pulses below stop value
|
|
|
|
Pwm_Limit: .byte 1 ; Maximum allowed pwm
|
|
Lipo_Cells: .byte 1 ; Number of lipo cells
|
|
Lipo_Adc_Limit_L: .byte 1 ; Low voltage limit adc minimum (lo byte)
|
|
Lipo_Adc_Limit_H: .byte 1 ; Low voltage limit adc minimum (hi byte)
|
|
|
|
Tx_Pgm_Func_No: .byte 1 ; Function number when doing programming by tx
|
|
Tx_Pgm_Paraval_No: .byte 1 ; Parameter value number when doing programming by tx
|
|
Tx_Pgm_Beep_No: .byte 1 ; Beep number when doing programming by tx
|
|
|
|
Pgm_Gov_P_Gain: .byte 1 ; Programmed governor P gain
|
|
Pgm_Gov_I_Gain: .byte 1 ; Programmed governor I gain
|
|
Pgm_Gov_Mode: .byte 1 ; Programmed governor mode
|
|
Pgm_Tail_Gain: .byte 1 ; Programmed tail gain
|
|
Pgm_Tail_Idle: .byte 1 ; Programmed tail idle speed
|
|
Pgm_Startup_Pwr: .byte 1 ; Programmed startup power
|
|
Pgm_Pwm_Freq: .byte 1 ; Programmed pwm frequency
|
|
|
|
.equ SRAM_BYTES = 100 ; Bytes available in SRAM. Used for number of bytes to reset
|
|
|
|
|
|
;**** **** **** **** ****
|
|
.eseg ; Eeprom segment
|
|
.org 0
|
|
Eep_Pgm_Gov_P_Gain: .byte 1 ; EEPROM copy of programmed governor P gain
|
|
Eep_Pgm_Gov_I_Gain: .byte 1 ; EEPROM copy of programmed governor I gain
|
|
Eep_Pgm_Gov_Mode: .byte 1 ; EEPROM copy of programmed governor mode
|
|
Eep_Pgm_Tail_Gain: .byte 1 ; EEPROM copy of programmed tail gain
|
|
Eep_Pgm_Tail_Idle: .byte 1 ; EEPROM copy of programmed tail idle speed
|
|
Eep_Pgm_Startup_Pwr: .byte 1 ; EEPROM copy of programmed startup power
|
|
Eep_Pgm_Pwm_Freq: .byte 1 ; EEPROM copy of programmed pwm frequency
|
|
Eep_Pgm_Input_Pol: .byte 1 ; EEPROM copy of programmed input polarity
|
|
Eep_Initialized_L: .byte 1 ; EEPROM initialized signature low byte
|
|
Eep_Initialized_H: .byte 1 ; EEPROM initialized signature high byte
|
|
|
|
;**** **** **** **** ****
|
|
.cseg ; Code segment
|
|
.org 0
|
|
Interrupt_Table_Definition ; ATmega interrupts
|
|
|
|
;**** **** **** **** ****
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; External interrupt 0 routine
|
|
;
|
|
; No assumptions
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
ext_int0:
|
|
.if ICP == 0
|
|
in I_Sreg, SREG
|
|
; Get timer1 values
|
|
Read_TCNT1L I_Temp1
|
|
Read_TCNT1H I_Temp2
|
|
; Check which edge it is
|
|
sbrc Flags2, RCP_EDGE_NO ; Was it a first edge trig?
|
|
rjmp rcpint_second_meas_pwm_freq ; No - branch to second
|
|
|
|
Rcp_Int_Second I_Temp3 ; Yes - set second edge trig
|
|
sbr Flags2, (1<<RCP_EDGE_NO) ; Set second edge flag
|
|
; Read RC signal level
|
|
Read_Rcp_Int I_Temp5
|
|
; Test RC signal level
|
|
sbrs I_Temp5, Rcp_In ; Is it high?
|
|
rjmp rcpint_fail_minimum ; No - jump to fail minimum
|
|
|
|
; RC pulse was high, store RC pulse start timestamp
|
|
mov Rcp_Prev_Edge_L, I_Temp1
|
|
mov Rcp_Prev_Edge_H, I_Temp2
|
|
rjmp rcpint_exit ; Exit
|
|
|
|
rcpint_fail_minimum:
|
|
; Prepare for next interrupt
|
|
Rcp_Int_First I_Temp3 ; Set interrupt trig to first again
|
|
Clear_Int_Flag I_Temp3 ; Clear interrupt flag
|
|
cbr Flags2, (1<<RCP_EDGE_NO) ; Set first edge flag
|
|
ldi I_Temp1, RCP_MIN ; Set RC pulse value to minimum
|
|
ldi I_Temp2, RCP_MIN
|
|
Read_Rcp_Int I_Temp5 ; Test RC signal level again
|
|
sbrc I_Temp5, Rcp_In ; Is it high?
|
|
rjmp rcpint_set_timeout ; Yes - set new timeout and exit
|
|
|
|
sts Prev_Rcp, I_Temp2 ; Store previous pulse length
|
|
sts New_Rcp, I_Temp1 ; Store new pulse length
|
|
rjmp rcpint_set_timeout ; Set new timeout and exit
|
|
|
|
rcpint_second_meas_pwm_freq:
|
|
; Prepare for next interrupt
|
|
Rcp_Int_First I_Temp3 ; Set first edge trig
|
|
cbr Flags2, (1<<RCP_EDGE_NO) ; Set first edge flag
|
|
; Check if pwm frequency shall be measured
|
|
sbrs Flags1, RCP_MEAS_PWM_FREQ ; Is measure RCP pwm frequency flag set?
|
|
rjmp rcpint_fall ; No - skip measurements
|
|
|
|
; Set second edge trig only during pwm frequency measurement
|
|
Rcp_Int_Second I_Temp3 ; Set second edge trig
|
|
Clear_Int_Flag I_Temp3 ; Clear interrupt flag
|
|
sbr Flags2, (1<<RCP_EDGE_NO) ; Set second edge flag
|
|
; Store edge data to RAM
|
|
sts Rcp_Edge_L, I_Temp1
|
|
sts Rcp_Edge_H, I_Temp2
|
|
; Calculate pwm frequency
|
|
lds I_Temp3, Rcp_PrePrev_Edge_L
|
|
lds I_Temp4, Rcp_PrePrev_Edge_H
|
|
sub I_Temp1, I_Temp3
|
|
sbc I_Temp2, I_Temp4
|
|
mov I_Temp4, Zero
|
|
; Check if pwm frequency is 8kHz
|
|
cpi I_Temp1, low(180) ; If below 180us, 8kHz pwm is assumed
|
|
ldi I_Temp3, high(180)
|
|
cpc I_Temp2, I_Temp3
|
|
brcc rcpint_check_4kHz
|
|
|
|
clr I_Temp3
|
|
sbr I_Temp3, (1<<RCP_PWM_FREQ_8KHZ)
|
|
mov I_Temp4, I_Temp3
|
|
rjmp rcpint_restore_edge
|
|
|
|
rcpint_check_4kHz:
|
|
; Check if pwm frequency is 4kHz
|
|
cpi I_Temp1, low(360) ; If below 360us, 4kHz pwm is assumed
|
|
ldi I_Temp3, high(360)
|
|
cpc I_Temp2, I_Temp3
|
|
brcc rcpint_check_2kHz
|
|
|
|
clr I_Temp3
|
|
sbr I_Temp3, (1<<RCP_PWM_FREQ_4KHZ)
|
|
mov I_Temp4, I_Temp3
|
|
rjmp rcpint_restore_edge
|
|
|
|
rcpint_check_2kHz:
|
|
; Check if pwm frequency is 2kHz
|
|
cpi I_Temp1, low(720) ; If below 720us, 2kHz pwm is assumed
|
|
ldi I_Temp3, high(720)
|
|
cpc I_Temp2, I_Temp3
|
|
brcc rcpint_check_1kHz
|
|
|
|
clr I_Temp3
|
|
sbr I_Temp3, (1<<RCP_PWM_FREQ_2KHZ)
|
|
mov I_Temp4, I_Temp3
|
|
rjmp rcpint_restore_edge
|
|
|
|
rcpint_check_1kHz:
|
|
; Check if pwm frequency is 1kHz
|
|
cpi I_Temp1, low(1440) ; If below 1440us, 1kHz pwm is assumed
|
|
ldi I_Temp3, high(1440)
|
|
cpc I_Temp2, I_Temp3
|
|
brcc rcpint_restore_edge
|
|
|
|
clr I_Temp3
|
|
sbr I_Temp3, (1<<RCP_PWM_FREQ_1KHZ)
|
|
mov I_Temp4, I_Temp3
|
|
|
|
rcpint_restore_edge:
|
|
; Restore edge data from RAM
|
|
lds I_Temp1, Rcp_Edge_L
|
|
lds I_Temp2, Rcp_Edge_H
|
|
; Store pre previous edge
|
|
sts Rcp_PrePrev_Edge_L, I_Temp1
|
|
sts Rcp_PrePrev_Edge_H, I_Temp2
|
|
|
|
rcpint_fall:
|
|
; RC pulse edge was second, calculate new pulse length
|
|
sub I_Temp1, Rcp_Prev_Edge_L
|
|
sbc I_Temp2, Rcp_Prev_Edge_H
|
|
sbrc Flags2, RCP_PWM_FREQ_8KHZ ; Is RC input pwm frequency 8kHz?
|
|
rjmp rcpint_pwm_mult ; Yes - branch forward
|
|
|
|
sbrc Flags2, RCP_PWM_FREQ_4KHZ ; Is RC input pwm frequency 4kHz?
|
|
rjmp rcpint_pwm_divide_done ; Yes - branch forward
|
|
|
|
lsr I_Temp2 ; No - 2kHz. Divide by 2 again
|
|
ror I_Temp1
|
|
|
|
sbrc Flags2, RCP_PWM_FREQ_2KHZ ; Is RC input pwm frequency 2kHz?
|
|
rjmp rcpint_pwm_divide_done ; Yes - branch forward
|
|
|
|
lsr I_Temp2 ; No - 1kHz. Divide by 2 again
|
|
ror I_Temp1
|
|
rjmp rcpint_pwm_divide_done ; Yes - branch forward
|
|
|
|
rcpint_pwm_mult:
|
|
lsl I_Temp1
|
|
rol I_Temp2
|
|
|
|
rcpint_pwm_divide_done:
|
|
; Check that RC pulse is within legal range
|
|
cpi I_Temp1, RCP_MAX
|
|
cpc I_Temp2, Zero
|
|
brcs rcpint_limited
|
|
|
|
ldi I_Temp1, RCP_MAX
|
|
|
|
rcpint_limited:
|
|
lds I_Temp2, New_Rcp ; Load pulse length to be used as previous
|
|
; RC pulse value accepted
|
|
sts Prev_Rcp, I_Temp2 ; Store previous pulse length
|
|
sts New_Rcp, I_Temp1 ; Store new pulse length
|
|
sbrs Flags1, RCP_MEAS_PWM_FREQ ; Is measure RCP pwm frequency flag set?
|
|
rjmp rcpint_set_timeout ; No - skip measurements
|
|
|
|
ldi I_Temp3, (1<<RCP_PWM_FREQ_1KHZ)+(1<<RCP_PWM_FREQ_2KHZ)+(1<<RCP_PWM_FREQ_4KHZ)+(1<<RCP_PWM_FREQ_8KHZ)
|
|
com I_Temp3
|
|
and Flags2, I_Temp3 ; Clear all pwm frequency flags
|
|
or Flags2, I_Temp4 ; Store pwm frequency value in flags
|
|
|
|
rcpint_set_timeout:
|
|
ldi I_Temp1, RCP_TIMEOUT ; Set timeout count to start value
|
|
mov Rcp_Timeout_Cnt, I_Temp1
|
|
sbr Flags2, (1<<RCP_UPDATED) ; Set updated flag
|
|
sbrc Flags1, RCP_MEAS_PWM_FREQ ; Is measure RCP pwm frequency flag set?
|
|
rjmp rcpint_exit ; Yes - exit
|
|
|
|
Rcp_Int_Disable I_Temp2 ; Disable RC pulse interrupt
|
|
|
|
rcpint_exit: ; Exit interrupt routine
|
|
ldi I_Temp2, RCP_SKIP_RATE ; Load number of skips
|
|
mov Rcp_Skip_Cnt, I_Temp2
|
|
out SREG, I_Sreg
|
|
.endif
|
|
reti
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Input capture pin1 interrupt routine
|
|
;
|
|
; No assumptions
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
icp1_int:
|
|
.if ICP == 1
|
|
in I_Sreg, SREG
|
|
; Get timer1 ICP values
|
|
Read_ICR1L I_Temp1
|
|
Read_ICR1H I_Temp2
|
|
; Check which edge it is
|
|
sbrc Flags2, RCP_EDGE_NO ; Was it a first edge trig?
|
|
rjmp rcpint_icp_second_meas_pwm_freq ; No - branch to second
|
|
|
|
Rcp_Icp_Int_Second I_Temp3 ; Yes - set second edge trig
|
|
sbr Flags2, (1<<RCP_EDGE_NO) ; Set second edge flag
|
|
; Read ICP RC signal level
|
|
Read_Rcp_Icp_Int I_Temp5
|
|
; Test RC signal level
|
|
sbrs I_Temp5, Rcp_Icp_In ; Is it high?
|
|
rjmp rcpint_icp_fail_minimum ; No - jump to fail minimum
|
|
|
|
; RC pulse was high, store RC pulse start timestamp
|
|
mov Rcp_Prev_Edge_L, I_Temp1
|
|
mov Rcp_Prev_Edge_H, I_Temp2
|
|
rjmp rcpint_icp_exit ; Exit
|
|
|
|
rcpint_icp_fail_minimum:
|
|
; Prepare for next interrupt
|
|
Rcp_Icp_Int_First I_Temp3 ; Set interrupt trig to first again
|
|
Clear_Icp_Int_Flag I_Temp3 ; Clear icp interrupt flag
|
|
cbr Flags2, (1<<RCP_EDGE_NO) ; Set first edge flag
|
|
ldi I_Temp1, RCP_MIN ; Set RC pulse value to minimum
|
|
ldi I_Temp2, RCP_MIN
|
|
Read_Rcp_Icp_Int I_Temp5 ; Test RC signal level again
|
|
sbrc I_Temp5, Rcp_Icp_In ; Is it high?
|
|
rjmp rcpint_icp_set_timeout ; Yes - set new timeout and exit
|
|
|
|
sts Prev_Rcp, I_Temp2 ; Store previous pulse length
|
|
sts New_Rcp, I_Temp1 ; Store new pulse length
|
|
rjmp rcpint_icp_set_timeout ; Set new timeout and exit
|
|
|
|
rcpint_icp_second_meas_pwm_freq:
|
|
; Prepare for next interrupt
|
|
Rcp_Icp_Int_First I_Temp3 ; Set first edge trig
|
|
cbr Flags2, (1<<RCP_EDGE_NO) ; Set first edge flag
|
|
; Check if pwm frequency shall be measured
|
|
sbrs Flags1, RCP_MEAS_PWM_FREQ ; Is measure RCP pwm frequency flag set?
|
|
rjmp rcpint_icp_fall ; No - skip measurements
|
|
|
|
; Set second edge trig only during pwm frequency measurement
|
|
Rcp_Icp_Int_Second I_Temp3 ; Set second edge trig
|
|
Clear_Icp_Int_Flag I_Temp3 ; Clear icp interrupt flag
|
|
sbr Flags2, (1<<RCP_EDGE_NO) ; Set second edge flag
|
|
; Store edge data to RAM
|
|
sts Rcp_Edge_L, I_Temp1
|
|
sts Rcp_Edge_H, I_Temp2
|
|
; Calculate pwm frequency
|
|
lds I_Temp3, Rcp_PrePrev_Edge_L
|
|
lds I_Temp4, Rcp_PrePrev_Edge_H
|
|
sub I_Temp1, I_Temp3
|
|
sbc I_Temp2, I_Temp4
|
|
mov I_Temp4, Zero
|
|
; Check if pwm frequency is 8kHz
|
|
cpi I_Temp1, low(180) ; If below 180us, 8kHz pwm is assumed
|
|
ldi I_Temp3, high(180)
|
|
cpc I_Temp2, I_Temp3
|
|
brcc rcpint_icp_check_4kHz
|
|
|
|
clr I_Temp3
|
|
sbr I_Temp3, (1<<RCP_PWM_FREQ_8KHZ)
|
|
mov I_Temp4, I_Temp3
|
|
rjmp rcpint_icp_restore_edge
|
|
|
|
rcpint_icp_check_4kHz:
|
|
; Check if pwm frequency is 4kHz
|
|
cpi I_Temp1, low(360) ; If below 360us, 4kHz pwm is assumed
|
|
ldi I_Temp3, high(360)
|
|
cpc I_Temp2, I_Temp3
|
|
brcc rcpint_icp_check_2kHz
|
|
|
|
clr I_Temp3
|
|
sbr I_Temp3, (1<<RCP_PWM_FREQ_4KHZ)
|
|
mov I_Temp4, I_Temp3
|
|
rjmp rcpint_icp_restore_edge
|
|
|
|
rcpint_icp_check_2kHz:
|
|
; Check if pwm frequency is 2kHz
|
|
cpi I_Temp1, low(720) ; If below 720us, 2kHz pwm is assumed
|
|
ldi I_Temp3, high(720)
|
|
cpc I_Temp2, I_Temp3
|
|
brcc rcpint_icp_check_1kHz
|
|
|
|
clr I_Temp3
|
|
sbr I_Temp3, (1<<RCP_PWM_FREQ_2KHZ)
|
|
mov I_Temp4, I_Temp3
|
|
rjmp rcpint_icp_restore_edge
|
|
|
|
rcpint_icp_check_1kHz:
|
|
; Check if pwm frequency is 1kHz
|
|
cpi I_Temp1, low(1440) ; If below 1440us, 1kHz pwm is assumed
|
|
ldi I_Temp3, high(1440)
|
|
cpc I_Temp2, I_Temp3
|
|
brcc rcpint_icp_restore_edge
|
|
|
|
clr I_Temp3
|
|
sbr I_Temp3, (1<<RCP_PWM_FREQ_1KHZ)
|
|
mov I_Temp4, I_Temp3
|
|
|
|
rcpint_icp_restore_edge:
|
|
; Restore edge data from RAM
|
|
lds I_Temp1, Rcp_Edge_L
|
|
lds I_Temp2, Rcp_Edge_H
|
|
; Store pre previous edge
|
|
sts Rcp_PrePrev_Edge_L, I_Temp1
|
|
sts Rcp_PrePrev_Edge_H, I_Temp2
|
|
|
|
rcpint_icp_fall:
|
|
; RC pulse edge was second, calculate new pulse length
|
|
sub I_Temp1, Rcp_Prev_Edge_L
|
|
sbc I_Temp2, Rcp_Prev_Edge_H
|
|
sbrc Flags2, RCP_PWM_FREQ_8KHZ ; Is RC input pwm frequency 8kHz?
|
|
rjmp rcpint_icp_pwm_mult ; Yes - branch forward
|
|
|
|
sbrc Flags2, RCP_PWM_FREQ_4KHZ ; Is RC input pwm frequency 4kHz?
|
|
rjmp rcpint_icp_pwm_divide_done ; Yes - branch forward
|
|
|
|
lsr I_Temp2 ; No - 2kHz. Divide by 2 again
|
|
ror I_Temp1
|
|
|
|
sbrc Flags2, RCP_PWM_FREQ_2KHZ ; Is RC input pwm frequency 2kHz?
|
|
rjmp rcpint_icp_pwm_divide_done ; Yes - branch forward
|
|
|
|
lsr I_Temp2 ; No - 1kHz. Divide by 2 again
|
|
ror I_Temp1
|
|
rjmp rcpint_icp_pwm_divide_done
|
|
|
|
rcpint_icp_pwm_mult:
|
|
lsl I_Temp1 ; Multiply by 2
|
|
rol I_Temp2
|
|
|
|
rcpint_icp_pwm_divide_done:
|
|
; Check that RC pulse is within legal range
|
|
cpi I_Temp1, RCP_MAX
|
|
cpc I_Temp2, Zero
|
|
brcs rcpint_icp_limited
|
|
|
|
ldi I_Temp1, RCP_MAX
|
|
|
|
rcpint_icp_limited:
|
|
lds I_Temp2, New_Rcp ; Load pulse length to be used as previous
|
|
; RC pulse value accepted
|
|
sts Prev_Rcp, I_Temp2 ; Store previous pulse length
|
|
sts New_Rcp, I_Temp1 ; Store new pulse length
|
|
sbrs Flags1, RCP_MEAS_PWM_FREQ ; Is measure RCP pwm frequency flag set?
|
|
rjmp rcpint_icp_set_timeout ; No - skip measurements
|
|
|
|
ldi I_Temp3, (1<<RCP_PWM_FREQ_1KHZ)+(1<<RCP_PWM_FREQ_2KHZ)+(1<<RCP_PWM_FREQ_4KHZ)+(1<<RCP_PWM_FREQ_8KHZ)
|
|
com I_Temp3
|
|
and Flags2, I_Temp3 ; Clear all pwm frequency flags
|
|
or Flags2, I_Temp4 ; Store pwm frequency value in flags
|
|
|
|
rcpint_icp_set_timeout:
|
|
ldi I_Temp1, RCP_TIMEOUT ; Set timeout count to start value
|
|
mov Rcp_Timeout_Cnt, I_Temp1
|
|
sbr Flags2, (1<<RCP_UPDATED) ; Set updated flag
|
|
sbrc Flags1, RCP_MEAS_PWM_FREQ ; Is measure RCP pwm frequency flag set?
|
|
rjmp rcpint_icp_exit ; Yes - exit
|
|
|
|
Rcp_Icp_Int_Disable I_Temp2 ; Disable RC pulse interrupt
|
|
|
|
rcpint_icp_exit: ; Exit interrupt routine
|
|
ldi I_Temp2, RCP_SKIP_RATE ; Load number of skips
|
|
mov Rcp_Skip_Cnt, I_Temp2
|
|
out SREG, I_Sreg
|
|
.endif
|
|
reti
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Timer 0 overflow interrupt
|
|
;
|
|
; No assumptions
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
t0ovfl_int: ; Happens every 256µs
|
|
in I_Sreg, SREG
|
|
; Check RC pulse timeout counter
|
|
tst Rcp_Timeout_Cnt ; RC pulse timeout count zero?
|
|
breq t0ovfl_pulses_absent ; Yes - pulses are absent
|
|
|
|
; Decrement timeout counter
|
|
dec Rcp_Timeout_Cnt ; Decrement
|
|
rjmp t0ovfl_skip_start
|
|
|
|
t0ovfl_pulses_absent:
|
|
; Timeout counter has reached zero, pulses are absent
|
|
ldi I_Temp1, RCP_MIN ; RCP_MIN as default
|
|
ldi I_Temp2, RCP_MIN
|
|
.if ICP == 0
|
|
Read_Rcp_Int I_Temp3 ; Look at value of Rcp_In
|
|
sbrc I_Temp3, Rcp_In ; Is it high?
|
|
ldi I_Temp1, RCP_MAX ; Yes - set RCP_MAX
|
|
Rcp_Int_First I_Temp3 ; Set interrupt trig to first again
|
|
Clear_Int_Flag I_Temp3 ; Clear interrupt flag
|
|
cbr Flags2, (1<<RCP_EDGE_NO) ; Set first edge flag
|
|
Read_Rcp_Int I_Temp3 ; Look once more at value of Rcp_In
|
|
sbrc I_Temp3, Rcp_In ; Is it high?
|
|
ldi I_Temp2, RCP_MAX ; Yes - set RCP_MAX
|
|
cp I_Temp1, I_Temp2 ; Compare the two readings of Rcp_In
|
|
brne t0ovfl_pulses_absent ; Go back if they are not equal
|
|
.else
|
|
Read_Rcp_Icp_Int I_Temp3 ; Look at value of Rcp_Icp_In
|
|
sbrc I_Temp3, Rcp_Icp_In ; Is it high?
|
|
ldi I_Temp1, RCP_MAX ; Yes - set RCP_MAX
|
|
Rcp_Icp_Int_First I_Temp3 ; Set interrupt trig to first again
|
|
Clear_Icp_Int_Flag I_Temp3 ; Clear icp interrupt flag
|
|
cbr Flags2, (1<<RCP_EDGE_NO) ; Set first edge flag
|
|
Read_Rcp_Icp_Int I_Temp3 ; Look once more at value of Rcp_In
|
|
sbrc I_Temp3, Rcp_Icp_In ; Is it high?
|
|
ldi I_Temp2, RCP_MAX ; Yes - set RCP_MAX
|
|
cp I_Temp1, I_Temp2 ; Compare the two readings of Rcp_In
|
|
brne t0ovfl_pulses_absent ; Go back if they are not equal
|
|
.endif
|
|
ldi I_Temp2, RCP_TIMEOUT ; Set timeout count to start value
|
|
mov Rcp_Timeout_Cnt, I_Temp2
|
|
sts Prev_Rcp, I_Temp1 ; Store previous pulse length
|
|
sts New_Rcp, I_Temp1 ; Store new pulse length
|
|
sbr Flags2, (1<<RCP_UPDATED) ; Set updated flag
|
|
|
|
t0ovfl_skip_start:
|
|
; Check RC pulse skip counter
|
|
tst Rcp_Skip_Cnt ; RC pulse skip count zero?
|
|
breq t0ovfl_skip_end ; Yes - end skipping RC pulse detection
|
|
|
|
; Decrement skip counter (only if edge counter is zero)
|
|
dec Rcp_Skip_Cnt ; Decrement
|
|
rjmp t0ovfl_rcp_update_start
|
|
|
|
t0ovfl_skip_end:
|
|
; Skip counter has reached zero, start looking for RC pulses again
|
|
.if ICP == 0
|
|
Rcp_Int_Enable I_Temp2 ; Enable RC pulse interrupt
|
|
Clear_Int_Flag I_Temp2 ; Clear interrupt flag
|
|
.else
|
|
Rcp_Icp_Int_Enable I_Temp2 ; Enable ICP RC pulse interrupt
|
|
Clear_Icp_Int_Flag I_Temp2 ; Clear icp interrupt flag
|
|
.endif
|
|
|
|
t0ovfl_rcp_update_start:
|
|
; Process updated RC pulse
|
|
sbrs Flags2, RCP_UPDATED ; Is there an updated RC pulse available?
|
|
rjmp t0ovfl_pwm_exit ; No - exit
|
|
|
|
lds I_Temp2, Prev_Rcp ; Load previous pulse value
|
|
lds I_Temp1, New_Rcp ; Load new pulse value
|
|
sbrc Flags2, RCP_PWM_FREQ_8KHZ ; Is RC input pwm frequency 8kHz?
|
|
add I_Temp1, I_Temp2 ; Yes - add the two pulse values
|
|
sbrc Flags2, RCP_PWM_FREQ_8KHZ ; Is RC input pwm frequency 8kHz?
|
|
ror I_Temp1 ; Yes - divide by 2 to compensate for the add
|
|
cbr Flags2, (1<<RCP_UPDATED) ; Flag that pulse has been evaluated
|
|
; Limit the maximum value to avoid wrap when scaled to pwm range
|
|
.if TAIL == 1
|
|
cpi I_Temp1, 240 ; 240 = (255/1.0625) Needs to be updated according to multiplication factor below
|
|
brlo t0ovfl_rcp_update_mult
|
|
|
|
ldi I_Temp1, 240 ; Set requested pwm to max
|
|
|
|
.endif
|
|
t0ovfl_rcp_update_mult:
|
|
.if TAIL == 1
|
|
; Multiply by 1.0625 (optional adjustment gyro gain)
|
|
mov I_Temp2, I_Temp1
|
|
swap I_Temp2 ; After this "0.0625"
|
|
andi I_Temp2, 0x0f
|
|
add I_Temp1, I_Temp2
|
|
; Adjust tail gain
|
|
lds I_Temp3, Pgm_Tail_Gain
|
|
cpi I_Temp3, 3 ; Is gain 1?
|
|
breq t0ovfl_pwm_min_run ; Yes - skip adjustment
|
|
|
|
mov I_Temp2, I_Temp1
|
|
lsr I_Temp2 ; After this "0.5"
|
|
lsr I_Temp2 ; After this "0.25"
|
|
sbrc I_Temp3, 0 ; Is bit 0 in gain set?
|
|
rjmp t0ovfl_rcp_tail_corr ; Yes - two gain correction steps
|
|
|
|
lsr I_Temp2 ; After this "0.125"
|
|
|
|
t0ovfl_rcp_tail_corr:
|
|
sbrc I_Temp3, 2 ; Is bit 2 in gain set?
|
|
rjmp t0ovfl_rcp_tail_gain_pos ; Yes - positive correction
|
|
|
|
sub I_Temp1, I_Temp2 ; Apply negative correction
|
|
rjmp t0ovfl_pwm_min_run
|
|
|
|
t0ovfl_rcp_tail_gain_pos:
|
|
add I_Temp1, I_Temp2 ; Apply positive correction
|
|
brcc t0ovfl_pwm_min_run ; Above max?
|
|
|
|
ldi I_Temp1, 0xff ; Yes - limit
|
|
.endif
|
|
|
|
t0ovfl_pwm_min_run:
|
|
; Limit minimum pwm
|
|
lds I_Temp2, Pgm_Tail_Idle ; Is requested pwm lower than minimum?
|
|
cp I_Temp1, I_Temp2
|
|
brcc t0ovfl_pwm_update ; No - branch
|
|
|
|
lds I_Temp1, Pgm_Tail_Idle ; Yes - limit pwm to Pgm_Tail_Idle
|
|
|
|
t0ovfl_pwm_update:
|
|
; Check if any startup mode flags are set
|
|
mov I_Temp2, Flags0
|
|
andi I_Temp2, (1<<SETTLE_MODE)+(1<<STEPPER_MODE)+(1<<AQUISITION_MODE)+(1<<INITIAL_RUN_MODE)
|
|
tst I_Temp2 ; Any startup mode set?
|
|
brne t0ovfl_pwm_exit ; Yes - exit (pwm controlled by set_startup_pwm)
|
|
|
|
; Update requested_pwm
|
|
mov Requested_Pwm, I_Temp1 ; Set requested pwm
|
|
|
|
lds I_Temp1, Pgm_Gov_Mode ; Governor mode?
|
|
cpi I_Temp1, 3
|
|
brcs t0ovfl_pwm_exit ; Yes - branch
|
|
|
|
; Update current pwm
|
|
mov Current_Pwm, Requested_Pwm
|
|
|
|
t0ovfl_pwm_exit:
|
|
out SREG, I_Sreg
|
|
reti
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Timer 1 output compare A interrupt
|
|
;
|
|
; No assumptions
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
t1oca_int:
|
|
in I_Sreg, SREG
|
|
cbr Flags0, (1<<OCA_PENDING) ; Flag that OCA value is passed
|
|
out SREG, I_Sreg
|
|
reti
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Timer 1 overflow interrupt
|
|
;
|
|
; No assumptions
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
t1ovfl_int: ; Happens every 65536µs
|
|
in I_Sreg, SREG
|
|
ldi I_Temp2, GOV_SPOOLRATE ; Load governor spool rate
|
|
; Check RC pulse against stop value
|
|
lds I_Temp1, New_Rcp ; Load new pulse value
|
|
subi I_Temp1, RCP_STOP ; Check if pulse is below stop value
|
|
brcs t1ovfl_rcp_stop
|
|
|
|
; RC pulse higher than stop value, reset stop counter
|
|
sts Rcp_Stop_Cnt, Zero ; Reset rcp stop counter
|
|
rjmp t1ovfl_rcp_gov_pwm
|
|
|
|
t1ovfl_rcp_stop:
|
|
; RC pulse less than stop value, increment stop counter
|
|
lds I_Temp1, Rcp_Stop_Cnt ; Load rcp stop counter
|
|
cpi I_Temp1, 255 ; Check if counter is max
|
|
breq t1ovfl_rcp_gov_pwm ; Branch if counter is equal to max
|
|
|
|
lds I_Temp1, Rcp_Stop_Cnt ; Increment stop counter
|
|
inc I_Temp1
|
|
sts Rcp_Stop_Cnt, I_Temp1
|
|
|
|
t1ovfl_rcp_gov_pwm:
|
|
.if TAIL == 0
|
|
lds I_Temp3, Pgm_Gov_Mode ; Governor target by arm mode?
|
|
cpi I_Temp3, 2
|
|
brne t1ovfl_rcp_gov_by_tx ; No - branch
|
|
|
|
lds Requested_Pwm, Gov_Arm_Target ; Yes - load arm target
|
|
|
|
t1ovfl_rcp_gov_by_tx:
|
|
cp Governor_Req_Pwm, Requested_Pwm ; Is governor requested pwm equal to requested pwm?
|
|
breq t1ovfl_rcp_exit; ; Yes - branch
|
|
|
|
brlo t1ovfl_rcp_gov_pwm_inc ; No - if lower then increment
|
|
|
|
dec Governor_Req_Pwm ; No - if higher then decrement
|
|
rjmp t1ovfl_rcp_gov_pwm_exit
|
|
|
|
t1ovfl_rcp_gov_pwm_inc:
|
|
inc Governor_Req_Pwm ; Increment
|
|
|
|
t1ovfl_rcp_gov_pwm_exit:
|
|
dec I_Temp2 ; Number of steps processed
|
|
brne t1ovfl_rcp_gov_pwm ; No - go back
|
|
.endif
|
|
|
|
t1ovfl_rcp_exit:
|
|
out SREG, I_Sreg
|
|
reti
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Timer 2 overflow interrupt
|
|
;
|
|
; Assumptions: Z register must be set to desired pwm_nfet_on label
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
t2ovfl_int:
|
|
in I_Sreg, SREG ; Store flags
|
|
; Check if second timer is zero
|
|
cp Pwm_Timer_Second, Zero ; Is second timer zero?
|
|
breq t2ovfl_int_execute ; Yes - execute pwm on/off
|
|
|
|
Set_TCNT2 Pwm_Timer_Second ; No - set counter to second timer
|
|
clr Pwm_Timer_Second ; Set second to zero
|
|
out SREG, I_Sreg ; Exit
|
|
reti
|
|
|
|
t2ovfl_int_execute:
|
|
sbrs Flags1, PWM_ON ; Is pwm on?
|
|
ijmp ; No - jump to pwm on routines. Z should be set to one of the pwm_nfet_on labels
|
|
|
|
; Pwm off cycle. Set timer2 for coming off cycle length
|
|
mov I_Temp1, Current_Pwm_Limited ; Load new timer setting
|
|
sec ; Set carry
|
|
sbrs Flags2, PGM_PWM_HIGH_FREQ ; High pwm frequency?
|
|
lsl I_Temp1 ; No - multiply by 2
|
|
brcs t2ovfl_int_off_no_second ; More than 256? - branch
|
|
|
|
mov Pwm_Timer_Second, I_Temp1 ; Set second timer
|
|
ldi I_Temp1, 0 ; Set next timer wait to max
|
|
|
|
t2ovfl_int_off_no_second:
|
|
; Write start point for timer2
|
|
Set_TCNT2 I_Temp1
|
|
; Read comparator output at the end of pwm on
|
|
Read_Comp_Out I_Temp2
|
|
; Clear pwm on flag
|
|
cbr Flags1, (1<<PWM_ON)
|
|
; Set full PWM (on all the time) if current PWM near max. This will give full power, but at the cost of a small "jump" in power
|
|
mov I_Temp3, Current_Pwm_Limited ; Load current pwm
|
|
cpi I_Temp3, 250 ; Above full pwm?
|
|
brsh t2ovfl_pwm_off_exit ; Yes - exit
|
|
All_nFETs_Off I_Temp1 ; No - switch off all nfets
|
|
|
|
; If damped operation, set all pmoses on in pwm_off
|
|
sbrs Flags1, PWM_OFF_DAMPED ; Damped operation?
|
|
rjmp t2ovfl_pwm_off_exit ; No - exit
|
|
|
|
; Delay to allow nFETs to go off before pFETs are turned on (only in damped mode)
|
|
ldi I_Temp1, 2
|
|
all_pfets_on_delay:
|
|
dec I_Temp1
|
|
brne all_pfets_on_delay
|
|
All_pFETs_On I_Temp1 ; Switch on all pfets
|
|
; Test comparator output at the end of pwm on (COMP_STORED is only used in damped operation)
|
|
sbr Flags1, (1<<COMP_STORED) ; Set comparator to high
|
|
sbrs I_Temp2, ACO ; Comparator output high?
|
|
cbr Flags1, (1<<COMP_STORED) ; No - clear comp_out
|
|
|
|
t2ovfl_pwm_off_exit: ; Exit from pwm off cycle
|
|
out SREG, I_Sreg
|
|
reti
|
|
|
|
pwm_nofet_on: ; Dummy pwm on cycle
|
|
sbr Flags1, (1<<PWM_ON) ; Set pwm on flag
|
|
rjmp t2ovfl_pwm_on_exit
|
|
|
|
pwm_afet_on: ; Pwm on cycle afet on (bfet off)
|
|
BnFET_off
|
|
AnFET_on
|
|
sbr Flags1, (1<<PWM_ON) ; Set pwm on flag
|
|
rjmp t2ovfl_pwm_on_exit
|
|
|
|
pwm_bfet_on: ; Pwm on cycle bfet on (cfet off)
|
|
CnFET_off
|
|
BnFET_on
|
|
sbr Flags1, (1<<PWM_ON) ; Set pwm on flag
|
|
rjmp t2ovfl_pwm_on_exit
|
|
|
|
pwm_cfet_on: ; Pwm on cycle cfet on (afet off)
|
|
AnFET_off
|
|
CnFET_on
|
|
sbr Flags1, (1<<PWM_ON) ; Set pwm on flag
|
|
rjmp t2ovfl_pwm_on_exit
|
|
|
|
pwm_anfet_bpfet_on: ; Pwm on cycle anfet on (bnfet off) and bpfet on (used in damped state 6)
|
|
; Delay from pFETs are turned off (only in damped mode) until nFET is turned on (pFETs are slow)
|
|
ldi I_Temp1, 15
|
|
an_bp:
|
|
ApFET_off
|
|
CpFET_off
|
|
dec I_Temp1
|
|
brne an_bp
|
|
BnFET_off ; Switch nFETs
|
|
AnFET_on
|
|
sbr Flags1, (1<<PWM_ON) ; Set pwm on flag
|
|
rjmp t2ovfl_pwm_on_exit
|
|
|
|
pwm_anfet_cpfet_on: ; Pwm on cycle anfet on (bnfet off) and cpfet on (used in damped state 5)
|
|
; Delay from pFETs are turned off (only in damped mode) until nFET is turned on (pFETs are slow)
|
|
ldi I_Temp1, 15
|
|
an_cp:
|
|
ApFET_off
|
|
BpFET_off
|
|
dec I_Temp1
|
|
brne an_cp
|
|
BnFET_off ; Switch nFETs
|
|
AnFET_on
|
|
sbr Flags1, (1<<PWM_ON) ; Set pwm on flag
|
|
rjmp t2ovfl_pwm_on_exit
|
|
|
|
pwm_bnfet_cpfet_on: ; Pwm on cycle bnfet on (cnfet off) and cpfet on (used in damped state 4)
|
|
; Delay from pFETs are turned off (only in damped mode) until nFET is turned on (pFETs are slow)
|
|
ldi I_Temp1, 15
|
|
bn_cp:
|
|
BpFET_off
|
|
ApFET_off
|
|
dec I_Temp1
|
|
brne bn_cp
|
|
CnFET_off ; Switch nFETs
|
|
BnFET_on
|
|
sbr Flags1, (1<<PWM_ON) ; Set pwm on flag
|
|
rjmp t2ovfl_pwm_on_exit
|
|
|
|
pwm_bnfet_apfet_on: ; Pwm on cycle bnfet on (cnfet off) and apfet on (used in damped state 3)
|
|
; Delay from pFETs are turned off (only in damped mode) until nFET is turned on (pFETs are slow)
|
|
ldi I_Temp1, 15
|
|
bn_ap:
|
|
BpFET_off
|
|
CpFET_off
|
|
dec I_Temp1
|
|
brne bn_ap
|
|
CnFET_off ; Switch nFETs
|
|
BnFET_on
|
|
sbr Flags1, (1<<PWM_ON) ; Set pwm on flag
|
|
rjmp t2ovfl_pwm_on_exit
|
|
|
|
pwm_cnfet_apfet_on: ; Pwm on cycle cnfet on (anfet off) and apfet on (used in damped state 2)
|
|
; Delay from pFETs are turned off (only in damped mode) until nFET is turned on (pFETs are slow)
|
|
ldi I_Temp1, 15
|
|
cn_ap:
|
|
CpFET_off
|
|
BpFET_off
|
|
dec I_Temp1
|
|
brne cn_ap
|
|
AnFET_off ; Switch nFETs
|
|
CnFET_on
|
|
sbr Flags1, (1<<PWM_ON) ; Set pwm on flag
|
|
rjmp t2ovfl_pwm_on_exit
|
|
|
|
pwm_cnfet_bpfet_on: ; Pwm on cycle cnfet on (anfet off) and bpfet on (used in damped state 1)
|
|
; Delay from pFETs are turned off (only in damped mode) until nFET is turned on (pFETs are slow)
|
|
ldi I_Temp1, 15
|
|
cn_bp:
|
|
CpFET_off
|
|
ApFET_off
|
|
dec I_Temp1
|
|
brne cn_bp
|
|
AnFET_off ; Switch nFETs
|
|
CnFET_on
|
|
sbr Flags1, (1<<PWM_ON) ; Set pwm on flag
|
|
rjmp t2ovfl_pwm_on_exit
|
|
|
|
t2ovfl_pwm_on_exit:
|
|
; Set timer2 for coming on cycle length
|
|
mov I_Temp1, Current_Pwm_Limited ; Load current pwm
|
|
com I_Temp1 ; com is 255-x
|
|
sec ; Set carry
|
|
sbrs Flags2, PGM_PWM_HIGH_FREQ ; High pwm frequency?
|
|
lsl I_Temp1 ; No - multiply by 2
|
|
brcs t2ovfl_int_on_no_second ; More than 256? - branch
|
|
|
|
mov Pwm_Timer_Second, I_Temp1 ; Set second timer
|
|
ldi I_Temp1, 0 ; Set next timer wait to max
|
|
|
|
t2ovfl_int_on_no_second:
|
|
Set_TCNT2 I_Temp1 ; Write start point for timer2
|
|
out SREG, I_Sreg
|
|
reti
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Wait 1us
|
|
;
|
|
; No assumptions
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
wait1us:
|
|
nop ; Assuming rcall is used for entry (rcall is 3 cycles, ret is 4 cycles plus 1 nop is 8 cycles)
|
|
ret
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Wait xms ~(x*4*250) (Different entry points)
|
|
;
|
|
; No assumptions
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
wait1ms:
|
|
ldi Temp3, 1
|
|
rjmp waitxms_o
|
|
|
|
wait3ms:
|
|
ldi Temp3, 3
|
|
rjmp waitxms_o
|
|
|
|
wait10ms:
|
|
ldi Temp3, 10
|
|
rjmp waitxms_o
|
|
|
|
wait30ms:
|
|
ldi Temp3, 30
|
|
rjmp waitxms_o
|
|
|
|
wait100ms:
|
|
ldi Temp3, 100
|
|
rjmp waitxms_o
|
|
|
|
wait200ms:
|
|
ldi Temp3, 200
|
|
rjmp waitxms_o
|
|
|
|
waitxms_o: ; Outer loop
|
|
ldi Temp2, 4
|
|
waitxms_m: ; Middle loop
|
|
ldi Temp1, 192 ; 250µs
|
|
waitxms_i: ; Inner loop
|
|
rcall wait1us
|
|
dec Temp1
|
|
brne waitxms_i
|
|
dec Temp2
|
|
brne waitxms_m
|
|
dec Temp3
|
|
brne waitxms_o
|
|
ret
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Beeper routines (4 different entry points)
|
|
;
|
|
; No assumptions
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
beep_f1: ; Entry point 1, load beeper frequency 1 settings
|
|
ldi Temp4, 21 ; Off wait loop length
|
|
ldi Temp3, 80 ; Number of beep pulses
|
|
rjmp beep
|
|
|
|
beep_f2: ; Entry point 2, load beeper frequency 2 settings
|
|
ldi Temp4, 18
|
|
ldi Temp3, 100
|
|
rjmp beep
|
|
|
|
beep_f3: ; Entry point 3, load beeper frequency 3 settings
|
|
ldi Temp4, 15
|
|
ldi Temp3, 120
|
|
rjmp beep
|
|
|
|
beep_f4: ; Entry point 4, load beeper frequency 4 settings
|
|
ldi Temp4, 12
|
|
ldi Temp3, 140
|
|
rjmp beep
|
|
|
|
beep: ; Beep loop start
|
|
BpFET_on ; BpFET on
|
|
ldi Temp1, 150 ; Allow some time after pfet is turned on
|
|
beep_wait_pfet_on:
|
|
dec Temp1
|
|
brne beep_wait_pfet_on
|
|
ldi Temp1, 12 ; 15µs on
|
|
AnFET_on ; AnFET on
|
|
beep_on_i: ; Fets on loop
|
|
rcall wait1us
|
|
dec Temp1
|
|
brne beep_on_i
|
|
AnFET_off ; AnFET off
|
|
ldi Temp2, 64
|
|
beep_off_o: ; Fets off, outer loop
|
|
mov Temp1, Temp4 ; Set off time
|
|
beep_off_i: ; Inner loop
|
|
rcall wait1us
|
|
dec Temp1
|
|
brne beep_off_i
|
|
dec Temp2
|
|
brne beep_off_o
|
|
dec Temp3
|
|
brne beep
|
|
BpFET_off ; BpFET off
|
|
ret
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Calculate governor routines
|
|
;
|
|
; No assumptions
|
|
;
|
|
; Governs headspeed based upon the Comm_Period4x variable and pwm
|
|
; The governor task is split into several routines in order to distribute processing time
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
; First governor routine - calculate governor target
|
|
calc_governor_target:
|
|
lds Temp1, Pgm_Gov_Mode ; Governor mode?
|
|
cpi Temp1, 3
|
|
brcs governor_speed_check ; Yes - branch
|
|
rjmp calc_governor_target_exit
|
|
|
|
governor_speed_check:
|
|
; Load comm period
|
|
lds Temp1, Comm_Period4x_L
|
|
lds Temp2, Comm_Period4x_H
|
|
; Check speed (do not run governor for low speeds)
|
|
ldi Temp3, 0x02 ; Is speed below min limit (~62500 eRPM)?
|
|
cpi Temp1, 0x80
|
|
cpc Temp2, Temp3
|
|
brcs governor_target_calc ; No - calculate governor target
|
|
|
|
mov Current_Pwm, Requested_Pwm ; Set current pwm to requested
|
|
sts Gov_Target_L, Zero ; Set target to zero
|
|
sts Gov_Target_H, Zero
|
|
sts Gov_Integral_L, Zero ; Set integral to zero
|
|
sts Gov_Integral_H, Zero
|
|
sts Gov_Integral_X, Zero
|
|
sts Gov_Active, Zero
|
|
rjmp calc_governor_target_exit
|
|
|
|
governor_target_calc:
|
|
; Governor calculations
|
|
mov Temp3, Governor_Req_Pwm ; Load governor requested pwm
|
|
com Temp3 ; Calculate 255-pwm (invert pwm)
|
|
; Calculate comm period target (1 + 2*((255-Requested_Pwm)/256) - 0.25)
|
|
rol Temp3 ; Msb to carry
|
|
rol Temp3 ; To bit0
|
|
mov Temp4, Temp3 ; Now 2 lsbs are valid for H
|
|
ror Temp3 ; Now 6 msbs are valid for L
|
|
andi Temp4, 0x01 ; Calculate H byte
|
|
inc Temp4
|
|
andi Temp3, 0xfe ; Calculate L byte
|
|
subi Temp3, 0x40 ; Subtract 0.25
|
|
sbc Temp4, Zero
|
|
; Store governor target
|
|
sts Gov_Target_L, Temp3
|
|
sts Gov_Target_H, Temp4
|
|
; Set governor active
|
|
ldi Temp1, 1
|
|
sts Gov_Active, Temp1
|
|
calc_governor_target_exit:
|
|
ret
|
|
|
|
|
|
; Second governor routine - calculate governor proportional error
|
|
calc_governor_prop_error:
|
|
; Exit if governor is inactive
|
|
lds Temp1, Gov_Active
|
|
cp Temp1, Zero
|
|
breq calc_governor_prop_error_exit
|
|
|
|
; Load governor target
|
|
lds Temp1, Gov_Target_L
|
|
lds Temp2, Gov_Target_H
|
|
; Load comm period
|
|
lds Temp3, Comm_Period4x_L
|
|
lds Temp4, Comm_Period4x_H
|
|
; Calculate error
|
|
sub Temp1, Temp3
|
|
sbc Temp2, Temp4
|
|
; Check error and limit (to low byte)
|
|
ldi Temp3, 0xff ; Is error too negative?
|
|
cpi Temp1, 0x80
|
|
cpc Temp2, Temp3
|
|
brlt governor_limit_prop_error_neg ; Yes - limit
|
|
|
|
ldi Temp3, 0x00 ; Is error too positive?
|
|
cpi Temp1, 0x7f
|
|
cpc Temp2, Temp3
|
|
brge governor_limit_prop_error_pos ; Yes - limit
|
|
rjmp governor_store_prop_error
|
|
|
|
governor_limit_prop_error_pos:
|
|
ldi Temp1, 0x7f ; Limit to max positive (2's complement)
|
|
ldi Temp2, 0x00
|
|
rjmp governor_store_prop_error
|
|
|
|
governor_limit_prop_error_neg:
|
|
ldi Temp1, 0x80 ; Limit to max negative (2's complement)
|
|
ldi Temp2, 0xff
|
|
|
|
governor_store_prop_error:
|
|
; Store proportional
|
|
sts Gov_Proportional_L, Temp1
|
|
sts Gov_Proportional_H, Temp2
|
|
calc_governor_prop_error_exit:
|
|
ret
|
|
|
|
|
|
; Third governor routine - calculate governor integral error
|
|
calc_governor_int_error:
|
|
; Exit if governor is inactive
|
|
lds Temp1, Gov_Active
|
|
cp Temp1, Zero
|
|
breq calc_governor_int_error_exit
|
|
|
|
; Load integral
|
|
lds Temp3, Gov_Integral_L
|
|
lds Temp4, Gov_Integral_H
|
|
lds Temp5, Gov_Integral_X
|
|
; Load proportional
|
|
lds Temp1, Gov_Proportional_L
|
|
lds Temp2, Gov_Proportional_H
|
|
; Add to integral
|
|
add Temp3, Temp1
|
|
adc Temp4, Temp2
|
|
adc Temp5, Temp2
|
|
|
|
; Check integral and limit
|
|
ldi Temp2, 0xf0 ; Is error too negative?
|
|
cp Temp5, Temp2
|
|
brlt governor_limit_int_error_neg ; Yes - limit
|
|
|
|
ldi Temp2, 0x0f ; Is error too positive?
|
|
cp Temp5, Temp2
|
|
brge governor_limit_int_error_pos ; Yes - limit
|
|
rjmp governor_check_pwm
|
|
|
|
governor_limit_int_error_pos:
|
|
ldi Temp3, 0xff ; Limit to max positive (2's complement)
|
|
ldi Temp4, 0xff
|
|
ldi Temp2, 0x0f
|
|
mov Temp5, Temp2
|
|
rjmp governor_check_pwm
|
|
|
|
governor_limit_int_error_neg:
|
|
ldi Temp3, 0x00 ; Limit to max negative (2's complement)
|
|
ldi Temp4, 0x00
|
|
ldi Temp2, 0xf0
|
|
mov Temp5, Temp2
|
|
|
|
governor_check_pwm:
|
|
; Check current pwm
|
|
lds Temp2, Pwm_Limit
|
|
cp Current_Pwm, Temp2 ; Is current pwm above pwm limit?
|
|
mov Temp2, Current_Pwm ; Load current pwm before continuing
|
|
brsh governor_int_max_pwm ; Yes
|
|
|
|
cpi Temp2, 1 ; Is current below pwm min?
|
|
brlo governor_int_min_pwm ; Yes
|
|
rjmp governor_store_int_error ; No - store integral error
|
|
|
|
governor_int_max_pwm:
|
|
tst Temp1 ; Is proportional error positive? (high byte is always zero)
|
|
brmi calc_governor_int_error_exit ; No - exit (do not integrate further)
|
|
rjmp governor_store_int_error ; Yes - store integral error
|
|
|
|
governor_int_min_pwm:
|
|
tst Temp1 ; Is proportional error positive? (high byte is always zero)
|
|
brpl calc_governor_int_error_exit ; Yes - exit (do not integrate further)
|
|
|
|
governor_store_int_error:
|
|
; Store integral
|
|
sts Gov_Integral_L, Temp3
|
|
sts Gov_Integral_H, Temp4
|
|
sts Gov_Integral_X, Temp5
|
|
calc_governor_int_error_exit:
|
|
ret
|
|
|
|
|
|
; Fourth governor routine - calculate governor proportional correction
|
|
calc_governor_prop_correction:
|
|
; Exit if governor is inactive
|
|
lds Temp1, Gov_Active
|
|
cp Temp1, Zero
|
|
breq calc_governor_prop_corr_exit
|
|
|
|
; Load proportional
|
|
lds Temp1, Gov_Proportional_L ; Only low byte required (high byte is always zero)
|
|
; Apply proportional gain
|
|
clr Temp2 ; Sign extend high byte
|
|
sbrc Temp1, 7
|
|
com Temp2
|
|
lsl Temp1 ; Nominal multiply by 2
|
|
rol Temp2
|
|
lds Temp4, Pgm_Gov_P_Gain ; Load proportional gain
|
|
sbrc Temp4, 0 ; Is lsb 1?
|
|
rjmp calc_governor_prop_corr_15 ; Yes - go to multiply by 1.5
|
|
|
|
cpi Temp4, 4 ; Is proportional gain 1?
|
|
breq governor_limit_prop_corr ; Yes - branch
|
|
|
|
asr Temp2 ; Gain is 0.5 - divide by 2
|
|
ror Temp1
|
|
rjmp governor_limit_prop_corr
|
|
|
|
calc_governor_prop_corr_15:
|
|
mov Temp6, Temp2 ; Load a copy
|
|
mov Temp5, Temp1
|
|
asr Temp6 ; Divide by 2
|
|
ror Temp5
|
|
add Temp1, Temp5 ; Add a half
|
|
adc Temp2, Temp6
|
|
cpi Temp4, 5 ; Is proportional gain 1.5?
|
|
breq governor_limit_prop_corr ; Yes - branch
|
|
|
|
asr Temp2 ; No - divide by 2
|
|
ror Temp1
|
|
cpi Temp4, 3 ; Is proportional gain 0.75?
|
|
breq governor_limit_prop_corr ; Yes - branch
|
|
|
|
asr Temp2 ; No - divide by 2
|
|
ror Temp1
|
|
|
|
governor_limit_prop_corr: ; Check error and limit (to low byte)
|
|
ldi Temp3, 0xff ; Is error too negative?
|
|
cpi Temp1, 0x80
|
|
cpc Temp2, Temp3
|
|
brlt governor_limit_prop_corr_neg ; Yes - limit
|
|
|
|
ldi Temp3, 0x00 ; Is error too positive?
|
|
cpi Temp1, 0x7f
|
|
cpc Temp2, Temp3
|
|
brge governor_limit_prop_corr_pos ; Yes - limit
|
|
rjmp governor_apply_prop_corr
|
|
|
|
governor_limit_prop_corr_pos:
|
|
ldi Temp1, 0x7f ; Limit to max positive (2's complement)
|
|
ldi Temp2, 0x00
|
|
rjmp governor_apply_prop_corr
|
|
|
|
governor_limit_prop_corr_neg:
|
|
ldi Temp1, 0x80 ; Limit to max negative (2's complement)
|
|
ldi Temp2, 0xff
|
|
|
|
governor_apply_prop_corr:
|
|
; Load requested pwm
|
|
mov Temp3, Governor_Req_Pwm
|
|
; Test proportional sign
|
|
tst Temp1 ; Is proportional negative?
|
|
brmi governor_corr_neg_prop ; Yes - go to correct negative
|
|
|
|
; Subtract positive proportional
|
|
sub Temp3, Temp1
|
|
; Check result
|
|
brcs governor_corr_prop_min_pwm ; Is result negative?
|
|
|
|
cpi Temp3, 1 ; Is result below pwm min?
|
|
brlo governor_corr_prop_min_pwm ; Yes
|
|
rjmp governor_store_prop_corr ; No - store proportional correction
|
|
|
|
governor_corr_prop_min_pwm:
|
|
ldi Temp3, 1 ; Load minimum pwm
|
|
rjmp governor_store_prop_corr
|
|
|
|
governor_corr_neg_prop:
|
|
; Add negative proportional
|
|
neg Temp1
|
|
add Temp3, Temp1
|
|
; Check result
|
|
brcs governor_corr_prop_max_pwm ; Is result above max?
|
|
rjmp governor_store_prop_corr ; No - store proportional correction
|
|
|
|
governor_corr_prop_max_pwm:
|
|
ldi Temp3, 255 ; Load maximum pwm
|
|
governor_store_prop_corr:
|
|
; Store proportional pwm
|
|
sts Gov_Prop_Pwm, Temp3
|
|
calc_governor_prop_corr_exit:
|
|
ret
|
|
|
|
|
|
; Fifth governor routine - calculate governor integral correction
|
|
calc_governor_int_correction:
|
|
; Exit if governor is inactive
|
|
lds Temp1, Gov_Active
|
|
cp Temp1, Zero
|
|
breq calc_governor_int_corr_exit
|
|
|
|
; Load integral
|
|
lds Temp2, Gov_Integral_H
|
|
lds Temp3, Gov_Integral_X
|
|
; Apply integrator gain
|
|
lds Temp4, Pgm_Gov_I_Gain ; Load integral gain
|
|
sbrc Temp4, 0 ; Is lsb 1?
|
|
rjmp calc_governor_int_corr_15 ; Yes - go to multiply by 1.5
|
|
|
|
cpi Temp4, 4 ; Is integral gain 1?
|
|
breq governor_limit_int_corr ; Yes - branch
|
|
|
|
asr Temp3 ; Gain is 0.5 - divide by 2
|
|
ror Temp2
|
|
rjmp governor_limit_int_corr
|
|
|
|
calc_governor_int_corr_15:
|
|
mov Temp6, Temp3 ; Load a copy
|
|
mov Temp5, Temp2
|
|
asr Temp6 ; Divide by 2
|
|
ror Temp5
|
|
add Temp2, Temp5 ; Add a half
|
|
adc Temp3, Temp6
|
|
cpi Temp4, 5 ; Is integral gain 1.5?
|
|
breq governor_limit_int_corr ; Yes - branch
|
|
|
|
asr Temp3 ; No - divide by 2
|
|
ror Temp2
|
|
cpi Temp4, 3 ; Is integral gain 0.75?
|
|
breq governor_limit_int_corr ; Yes - branch
|
|
|
|
asr Temp3 ; No - divide by 2
|
|
ror Temp2
|
|
|
|
governor_limit_int_corr:
|
|
; Check integral and limit
|
|
ldi Temp1, 0xff ; Is integral too negative?
|
|
cpi Temp2, 0x00
|
|
cpc Temp3, Temp1
|
|
brlt governor_limit_int_corr_neg ; Yes - limit
|
|
|
|
ldi Temp1, 0x00 ; Is integral too positive?
|
|
cpi Temp2, 0xff
|
|
cpc Temp3, Temp1
|
|
brge governor_limit_int_corr_pos ; Yes - limit
|
|
rjmp governor_apply_int_corr
|
|
|
|
governor_limit_int_corr_pos:
|
|
ldi Temp2, 0xff ; Limit to max positive (2's complement)
|
|
ldi Temp3, 0x00
|
|
rjmp governor_apply_int_corr
|
|
|
|
governor_limit_int_corr_neg:
|
|
ldi Temp2, 0x00 ; Limit to max negative (2's complement)
|
|
ldi Temp3, 0xff
|
|
|
|
governor_apply_int_corr:
|
|
; Load proportional pwm
|
|
lds Temp1, Gov_Prop_Pwm
|
|
; Test integral sign
|
|
tst Temp3 ; Is integral negative?
|
|
brmi governor_corr_neg_int ; Yes - go to correct negative
|
|
|
|
; Subtract positive integral
|
|
sub Temp1, Temp2
|
|
; Check result
|
|
brcs governor_corr_int_min_pwm ; Is result negative?
|
|
cpi Temp1, 1 ; Is result below pwm min?
|
|
brlo governor_corr_int_min_pwm ; Yes
|
|
rjmp governor_store_int_corr ; No - store correction
|
|
|
|
governor_corr_int_min_pwm:
|
|
ldi Temp1, 1 ; Load minimum pwm
|
|
rjmp governor_store_int_corr
|
|
|
|
governor_corr_neg_int:
|
|
; Add negative integral
|
|
com Temp2 ; Invert
|
|
inc Temp2 ; Add one
|
|
add Temp1, Temp2
|
|
; Check result
|
|
brcs governor_corr_int_max_pwm ; Is result above max?
|
|
rjmp governor_store_int_corr ; No - store correction
|
|
|
|
governor_corr_int_max_pwm:
|
|
ldi Temp1, 255 ; Load maximum pwm
|
|
governor_store_int_corr:
|
|
; Store current pwm
|
|
mov Current_Pwm, Temp1
|
|
|
|
calc_governor_int_corr_exit:
|
|
ret
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Measure lipo cells
|
|
;
|
|
; No assumptions
|
|
;
|
|
; Measure voltage and calculate lipo cells
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
measure_lipo_cells:
|
|
.if TAIL == 0
|
|
Start_Adc Temp1
|
|
; Wait for ADC conversion to complete
|
|
Get_Adc_Status Temp3
|
|
sbrc Temp3, ADSC
|
|
rjmp measure_lipo_cells
|
|
; Read ADC result
|
|
Read_Adc_Result Temp1, Temp2
|
|
; Stop ADC
|
|
Stop_Adc Temp3
|
|
; Set 1S
|
|
ldi Temp3, ADC_LIMIT_L
|
|
sts Lipo_Adc_Limit_L, Temp3
|
|
ldi Temp3, ADC_LIMIT_H
|
|
sts Lipo_Adc_Limit_H, Temp3
|
|
ldi Temp3, 1
|
|
sts Lipo_Cells, Temp3
|
|
; Check voltage against 2S limit
|
|
ldi Temp3, ADC_LIMIT_L ; Load limit
|
|
ldi Temp4, ADC_LIMIT_H
|
|
lsl Temp3 ; Multiply limit by 2
|
|
rol Temp4
|
|
cp Temp1, Temp3 ; Voltage above limit?
|
|
cpc Temp2, Temp4
|
|
brcs measure_lipo_exit ; No - exit
|
|
|
|
sts Lipo_Adc_Limit_L, Temp3 ; Set 2S
|
|
sts Lipo_Adc_Limit_H, Temp4
|
|
ldi Temp3, 2
|
|
sts Lipo_Cells, Temp3
|
|
; Check voltage against 3S limit
|
|
ldi Temp3, ADC_LIMIT_L ; Load limit
|
|
ldi Temp4, ADC_LIMIT_H
|
|
mov Temp5, Temp3 ; Make a copy
|
|
mov Temp6, Temp4
|
|
lsl Temp3 ; Multiply limit by 2
|
|
rol Temp4
|
|
add Temp3, Temp5 ; Add limit
|
|
adc Temp4, Temp6
|
|
cp Temp1, Temp3 ; Voltage above limit?
|
|
cpc Temp2, Temp4
|
|
brcs measure_lipo_exit ; No - exit
|
|
|
|
sts Lipo_Adc_Limit_L, Temp3 ; Set 3S
|
|
sts Lipo_Adc_Limit_H, Temp4
|
|
ldi Temp3, 3
|
|
sts Lipo_Cells, Temp3
|
|
|
|
measure_lipo_exit:
|
|
.endif
|
|
ret
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Start ADC conversion
|
|
;
|
|
; No assumptions
|
|
;
|
|
; Start conversion used for measuring power supply voltage
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
start_adc_conversion:
|
|
.if TAIL == 0
|
|
Start_Adc Temp1
|
|
.endif
|
|
ret
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Check power supply voltage and limit power
|
|
;
|
|
; No assumptions
|
|
;
|
|
; Used to limit main motor power in order to maintain the required voltage
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
check_voltage_and_limit_power:
|
|
.if TAIL == 0
|
|
; Wait for ADC conversion to complete
|
|
Get_Adc_Status Temp3
|
|
sbrc Temp3, ADSC
|
|
rjmp check_voltage_and_limit_power
|
|
; Read ADC result
|
|
Read_Adc_Result Temp1, Temp2
|
|
; Stop ADC
|
|
Stop_Adc Temp3
|
|
; Check if ADC is saturated
|
|
cpi Temp1, 0xff
|
|
ldi Temp3, 0x03
|
|
cpc Temp2, Temp3
|
|
brcc check_voltage_good ; ADC saturated, can not make judgement
|
|
|
|
; Check voltage against limit
|
|
lds Temp3, Lipo_Adc_Limit_L ; Load limit
|
|
lds Temp4, Lipo_Adc_Limit_H
|
|
cp Temp1, Temp3 ; Voltage below limit?
|
|
cpc Temp2, Temp4
|
|
brcc check_voltage_good ; No - branch
|
|
|
|
; Decrease pwm limit
|
|
lds Temp1, Pwm_Limit
|
|
tst Temp1 ; Limit above zero?
|
|
breq check_voltage_lim ; No - branch
|
|
|
|
dec Temp1 ; Yes - decrement limit
|
|
sts Pwm_Limit, Temp1
|
|
rjmp check_voltage_lim
|
|
|
|
check_voltage_good:
|
|
; Increase pwm limit
|
|
lds Temp1, Pwm_Limit
|
|
cpi Temp1, 0xff ; Limit below max?
|
|
breq check_voltage_lim ; No - branch
|
|
|
|
inc Temp1 ; Yes - increment limit
|
|
sts Pwm_Limit, Temp1
|
|
|
|
check_voltage_lim:
|
|
cp Current_Pwm, Temp1 ; Current pwm above limit?
|
|
brsh check_voltage_exit ; Yes - branch and limit
|
|
|
|
mov Temp1, Current_Pwm ; No - set current pwm
|
|
|
|
check_voltage_exit:
|
|
mov Current_Pwm_Limited, Temp1
|
|
.else
|
|
mov Current_Pwm_Limited, Current_Pwm ; Direct transfer for tail
|
|
.endif
|
|
ret
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Set startup PWM routine
|
|
;
|
|
; No assumptions
|
|
;
|
|
; Used for pwm control during startup
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
set_startup_pwm:
|
|
; Set pwm values according to startup mode flags
|
|
sbrc Flags0, SETTLE_MODE ; Is it motor start settle mode?
|
|
ldi Temp1, PWM_SETTLE ; Yes - set settle power
|
|
sbrc Flags0, STEPPER_MODE ; Is it motor start stepper mode?
|
|
ldi Temp1, PWM_STEPPER ; Yes - set stepper power
|
|
sbrc Flags0, AQUISITION_MODE ; Is it motor start aquisition mode?
|
|
ldi Temp1, PWM_AQUISITION ; Yes - set aquisition power
|
|
sbrc Flags0, INITIAL_RUN_MODE ; Is it initial run mode?
|
|
ldi Temp1, PWM_INITIAL_RUN ; Yes - set initial run power
|
|
|
|
; Update pwm variables if any startup mode flag is set
|
|
mov Temp2, Flags0
|
|
andi Temp2, (1<<SETTLE_MODE)+(1<<STEPPER_MODE)+(1<<AQUISITION_MODE)+(1<<INITIAL_RUN_MODE)
|
|
tst Temp2 ; Any startup mode set?
|
|
breq startup_pwm_exit ; No - exit
|
|
|
|
; Adjust startup power
|
|
lds Temp3, Pgm_Startup_Pwr
|
|
cpi Temp3, 3 ; Is gain 1?
|
|
breq startup_pwm_set_pwm ; Yes - skip adjustment
|
|
|
|
mov Temp2, Temp1
|
|
lsr Temp2 ; After this "0.5"
|
|
sbrc Temp3, 0 ; Is bit 0 in gain set?
|
|
rjmp startup_pwm_corr ; Yes - two gain correction steps
|
|
|
|
lsr Temp2 ; After this "0.25"
|
|
|
|
startup_pwm_corr:
|
|
sbrc Temp3, 2 ; Is bit 2 in gain set?
|
|
rjmp startup_pwm_gain_pos ; Yes - positive correction
|
|
|
|
sub Temp1, Temp2 ; Apply negative correction
|
|
rjmp startup_pwm_set_pwm
|
|
|
|
startup_pwm_gain_pos:
|
|
add Temp1, Temp2 ; Apply positive correction
|
|
brcc startup_pwm_set_pwm ; Above max?
|
|
|
|
ldi Temp1, 0xff ; Yes - limit
|
|
|
|
startup_pwm_set_pwm:
|
|
; Set pwm variables
|
|
mov Requested_Pwm, Temp1 ; Update requested pwm
|
|
mov Current_Pwm, Temp1 ; Update current pwm
|
|
mov Current_Pwm_Limited, Temp1 ; Update limited version of current pwm
|
|
|
|
startup_pwm_exit:
|
|
ret
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Initialize all timings routine
|
|
;
|
|
; No assumptions
|
|
;
|
|
; Part of initialization before motor start
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
initialize_all_timings:
|
|
ldi YL, low(STEPPER_STEP_BEG)
|
|
ldi YH, high(STEPPER_STEP_BEG)
|
|
sts Wt_Stepper_Step_L, YL ; Initialize stepper step time
|
|
sts Wt_Stepper_Step_H, YH
|
|
ldi Temp3, 0xff ; Initialization value ~8.2ms
|
|
ldi Temp4, 0x1f
|
|
sts Wt_Comm_L, Temp3 ; Initialize wait from zero cross to commutation
|
|
sts Wt_Comm_H, Temp4
|
|
sts Wt_Advance_L, Temp3 ; Initialize wait for timing advance
|
|
sts Wt_Advance_H, Temp4
|
|
lsr Temp4 ; Divide by 2
|
|
ror Temp3
|
|
sts Wt_Zc_Scan_L, Temp3 ; Initialize wait before zero cross scan
|
|
sts Wt_Zc_Scan_H, Temp4
|
|
ldi Temp2, 0xff ; Set commutation period registers to very slow timing
|
|
sts Comm_Period4x_H, Temp2
|
|
ldi Temp1, 0xff
|
|
sts Comm_Period4x_L, Temp1
|
|
ret
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Calculate next commutation timing routine
|
|
;
|
|
; No assumptions
|
|
;
|
|
; Called immediately after each commutation
|
|
; Also sets up timer 1 to wait advance timing
|
|
; Two entry points are used
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
calc_next_comm_timing_start: ; Entry point for startup
|
|
lds YL, Wt_Stepper_Step_L ; Set up stepper step wait
|
|
lds YH, Wt_Stepper_Step_H
|
|
rjmp read_timer
|
|
|
|
calc_next_comm_timing: ; Entry point for run mode
|
|
lds YL, Wt_Advance_L ; Set up advance timing wait
|
|
lds YH, Wt_Advance_H
|
|
read_timer:
|
|
cli ; Disable interrupts while reading timer 1
|
|
Read_TCNT1L Temp1
|
|
Read_TCNT1H Temp2
|
|
add YL, Temp1 ; Set new output compare value
|
|
adc YH, Temp2
|
|
Set_OCR1AH YH ; Update high byte first to avoid false output compare
|
|
Set_OCR1AL YL
|
|
sei ; Enable interrupts
|
|
sbr Flags0, (1<<OCA_PENDING) ; Set timer output compare pending flag
|
|
; Calculate this commutation time
|
|
lds Temp3, Prev_Comm_L
|
|
lds Temp4, Prev_Comm_H
|
|
sts Prev_Comm_L, Temp1 ; Store timestamp as previous commutation
|
|
sts Prev_Comm_H, Temp2
|
|
sub Temp1, Temp3 ; Calculate the new commutation time
|
|
sbc Temp2, Temp4
|
|
; Calculate next zero cross scan timeout
|
|
lds Temp3, Comm_Period4x_L ; Comm_Period4x(-l-h-x) holds the time of 4 commutations
|
|
lds Temp4, Comm_Period4x_H
|
|
movw YL, Temp3 ; Copy timing to Y
|
|
lsr YH ; Divide by 2
|
|
ror YL
|
|
lsr YH ; Divide by 2 again
|
|
ror YL
|
|
sub Temp3, YL ; Subtract a quarter
|
|
sbc Temp4, YH
|
|
add Temp3, Temp1 ; Add the new time
|
|
adc Temp4, Temp2
|
|
sts Comm_Period4x_L, Temp3 ; Store Comm_Period4x_X
|
|
sts Comm_Period4x_H, Temp4
|
|
brcs calc_next_comm_slow ; Yes - go to slow case
|
|
ret
|
|
|
|
calc_next_comm_slow:
|
|
ldi Temp2, 0xff ; Set commutation period registers to very slow timing (0xffff)
|
|
sts Comm_Period4x_H, Temp2
|
|
ldi Temp1, 0xff
|
|
sts Comm_Period4x_L, Temp1
|
|
ret
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Wait advance timing routine
|
|
;
|
|
; No assumptions
|
|
;
|
|
; Waits for the advance timing to elapse
|
|
; Also sets up timer 1 to wait the zero cross scan wait time
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
wait_advance_timing:
|
|
sbrc Flags0, OCA_PENDING
|
|
rjmp wait_advance_timing
|
|
|
|
lds YL, Wt_Zc_Scan_L ; Set wait to zero cross scan value
|
|
lds YH, Wt_Zc_Scan_H
|
|
cli ; Disable interrupts while reading timer 1
|
|
Read_TCNT1L Temp1
|
|
Read_TCNT1H Temp2
|
|
add Temp1, YL ; Set new output compare value
|
|
adc Temp2, YH
|
|
Set_OCR1AH Temp2 ; Update high byte first to avoid false output compare
|
|
Set_OCR1AL Temp1
|
|
sei ; Enable interrupts
|
|
sbr Flags0, (1<<OCA_PENDING)
|
|
ret
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Calculate new wait times routine
|
|
;
|
|
; No assumptions
|
|
;
|
|
; Calculates new wait times
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
calc_new_wait_times:
|
|
lds Temp1, Comm_Period4x_L ; Load Comm_Period4x
|
|
lds Temp2, Comm_Period4x_H
|
|
lsr Temp2 ; Divide by 2
|
|
ror Temp1
|
|
lsr Temp2 ; Divide by 2 again
|
|
ror Temp1
|
|
lsr Temp2 ; Divide by 2 again (prior to this it is average commutation time)
|
|
ror Temp1
|
|
lsr Temp2 ; Divide by 2 again
|
|
ror Temp1
|
|
subi Temp1, COMM_TIME_RED
|
|
sbc Temp2, Zero
|
|
brcs load_min_time ; Check that result is still positive
|
|
cpi Temp1, COMM_TIME_MIN
|
|
cpc Temp2, Zero
|
|
brcc store_times ; Check that result is still above minumum
|
|
|
|
load_min_time:
|
|
ldi Temp1, COMM_TIME_MIN
|
|
clr Temp2
|
|
|
|
store_times:
|
|
sts Wt_Comm_L, Temp1 ; Now commutation time (~60°) divided by 4 (~15°)
|
|
sts Wt_Comm_H, Temp2
|
|
sts Wt_Advance_L, Temp1 ; New commutation advance time (15°)
|
|
sts Wt_Advance_H, Temp2
|
|
lsr Temp2 ; Divide by 2
|
|
ror Temp1
|
|
sts Wt_Zc_Scan_L, Temp1 ; Use this value for zero cross scan delay (7.5°)
|
|
sts Wt_Zc_Scan_H, Temp2
|
|
ret
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Wait before zero cross scan routine
|
|
;
|
|
; No assumptions
|
|
;
|
|
; Waits for the zero cross scan wait time to elapse
|
|
; Also sets up timer 1 to wait the zero cross scan timeout time
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
wait_before_zc_scan:
|
|
sbrc Flags0, OCA_PENDING
|
|
rjmp wait_before_zc_scan
|
|
|
|
lds YL, Comm_Period4x_L ; Set wait to zero comm period 4x value
|
|
lds YH, Comm_Period4x_H
|
|
cli ; Disable interrupts while reading timer 1
|
|
Read_TCNT1L Temp1
|
|
Read_TCNT1H Temp2
|
|
add Temp1, YL ; Set new output compare value
|
|
adc Temp2, YH
|
|
Set_OCR1AH Temp2 ; Update high byte first to avoid false output compare
|
|
Set_OCR1AL Temp1
|
|
sei ; Enable interrupts
|
|
sbr Flags0, (1<<OCA_PENDING)
|
|
ret
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Wait for comparator to go low/high routines
|
|
;
|
|
; No assumptions
|
|
;
|
|
; Waits for the zero cross scan wait time to elapse
|
|
; Then scans for comparator going low/high
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
wait_for_comp_out_low:
|
|
sei ; Enable interrupts
|
|
sbrs Flags0, OCA_PENDING ; Has zero cross scan timeout elapsed?
|
|
ret ; Yes - return
|
|
sbrc Flags1, PWM_OFF_DAMPED ; Is it damped operation?
|
|
rjmp use_stored_comp_out_low ; Yes - use stored comparator output
|
|
rjmp read_comp_out_low ; No - read comparator output
|
|
|
|
use_stored_comp_out_low:
|
|
sbrc Flags1, COMP_STORED ; Is comparator low?
|
|
rjmp wait_for_comp_out_low ; No - loop, while high
|
|
sei ; Enable interrupts
|
|
ret
|
|
|
|
read_comp_out_low:
|
|
ldi Temp1, 6 ; Load number of wait cycles
|
|
; Select number of comparator readings based upon current pwm
|
|
mov Temp3, Current_Pwm_Limited ; Load current pwm
|
|
com Temp3 ; Invert
|
|
swap Temp3 ; Swap nibbles (bits7:4 go to bits3:0)
|
|
lsr Temp3 ; Shift right (original bits7:5 will now be in bits2:0)
|
|
andi Temp3, 0x07 ; Take 3 lsbs (that were originally msbs)
|
|
inc Temp3 ; Add 1 to ensure always 1 or higher
|
|
cli ; Disable interrupts
|
|
|
|
pwm_wait_low: ; Wait some cycles after pwm has been switched on (motor wire electrical settling)
|
|
dec Temp1
|
|
brne pwm_wait_low
|
|
|
|
comp_read_low:
|
|
Read_Comp_Out Temp2 ; Read comparator output
|
|
sbrc Temp2, ACO ; Is comparator output low?
|
|
rjmp wait_for_comp_out_low ; No - go back
|
|
dec Temp3 ; Decrement readings counter
|
|
brne comp_read_low ; Repeat comparator reading if not zero
|
|
sei ; Enable interrupts
|
|
ret ; Yes - return
|
|
|
|
wait_for_comp_out_high:
|
|
sei ; Enable interrupts
|
|
sbrs Flags0, OCA_PENDING ; Has zero cross scan timeout elapsed?
|
|
ret ; Yes - return
|
|
sbrc Flags1, PWM_OFF_DAMPED ; Is it damped operation?
|
|
rjmp use_stored_comp_out_high ; Yes - use stored comparator output
|
|
rjmp read_comp_out_high ; No - read comparator output
|
|
|
|
use_stored_comp_out_high:
|
|
sbrs Flags1, COMP_STORED ; Is comparator high?
|
|
rjmp wait_for_comp_out_high ; No - loop, while low
|
|
sei ; Enable interrupts
|
|
ret ; Yes - return
|
|
|
|
read_comp_out_high:
|
|
ldi Temp1, 6 ; Load number of wait cycles
|
|
; Select number of comparator readings based upon current pwm
|
|
mov Temp3, Current_Pwm_Limited ; Load current pwm
|
|
com Temp3 ; Invert
|
|
swap Temp3 ; Swap nibbles (bits7:4 go to bits3:0)
|
|
lsr Temp3 ; Shift right (original bits7:5 will now be in bits2:0)
|
|
andi Temp3, 0x07 ; Take 3 lsbs (that were originally msbs)
|
|
inc Temp3 ; Add 1 to ensure always 1 or higher
|
|
cli ; Disable interrupts
|
|
|
|
pwm_wait_high: ; Wait some cycles after pwm has been switched on (motor wire electrical settling)
|
|
dec Temp1
|
|
brne pwm_wait_high
|
|
|
|
comp_read_high:
|
|
Read_Comp_Out Temp2 ; Read comparator output
|
|
sbrs Temp2, ACO ; Is comparator output high?
|
|
rjmp wait_for_comp_out_high ; No - go back
|
|
dec Temp3 ; Decrement readings counter
|
|
brne comp_read_high ; Repeat comparator reading if not zero
|
|
sei ; Enable interrupts
|
|
ret ; Yes - return
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Setup commutation timing routine
|
|
;
|
|
; No assumptions
|
|
;
|
|
; Sets up and starts wait from commutation to zero cross
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
setup_comm_wait:
|
|
lds YL, Wt_Comm_L ; Set wait commutation value
|
|
lds YH, Wt_Comm_H
|
|
cli ; Disable interrupts while reading timer 1
|
|
Read_TCNT1L Temp1
|
|
Read_TCNT1H Temp2
|
|
add Temp1, YL ; Set new output compare value
|
|
adc Temp2, YH
|
|
Set_OCR1AH Temp2 ; Update high byte first to avoid false output compare
|
|
Set_OCR1AL Temp1
|
|
sei ; Enable interrupts
|
|
sbr Flags0, (1<<OCA_PENDING)
|
|
ret
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Wait for commutation routine
|
|
;
|
|
; No assumptions
|
|
;
|
|
; Waits from zero cross to commutation
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
wait_for_comm:
|
|
sbrc Flags0, OCA_PENDING
|
|
rjmp wait_for_comm
|
|
ret
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Commutation routines
|
|
;
|
|
; No assumptions
|
|
;
|
|
; Performs commutation switching
|
|
; Damped routines uses all pfets on when in pwm off to dampen the motor
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
comm1comm2damped:
|
|
cli ; Disable interrupts
|
|
ldi XL, low(pwm_cnfet_apfet_on)
|
|
ldi XH, high(pwm_cnfet_apfet_on)
|
|
movw ZL, XL ; Atomic set (read by ISR)
|
|
BpFET_off ; Yes - Bp off
|
|
ApFET_on ; Ap on
|
|
Set_Comp_Phase_B Temp1 ; Set comparator to phase B
|
|
sei ; Enable interrupts
|
|
ret
|
|
|
|
comm2comm3damped:
|
|
cli ; Disable interrupts
|
|
ldi XL, low(pwm_bnfet_apfet_on)
|
|
ldi XH, high(pwm_bnfet_apfet_on)
|
|
movw ZL, XL ; Atomic set (read by ISR)
|
|
CnFET_off ; Cn off
|
|
sbrc Flags1, PWM_ON ; Is pwm on?
|
|
BnFET_on ; Yes Bn on
|
|
Set_Comp_Phase_C Temp1 ; Set comparator to phase C
|
|
sei ; Enable interrupts
|
|
ret
|
|
|
|
comm3comm4damped:
|
|
cli ; Disable interrupts
|
|
ldi XL, low(pwm_bnfet_cpfet_on)
|
|
ldi XH, high(pwm_bnfet_cpfet_on)
|
|
movw ZL, XL ; Atomic set (read by ISR)
|
|
ApFET_off ; Yes - Ap off
|
|
CpFET_on ; Cp on
|
|
Set_Comp_Phase_A Temp1 ; Set comparator to phase A
|
|
sei ; Enable interrupts
|
|
ret
|
|
|
|
comm4comm5damped:
|
|
cli ; Disable interrupts
|
|
ldi XL, low(pwm_anfet_cpfet_on)
|
|
ldi XH, high(pwm_anfet_cpfet_on)
|
|
movw ZL, XL ; Atomic set (read by ISR)
|
|
BnFET_off ; Bn off
|
|
sbrc Flags1, PWM_ON ; Is pwm on?
|
|
AnFET_on ; Yes An on
|
|
Set_Comp_Phase_B Temp1 ; Set comparator to phase B
|
|
sei ; Enable interrupts
|
|
ret
|
|
|
|
comm5comm6damped:
|
|
cli ; Disable interrupts
|
|
ldi XL, low(pwm_anfet_bpfet_on)
|
|
ldi XH, high(pwm_anfet_bpfet_on)
|
|
movw ZL, XL ; Atomic set (read by ISR)
|
|
CpFET_off ; Yes - Cp off
|
|
BpFET_on ; Bp on
|
|
Set_Comp_Phase_C Temp1 ; Set comparator to phase C
|
|
sei ; Enable interrupts
|
|
ret
|
|
|
|
comm6comm1damped:
|
|
cli ; Disable interrupts
|
|
ldi XL, low(pwm_cnfet_bpfet_on)
|
|
ldi XH, high(pwm_cnfet_bpfet_on)
|
|
movw ZL, XL ; Atomic set (read by ISR)
|
|
AnFET_off ; An off
|
|
sbrc Flags1, PWM_ON ; Is pwm on?
|
|
CnFET_on ; Yes Cn on
|
|
Set_Comp_Phase_A Temp1 ; Set comparator to phase A
|
|
sei ; Enable interrupts
|
|
ret
|
|
|
|
comm1comm2:
|
|
cli ; Disable interrupts
|
|
BpFET_off ; Bp off
|
|
ApFET_on ; Ap on
|
|
Set_Comp_Phase_B Temp1 ; Set comparator to phase B
|
|
sei ; Enable interrupts
|
|
ret
|
|
|
|
comm2comm3:
|
|
cli ; Disable interrupts
|
|
ldi XL, low(pwm_bfet_on) ; Set Z register to desired pwm_nfet_on label
|
|
ldi XH, high(pwm_bfet_on)
|
|
movw ZL, XL ; Atomic set (read by ISR)
|
|
CnFET_off ; Cn off
|
|
sbrc Flags1, PWM_ON ; Is pwm on?
|
|
BnFET_on ; Yes Bn on
|
|
Set_Comp_Phase_C Temp1 ; Set comparator to phase C
|
|
sei ; Enable interrupts
|
|
ret
|
|
|
|
comm3comm4:
|
|
cli ; Disable interrupts
|
|
ApFET_off ; Ap off
|
|
CpFET_on ; Cp on
|
|
Set_Comp_Phase_A Temp1 ; Set comparator to phase A
|
|
sei ; Enable interrupts
|
|
ret
|
|
|
|
comm4comm5:
|
|
cli ; Disable interrupts
|
|
ldi XL, low(pwm_afet_on) ; Set Z register to desired pwm_nfet_on label
|
|
ldi XH, high(pwm_afet_on)
|
|
movw ZL, XL ; Atomic set (read by ISR)
|
|
BnFET_off ; Bn off
|
|
sbrc Flags1, PWM_ON ; Is pwm on?
|
|
AnFET_on ; Yes An on
|
|
Set_Comp_Phase_B Temp1 ; Set comparator to phase B
|
|
sei ; Enable interrupts
|
|
ret
|
|
|
|
comm5comm6:
|
|
cli ; Disable interrupts
|
|
CpFET_off ; Cp off
|
|
BpFET_on ; Bp on
|
|
Set_Comp_Phase_C Temp1 ; Set comparator to phase C
|
|
sei ; Enable interrupts
|
|
ret
|
|
|
|
comm6comm1:
|
|
cli ; Disable interrupts
|
|
ldi XL, low(pwm_cfet_on) ; Set Z register to desired pwm_nfet_on label
|
|
ldi XH, high(pwm_cfet_on)
|
|
movw ZL, XL ; Atomic set (read by ISR)
|
|
AnFET_off ; An off
|
|
sbrc Flags1, PWM_ON ; Is pwm on?
|
|
CnFET_on ; Yes Cn on
|
|
Set_Comp_Phase_A Temp1 ; Set comparator to phase A
|
|
sei ; Enable interrupts
|
|
ret
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Switch power off routine
|
|
;
|
|
; No assumptions
|
|
;
|
|
; Switches all fets off
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
switch_power_off:
|
|
ldi XL, low(pwm_nofet_on) ; Set Z register to desired pwm_nfet_on label
|
|
ldi XH, high(pwm_nofet_on)
|
|
movw ZL, XL ; Atomic set (read by ISR)
|
|
All_nFETs_Off Temp1 ; Turn off all nfets
|
|
All_pFETs_Off Temp1 ; Turn off all pfets
|
|
ret
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Decrement stepper step routine
|
|
;
|
|
; No assumptions
|
|
;
|
|
; Decrements the stepper step
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
decrement_stepper_step:
|
|
lds Temp1, Wt_Stepper_Step_L
|
|
lds Temp2, Wt_Stepper_Step_H
|
|
cpi Temp1, low(STEPPER_STEP_END) ; Minimum STEPPER_STEP_END
|
|
ldi Temp3, high(STEPPER_STEP_END)
|
|
cpc Temp2, Temp3
|
|
brsh decrement_step ; Branch if same or higher than minimum
|
|
ret
|
|
|
|
decrement_step:
|
|
lds Temp1, Wt_Stepper_Step_L
|
|
lds Temp2, Wt_Stepper_Step_H
|
|
subi Temp1, low(STEPPER_STEP_DECREMENT)
|
|
sbci Temp2, high(STEPPER_STEP_DECREMENT)
|
|
sts Wt_Stepper_Step_L, Temp1
|
|
sts Wt_Stepper_Step_H, Temp2
|
|
ret
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Stepper timer wait
|
|
;
|
|
; No assumptions
|
|
;
|
|
; Waits for the stepper step timer to elapse
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
stepper_timer_wait:
|
|
sbrc Flags0, OCA_PENDING ; Timer pending?
|
|
rjmp stepper_timer_wait ; Yes, go back
|
|
ret
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Set default parameters
|
|
;
|
|
; No assumptions
|
|
;
|
|
; Sets default programming parameters
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
set_default_parameters:
|
|
.if TAIL == 0
|
|
ldi Temp1, 4
|
|
sts Pgm_Gov_P_Gain, Temp1
|
|
ldi Temp1, 4
|
|
sts Pgm_Gov_I_Gain, Temp1
|
|
ldi Temp1, 1
|
|
sts Pgm_Gov_Mode, Temp1
|
|
ldi Temp1, 3
|
|
sts Pgm_Startup_Pwr, Temp1
|
|
sbr Flags2, (1<<PGM_PWM_HIGH_FREQ)
|
|
cbr Flags2, (1<<PGM_RCP_PWM_POL)
|
|
.else
|
|
ldi Temp1, 3
|
|
sts Pgm_Tail_Gain, Temp1
|
|
ldi Temp1, 3
|
|
sts Pgm_Tail_Idle, Temp1
|
|
ldi Temp1, 3
|
|
sts Pgm_Startup_Pwr, Temp1
|
|
sbr Flags2, (1<<PGM_PWM_HIGH_FREQ)
|
|
cbr Flags2, (1<<PGM_RCP_PWM_POL)
|
|
ldi Temp1, 3
|
|
sts Pgm_Gov_Mode, Temp1
|
|
.endif
|
|
ret
|
|
|
|
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Main program start
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
|
|
.if TX_PGM == 1
|
|
.include "BLHeliTxPgm.inc" ; Include source code for programming the ESC with the TX
|
|
.endif
|
|
|
|
;**** **** **** **** ****
|
|
reset:
|
|
; Disable watchdog
|
|
Disable_Watchdog Temp1
|
|
; Initialize MCU
|
|
Initialize_MCU Temp1
|
|
; Initialize stack
|
|
ldi Temp1, high(RAMEND) ; Stack = RAMEND
|
|
out SPH, Temp1
|
|
ldi Temp1, low(RAMEND)
|
|
out SPL, Temp1
|
|
; Switch power off
|
|
rcall switch_power_off
|
|
; PortB initialization
|
|
ldi Temp1, INIT_PB
|
|
out PORTB, Temp1
|
|
ldi Temp1, DIR_PB
|
|
out DDRB, Temp1
|
|
; PortC initialization
|
|
ldi Temp1, INIT_PC
|
|
out PORTC, Temp1
|
|
ldi Temp1, DIR_PC
|
|
out DDRC, Temp1
|
|
; PortD initialization
|
|
ldi Temp1, INIT_PD
|
|
out PORTD, Temp1
|
|
ldi Temp1, DIR_PD
|
|
out DDRD, Temp1
|
|
; Clear registers r0 through r25
|
|
clr Zero
|
|
ldi XL, low(0) ; Register number
|
|
ldi XH, low(0)
|
|
clear_regs:
|
|
st X+, Zero ; Clear register and post increment register number
|
|
cpi XL, 26 ; Check register number - last register?
|
|
brne clear_regs ; If not last register, go back
|
|
; Clear RAM
|
|
ldi XL, low(SRAM_START)
|
|
ldi XH, high(SRAM_START)
|
|
ldi Temp1, SRAM_BYTES
|
|
clear_ram:
|
|
st X+, Zero
|
|
dec Temp1
|
|
brne clear_ram
|
|
; Switch power off
|
|
rcall switch_power_off
|
|
; Timer0: clk/8 for beep control, waiting and RC pulse timeouts
|
|
ldi Temp1, (1<<CS01)
|
|
Set_Timer0_CS0 Temp1
|
|
; Timer1: clk/8 for commutation control and RC pulse measurement
|
|
ldi Temp1, (1<<CS11)
|
|
Set_Timer1_CS1 Temp1
|
|
; Timer2: clk/1 for pwm
|
|
ldi Temp1, (1<<CS20)
|
|
Set_Timer2_CS2 Temp1
|
|
; Set default programmed parameters
|
|
rcall set_default_parameters
|
|
.if TX_PGM == 1
|
|
; Read programmed parameters
|
|
rcall read_eeprom_parameters
|
|
.endif
|
|
; Initializing beep
|
|
rcall wait200ms
|
|
rcall beep_f1
|
|
rcall wait30ms
|
|
rcall beep_f2
|
|
rcall wait30ms
|
|
rcall beep_f3
|
|
rcall wait30ms
|
|
; Initialize interrupts and registers
|
|
Initialize_Interrupts ; Set all interrupt enable bits
|
|
; Initialize ADC
|
|
Initialize_Adc ; Initialize ADC operation
|
|
sei ; Enable all interrupts
|
|
; Measure number of lipo cells
|
|
rcall Measure_Lipo_Cells ; Measure number of lipo cells
|
|
; Initialize rc pulse
|
|
.if ICP == 0
|
|
Rcp_Int_First Temp1 ; Set interrupt to first edge
|
|
Rcp_Int_Enable Temp1 ; Enable interrupt
|
|
Clear_Int_Flag Temp1 ; Clear interrupt flag
|
|
cbr Flags2, (1<<RCP_EDGE_NO) ; Set first edge flag
|
|
.else
|
|
Rcp_Icp_Int_First Temp1 ; Set interrupt to first edge
|
|
Rcp_Icp_Int_Enable Temp1 ; Enable ICP interrupt
|
|
Clear_Icp_Int_Flag Temp1 ; Clear interrupt flag
|
|
cbr Flags2, (1<<RCP_EDGE_NO) ; Set first edge flag
|
|
.endif
|
|
rcall wait200ms
|
|
|
|
; Wait for zero throttle
|
|
wait_for_zero_throttle:
|
|
rcall wait3ms ; Wait for next pulse (NB: Uses Temp1/2/3!)
|
|
lds Temp1, New_Rcp ; Load value
|
|
cpi Temp1, RCP_STOP ; Below stop?
|
|
brsh wait_for_zero_throttle ; No - start over
|
|
rcall wait200ms
|
|
|
|
; Validate RC pulse and measure PWM frequency
|
|
sbr Flags1, (1<<RCP_MEAS_PWM_FREQ) ; Set measure pwm frequency flag
|
|
validate_rcp_start:
|
|
ldi Temp2, 5 ; Number of pulses to validate
|
|
mov Temp5, Temp2
|
|
validate_rcp_loop:
|
|
rcall wait3ms ; Wait for next pulse (NB: Uses Temp1/2/3!)
|
|
validate_rcp_wait_updated:
|
|
sbrs Flags2, RCP_UPDATED ; Is there an updated RC pulse available?
|
|
rjmp validate_rcp_wait_updated ; No - wait for it
|
|
|
|
lds Temp1, New_Rcp ; Yes - load value
|
|
cpi Temp1, RCP_VALIDATE ; Higher than validate level?
|
|
brlo validate_rcp_start ; No - start over
|
|
mov Temp3, Flags2 ; Check pwm frequency flags
|
|
andi Temp3, (1<<RCP_PWM_FREQ_1KHZ)+(1<<RCP_PWM_FREQ_2KHZ)+(1<<RCP_PWM_FREQ_4KHZ)+(1<<RCP_PWM_FREQ_8KHZ)
|
|
lds Temp4, Prev_Rcp_Pwm_Freq ; Load previous flags
|
|
sts Prev_Rcp_Pwm_Freq, Temp3 ; Store previous flags for next pulse
|
|
cp Temp3, Temp4 ; New flags same as previous?
|
|
brne validate_rcp_start ; No - start over
|
|
|
|
dec Temp5 ; Required number of pulses seen?
|
|
brne validate_rcp_loop ; No - find more
|
|
|
|
cbr Flags1, (1<<RCP_MEAS_PWM_FREQ) ; Clear measure pwm frequency flag
|
|
; Set up RC pulse interrupts after pwm frequency measurement
|
|
.if ICP == 0
|
|
Rcp_Int_First Temp1 ; Set interrupt trig to first again
|
|
Clear_Int_Flag Temp1 ; Clear interrupt flag
|
|
.else
|
|
Rcp_Icp_Int_First Temp1 ; Set interrupt trig to first again
|
|
Clear_Icp_Int_Flag Temp1 ; Clear icp interrupt flag
|
|
.endif
|
|
cbr Flags2, (1<<RCP_EDGE_NO) ; Set first edge flag
|
|
|
|
; Beep arm sequence start signal
|
|
cli ; Disable all interrupts
|
|
rcall beep_f1 ; Signal that RC pulse is ready
|
|
rcall beep_f1
|
|
rcall beep_f1
|
|
sei ; Enable all interrupts
|
|
rcall wait200ms
|
|
|
|
; Arming sequence start
|
|
sts Gov_Arm_Target, Zero ; Clear governor arm target
|
|
arming_start:
|
|
rcall wait3ms
|
|
lds Temp1, New_Rcp ; Load new RC pulse value
|
|
cpi Temp1, RCP_MAX ; Is RC pulse max?
|
|
brlo program_by_tx_checked ; No - branch
|
|
|
|
.if TX_PGM == 1
|
|
rjmp program_by_tx ; Yes - start programming mode entry
|
|
.endif
|
|
|
|
program_by_tx_checked:
|
|
lds Temp2, Gov_Arm_Target ; Load governor arm target
|
|
cp Temp1, Temp2 ; Is RC pulse larger than arm target?
|
|
brlo arm_target_updated ; No - do not update
|
|
|
|
sts Gov_Arm_Target, Temp1 ; Yes - update arm target
|
|
|
|
arm_target_updated:
|
|
cpi Temp1, RCP_STOP ; Below stop?
|
|
brsh arming_start ; No - start over
|
|
|
|
; Beep arm sequence end signal
|
|
cli ; Disable all interrupts
|
|
rcall beep_f4 ; Signal that rcpulse is ready
|
|
rcall beep_f4
|
|
rcall beep_f4
|
|
sei ; Enable all interrupts
|
|
rcall wait200ms
|
|
|
|
; Armed and waiting for power on
|
|
wait_for_power_on:
|
|
rcall wait3ms
|
|
lds Temp1, New_Rcp ; Load value
|
|
cpi Temp1, RCP_STOP ; Higher than stop?
|
|
brlo wait_for_power_on ; No - start over
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Start entry point
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
init_start:
|
|
rcall switch_power_off
|
|
clr Requested_Pwm ; Set requested pwm to zero
|
|
clr Governor_Req_Pwm ; Set governor requested pwm to zero
|
|
clr Current_Pwm ; Set current pwm to zero
|
|
clr Current_Pwm_Limited ; Set limited current pwm to zero
|
|
ldi Temp1, 0xff ; Set pwm limit to max
|
|
sts Pwm_Limit, Temp1
|
|
sts Gov_Target_L, Zero ; Set target to zero
|
|
sts Gov_Target_H, Zero
|
|
sts Gov_Integral_L, Zero ; Set integral to zero
|
|
sts Gov_Integral_H, Zero
|
|
sts Gov_Integral_X, Zero
|
|
sts Gov_Active, Zero
|
|
clr Flags0 ; Clear flags0
|
|
clr Flags1 ; Clear flags1
|
|
sts Rcp_Stop_Cnt, Zero ; Set RC pulse stop count to zero
|
|
Comp_Init Temp1 ; Initialize comparator
|
|
rcall initialize_all_timings ; Initialize timing
|
|
|
|
;**** **** **** **** ****
|
|
; Settle mode beginning
|
|
;**** **** **** **** ****
|
|
sbr Flags1, (1<<PWM_OFF_DAMPED) ; Set damped operation
|
|
sbr Flags0, (1<<SETTLE_MODE) ; Set motor start settling mode flag
|
|
rcall set_startup_pwm
|
|
rcall comm6comm1damped ; Initialize commutation
|
|
rcall wait1ms
|
|
rcall comm1comm2damped
|
|
rcall wait1ms
|
|
rcall wait1ms
|
|
rcall comm2comm3damped
|
|
rcall wait3ms
|
|
rcall comm3comm4damped
|
|
rcall wait3ms
|
|
rcall wait3ms
|
|
rcall comm4comm5damped
|
|
rcall wait10ms ; Settle rotor
|
|
rcall comm5comm6damped
|
|
rcall wait3ms
|
|
rcall wait1ms
|
|
cbr Flags0, (1<<SETTLE_MODE) ; Clear settling mode flag
|
|
sbr Flags0, (1<<STEPPER_MODE) ; Set motor start stepper mode flag
|
|
rcall set_startup_pwm
|
|
|
|
;**** **** **** **** ****
|
|
; Stepper mode beginning
|
|
;**** **** **** **** ****
|
|
stepper_rot_beg:
|
|
rcall comm6comm1damped ; Commutate
|
|
rcall calc_next_comm_timing_start ; Update timing and set timer
|
|
rcall calc_new_wait_times
|
|
rcall decrement_stepper_step
|
|
rcall stepper_timer_wait
|
|
|
|
rcall comm1comm2damped
|
|
rcall calc_next_comm_timing_start
|
|
rcall calc_new_wait_times
|
|
rcall decrement_stepper_step
|
|
rcall stepper_timer_wait
|
|
|
|
rcall comm2comm3damped
|
|
rcall calc_next_comm_timing_start
|
|
rcall calc_new_wait_times
|
|
rcall decrement_stepper_step
|
|
rcall stepper_timer_wait
|
|
|
|
rcall comm3comm4damped
|
|
rcall calc_next_comm_timing_start
|
|
rcall calc_new_wait_times
|
|
rcall decrement_stepper_step
|
|
rcall stepper_timer_wait
|
|
|
|
rcall comm4comm5damped
|
|
rcall calc_next_comm_timing_start
|
|
rcall calc_new_wait_times
|
|
rcall decrement_stepper_step
|
|
rcall stepper_timer_wait
|
|
|
|
rcall comm5comm6damped
|
|
rcall calc_next_comm_timing_start
|
|
rcall calc_new_wait_times
|
|
rcall decrement_stepper_step
|
|
; Check stepper step versus end criteria
|
|
lds Temp1, Wt_Stepper_Step_L
|
|
lds Temp2, Wt_Stepper_Step_H
|
|
cpi Temp1, low(STEPPER_STEP_END) ; Minimum STEPPER_STEP_END
|
|
ldi Temp3, high(STEPPER_STEP_END)
|
|
cpc Temp2, Temp3
|
|
brlo stepper_rot_exit ; Branch if lower than minimum
|
|
; Wait for step
|
|
rcall stepper_timer_wait
|
|
rjmp stepper_rot_beg ; Next rotation
|
|
|
|
stepper_rot_exit:
|
|
; Set aquisition mode
|
|
cbr Flags0, (1<<STEPPER_MODE) ; Clear motor start stepper mode flag
|
|
sbr Flags0, (1<<AQUISITION_MODE) ; Set aquisition mode flag
|
|
rcall set_startup_pwm
|
|
; Set aquisition rotation count
|
|
ldi Temp1, AQUISITION_ROTATIONS
|
|
sts Startup_Rot_Cnt, Temp1
|
|
; Wait for step
|
|
rcall stepper_timer_wait ; As the last part of stepper mode
|
|
|
|
;**** **** **** **** ****
|
|
; Aquisition mode beginning
|
|
;**** **** **** **** ****
|
|
aquisition_rot_beg:
|
|
rcall comm6comm1damped ; Commutate
|
|
rcall calc_next_comm_timing_start ; Update timing and set timer
|
|
rcall calc_new_wait_times
|
|
rcall decrement_stepper_step
|
|
rcall stepper_timer_wait
|
|
|
|
rcall comm1comm2damped
|
|
rcall calc_next_comm_timing_start
|
|
rcall calc_new_wait_times
|
|
rcall decrement_stepper_step
|
|
rcall stepper_timer_wait
|
|
|
|
rcall comm2comm3damped
|
|
rcall calc_next_comm_timing_start
|
|
rcall calc_new_wait_times
|
|
rcall decrement_stepper_step
|
|
rcall stepper_timer_wait
|
|
|
|
rcall comm3comm4damped
|
|
rcall calc_next_comm_timing_start
|
|
rcall calc_new_wait_times
|
|
rcall decrement_stepper_step
|
|
rcall stepper_timer_wait
|
|
|
|
rcall comm4comm5damped
|
|
rcall calc_next_comm_timing_start
|
|
rcall calc_new_wait_times
|
|
rcall decrement_stepper_step
|
|
rcall stepper_timer_wait
|
|
|
|
rcall comm5comm6damped
|
|
rcall calc_next_comm_timing_start
|
|
rcall calc_new_wait_times
|
|
rcall decrement_stepper_step
|
|
; Decrement startup rotation count
|
|
lds Temp1, Startup_Rot_Cnt
|
|
dec Temp1
|
|
; Check number of aquisition rotations
|
|
breq aquisition_rot_exit ; Branch if counter is zero
|
|
|
|
; Store counter
|
|
sts Startup_Rot_Cnt, Temp1
|
|
; Wait for step
|
|
rcall stepper_timer_wait
|
|
rjmp aquisition_rot_beg ; Next rotation
|
|
|
|
aquisition_rot_exit:
|
|
cbr Flags0, (1<<AQUISITION_MODE) ; Clear aquisition mode flag
|
|
sbr Flags0, (1<<INITIAL_RUN_MODE) ; Set initial run mode flag
|
|
rcall set_startup_pwm
|
|
rcall stepper_timer_wait ; As the last part of aquisition mode
|
|
|
|
rcall comm6comm1damped
|
|
rcall calc_next_comm_timing
|
|
rcall wait_advance_timing ; Wait advance timing and start zero cross wait
|
|
rcall calc_new_wait_times
|
|
rcall wait_before_zc_scan ; Wait zero cross wait and start zero cross timeout
|
|
|
|
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
;
|
|
; Run entry point
|
|
;
|
|
;**** **** **** **** **** **** **** **** **** **** **** **** ****
|
|
; Set damped run rotation count
|
|
ldi Temp1, DAMPED_RUN_ROTATIONS
|
|
sts Startup_Rot_Cnt, Temp1
|
|
|
|
; Damped run 1 = B(p-on) + C(n-choppered) - comparator A evaluated
|
|
; Out_cA changes from high to low
|
|
damped_run1:
|
|
rcall wait_for_comp_out_low ; Wait zero cross wait and wait for low
|
|
sbrs Flags0, OCA_PENDING ; Has timeout elapsed?
|
|
rjmp run_to_wait_for_power_on ; Yes - exit run mode
|
|
|
|
rcall setup_comm_wait ; Setup wait time from zero cross to commutation
|
|
rcall wait_for_comm ; Wait from zero cross to commutation
|
|
rcall comm1comm2damped ; Commutate
|
|
rcall calc_next_comm_timing ; Calculate next timing and start advance timing wait
|
|
rcall wait_advance_timing ; Wait advance timing and start zero cross wait
|
|
rcall calc_new_wait_times
|
|
rcall wait_before_zc_scan ; Wait zero cross wait and start zero cross timeout
|
|
|
|
; Damped run 2 = A(p-on) + C(n-choppered) - comparator B evaluated
|
|
; Out_cB changes from low to high
|
|
damped_run2:
|
|
rcall wait_for_comp_out_high
|
|
sbrs Flags0, OCA_PENDING
|
|
rjmp run_to_wait_for_power_on
|
|
|
|
rcall setup_comm_wait
|
|
rcall wait_for_comm
|
|
rcall comm2comm3damped
|
|
rcall calc_next_comm_timing
|
|
rcall wait_advance_timing
|
|
rcall calc_new_wait_times
|
|
rcall wait_before_zc_scan
|
|
|
|
; Damped run 3 = A(p-on) + B(n-choppered) - comparator C evaluated
|
|
; Out_cC changes from high to low
|
|
damped_run3:
|
|
rcall wait_for_comp_out_low
|
|
sbrs Flags0, OCA_PENDING
|
|
rjmp run_to_wait_for_power_on
|
|
|
|
rcall setup_comm_wait
|
|
rcall wait_for_comm
|
|
rcall comm3comm4damped
|
|
rcall calc_next_comm_timing
|
|
rcall wait_advance_timing
|
|
rcall calc_new_wait_times
|
|
rcall wait_before_zc_scan
|
|
|
|
; Damped run 4 = C(p-on) + B(n-choppered) - comparator A evaluated
|
|
; Out_cA changes from low to high
|
|
damped_run4:
|
|
rcall wait_for_comp_out_high
|
|
sbrs Flags0, OCA_PENDING
|
|
rjmp run_to_wait_for_power_on
|
|
|
|
rcall setup_comm_wait
|
|
rcall wait_for_comm
|
|
rcall comm4comm5damped
|
|
rcall calc_next_comm_timing
|
|
rcall wait_advance_timing
|
|
rcall calc_new_wait_times
|
|
rcall wait_before_zc_scan
|
|
|
|
; Damped run 5 = C(p-on) + A(n-choppered) - comparator B evaluated
|
|
; Out_cB changes from high to low
|
|
damped_run5:
|
|
rcall wait_for_comp_out_low
|
|
sbrs Flags0, OCA_PENDING
|
|
rjmp run_to_wait_for_power_on
|
|
|
|
rcall setup_comm_wait
|
|
rcall wait_for_comm
|
|
rcall comm5comm6damped
|
|
rcall calc_next_comm_timing
|
|
rcall wait_advance_timing
|
|
rcall calc_new_wait_times
|
|
rcall wait_before_zc_scan
|
|
|
|
; Damped run 6 = B(p-on) + A(n-choppered) - comparator C evaluated
|
|
; Out_cC changes from low to high
|
|
damped_run6:
|
|
rcall wait_for_comp_out_high
|
|
sbrs Flags0, OCA_PENDING
|
|
rjmp run_to_wait_for_power_on
|
|
|
|
rcall setup_comm_wait
|
|
rcall wait_for_comm
|
|
rcall comm6comm1damped
|
|
rcall calc_next_comm_timing
|
|
rcall wait_advance_timing
|
|
rcall calc_new_wait_times
|
|
rcall wait_before_zc_scan
|
|
|
|
; Decrement startup rotaton count
|
|
lds Temp1, Startup_Rot_Cnt
|
|
dec Temp1
|
|
; Check number of aquisition rotations
|
|
breq non_damped_run ; Branch if counter is zero
|
|
|
|
sts Startup_Rot_Cnt, Temp1 ; No - store counter
|
|
rjmp damped_run1 ; Continue to run damped
|
|
|
|
non_damped_run:
|
|
; Transition from damped to non-damped
|
|
cli ; Disable interrupts
|
|
cbr Flags1, (1<<PWM_OFF_DAMPED) ; Clear damped flag
|
|
All_pFETs_Off Temp1 ; Turn off all pfets
|
|
BpFET_on ; Bp on
|
|
ldi XL, low(pwm_cfet_on) ; Set Z register to desired pwm_nfet_on label
|
|
ldi XH, high(pwm_cfet_on)
|
|
movw ZL, XL ; Atomic set (read by ISR)
|
|
sei ; Enable interrupts
|
|
|
|
; Run 1 = B(p-on) + C(n-choppered) - comparator A evaluated
|
|
; Out_cA changes from high to low
|
|
run1:
|
|
rcall wait_for_comp_out_low ; Wait zero cross wait and wait for low
|
|
sbrs Flags0, OCA_PENDING ; Has timeout elapsed?
|
|
rjmp run_to_wait_for_power_on ; Yes - exit run mode
|
|
|
|
rcall setup_comm_wait ; Setup wait time from zero cross to commutation
|
|
rcall calc_governor_target ; Calculate governor target
|
|
rcall wait_for_comm ; Wait from zero cross to commutation
|
|
rcall comm1comm2 ; Commutate
|
|
rcall calc_next_comm_timing ; Calculate next timing and start advance timing wait
|
|
rcall wait_advance_timing ; Wait advance timing and start zero cross wait
|
|
rcall calc_new_wait_times
|
|
rcall wait_before_zc_scan ; Wait zero cross wait and start zero cross timeout
|
|
|
|
; Run 2 = A(p-on) + C(n-choppered) - comparator B evaluated
|
|
; Out_cB changes from low to high
|
|
run2:
|
|
rcall wait_for_comp_out_high
|
|
sbrs Flags0, OCA_PENDING
|
|
rjmp run_to_wait_for_power_on
|
|
|
|
rcall setup_comm_wait
|
|
rcall calc_governor_prop_error
|
|
rcall wait_for_comm
|
|
rcall comm2comm3
|
|
rcall calc_next_comm_timing
|
|
rcall wait_advance_timing
|
|
rcall calc_new_wait_times
|
|
rcall wait_before_zc_scan
|
|
|
|
; Run 3 = A(p-on) + B(n-choppered) - comparator C evaluated
|
|
; Out_cC changes from high to low
|
|
run3:
|
|
rcall wait_for_comp_out_low
|
|
sbrs Flags0, OCA_PENDING
|
|
rjmp run_to_wait_for_power_on
|
|
|
|
rcall setup_comm_wait
|
|
rcall calc_governor_int_error
|
|
rcall wait_for_comm
|
|
rcall comm3comm4
|
|
rcall calc_next_comm_timing
|
|
rcall wait_advance_timing
|
|
rcall calc_new_wait_times
|
|
rcall wait_before_zc_scan
|
|
|
|
; Run 4 = C(p-on) + B(n-choppered) - comparator A evaluated
|
|
; Out_cA changes from low to high
|
|
run4:
|
|
rcall wait_for_comp_out_high
|
|
sbrs Flags0, OCA_PENDING
|
|
rjmp run_to_wait_for_power_on
|
|
|
|
rcall setup_comm_wait
|
|
rcall calc_governor_prop_correction
|
|
rcall wait_for_comm
|
|
rcall comm4comm5
|
|
rcall calc_next_comm_timing
|
|
rcall wait_advance_timing
|
|
rcall calc_new_wait_times
|
|
rcall wait_before_zc_scan
|
|
|
|
; Run 5 = C(p-on) + A(n-choppered) - comparator B evaluated
|
|
; Out_cB changes from high to low
|
|
run5:
|
|
rcall wait_for_comp_out_low
|
|
sbrs Flags0, OCA_PENDING
|
|
rjmp run_to_wait_for_power_on
|
|
|
|
rcall setup_comm_wait
|
|
rcall calc_governor_int_correction
|
|
rcall wait_for_comm
|
|
rcall comm5comm6
|
|
rcall calc_next_comm_timing
|
|
rcall wait_advance_timing
|
|
rcall calc_new_wait_times
|
|
rcall wait_before_zc_scan
|
|
|
|
; Run 6 = B(p-on) + A(n-choppered) - comparator C evaluated
|
|
; Out_cC changes from low to high
|
|
run6:
|
|
rcall wait_for_comp_out_high
|
|
rcall start_adc_conversion
|
|
sbrs Flags0, OCA_PENDING
|
|
rjmp run_to_wait_for_power_on
|
|
|
|
rcall setup_comm_wait
|
|
rcall check_voltage_and_limit_power
|
|
rcall wait_for_comm
|
|
rcall comm6comm1
|
|
rcall calc_next_comm_timing
|
|
rcall wait_advance_timing
|
|
rcall calc_new_wait_times
|
|
rcall wait_before_zc_scan
|
|
|
|
cbr Flags0, (1<<INITIAL_RUN_MODE) ; Clear initial run mode flag
|
|
|
|
lds Temp1, Rcp_Stop_Cnt ; Load stop RC pulse counter value
|
|
cpi Temp1, RCP_STOP_LIMIT ; Is number of stop RC pulses above limit?
|
|
brcc run_to_wait_for_power_on ; Yes, go back to wait for poweron
|
|
|
|
lds Temp1, Comm_Period4x_H
|
|
sbrc Temp1, 7 ; Is Comm_Period4x more than 32ms (~1220 eRPM)?
|
|
brne run_to_wait_for_power_on ; Yes - go back to motor start
|
|
rjmp run1 ; Go back to run 1
|
|
|
|
run_to_wait_for_power_on:
|
|
rcall switch_power_off
|
|
clr Requested_Pwm ; Set requested pwm to zero
|
|
clr Governor_Req_Pwm ; Set governor requested pwm to zero
|
|
clr Current_Pwm ; Set current pwm to zero
|
|
clr Current_Pwm_Limited ; Set limited current pwm to zero
|
|
.if TAIL == 1
|
|
rjmp wait_for_power_on ; Tail - Go back to wait for power on
|
|
.else
|
|
rjmp validate_rcp_start ; Main - Go back to validate RC pulse
|
|
.endif
|
|
|
|
.exit
|