;**** **** **** **** ****
;
; 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 .
;
;**** **** **** **** ****
;
; 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<