$NOMOD51 ;**** **** **** **** **** ; ; BLHeli program for controlling brushless motors in multirotors ; ; 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 was initially designed for use with Eflite mCP X, but is now adapted to copters/planes in general ; ; The software was inspired by and started from from Bernard Konze's BLMC: http://home.versanet.de/~bkonze/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 code is designed for multirotor applications, running damped light mode ; ; The input signal can be Normal (1-2ms), OneShot125 (125-250us), OneShot42 (41.7-83.3us) or Multishot (5-25us) at rates as high as allowed by the format. ; The code autodetects normal, OneShot125, Oneshot42 or Multishot. ; ; The first lines of the software must be modified according to the chosen environment: ; ESCNO EQU "ESC" ; MCU_48MHZ EQU "N" ; FETON_DELAY EQU "N" ; ;**** **** **** **** **** ; Revision history: ; - Rev16.0 Started. Built upon rev 14.5 of base code ; Using hardware pwm for very smooth throttle response, silent running and support of very high rpms ; Implemented reverse bidirectional mode ; Implemented separate throttle gains fwd and rev in bidirectional mode ; Implemented support for Oneshot42 and Multishot ; - Rev16.1 Made low rpm power limiting programmable through the startup power parameter ; - Rev16.2 Fixed bug that prevented temperature protection ; Improved robustness to very high input signal rates ; Beeps can be turned off by programming beep strength to 1 ; Throttle cal difference is checked to be above required minimum before storing. Throttle cal max is not stored until successful min throttle cal ; - Rev16.3 Implemented programmable temperature protection ; Improved protection of bootloader and generally reduced risk of flash corruption ; Some small changes for improved sync hold ; ; ;**** **** **** **** **** ; Minimum 8K Bytes of In-System Self-Programmable Flash ; Minimum 512 Bytes Internal SRAM ; ;**** **** **** **** **** ; Master clock is internal 24MHz oscillator (or 48MHz, for which the times below are halved) ; Although 24/48 are used in the code, the exact clock frequencies are 24.5MHz or 49.0 MHz ; Timer 0 (41.67ns counts) always counts up and is used for ; - RC pulse measurement ; Timer 2 (500ns counts) always counts up and is used for ; - RC pulse timeout counts and commutation times ; Timer 3 (500ns counts) always counts up and is used for ; - Commutation timeouts ; PCA0 (41.67ns counts) always counts up and is used for ; - Hardware PWM generation ; ;**** **** **** **** **** ; Interrupt handling ; The C8051 does not disable interrupts when entering an interrupt routine. ; Also some interrupt flags need to be cleared by software ; The code disables interrupts in some interrupt routines ; - Interrupts are disabled during beeps, to avoid audible interference from interrupts ; ;**** **** **** **** **** ; Motor control: ; - Brushless motor control with 6 states for each electrical 360 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 nominally ; - Motor pwm is always damped light (aka complementary pwm, regenerative braking) ; 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: ; There is a startup phase and an initial run phase, before normal bemf commutation run begins. ; ;**** **** **** **** **** ; List of enumerated supported ESCs A_ EQU 1 ; X X RC X MC MB MA CC X X Cc Cp Bc Bp Ac Ap B_ EQU 2 ; X X RC X MC MB MA CC X X Ap Ac Bp Bc Cp Cc C_ EQU 3 ; Ac Ap MC MB MA CC X RC X X X X Cc Cp Bc Bp D_ EQU 4 ; X X RC X CC MA MC MB X X Cc Cp Bc Bp Ac Ap Com fets inverted E_ EQU 5 ; L1 L0 RC X MC MB MA CC X L2 Cc Cp Bc Bp Ac Ap A with LEDs F_ EQU 6 ; X X RC X MA MB MC CC X X Cc Cp Bc Bp Ac Ap G_ EQU 7 ; X X RC X CC MA MC MB X X Cc Cp Bc Bp Ac Ap Like D, but noninverted com fets H_ EQU 8 ; RC X X X MA MB CC MC X Ap Bp Cp X Ac Bc Cc I_ EQU 9 ; X X RC X MC MB MA CC X X Ac Bc Cc Ap Bp Cp J_ EQU 10 ; L2 L1 L0 RC CC MB MC MA X X Cc Bc Ac Cp Bp Ap K_ EQU 11 ; X X MC X MB CC MA RC X X Ap Bp Cp Cc Bc Ac Com fets inverted L_ EQU 12 ; X X RC X CC MA MB MC X X Ac Bc Cc Ap Bp Cp ;**** **** **** **** **** ; Select the port mapping to use (or unselect all for use with external batch compile file) ;ESCNO EQU A_ ;ESCNO EQU B_ ;ESCNO EQU C_ ;ESCNO EQU D_ ;ESCNO EQU E_ ;ESCNO EQU F_ ;ESCNO EQU G_ ;ESCNO EQU H_ ;ESCNO EQU I_ ;ESCNO EQU J_ ;ESCNO EQU K_ ;ESCNO EQU L_ ;**** **** **** **** **** ; Select the MCU type (or unselect for use with external batch compile file) ;MCU_48MHZ EQU 1 ;**** **** **** **** **** ; Select the fet deadtime (or unselect for use with external batch compile file) ;FETON_DELAY EQU 50 ; 20.4ns per step ;**** **** **** **** **** ; ESC selection statements IF ESCNO == A_ $include (A.inc) ; Select pinout A ENDIF IF ESCNO == B_ $include (B.inc) ; Select pinout B ENDIF IF ESCNO == C_ $include (C.inc) ; Select pinout C ENDIF IF ESCNO == D_ $include (D.inc) ; Select pinout D ENDIF IF ESCNO == E_ $include (E.inc) ; Select pinout E ENDIF IF ESCNO == F_ $include (F.inc) ; Select pinout F ENDIF IF ESCNO == G_ $include (G.inc) ; Select pinout G ENDIF IF ESCNO == H_ $include (H.inc) ; Select pinout H ENDIF IF ESCNO == I_ $include (I.inc) ; Select pinout I ENDIF IF ESCNO == J_ $include (J.inc) ; Select pinout J ENDIF IF ESCNO == K_ $include (K.inc) ; Select pinout K ENDIF IF ESCNO == L_ $include (L.inc) ; Select pinout L ENDIF ;**** **** **** **** **** ; Programming defaults ; DEFAULT_PGM_STARTUP_PWR EQU 9 ; 1=0.031 2=0.047 3=0.063 4=0.094 5=0.125 6=0.188 7=0.25 8=0.38 9=0.50 10=0.75 11=1.00 12=1.25 13=1.50 DEFAULT_PGM_COMM_TIMING EQU 3 ; 1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=High DEFAULT_PGM_DEMAG_COMP EQU 2 ; 1=Disabled 2=Low 3=High DEFAULT_PGM_DIRECTION EQU 1 ; 1=Normal 2=Reversed 3=Bidir 4=Bidir rev DEFAULT_PGM_BEEP_STRENGTH EQU 40 ; Beep strength DEFAULT_PGM_BEACON_STRENGTH EQU 80 ; Beacon strength DEFAULT_PGM_BEACON_DELAY EQU 4 ; 1=1m 2=2m 3=5m 4=10m 5=Infinite ; COMMON DEFAULT_PGM_ENABLE_TX_PROGRAM EQU 1 ; 1=Enabled 0=Disabled DEFAULT_PGM_MIN_THROTTLE EQU 37 ; 4*37+1000=1148 DEFAULT_PGM_MAX_THROTTLE EQU 208 ; 4*208+1000=1832 DEFAULT_PGM_CENTER_THROTTLE EQU 122 ; 4*122+1000=1488 (used in bidirectional mode) DEFAULT_PGM_ENABLE_TEMP_PROT EQU 7 ; 0=Disabled 1=80C 2=90C 3=100C 4=110C 5=120C 6=130C 7=140C DEFAULT_PGM_ENABLE_POWER_PROT EQU 1 ; 1=Enabled 0=Disabled DEFAULT_PGM_BRAKE_ON_STOP EQU 0 ; 1=Enabled 0=Disabled DEFAULT_PGM_LED_CONTROL EQU 0 ; Byte for LED control. 2bits per LED, 0=Off, 1=On ;**** **** **** **** **** ; Temporary register definitions Temp1 EQU R0 Temp2 EQU R1 Temp3 EQU R2 Temp4 EQU R3 Temp5 EQU R4 Temp6 EQU R5 Temp7 EQU R6 Temp8 EQU R7 ;**** **** **** **** **** ; Register definitions DSEG AT 20h ; Variables segment Bit_Access: DS 1 ; MUST BE AT THIS ADDRESS. Variable at bit accessible address (for non interrupt routines) Bit_Access_Int: DS 1 ; Variable at bit accessible address (for interrupts) Rcp_Outside_Range_Cnt: DS 1 ; RC pulse outside range counter (incrementing) Rcp_Timeout_Cntd: DS 1 ; RC pulse timeout counter (decrementing) Flags0: DS 1 ; State flags. Reset upon init_start T3_PENDING EQU 0 ; Timer3 pending flag DEMAG_DETECTED EQU 1 ; Set when excessive demag time is detected DEMAG_CUT_POWER EQU 2 ; Set when demag compensation cuts power COMP_TIMED_OUT EQU 3 ; Set when comparator reading timed out ; EQU 4 ; EQU 5 ; EQU 6 ; EQU 7 Flags1: DS 1 ; State flags. Reset upon init_start STARTUP_PHASE EQU 0 ; Set when in startup phase INITIAL_RUN_PHASE EQU 1 ; Set when in initial run phase, before synchronized run is achieved MOTOR_STARTED EQU 2 ; Set when motor is started DIR_CHANGE_BRAKE EQU 3 ; Set when braking before direction change HIGH_RPM EQU 4 ; Set when motor rpm is high (Comm_Period4x_H less than 2) ; EQU 5 ; EQU 6 ; EQU 7 Flags2: DS 1 ; State flags. NOT reset upon init_start RCP_UPDATED EQU 0 ; New RC pulse length value available RCP_ONESHOT125 EQU 1 ; RC pulse input is OneShot125 (125-250us) RCP_ONESHOT42 EQU 2 ; RC pulse input is OneShot42 (41.67-83us) RCP_MULTISHOT EQU 3 ; RC pulse input is Multishot (5-25us) RCP_DIR_REV EQU 4 ; RC pulse direction in bidirectional mode RCP_FULL_RANGE EQU 5 ; When set full input signal range is used (1000-2000us) and stored calibration values are ignored ; EQU 6 ; EQU 7 Flags3: DS 1 ; State flags. NOT reset upon init_start PGM_DIR_REV EQU 0 ; Programmed direction. 0=normal, 1=reversed PGM_BIDIR_REV EQU 1 ; Programmed bidirectional direction. 0=normal, 1=reversed PGM_BIDIR EQU 2 ; Programmed bidirectional operation. 0=normal, 1=bidirectional ; EQU 3 ; EQU 4 ; EQU 5 ; EQU 6 ; EQU 7 ;**** **** **** **** **** ; RAM definitions DSEG AT 30h ; Ram data segment, direct addressing Initial_Arm: DS 1 ; Variable that is set during the first arm sequence after power on Min_Throttle_L: DS 1 ; Minimum throttle scaled (lo byte) Min_Throttle_H: DS 1 ; Minimum throttle scaled (hi byte) Center_Throttle_L: DS 1 ; Center throttle scaled (lo byte) Center_Throttle_H: DS 1 ; Center throttle scaled (hi byte) Max_Throttle_L: DS 1 ; Maximum throttle scaled (lo byte) Max_Throttle_H: DS 1 ; Maximum throttle scaled (hi byte) Power_On_Wait_Cnt_L: DS 1 ; Power on wait counter (lo byte) Power_On_Wait_Cnt_H: DS 1 ; Power on wait counter (hi byte) Startup_Cnt: DS 1 ; Startup phase commutations counter (incrementing) Startup_Zc_Timeout_Cntd: DS 1 ; Startup zero cross timeout counter (decrementing) Initial_Run_Rot_Cntd: DS 1 ; Initial run rotations counter (decrementing) Stall_Cnt: DS 1 ; Counts start/run attempts that resulted in stall. Reset upon a proper stop Demag_Detected_Metric: DS 1 ; Metric used to gauge demag event frequency Demag_Pwr_Off_Thresh: DS 1 ; Metric threshold above which power is cut Low_Rpm_Pwr_Slope: DS 1 ; Sets the slope of power increase for low rpms Timer0_X: DS 1 ; Timer 0 extended byte Timer2_X: DS 1 ; Timer 2 extended byte Prev_Comm_L: DS 1 ; Previous commutation timer3 timestamp (lo byte) Prev_Comm_H: DS 1 ; Previous commutation timer3 timestamp (hi byte) Prev_Comm_X: DS 1 ; Previous commutation timer3 timestamp (ext byte) Prev_Prev_Comm_L: DS 1 ; Pre-previous commutation timer3 timestamp (lo byte) Prev_Prev_Comm_H: DS 1 ; Pre-previous commutation timer3 timestamp (hi byte) Comm_Period4x_L: DS 1 ; Timer3 counts between the last 4 commutations (lo byte) Comm_Period4x_H: DS 1 ; Timer3 counts between the last 4 commutations (hi byte) Comparator_Read_Cnt: DS 1 ; Number of comparator reads done Wt_Adv_Start_L: DS 1 ; Timer3 start point for commutation advance timing (lo byte) Wt_Adv_Start_H: DS 1 ; Timer3 start point for commutation advance timing (hi byte) Wt_Zc_Scan_Start_L: DS 1 ; Timer3 start point from commutation to zero cross scan (lo byte) Wt_Zc_Scan_Start_H: DS 1 ; Timer3 start point from commutation to zero cross scan (hi byte) Wt_Zc_Tout_Start_L: DS 1 ; Timer3 start point for zero cross scan timeout (lo byte) Wt_Zc_Tout_Start_H: DS 1 ; Timer3 start point for zero cross scan timeout (hi byte) Wt_Comm_Start_L: DS 1 ; Timer3 start point from zero cross to commutation (lo byte) Wt_Comm_Start_H: DS 1 ; Timer3 start point from zero cross to commutation (hi byte) New_Rcp: DS 1 ; New RC pulse value in pca counts Rcp_Stop_Cnt: DS 1 ; Counter for RC pulses below stop value Power_Pwm_Reg_L: DS 1 ; Power pwm register setting (lo byte) Power_Pwm_Reg_H: DS 1 ; Power pwm register setting (hi byte). 0x3F is minimum power Damp_Pwm_Reg_L: DS 1 ; Damping pwm register setting (lo byte) Damp_Pwm_Reg_H: DS 1 ; Damping pwm register setting (hi byte) Current_Power_Pwm_Reg_H: DS 1 ; Current power pwm register setting that is loaded in the PCA register (hi byte) Pwm_Limit: DS 1 ; Maximum allowed pwm Pwm_Limit_By_Rpm: DS 1 ; Maximum allowed pwm for low or high rpms Pwm_Limit_Beg: DS 1 ; Initial pwm limit Adc_Conversion_Cnt: DS 1 ; Adc conversion counter Current_Average_Temp: DS 1 ; Current average temperature (lo byte ADC reading, assuming hi byte is 1) Throttle_Gain: DS 1 ; Gain to be applied to RCP value Throttle_Gain_M: DS 1 ; Gain to be applied to RCP value (multiplier 0=1x, 1=2x, 2=4x etc)) Throttle_Gain_BD_Rev: DS 1 ; Gain to be applied to RCP value for reverse direction in bidirectional mode Throttle_Gain_BD_Rev_M: DS 1 ; Gain to be applied to RCP value for reverse direction in bidirectional mode (multiplier 0=1x, 1=2x, 2=4x etc) Beep_Strength: DS 1 ; Strength of beeps Skip_T2_Int: DS 1 ; Set for 48MHz MCUs when timer 2 interrupt shall be ignored Clock_Set_At_48MHz: DS 1 ; Variable set if 48MHz MCUs run at 48MHz Flash_Key_1: DS 1 ; Flash key one Flash_Key_2: DS 1 ; Flash key two Temp_Prot_Limit: DS 1 ; Temperature protection limit ; Indirect addressing data segment. The variables below must be in this sequence ISEG AT 080h _Pgm_Gov_P_Gain: DS 1 ; Programmed governor P gain _Pgm_Gov_I_Gain: DS 1 ; Programmed governor I gain _Pgm_Gov_Mode: DS 1 ; Programmed governor mode _Pgm_Low_Voltage_Lim: DS 1 ; Programmed low voltage limit _Pgm_Motor_Gain: DS 1 ; Programmed motor gain _Pgm_Motor_Idle: DS 1 ; Programmed motor idle speed Pgm_Startup_Pwr: DS 1 ; Programmed startup power _Pgm_Pwm_Freq: DS 1 ; Programmed pwm frequency Pgm_Direction: DS 1 ; Programmed rotation direction Pgm_Input_Pol: DS 1 ; Programmed input pwm polarity Initialized_L_Dummy: DS 1 ; Place holder Initialized_H_Dummy: DS 1 ; Place holder Pgm_Enable_TX_Program: DS 1 ; Programmed enable/disable value for TX programming _Pgm_Main_Rearm_Start: DS 1 ; Programmed enable/disable re-arming main every start _Pgm_Gov_Setup_Target: DS 1 ; Programmed main governor setup target _Pgm_Startup_Rpm: DS 1 ; Programmed startup rpm (unused - place holder) _Pgm_Startup_Accel: DS 1 ; Programmed startup acceleration (unused - place holder) _Pgm_Volt_Comp: DS 1 ; Place holder Pgm_Comm_Timing: DS 1 ; Programmed commutation timing _Pgm_Damping_Force: DS 1 ; Programmed damping force (unused - place holder) _Pgm_Gov_Range: DS 1 ; Programmed governor range _Pgm_Startup_Method: DS 1 ; Programmed startup method (unused - place holder) Pgm_Min_Throttle: DS 1 ; Programmed throttle minimum Pgm_Max_Throttle: DS 1 ; Programmed throttle maximum Pgm_Beep_Strength: DS 1 ; Programmed beep strength Pgm_Beacon_Strength: DS 1 ; Programmed beacon strength Pgm_Beacon_Delay: DS 1 ; Programmed beacon delay _Pgm_Throttle_Rate: DS 1 ; Programmed throttle rate (unused - place holder) Pgm_Demag_Comp: DS 1 ; Programmed demag compensation _Pgm_BEC_Voltage_High: DS 1 ; Programmed BEC voltage Pgm_Center_Throttle: DS 1 ; Programmed throttle center (in bidirectional mode) _Pgm_Main_Spoolup_Time: DS 1 ; Programmed main spoolup time Pgm_Enable_Temp_Prot: DS 1 ; Programmed temperature protection enable Pgm_Enable_Power_Prot: DS 1 ; Programmed low rpm power protection enable _Pgm_Enable_Pwm_Input: DS 1 ; Programmed PWM input signal enable _Pgm_Pwm_Dither: DS 1 ; Programmed output PWM dither Pgm_Brake_On_Stop: DS 1 ; Programmed braking when throttle is zero Pgm_LED_Control: DS 1 ; Programmed LED control ; The sequence of the variables below is no longer of importance Pgm_Startup_Pwr_Decoded: DS 1 ; Programmed startup power decoded ; Indirect addressing data segment ISEG AT 0D0h Tag_Temporary_Storage: DS 48 ; Temporary storage for tags when updating "Eeprom" ;**** **** **** **** **** CSEG AT 1A00h ; "Eeprom" segment EEPROM_FW_MAIN_REVISION EQU 16 ; Main revision of the firmware EEPROM_FW_SUB_REVISION EQU 3 ; Sub revision of the firmware EEPROM_LAYOUT_REVISION EQU 33 ; Revision of the EEPROM layout Eep_FW_Main_Revision: DB EEPROM_FW_MAIN_REVISION ; EEPROM firmware main revision number Eep_FW_Sub_Revision: DB EEPROM_FW_SUB_REVISION ; EEPROM firmware sub revision number Eep_Layout_Revision: DB EEPROM_LAYOUT_REVISION ; EEPROM layout revision number _Eep_Pgm_Gov_P_Gain: DB 0FFh _Eep_Pgm_Gov_I_Gain: DB 0FFh _Eep_Pgm_Gov_Mode: DB 0FFh _Eep_Pgm_Low_Voltage_Lim: DB 0FFh _Eep_Pgm_Motor_Gain: DB 0FFh _Eep_Pgm_Motor_Idle: DB 0FFh Eep_Pgm_Startup_Pwr: DB DEFAULT_PGM_STARTUP_PWR ; EEPROM copy of programmed startup power _Eep_Pgm_Pwm_Freq: DB 0FFh Eep_Pgm_Direction: DB DEFAULT_PGM_DIRECTION ; EEPROM copy of programmed rotation direction _Eep_Pgm_Input_Pol: DB 0FFh Eep_Initialized_L: DB 055h ; EEPROM initialized signature low byte Eep_Initialized_H: DB 0AAh ; EEPROM initialized signature high byte Eep_Enable_TX_Program: DB DEFAULT_PGM_ENABLE_TX_PROGRAM ; EEPROM TX programming enable _Eep_Main_Rearm_Start: DB 0FFh _Eep_Pgm_Gov_Setup_Target: DB 0FFh _Eep_Pgm_Startup_Rpm: DB 0FFh _Eep_Pgm_Startup_Accel: DB 0FFh _Eep_Pgm_Volt_Comp: DB 0FFh Eep_Pgm_Comm_Timing: DB DEFAULT_PGM_COMM_TIMING ; EEPROM copy of programmed commutation timing _Eep_Pgm_Damping_Force: DB 0FFh _Eep_Pgm_Gov_Range: DB 0FFh _Eep_Pgm_Startup_Method: DB 0FFh Eep_Pgm_Min_Throttle: DB DEFAULT_PGM_MIN_THROTTLE ; EEPROM copy of programmed minimum throttle Eep_Pgm_Max_Throttle: DB DEFAULT_PGM_MAX_THROTTLE ; EEPROM copy of programmed minimum throttle Eep_Pgm_Beep_Strength: DB DEFAULT_PGM_BEEP_STRENGTH ; EEPROM copy of programmed beep strength Eep_Pgm_Beacon_Strength: DB DEFAULT_PGM_BEACON_STRENGTH ; EEPROM copy of programmed beacon strength Eep_Pgm_Beacon_Delay: DB DEFAULT_PGM_BEACON_DELAY ; EEPROM copy of programmed beacon delay _Eep_Pgm_Throttle_Rate: DB 0FFh Eep_Pgm_Demag_Comp: DB DEFAULT_PGM_DEMAG_COMP ; EEPROM copy of programmed demag compensation _Eep_Pgm_BEC_Voltage_High: DB 0FFh Eep_Pgm_Center_Throttle: DB DEFAULT_PGM_CENTER_THROTTLE ; EEPROM copy of programmed center throttle _Eep_Pgm_Main_Spoolup_Time: DB 0FFh Eep_Pgm_Temp_Prot_Enable: DB DEFAULT_PGM_ENABLE_TEMP_PROT ; EEPROM copy of programmed temperature protection enable Eep_Pgm_Enable_Power_Prot: DB DEFAULT_PGM_ENABLE_POWER_PROT ; EEPROM copy of programmed low rpm power protection enable _Eep_Pgm_Enable_Pwm_Input: DB 0FFh _Eep_Pgm_Pwm_Dither: DB 0FFh Eep_Pgm_Brake_On_Stop: DB DEFAULT_PGM_BRAKE_ON_STOP ; EEPROM copy of programmed braking when throttle is zero Eep_Pgm_LED_Control: DB DEFAULT_PGM_LED_CONTROL ; EEPROM copy of programmed LED control Eep_Dummy: DB 0FFh ; EEPROM address for safety reason CSEG AT 1A60h Eep_Name: DB " " ; Name tag (16 Bytes) ;**** **** **** **** **** Interrupt_Table_Definition ; SiLabs interrupts CSEG AT 80h ; Code segment after interrupt vectors ;**** **** **** **** **** ; Table definitions STARTUP_POWER_TABLE: DB 04h, 06h, 08h, 0Ch, 10h, 18h, 20h, 30h, 40h, 60h, 80h, 0A0h, 0C0h ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Timer0 interrupt routine ; ; No assumptions ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** IF MCU_48MHZ == 1 t0_int: clr TCON_TF0 ; Clear interrupt flag inc Timer0_X reti ENDIF ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Timer2 interrupt routine ; ; No assumptions ; Requirements: Temp variables can NOT be used since PSW.x is not set ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** t2_int: ; Happens every 32ms push PSW ; Preserve registers through interrupt push ACC clr TMR2CN0_TF2H ; Clear interrupt flag inc Timer2_X IF MCU_48MHZ == 1 mov A, Clock_Set_At_48MHz jz t2_int_start ; Check skip variable mov A, Skip_T2_Int jz t2_int_start ; Execute this interrupt mov Skip_T2_Int, #0 ajmp t2_int_exit t2_int_start: mov Skip_T2_Int, #1 ; Skip next interrupt ENDIF ; Update RC pulse timeout counter mov A, Rcp_Timeout_Cntd ; RC pulse timeout count zero? jz t2_int_exit ; Yes - do not decrement dec Rcp_Timeout_Cntd ; No decrement t2_int_exit: pop ACC ; Restore preserved registers pop PSW reti ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Timer3 interrupt routine ; ; No assumptions ; Requirements: Temp variables can NOT be used since PSW.x is not set ; ACC can not be used, as it is not pushed to stack ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** t3_int: ; Used for commutation timing clr IE_EA ; Disable all interrupts anl EIE1, #7Fh ; Disable timer3 interrupts mov TMR3RLL, #0FAh ; Set a short delay before next interrupt mov TMR3RLH, #0FFh clr Flags0.T3_PENDING ; Flag that timer has wrapped anl TMR3CN0, #07Fh ; Timer3 interrupt flag cleared setb IE_EA ; Enable all interrupts reti ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Int0 interrupt routine ; ; No assumptions ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** int0_int: ; Used for RC pulse timing clr IE_EA anl EIE1, #0EFh ; Disable pca interrupts push PSW ; Preserve registers through interrupt push ACC push B setb PSW.3 ; Select register bank 1 for this interrupt setb IE_EA ; Get the counter values Get_Rcp_Capture_Values ; Scale down to 10 bits (for 24MHz, and 11 bits for 48MHz) jnb Flags2.RCP_MULTISHOT, int0_int_fall_not_multishot ; Multishot - Multiply by 2 and add 1/16 and 1/32 mov A, Temp1 ; Divide by 16 swap A anl A, #0Fh mov Temp3, A mov A, Temp2 swap A anl A, #0F0h orl A, Temp3 mov Temp3, A clr C ; Make divided by 32 rrc A add A, Temp3 ; Add 1/16 to 1/32 mov Temp3, A clr C ; Multiply by 2 mov A, Temp1 rlc A mov Temp1, A mov A, Temp2 rlc A mov Temp2, A mov A, Temp1 ; Add 1/16 and 1/32 add A, Temp3 mov Temp3, A mov A, Temp2 IF MCU_48MHZ == 0 addc A, #03h ; Add to low end, to make signal look like 20-40us ELSE addc A, #06h ENDIF mov Temp4, A ajmp int0_int_fall_gain_done int0_int_fall_not_multishot: jnb Flags2.RCP_ONESHOT42, int0_int_fall_not_oneshot_42 ; Oneshot42 - Add 2/256 clr C mov A, Temp1 rlc A mov A, Temp2 rlc A mov Temp3, A mov A, Temp1 add A, Temp3 mov Temp3, A mov A, Temp2 addc A, #0 mov Temp4, A ajmp int0_int_fall_gain_done int0_int_fall_not_oneshot_42: jnb Flags2.RCP_ONESHOT125, int0_int_fall_not_oneshot_125 ; Oneshot125 - multiply by 86/256 mov A, Temp1 ; Multiply by 86 and divide by 256 mov B, #56h mul AB mov Temp3, B mov A, Temp2 mov B, #56h mul AB add A, Temp3 mov Temp3, A xch A, B addc A, #0 mov Temp4, A ajmp int0_int_fall_gain_done int0_int_fall_not_oneshot_125: ; Regular signal - multiply by 43/1024 IF MCU_48MHZ == 1 clr C mov A, Temp3 ; Divide by 2 rrc A mov Temp3, A mov A, Temp2 rrc A mov Temp2, A mov A, Temp1 rrc A mov Temp1, A ENDIF mov A, Temp1 ; Multiply by 43 and divide by 1024 IF MCU_48MHZ == 0 mov B, #2Bh ELSE mov B, #56h ; Multiply by 86 ENDIF mul AB mov Temp3, B mov A, Temp2 IF MCU_48MHZ == 0 mov B, #2Bh ELSE mov B, #56h ; Multiply by 86 ENDIF mul AB add A, Temp3 mov Temp3, A xch A, B addc A, #0 clr C rrc A ; Divide by 2 for total 512 mov Temp4, A mov A, Temp3 rrc A mov Temp3, A clr C mov A, Temp4 ; Divide by 2 for total 1024 rrc A mov Temp4, A mov A, Temp3 rrc A mov Temp3, A int0_int_fall_gain_done: ; Check if 2235us or above (in order to ignore false pulses) clr C mov A, Temp4 ; Is pulse 2235us or higher? IF MCU_48MHZ == 0 subb A, #09h ELSE subb A, #12h ENDIF jnc int0_int_outside_range ; Yes - ignore pulse ; Check if below 745us (in order to ignore false pulses) clr C mov A, Temp4 ; Is pulse below 745us? IF MCU_48MHZ == 0 subb A, #03h ELSE subb A, #06h ENDIF jnc int0_int_check_full_range ; No - proceed int0_int_outside_range: inc Rcp_Outside_Range_Cnt mov A, Rcp_Outside_Range_Cnt jnz ($+4) dec Rcp_Outside_Range_Cnt clr C mov A, Rcp_Outside_Range_Cnt subb A, #10 ; Allow a given number of outside pulses jnc ($+4) ajmp int0_int_set_timeout ; If outside limits - ignore pulse mov New_Rcp, #0 ; Set pulse length to zero mov Temp3, #0 mov Temp4, #0 ajmp int0_int_set_pwm_registers int0_int_check_full_range: ; Decrement outside range counter mov A, Rcp_Outside_Range_Cnt jz ($+4) dec Rcp_Outside_Range_Cnt ; Calculate "1000us" plus throttle minimum jnb Flags2.RCP_FULL_RANGE, int0_int_set_min ; Check if full range is chosen mov Temp5, #0 ; Set 1000us as default minimum IF MCU_48MHZ == 0 mov Temp6, #4 ELSE mov Temp6, #8 ENDIF ajmp int0_int_calculate int0_int_set_min: mov Temp5, Min_Throttle_L ; Min throttle value scaled mov Temp6, Min_Throttle_H jnb Flags3.PGM_BIDIR, ($+7) mov Temp5, Center_Throttle_L ; Center throttle value scaled mov Temp6, Center_Throttle_H int0_int_calculate: clr C mov A, Temp3 ; Subtract minimum subb A, Temp5 mov Temp3, A mov A, Temp4 subb A, Temp6 mov Temp4, A mov Bit_Access_Int.0, C mov Temp7, Throttle_Gain ; Load Temp7/Temp8 with throttle gain mov Temp8, Throttle_Gain_M jnb Flags3.PGM_BIDIR, int0_int_not_bidir ; If not bidirectional operation - branch jnc int0_int_bidir_fwd ; If result is positive - branch int0_int_bidir_rev: jb Flags2.RCP_DIR_REV, int0_int_bidir_rev_chk ; If same direction - branch setb Flags2.RCP_DIR_REV ajmp int0_int_bidir_rev_chk int0_int_bidir_fwd: jnb Flags2.RCP_DIR_REV, int0_int_bidir_rev_chk ; If same direction - branch clr Flags2.RCP_DIR_REV int0_int_bidir_rev_chk: jnb Flags2.RCP_DIR_REV, ($+7) mov Temp7, Throttle_Gain_BD_Rev ; Load Temp7/Temp8 with throttle gain for bidirectional reverse mov Temp8, Throttle_Gain_BD_Rev_M jb Flags3.PGM_BIDIR_REV, ($+5) cpl Flags2.RCP_DIR_REV clr C ; Multiply throttle value by 2 mov A, Temp3 rlc A mov Temp3, A mov A, Temp4 rlc A mov Temp4, A mov C, Bit_Access_Int.0 jnc int0_int_bidir_do_deadband ; If result is positive - branch mov A, Temp3 ; Change sign cpl A add A, #1 mov Temp3, A mov A, Temp4 cpl A addc A, #0 mov Temp4, A int0_int_bidir_do_deadband: clr C ; Subtract deadband mov A, Temp3 IF MCU_48MHZ == 0 subb A, #40 ELSE subb A, #80 ENDIF mov Temp3, A mov A, Temp4 subb A, #0 mov Temp4, A jnc int0_int_do_throttle_gain mov Temp1, #0 mov Temp3, #0 mov Temp4, #0 ajmp int0_int_do_throttle_gain int0_int_not_bidir: mov C, Bit_Access_Int.0 jnc int0_int_do_throttle_gain ; If result is positive - branch int0_int_unidir_neg: mov Temp1, #0 ; Yes - set to minimum mov Temp3, #0 mov Temp4, #0 ajmp int0_int_pulse_ready int0_int_do_throttle_gain: ; Boost pwm during direct start mov A, Flags1 anl A, #((1 SHL STARTUP_PHASE)+(1 SHL INITIAL_RUN_PHASE)) jz int0_int_startup_boosted jb Flags1.MOTOR_STARTED, int0_int_startup_boosted ; Do not boost when changing direction in bidirectional mode mov A, Pwm_Limit_Beg ; Set 25% of max startup power as minimum power IF MCU_48MHZ == 1 rlc A ENDIF mov Temp2, A mov A, Temp4 jnz int0_int_startup_boost_stall clr C mov A, Temp2 subb A, Temp3 jc int0_int_startup_boost_stall mov A, Temp2 mov Temp3, A int0_int_startup_boost_stall: mov A, Stall_Cnt ; Add an extra power boost during start swap A IF MCU_48MHZ == 1 rlc A ENDIF add A, Temp3 mov Temp3, A mov A, Temp4 addc A, #0 mov Temp4, A int0_int_startup_boosted: mov A, Temp3 ; Multiply throttle value by throttle gain mov B, Temp7 ; Temp7 has Throttle_Gain mul AB mov Temp2, A mov Temp3, B mov A, Temp4 mov B, Temp7 ; Temp7 has Throttle_Gain mul AB add A, Temp3 mov Temp3, A xch A, B addc A, #0 mov Temp4, A clr C ; Generate 8bit number mov A, Temp4 rrc A mov Temp6, A mov A, Temp3 rrc A mov Temp1, A IF MCU_48MHZ == 1 clr C mov A, Temp6 rrc A mov Temp6, A mov A, Temp1 rrc A mov Temp1, A ENDIF inc Temp8 ; Temp8 has Throttle_Gain_M int0_int_gain_loop: mov A, Temp8 dec A jz int0_int_gain_rcp_done ; Skip one multiply by 2 of New_Rcp clr C mov A, Temp1 ; Multiply New_Rcp by 2 rlc A mov Temp1, A int0_int_gain_rcp_done: clr C mov A, Temp2 ; Multiply pwm by 2 rlc A mov A, Temp3 rlc A mov Temp3, A mov A, Temp4 rlc A mov Temp4, A djnz Temp8, int0_int_gain_loop mov A, Temp4 IF MCU_48MHZ == 0 jnb ACC.2, int0_int_pulse_ready ; Check that RC pulse is within legal range ELSE jnb ACC.3, int0_int_pulse_ready ENDIF mov Temp1, #0FFh mov Temp3, #0FFh IF MCU_48MHZ == 0 mov Temp4, #3 ELSE mov Temp4, #7 ENDIF int0_int_pulse_ready: mov New_Rcp, Temp1 ; Store new pulse length setb Flags2.RCP_UPDATED ; Set updated flag ; Set pwm limit clr C mov A, Pwm_Limit ; Limit to the smallest mov Temp5, A ; Store limit in Temp5 subb A, Pwm_Limit_By_Rpm jc ($+4) mov Temp5, Pwm_Limit_By_Rpm ; Check against limit clr C mov A, Temp5 subb A, New_Rcp jnc int0_int_set_pwm_registers mov A, Temp5 ; Multiply limit by 4 (8 for 48MHz MCUs) IF MCU_48MHZ == 0 mov B, #4 ELSE mov B, #8 ENDIF mul AB mov Temp3, A mov Temp4, B int0_int_set_pwm_registers: mov A, Temp3 cpl A mov Temp1, A mov A, Temp4 cpl A IF MCU_48MHZ == 0 anl A, #3 ELSE anl A, #7 ENDIF mov Temp2, A IF FETON_DELAY != 0 clr C mov A, Temp1 ; Skew damping fet timing IF MCU_48MHZ == 0 subb A, #FETON_DELAY ELSE subb A, #(FETON_DELAY SHL 1) ENDIF mov Temp3, A mov A, Temp2 subb A, #0 mov Temp4, A jnc int0_int_set_pwm_damp_set mov Temp3, #0 mov Temp4, #0 int0_int_set_pwm_damp_set: ENDIF mov Power_Pwm_Reg_L, Temp1 mov Power_Pwm_Reg_H, Temp2 IF FETON_DELAY != 0 mov Damp_Pwm_Reg_L, Temp3 mov Damp_Pwm_Reg_H, Temp4 ENDIF mov Rcp_Timeout_Cntd, #10 ; Set timeout count IF FETON_DELAY != 0 pop B ; Restore preserved registers pop ACC pop PSW Clear_COVF_Interrupt Enable_COVF_Interrupt ; Generate a pca interrupt orl EIE1, #10h ; Enable pca interrupts reti ELSE mov A, Current_Power_Pwm_Reg_H IF MCU_48MHZ == 0 jnb ACC.1, int0_int_set_pca_int_hi_pwm ELSE jnb ACC.2, int0_int_set_pca_int_hi_pwm ENDIF pop B ; Restore preserved registers pop ACC pop PSW Clear_COVF_Interrupt Enable_COVF_Interrupt ; Generate a pca interrupt orl EIE1, #10h ; Enable pca interrupts reti int0_int_set_pca_int_hi_pwm: pop B ; Restore preserved registers pop ACC pop PSW Clear_CCF_Interrupt Enable_CCF_Interrupt ; Generate pca interrupt orl EIE1, #10h ; Enable pca interrupts reti ENDIF int0_int_set_timeout: mov Rcp_Timeout_Cntd, #10 ; Set timeout count pop B ; Restore preserved registers pop ACC pop PSW orl EIE1, #10h ; Enable pca interrupts reti ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; PCA interrupt routine ; ; No assumptions ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** pca_int: ; Used for setting pwm registers clr IE_EA anl IE, #0FEh ; Disable int0 interrupts push PSW ; Preserve registers through interrupt push ACC setb PSW.3 ; Select register bank 1 for this interrupt IF FETON_DELAY != 0 ; HI/LO enable style drivers mov Temp1, PCA0L ; Read low byte, to transfer high byte to holding register mov A, Current_Power_Pwm_Reg_H IF MCU_48MHZ == 0 jnb ACC.1, pca_int_hi_pwm ELSE jnb ACC.2, pca_int_hi_pwm ENDIF mov A, PCA0H IF MCU_48MHZ == 0 jb ACC.1, pca_int_exit ; Power below 50%, update pca in the 0x00-0x0F range jb ACC.0, pca_int_exit ELSE jb ACC.2, pca_int_exit jb ACC.1, pca_int_exit ENDIF ajmp pca_int_set_pwm pca_int_hi_pwm: mov A, PCA0H IF MCU_48MHZ == 0 jnb ACC.1, pca_int_exit ; Power above 50%, update pca in the 0x20-0x2F range jb ACC.0, pca_int_exit ELSE jnb ACC.2, pca_int_exit jb ACC.1, pca_int_exit ENDIF pca_int_set_pwm: Set_Power_Pwm_Regs Set_Damp_Pwm_Regs mov Current_Power_Pwm_Reg_H, Power_Pwm_Reg_H Disable_COVF_Interrupt ELSE ; EN/PWM style drivers Set_Power_Pwm_Regs mov Current_Power_Pwm_Reg_H, Power_Pwm_Reg_H Disable_COVF_Interrupt Disable_CCF_Interrupt ENDIF ; Check RC pulse against stop value clr C mov A, New_Rcp ; Load new pulse value subb A, #1 ; Check if pulse is below stop value jc pca_int_rcp_stop ; RC pulse higher than stop value, reset stop counter mov Rcp_Stop_Cnt, #0 ; Reset rcp stop counter ajmp pca_int_exit pca_int_rcp_stop: ; RC pulse less than stop value mov A, Rcp_Stop_Cnt ; Increment stop counter add A, #1 mov Rcp_Stop_Cnt, A jnc ($+5) ; Branch if counter has not wrapped mov Rcp_Stop_Cnt, #0FFh ; Set stop counter to max pca_int_exit: Clear_COVF_Interrupt IF FETON_DELAY == 0 Clear_CCF_Interrupt ENDIF pop ACC ; Restore preserved registers pop PSW orl IE, #01h ; Enable int0 interrupts setb IE_EA reti ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Wait xms ~(x*4*250) (Different entry points) ; ; No assumptions ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** wait1ms: mov Temp2, #1 jmp waitxms_o wait3ms: mov Temp2, #3 jmp waitxms_o wait10ms: mov Temp2, #10 jmp waitxms_o wait30ms: mov Temp2, #30 jmp waitxms_o wait100ms: mov Temp2, #100 jmp waitxms_o wait200ms: mov Temp2, #200 jmp waitxms_o waitxms_o: ; Outer loop mov Temp1, #23 waitxms_m: ; Middle loop clr A djnz ACC, $ ; Inner loop (42.7us - 1024 cycles) djnz Temp1, waitxms_m djnz Temp2, waitxms_o ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Beeper routines (4 different entry points) ; ; No assumptions ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** beep_f1: ; Entry point 1, load beeper frequency 1 settings mov Temp3, #20 ; Off wait loop length mov Temp4, #120 ; Number of beep pulses jmp beep beep_f2: ; Entry point 2, load beeper frequency 2 settings mov Temp3, #16 mov Temp4, #140 jmp beep beep_f3: ; Entry point 3, load beeper frequency 3 settings mov Temp3, #13 mov Temp4, #180 jmp beep beep_f4: ; Entry point 4, load beeper frequency 4 settings mov Temp3, #11 mov Temp4, #200 jmp beep beep: ; Beep loop start mov A, Beep_Strength djnz ACC, beep_start ret beep_start: mov Temp2, #2 beep_onoff: clr A BcomFET_off ; BcomFET off djnz ACC, $ ; Allow some time after comfet is turned off BpwmFET_on ; BpwmFET on (in order to charge the driver of the BcomFET) djnz ACC, $ ; Let the pwmfet be turned on a while BpwmFET_off ; BpwmFET off again djnz ACC, $ ; Allow some time after pwmfet is turned off BcomFET_on ; BcomFET on djnz ACC, $ ; Allow some time after comfet is turned on ; Turn on pwmfet mov A, Temp2 jb ACC.0, beep_apwmfet_on ApwmFET_on ; ApwmFET on beep_apwmfet_on: jnb ACC.0, beep_cpwmfet_on CpwmFET_on ; CpwmFET on beep_cpwmfet_on: mov A, Beep_Strength djnz ACC, $ ; Turn off pwmfet mov A, Temp2 jb ACC.0, beep_apwmfet_off ApwmFET_off ; ApwmFET off beep_apwmfet_off: jnb ACC.0, beep_cpwmfet_off CpwmFET_off ; CpwmFET off beep_cpwmfet_off: mov A, #150 ; 25µs off djnz ACC, $ djnz Temp2, beep_onoff ; Copy variable mov A, Temp3 mov Temp1, A beep_off: ; Fets off loop djnz ACC, $ djnz Temp1, beep_off djnz Temp4, beep BcomFET_off ; BcomFET off ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Set pwm limit low rpm ; ; No assumptions ; ; Sets power limit for low rpms and disables demag for low rpms ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** set_pwm_limit_low_rpm: ; Set pwm limit and demag disable for low rpms mov Temp1, #0FFh ; Default full power jb Flags1.STARTUP_PHASE, set_pwm_limit_low_rpm_exit ; Exit if startup phase set mov Temp2, #Pgm_Enable_Power_Prot ; Check if low RPM power protection is enabled mov A, @Temp2 jz set_pwm_limit_low_rpm_exit ; Exit if disabled mov A, Comm_Period4x_H jz set_pwm_limit_low_rpm_exit ; Avoid divide by zero mov A, #255 ; Divide 255 by Comm_Period4x_H mov B, Comm_Period4x_H div AB mov B, Low_Rpm_Pwr_Slope ; Multiply by slope jnb Flags1.INITIAL_RUN_PHASE, ($+6) ; More protection for initial run phase mov B, #5 mul AB mov Temp1, A ; Set new limit xch A, B jz ($+4) ; Limit to max mov Temp1, #0FFh clr C mov A, Temp1 ; Limit to min subb A, Pwm_Limit_Beg jnc set_pwm_limit_low_rpm_exit mov Temp1, Pwm_Limit_Beg set_pwm_limit_low_rpm_exit: mov Pwm_Limit_By_Rpm, Temp1 ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Set pwm limit high rpm ; ; No assumptions ; ; Sets power limit for high rpms ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** set_pwm_limit_high_rpm: IF MCU_48MHZ == 1 clr C mov A, Comm_Period4x_L subb A, #0A0h ; Limit Comm_Period to 160, which is 500k erpm mov A, Comm_Period4x_H subb A, #00h ELSE clr C mov A, Comm_Period4x_L subb A, #0E4h ; Limit Comm_Period to 228, which is 350k erpm mov A, Comm_Period4x_H subb A, #00h ENDIF mov A, Pwm_Limit_By_Rpm jnc set_pwm_limit_high_rpm_inc_limit dec A ajmp set_pwm_limit_high_rpm_store set_pwm_limit_high_rpm_inc_limit: inc A set_pwm_limit_high_rpm_store: jz ($+4) mov Pwm_Limit_By_Rpm, A ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Start ADC conversion ; ; No assumptions ; ; Start conversion used for measuring power supply voltage ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** start_adc_conversion: ; Start adc Start_Adc ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Check temperature, power supply voltage and limit power ; ; No assumptions ; ; Used to limit main motor power in order to maintain the required voltage ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** check_temp_voltage_and_limit_power: inc Adc_Conversion_Cnt ; Increment conversion counter clr C mov A, Adc_Conversion_Cnt ; Is conversion count equal to temp rate? subb A, #8 jc check_voltage_start ; No - check voltage ; Wait for ADC conversion to complete jnb ADC0CN0_ADINT, check_temp_voltage_and_limit_power ; Read ADC result Read_Adc_Result ; Stop ADC Stop_Adc mov Adc_Conversion_Cnt, #0 ; Yes - temperature check. Reset counter mov A, Temp2 ; Move ADC MSB to Temp3 mov Temp3, A mov Temp2, #Pgm_Enable_Temp_Prot ; Is temp protection enabled? mov A, @Temp2 jz temp_check_exit ; No - branch mov A, Temp3 ; Is temperature reading below 256? jnz temp_average_inc_dec ; No - proceed mov A, Current_Average_Temp ; Yes - decrement average jz temp_average_updated ; Already zero - no change jmp temp_average_dec ; Decrement temp_average_inc_dec: clr C mov A, Temp1 ; Check if current temperature is above or below average subb A, Current_Average_Temp jz temp_average_updated_load_acc ; Equal - no change mov A, Current_Average_Temp ; Above - increment average jnc temp_average_inc jz temp_average_updated ; Below - decrement average if average is not already zero temp_average_dec: dec A ; Decrement average jmp temp_average_updated temp_average_inc: inc A ; Increment average jz temp_average_dec jmp temp_average_updated temp_average_updated_load_acc: mov A, Current_Average_Temp temp_average_updated: mov Current_Average_Temp, A clr C subb A, Temp_Prot_Limit ; Is temperature below first limit? jc temp_check_exit ; Yes - exit mov Pwm_Limit, #192 ; No - limit pwm clr C subb A, #(TEMP_LIMIT_STEP/2) ; Is temperature below second limit jc temp_check_exit ; Yes - exit mov Pwm_Limit, #128 ; No - limit pwm clr C subb A, #(TEMP_LIMIT_STEP/2) ; Is temperature below third limit jc temp_check_exit ; Yes - exit mov Pwm_Limit, #64 ; No - limit pwm clr C subb A, #(TEMP_LIMIT_STEP/2) ; Is temperature below final limit jc temp_check_exit ; Yes - exit mov Pwm_Limit, #0 ; No - limit pwm temp_check_exit: ret check_voltage_start: ; Increase pwm limit mov A, Pwm_Limit add A, #16 jnc ($+4) ; If not max - branch mov A, #255 mov Pwm_Limit, A ; Increment limit ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Set startup PWM routine ; ; Either the SETTLE_PHASE or the STEPPER_PHASE flag must be set ; ; Used for pwm control during startup ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** set_startup_pwm: ; Adjust startup power mov A, #50 ; Set power mov Temp2, #Pgm_Startup_Pwr_Decoded mov B, @Temp2 mul AB xch A, B mov C, B.7 ; Multiply result by 2 (unity gain is 128) rlc A mov Temp1, A ; Transfer to Temp1 clr C mov A, Temp1 ; Check against limit subb A, Pwm_Limit jc startup_pwm_set_pwm ; If pwm below limit - branch mov Temp1, Pwm_Limit ; Limit pwm startup_pwm_set_pwm: mov Pwm_Limit_Beg, Temp1 ; Set initial pwm limit ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Initialize timing routine ; ; No assumptions ; ; Part of initialization before motor start ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** initialize_timing: mov Comm_Period4x_L, #00h ; Set commutation period registers mov Comm_Period4x_H, #0F0h ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Calculate next commutation timing routine ; ; No assumptions ; ; Called immediately after each commutation ; Also sets up timer 3 to wait advance timing ; Two entry points are used ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** calc_next_comm_timing: ; Entry point for run phase ; Read commutation time clr IE_EA mov TMR2CN0, #00h ; Timer2 disabled mov Temp1, TMR2L ; Load timer value mov Temp2, TMR2H mov Temp3, Timer2_X jnb TMR2CN0_TF2H, ($+4) ; Check if interrupt is pending inc Temp3 ; If it is pending, then timer has already wrapped mov TMR2CN0, #04h ; Timer2 enabled setb IE_EA IF MCU_48MHZ == 1 clr C mov A, Temp3 rrc A mov Temp3, A mov A, Temp2 rrc A mov Temp2, A mov A, Temp1 rrc A mov Temp1, A ENDIF ; Calculate this commutation time mov Temp4, Prev_Comm_L mov Temp5, Prev_Comm_H mov Prev_Comm_L, Temp1 ; Store timestamp as previous commutation mov Prev_Comm_H, Temp2 clr C mov A, Temp1 subb A, Temp4 ; Calculate the new commutation time mov Temp1, A mov A, Temp2 subb A, Temp5 jb Flags1.STARTUP_PHASE, calc_next_comm_startup IF MCU_48MHZ == 1 anl A, #7Fh ENDIF mov Temp2, A jnb Flags1.HIGH_RPM, ($+5) ; Branch if high rpm ajmp calc_next_comm_timing_fast ajmp calc_next_comm_normal calc_next_comm_startup: mov Temp6, Prev_Comm_X mov Prev_Comm_X, Temp3 ; Store extended timestamp as previous commutation mov Temp2, A mov A, Temp3 subb A, Temp6 ; Calculate the new extended commutation time IF MCU_48MHZ == 1 anl A, #7Fh ENDIF mov Temp3, A jz ($+6) mov Temp1, #0FFh mov Temp2, #0FFh mov Temp7, Prev_Prev_Comm_L mov Temp8, Prev_Prev_Comm_H mov Prev_Prev_Comm_L, Temp4 mov Prev_Prev_Comm_H, Temp5 mov Temp1, Prev_Comm_L ; Reload this commutation time mov Temp2, Prev_Comm_H clr C mov A, Temp1 subb A, Temp7 ; Calculate the new commutation time based upon the two last commutations (to reduce sensitivity to offset) mov Temp1, A mov A, Temp2 subb A, Temp8 mov Temp2, A clr C mov A, Comm_Period4x_H ; Average with previous and save rrc A mov Temp4, A mov A, Comm_Period4x_L rrc A mov Temp3, A mov A, Temp1 add A, Temp3 mov Comm_Period4x_L, A mov A, Temp2 addc A, Temp4 mov Comm_Period4x_H, A jnc ($+8) mov Comm_Period4x_L, #0FFh mov Comm_Period4x_H, #0FFh ajmp calc_new_wait_times_setup calc_next_comm_normal: ; Calculate new commutation time mov Temp3, Comm_Period4x_L ; Comm_Period4x(-l-h) holds the time of 4 commutations mov Temp4, Comm_Period4x_H mov Temp5, Comm_Period4x_L ; Copy variables mov Temp6, Comm_Period4x_H mov Temp7, #4 ; Divide Comm_Period4x 4 times as default mov Temp8, #2 ; Divide new commutation time 2 times as default clr C mov A, Temp4 subb A, #04h jc calc_next_comm_avg_period_div dec Temp7 ; Reduce averaging time constant for low speeds dec Temp8 clr C mov A, Temp4 subb A, #08h jc calc_next_comm_avg_period_div jb Flags1.INITIAL_RUN_PHASE, calc_next_comm_avg_period_div ; Do not average very fast during initial run dec Temp7 ; Reduce averaging time constant more for even lower speeds dec Temp8 calc_next_comm_avg_period_div: clr C mov A, Temp6 rrc A ; Divide by 2 mov Temp6, A mov A, Temp5 rrc A mov Temp5, A djnz Temp7, calc_next_comm_avg_period_div clr C mov A, Temp3 subb A, Temp5 ; Subtract a fraction mov Temp3, A mov A, Temp4 subb A, Temp6 mov Temp4, A mov A, Temp8 ; Divide new time jz calc_next_comm_new_period_div_done calc_next_comm_new_period_div: clr C mov A, Temp2 rrc A ; Divide by 2 mov Temp2, A mov A, Temp1 rrc A mov Temp1, A djnz Temp8, calc_next_comm_new_period_div calc_next_comm_new_period_div_done: mov A, Temp3 add A, Temp1 ; Add the divided new time mov Temp3, A mov A, Temp4 addc A, Temp2 mov Temp4, A mov Comm_Period4x_L, Temp3 ; Store Comm_Period4x_X mov Comm_Period4x_H, Temp4 jnc calc_new_wait_times_setup; If period larger than 0xffff - go to slow case mov Temp4, #0FFh mov Comm_Period4x_L, Temp4 ; Set commutation period registers to very slow timing (0xffff) mov Comm_Period4x_H, Temp4 calc_new_wait_times_setup: ; Set high rpm bit (if above 156k erpm) clr C mov A, Temp4 subb A, #2 jnc ($+4) setb Flags1.HIGH_RPM ; Set high rpm bit ; Load programmed commutation timing jnb Flags1.STARTUP_PHASE, calc_new_wait_per_startup_done ; Set dedicated timing during startup mov Temp8, #3 ajmp calc_new_wait_per_demag_done calc_new_wait_per_startup_done: mov Temp1, #Pgm_Comm_Timing ; Load timing setting mov A, @Temp1 mov Temp8, A ; Store in Temp8 clr C mov A, Demag_Detected_Metric ; Check demag metric subb A, #130 jc calc_new_wait_per_demag_done inc Temp8 ; Increase timing clr C mov A, Demag_Detected_Metric subb A, #160 jc ($+3) inc Temp8 ; Increase timing again clr C mov A, Temp8 ; Limit timing to max subb A, #6 jc ($+4) mov Temp8, #5 ; Set timing to max calc_new_wait_per_demag_done: ; Set timing reduction mov Temp7, #2 ; Load current commutation timing mov A, Comm_Period4x_H ; Divide 4 times swap A anl A, #00Fh mov Temp2, A mov A, Comm_Period4x_H swap A anl A, #0F0h mov Temp1, A mov A, Comm_Period4x_L swap A anl A, #00Fh add A, Temp1 mov Temp1, A clr C mov A, Temp1 subb A, Temp7 mov Temp3, A mov A, Temp2 subb A, #0 mov Temp4, A jc load_min_time ; Check that result is still positive clr C mov A, Temp3 subb A, #1 mov A, Temp4 subb A, #0 jnc calc_new_wait_times_exit ; Check that result is still above minumum load_min_time: mov Temp3, #1 clr A mov Temp4, A calc_new_wait_times_exit: ajmp wait_advance_timing ; Fast calculation (Comm_Period4x_H less than 2) calc_next_comm_timing_fast: ; Calculate new commutation time mov Temp3, Comm_Period4x_L ; Comm_Period4x(-l-h) holds the time of 4 commutations mov Temp4, Comm_Period4x_H mov A, Temp4 ; Divide by 2 4 times swap A mov Temp7, A mov A, Temp3 swap A anl A, #0Fh orl A, Temp7 mov Temp5, A clr C mov A, Temp3 ; Subtract a fraction subb A, Temp5 mov Temp3, A mov A, Temp4 subb A, #0 mov Temp4, A clr C mov A, Temp1 rrc A ; Divide by 2 2 times clr C rrc A mov Temp1, A mov A, Temp3 ; Add the divided new time add A, Temp1 mov Temp3, A mov A, Temp4 addc A, #0 mov Temp4, A mov Comm_Period4x_L, Temp3 ; Store Comm_Period4x_X mov Comm_Period4x_H, Temp4 clr C mov A, Temp4 ; If erpm below 156k - go to normal case subb A, #2 jc ($+4) clr Flags1.HIGH_RPM ; Clear high rpm bit ; Set timing reduction mov Temp1, #2 mov A, Temp4 ; Divide by 2 4 times swap A mov Temp7, A mov Temp4, #0 mov A, Temp3 swap A anl A, #0Fh orl A, Temp7 mov Temp3, A clr C mov A, Temp3 subb A, Temp1 mov Temp3, A jc load_min_time_fast ; Check that result is still positive clr C subb A, #1 jnc calc_new_wait_times_fast_done ; Check that result is still above minumum load_min_time_fast: mov Temp3, #1 calc_new_wait_times_fast_done: mov Temp1, #Pgm_Comm_Timing ; Load timing setting mov A, @Temp1 mov Temp8, A ; Store in Temp8 ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Wait advance timing routine ; ; No assumptions ; NOTE: Be VERY careful if using temp registers. They are passed over this routine ; ; Waits for the advance timing to elapse and sets up the next zero cross wait ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** wait_advance_timing: jnb Flags0.T3_PENDING, ($+5) ajmp wait_advance_timing ; Setup next wait time mov TMR3RLL, Wt_ZC_Tout_Start_L mov TMR3RLH, Wt_ZC_Tout_Start_H setb Flags0.T3_PENDING orl EIE1, #80h ; Enable timer3 interrupts ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Calculate new wait times routine ; ; No assumptions ; ; Calculates new wait times ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** calc_new_wait_times: clr C clr A subb A, Temp3 ; Negate mov Temp1, A clr A subb A, Temp4 mov Temp2, A IF MCU_48MHZ == 1 clr C mov A, Temp1 ; Multiply by 2 rlc A mov Temp1, A mov A, Temp2 rlc A mov Temp2, A ENDIF jnb Flags1.HIGH_RPM, ($+5) ; Branch if high rpm ajmp calc_new_wait_times_fast mov A, Temp1 ; Copy values mov Temp3, A mov A, Temp2 mov Temp4, A setb C ; Negative numbers - set carry mov A, Temp2 rrc A ; Divide by 2 mov Temp6, A mov A, Temp1 rrc A mov Temp5, A mov Wt_Zc_Tout_Start_L, Temp1; Set 15deg time for zero cross scan timeout mov Wt_Zc_Tout_Start_H, Temp2 clr C mov A, Temp8 ; (Temp8 has Pgm_Comm_Timing) subb A, #3 ; Is timing normal? jz store_times_decrease ; Yes - branch mov A, Temp8 jb ACC.0, adjust_timing_two_steps ; If an odd number - branch mov A, Temp1 ; Add 7.5deg and store in Temp1/2 add A, Temp5 mov Temp1, A mov A, Temp2 addc A, Temp6 mov Temp2, A mov A, Temp5 ; Store 7.5deg in Temp3/4 mov Temp3, A mov A, Temp6 mov Temp4, A jmp store_times_up_or_down adjust_timing_two_steps: mov A, Temp1 ; Add 15deg and store in Temp1/2 add A, Temp1 mov Temp1, A mov A, Temp2 addc A, Temp2 mov Temp2, A clr C mov A, Temp1 add A, #1 mov Temp1, A mov A, Temp2 addc A, #0 mov Temp2, A mov Temp3, #-1 ; Store minimum time in Temp3/4 mov Temp4, #0FFh store_times_up_or_down: clr C mov A, Temp8 subb A, #3 ; Is timing higher than normal? jc store_times_decrease ; No - branch store_times_increase: mov Wt_Comm_Start_L, Temp3 ; Now commutation time (~60deg) divided by 4 (~15deg nominal) mov Wt_Comm_Start_H, Temp4 mov Wt_Adv_Start_L, Temp1 ; New commutation advance time (~15deg nominal) mov Wt_Adv_Start_H, Temp2 mov Wt_Zc_Scan_Start_L, Temp5 ; Use this value for zero cross scan delay (7.5deg) mov Wt_Zc_Scan_Start_H, Temp6 ajmp wait_before_zc_scan store_times_decrease: mov Wt_Comm_Start_L, Temp1 ; Now commutation time (~60deg) divided by 4 (~15deg nominal) mov Wt_Comm_Start_H, Temp2 mov Wt_Adv_Start_L, Temp3 ; New commutation advance time (~15deg nominal) mov Wt_Adv_Start_H, Temp4 mov Wt_Zc_Scan_Start_L, Temp5 ; Use this value for zero cross scan delay (7.5deg) mov Wt_Zc_Scan_Start_H, Temp6 jnb Flags1.STARTUP_PHASE, store_times_exit mov Wt_Comm_Start_L, #0F0h ; Set very short delays for all but advance time during startup, in order to widen zero cross capture range mov Wt_Comm_Start_H, #0FFh mov Wt_Zc_Scan_Start_L, #0F0h mov Wt_Zc_Scan_Start_H, #0FFh mov Wt_Zc_Tout_Start_L, #0F0h mov Wt_Zc_Tout_Start_H, #0FFh store_times_exit: ajmp wait_before_zc_scan calc_new_wait_times_fast: mov A, Temp1 ; Copy values mov Temp3, A setb C ; Negative numbers - set carry mov A, Temp1 ; Divide by 2 rrc A mov Temp5, A mov Wt_Zc_Tout_Start_L, Temp1; Set 15deg time for zero cross scan timeout clr C mov A, Temp8 ; (Temp8 has Pgm_Comm_Timing) subb A, #3 ; Is timing normal? jz store_times_decrease_fast; Yes - branch mov A, Temp8 jb ACC.0, adjust_timing_two_steps_fast ; If an odd number - branch mov A, Temp1 ; Add 7.5deg and store in Temp1 add A, Temp5 mov Temp1, A mov A, Temp5 ; Store 7.5deg in Temp3 mov Temp3, A ajmp store_times_up_or_down_fast adjust_timing_two_steps_fast: mov A, Temp1 ; Add 15deg and store in Temp1 add A, Temp1 add A, #1 mov Temp1, A mov Temp3, #-1 ; Store minimum time in Temp3 store_times_up_or_down_fast: clr C mov A, Temp8 subb A, #3 ; Is timing higher than normal? jc store_times_decrease_fast; No - branch store_times_increase_fast: mov Wt_Comm_Start_L, Temp3 ; Now commutation time (~60deg) divided by 4 (~15deg nominal) mov Wt_Adv_Start_L, Temp1 ; New commutation advance time (~15deg nominal) mov Wt_Zc_Scan_Start_L, Temp5 ; Use this value for zero cross scan delay (7.5deg) ajmp wait_before_zc_scan store_times_decrease_fast: mov Wt_Comm_Start_L, Temp1 ; Now commutation time (~60deg) divided by 4 (~15deg nominal) mov Wt_Adv_Start_L, Temp3 ; New commutation advance time (~15deg nominal) mov Wt_Zc_Scan_Start_L, Temp5 ; Use this value for zero cross scan delay (7.5deg) ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Wait before zero cross scan routine ; ; No assumptions ; ; Waits for the zero cross scan wait time to elapse ; Also sets up timer 3 for the zero cross scan timeout time ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** wait_before_zc_scan: jnb Flags0.T3_PENDING, ($+5) ajmp wait_before_zc_scan mov Startup_Zc_Timeout_Cntd, #2 setup_zc_scan_timeout: setb Flags0.T3_PENDING orl EIE1, #80h ; Enable timer3 interrupts mov A, Flags1 anl A, #((1 SHL STARTUP_PHASE)+(1 SHL INITIAL_RUN_PHASE)) jz wait_before_zc_scan_exit mov Temp1, Comm_Period4x_L ; Set long timeout when starting mov Temp2, Comm_Period4x_H clr C mov A, Temp2 rrc A mov Temp2, A mov A, Temp1 rrc A mov Temp1, A IF MCU_48MHZ == 0 clr C mov A, Temp2 rrc A mov Temp2, A mov A, Temp1 rrc A mov Temp1, A ENDIF jnb Flags1.STARTUP_PHASE, setup_zc_scan_timeout_startup_done mov A, Temp2 add A, #40h ; Increase timeout somewhat to avoid false wind up mov Temp2, A setup_zc_scan_timeout_startup_done: clr IE_EA anl EIE1, #7Fh ; Disable timer3 interrupts mov TMR3CN0, #00h ; Timer3 disabled and interrupt flag cleared clr C clr A subb A, Temp1 ; Set timeout mov TMR3L, A clr A subb A, Temp2 mov TMR3H, A mov TMR3CN0, #04h ; Timer3 enabled and interrupt flag cleared setb Flags0.T3_PENDING orl EIE1, #80h ; Enable timer3 interrupts setb IE_EA wait_before_zc_scan_exit: 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: setb Flags0.DEMAG_DETECTED ; Set demag detected flag as default mov Comparator_Read_Cnt, #0 ; Reset number of comparator reads mov Bit_Access, #00h ; Desired comparator output jnb Flags1.DIR_CHANGE_BRAKE, ($+6) mov Bit_Access, #40h jmp wait_for_comp_out_start wait_for_comp_out_high: setb Flags0.DEMAG_DETECTED ; Set demag detected flag as default mov Comparator_Read_Cnt, #0 ; Reset number of comparator reads mov Bit_Access, #40h ; Desired comparator output jnb Flags1.DIR_CHANGE_BRAKE, ($+6) mov Bit_Access, #00h wait_for_comp_out_start: ; Set number of comparator readings mov Temp1, #1 ; Number of OK readings required mov Temp2, #1 ; Max number of readings required jb Flags1.HIGH_RPM, comp_scale_samples ; Branch if high rpm mov A, Flags1 ; Clear demag detected flag if start phases anl A, #((1 SHL STARTUP_PHASE)+(1 SHL INITIAL_RUN_PHASE)) jz ($+4) clr Flags0.DEMAG_DETECTED mov Temp2, #20 ; Too low value (~<15) causes rough running at pwm harmonics. Too high a value (~>35) causes the RCT4215 630 to run rough on full throttle mov A, Comm_Period4x_H ; Set number of readings higher for lower speeds clr C rrc A jnz ($+3) inc A mov Temp1, A clr C subb A, #20 jc ($+4) mov Temp1, #20 jnb Flags1.STARTUP_PHASE, comp_scale_samples mov Temp1, #27 ; Set many samples during startup, approximately one pwm period mov Temp2, #27 comp_scale_samples: IF MCU_48MHZ == 1 clr C mov A, Temp1 rlc A mov Temp1, A clr C mov A, Temp2 rlc A mov Temp2, A ENDIF comp_check_timeout: jb Flags0.T3_PENDING, comp_check_timeout_not_timed_out ; Has zero cross scan timeout elapsed? mov A, Comparator_Read_Cnt ; Check that comparator has been read jz comp_check_timeout_not_timed_out ; If not read - branch jnb Flags1.STARTUP_PHASE, comp_check_timeout_timeout_extended ; Extend timeout during startup djnz Startup_Zc_Timeout_Cntd, comp_check_timeout_extend_timeout comp_check_timeout_timeout_extended: setb Flags0.COMP_TIMED_OUT ajmp setup_comm_wait comp_check_timeout_extend_timeout: call setup_zc_scan_timeout comp_check_timeout_not_timed_out: inc Comparator_Read_Cnt ; Increment comparator read count Read_Comp_Out ; Read comparator output anl A, #40h cjne A, Bit_Access, comp_read_wrong ajmp comp_read_ok comp_read_wrong: jnb Flags1.STARTUP_PHASE, comp_read_wrong_not_startup inc Temp1 ; Increment number of OK readings required clr C mov A, Temp1 subb A, Temp2 ; If above initial requirement - go back and restart jc ($+3) dec Temp1 ajmp comp_check_timeout ; Continue to look for good ones comp_read_wrong_not_startup: jb Flags0.DEMAG_DETECTED, comp_read_wrong_extend_timeout inc Temp1 ; Increment number of OK readings required clr C mov A, Temp1 subb A, Temp2 jc ($+4) ajmp wait_for_comp_out_start ; If above initial requirement - go back and restart ajmp comp_check_timeout ; Otherwise - take another reading comp_read_wrong_extend_timeout: clr Flags0.DEMAG_DETECTED ; Clear demag detected flag clr IE_EA anl EIE1, #7Fh ; Disable timer3 interrupts mov TMR3CN0, #00h ; Timer3 disabled and interrupt flag cleared jnb Flags1.HIGH_RPM, comp_read_wrong_low_rpm ; Branch if not high rpm mov TMR3L, #00h ; Set timeout to ~1ms IF MCU_48MHZ == 1 mov TMR3H, #0F0h ELSE mov TMR3H, #0F8h ENDIF comp_read_wrong_timeout_set: mov TMR3CN0, #04h ; Timer3 enabled and interrupt flag cleared setb Flags0.T3_PENDING orl EIE1, #80h ; Enable timer3 interrupts setb IE_EA ajmp wait_for_comp_out_start ; If comparator output is not correct - go back and restart comp_read_wrong_low_rpm: mov A, Comm_Period4x_H ; Set timeout to ~4x comm period 4x value mov Temp7, #0FFh ; Default to long IF MCU_48MHZ == 1 clr C rlc A jc comp_read_wrong_load_timeout ENDIF clr C rlc A jc comp_read_wrong_load_timeout clr C rlc A jc comp_read_wrong_load_timeout mov Temp7, A comp_read_wrong_load_timeout: clr C clr A subb A, Temp7 mov TMR3L, #0 mov TMR3H, A ajmp comp_read_wrong_timeout_set comp_read_ok: clr C mov A, Startup_Cnt ; Force a timeout for the first commutation subb A, #1 jnc ($+4) ajmp wait_for_comp_out_start jnb Flags0.DEMAG_DETECTED, ($+5) ; Do not accept correct comparator output if it is demag ajmp wait_for_comp_out_start djnz Temp1, comp_read_ok_jmp ; Decrement readings counter - repeat comparator reading if not zero ajmp ($+4) comp_read_ok_jmp: ajmp comp_check_timeout clr Flags0.COMP_TIMED_OUT ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Setup commutation timing routine ; ; No assumptions ; ; Sets up and starts wait from commutation to zero cross ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** setup_comm_wait: clr IE_EA anl EIE1, #7Fh ; Disable timer3 interrupts mov TMR3CN0, #00h ; Timer3 disabled and interrupt flag cleared mov TMR3L, Wt_Comm_Start_L mov TMR3H, Wt_Comm_Start_H mov TMR3CN0, #04h ; Timer3 enabled and interrupt flag cleared ; Setup next wait time mov TMR3RLL, Wt_Adv_Start_L mov TMR3RLH, Wt_Adv_Start_H setb Flags0.T3_PENDING orl EIE1, #80h ; Enable timer3 interrupts setb IE_EA ; Enable interrupts again ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Evaluate comparator integrity ; ; No assumptions ; ; Checks comparator signal behaviour versus expected behaviour ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** evaluate_comparator_integrity: mov A, Flags1 anl A, #((1 SHL STARTUP_PHASE)+(1 SHL INITIAL_RUN_PHASE)) jz eval_comp_check_timeout jb Flags1.INITIAL_RUN_PHASE, ($+5) ; Do not increment beyond startup phase inc Startup_Cnt ; Increment counter jmp eval_comp_exit eval_comp_check_timeout: jnb Flags0.COMP_TIMED_OUT, eval_comp_exit ; Has timeout elapsed? jb Flags1.DIR_CHANGE_BRAKE, eval_comp_exit ; Do not exit run mode if it is braking jb Flags0.DEMAG_DETECTED, eval_comp_exit ; Do not exit run mode if it is a demag situation dec SP ; Routine exit without "ret" command dec SP ljmp run_to_wait_for_power_on_fail ; Yes - exit run mode eval_comp_exit: ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Wait for commutation routine ; ; No assumptions ; ; Waits from zero cross to commutation ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** wait_for_comm: ; Update demag metric mov Temp1, #0 jnb Flags0.DEMAG_DETECTED, ($+5) mov Temp1, #1 mov A, Demag_Detected_Metric ; Sliding average of 8, 256 when demag and 0 when not. Limited to minimum 120 mov B, #7 mul AB ; Multiply by 7 mov Temp2, A mov A, B ; Add new value for current demag status add A, Temp1 mov B, A mov A, Temp2 mov C, B.0 ; Divide by 8 rrc A mov C, B.1 rrc A mov C, B.2 rrc A mov Demag_Detected_Metric, A clr C subb A, #120 ; Limit to minimum 120 jnc ($+5) mov Demag_Detected_Metric, #120 clr C mov A, Demag_Detected_Metric ; Check demag metric subb A, Demag_Pwr_Off_Thresh jc wait_for_comm_wait ; Cut power if many consecutive demags. This will help retain sync during hard accelerations setb Flags0.DEMAG_CUT_POWER ; Set demag power cut flag All_pwmFETs_off Set_Pwms_Off wait_for_comm_wait: jnb Flags0.T3_PENDING, ($+5) ajmp wait_for_comm_wait ; Setup next wait time mov TMR3RLL, Wt_Zc_Scan_Start_L mov TMR3RLH, Wt_Zc_Scan_Start_H setb Flags0.T3_PENDING orl EIE1, #80h ; Enable timer3 interrupts ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Commutation routines ; ; No assumptions ; ; Performs commutation switching ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; Comm phase 1 to comm phase 2 comm1comm2: Set_RPM_Out jb Flags3.PGM_DIR_REV, comm12_rev clr IE_EA ; Disable all interrupts BcomFET_off ; Turn off comfet AcomFET_on ; Turn on comfet Set_Pwm_C ; To reapply power after a demag cut setb IE_EA Set_Comp_Phase_B ; Set comparator phase jmp comm_exit comm12_rev: clr IE_EA ; Disable all interrupts BcomFET_off ; Turn off comfet CcomFET_on ; Turn on comfet (reverse) Set_Pwm_A ; To reapply power after a demag cut setb IE_EA Set_Comp_Phase_B ; Set comparator phase jmp comm_exit ; Comm phase 2 to comm phase 3 comm2comm3: Clear_RPM_Out jb Flags3.PGM_DIR_REV, comm23_rev clr IE_EA ; Disable all interrupts CpwmFET_off ; Turn off pwmfet Set_Pwm_B AcomFET_on ; To reapply power after a demag cut setb IE_EA Set_Comp_Phase_C ; Set comparator phase ljmp comm_exit comm23_rev: clr IE_EA ; Disable all interrupts ApwmFET_off ; Turn off pwmfet (reverse) Set_Pwm_B CcomFET_on ; To reapply power after a demag cut setb IE_EA Set_Comp_Phase_A ; Set comparator phase (reverse) ljmp comm_exit ; Comm phase 3 to comm phase 4 comm3comm4: Set_RPM_Out jb Flags3.PGM_DIR_REV, comm34_rev clr IE_EA ; Disable all interrupts AcomFET_off ; Turn off comfet CcomFET_on ; Turn on comfet Set_Pwm_B ; To reapply power after a demag cut setb IE_EA Set_Comp_Phase_A ; Set comparator phase jmp comm_exit comm34_rev: clr IE_EA ; Disable all interrupts CcomFET_off ; Turn off comfet (reverse) AcomFET_on ; Turn on comfet (reverse) Set_Pwm_B ; To reapply power after a demag cut setb IE_EA Set_Comp_Phase_C ; Set comparator phase (reverse) jmp comm_exit ; Comm phase 4 to comm phase 5 comm4comm5: Clear_RPM_Out jb Flags3.PGM_DIR_REV, comm45_rev clr IE_EA ; Disable all interrupts BpwmFET_off ; Turn off pwmfet Set_Pwm_A CcomFET_on ; To reapply power after a demag cut setb IE_EA Set_Comp_Phase_B ; Set comparator phase ljmp comm_exit comm45_rev: clr IE_EA ; Disable all interrupts BpwmFET_off ; Turn off pwmfet Set_Pwm_C AcomFET_on ; To reapply power after a demag cut setb IE_EA Set_Comp_Phase_B ; Set comparator phase ljmp comm_exit ; Comm phase 5 to comm phase 6 comm5comm6: Set_RPM_Out jb Flags3.PGM_DIR_REV, comm56_rev clr IE_EA ; Disable all interrupts CcomFET_off ; Turn off comfet BcomFET_on ; Turn on comfet Set_Pwm_A ; To reapply power after a demag cut setb IE_EA Set_Comp_Phase_C ; Set comparator phase jmp comm_exit comm56_rev: clr IE_EA ; Disable all interrupts AcomFET_off ; Turn off comfet (reverse) BcomFET_on ; Turn on comfet Set_Pwm_C ; To reapply power after a demag cut setb IE_EA Set_Comp_Phase_A ; Set comparator phase (reverse) jmp comm_exit ; Comm phase 6 to comm phase 1 comm6comm1: Clear_RPM_Out jb Flags3.PGM_DIR_REV, comm61_rev clr IE_EA ; Disable all interrupts ApwmFET_off ; Turn off pwmfet Set_Pwm_C BcomFET_on ; To reapply power after a demag cut setb IE_EA Set_Comp_Phase_A ; Set comparator phase jmp comm_exit comm61_rev: clr IE_EA ; Disable all interrupts CpwmFET_off ; Turn off pwmfet (reverse) Set_Pwm_A BcomFET_on ; To reapply power after a demag cut setb IE_EA Set_Comp_Phase_C ; Set comparator phase (reverse) comm_exit: clr Flags0.DEMAG_CUT_POWER ; Clear demag power cut flag ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Switch power off routine ; ; No assumptions ; ; Switches all fets off ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** switch_power_off: All_pwmFETs_Off ; Turn off all pwm fets All_comFETs_Off ; Turn off all commutation fets Set_Pwms_Off ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Set default parameters ; ; No assumptions ; ; Sets default programming parameters ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** set_default_parameters: mov Temp1, #_Pgm_Gov_P_Gain mov @Temp1, #0FFh ; Governor P gain inc Temp1 mov @Temp1, #0FFh ; Governor I gain inc Temp1 mov @Temp1, #0FFh ; Governor mode inc Temp1 mov @Temp1, #0FFh ; Low voltage limit inc Temp1 mov @Temp1, #0FFh ; Multi gain inc Temp1 mov @Temp1, #0FFh inc Temp1 mov @Temp1, #DEFAULT_PGM_STARTUP_PWR inc Temp1 mov @Temp1, #0FFh ; Pwm freq inc Temp1 mov @Temp1, #DEFAULT_PGM_DIRECTION mov Temp1, #Pgm_Enable_TX_Program mov @Temp1, #DEFAULT_PGM_ENABLE_TX_PROGRAM inc Temp1 mov @Temp1, #0FFh ; Main rearm start inc Temp1 mov @Temp1, #0FFh ; Governor setup target inc Temp1 mov @Temp1, #0FFh ; Startup rpm inc Temp1 mov @Temp1, #0FFh ; Startup accel inc Temp1 mov @Temp1, #0FFh ; Voltage comp inc Temp1 mov @Temp1, #DEFAULT_PGM_COMM_TIMING inc Temp1 mov @Temp1, #0FFh ; Damping force inc Temp1 mov @Temp1, #0FFh ; Governor range inc Temp1 mov @Temp1, #0FFh ; Startup method inc Temp1 mov @Temp1, #DEFAULT_PGM_MIN_THROTTLE inc Temp1 mov @Temp1, #DEFAULT_PGM_MAX_THROTTLE inc Temp1 mov @Temp1, #DEFAULT_PGM_BEEP_STRENGTH inc Temp1 mov @Temp1, #DEFAULT_PGM_BEACON_STRENGTH inc Temp1 mov @Temp1, #DEFAULT_PGM_BEACON_DELAY inc Temp1 mov @Temp1, #0FFh ; Throttle rate inc Temp1 mov @Temp1, #DEFAULT_PGM_DEMAG_COMP inc Temp1 mov @Temp1, #0FFh ; Bec voltage high inc Temp1 mov @Temp1, #DEFAULT_PGM_CENTER_THROTTLE inc Temp1 mov @Temp1, #0FFh inc Temp1 mov @Temp1, #DEFAULT_PGM_ENABLE_TEMP_PROT inc Temp1 mov @Temp1, #DEFAULT_PGM_ENABLE_POWER_PROT inc Temp1 mov @Temp1, #0FFh ; Enable pwm input inc Temp1 mov @Temp1, #0FFh ; Pwm dither inc Temp1 mov @Temp1, #DEFAULT_PGM_BRAKE_ON_STOP inc Temp1 mov @Temp1, #DEFAULT_PGM_LED_CONTROL ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Scale throttle cal ; ; No assumptions ; ; Scales a throttle cal value ; Input is ACC, output is Temp2/Temp1 ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** scale_throttle_cal: mov Temp3, A mov B, #0Ch ; Calculate "3%" (for going from 1000us to numerical 1024) mul AB mov Temp4, B mov A, Temp3 clr C ; Shift to 9 bits rlc A mov Temp1, A mov A, #1 rlc A mov Temp2, A mov A, Temp1 ; Shift to 10 bits clr C rlc A mov Temp1, A mov A, Temp2 rlc A mov Temp2, A mov A, Temp1 ; Add "3%" clr C add A, Temp4 mov Temp1, A mov A, Temp2 addc A, #0 mov Temp2, A IF MCU_48MHZ == 1 mov A, Temp1 ; Shift to 11 bits clr C rlc A mov Temp1, A mov A, Temp2 rlc A mov Temp2, A ENDIF ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Decode settings ; ; No assumptions ; ; Decodes various settings ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** decode_settings: ; Load programmed direction mov Temp1, #Pgm_Direction mov A, @Temp1 clr C subb A, #3 setb Flags3.PGM_BIDIR jnc ($+4) clr Flags3.PGM_BIDIR clr Flags3.PGM_DIR_REV mov A, @Temp1 jnb ACC.1, ($+5) setb Flags3.PGM_DIR_REV mov C, Flags3.PGM_DIR_REV mov Flags3.PGM_BIDIR_REV, C ; Decode startup power mov Temp1, #Pgm_Startup_Pwr mov A, @Temp1 dec A mov DPTR, #STARTUP_POWER_TABLE movc A, @A+DPTR mov Temp1, #Pgm_Startup_Pwr_Decoded mov @Temp1, A ; Decode low rpm power slope mov Temp1, #Pgm_Startup_Pwr mov A, @Temp1 mov Low_Rpm_Pwr_Slope, A clr C subb A, #2 jnc ($+5) mov Low_Rpm_Pwr_Slope, #2 ; Decode demag compensation mov Temp1, #Pgm_Demag_Comp mov A, @Temp1 mov Demag_Pwr_Off_Thresh, #255 ; Set default cjne A, #2, decode_demag_high mov Demag_Pwr_Off_Thresh, #160 ; Settings for demag comp low decode_demag_high: cjne A, #3, decode_demag_done mov Demag_Pwr_Off_Thresh, #130 ; Settings for demag comp high decode_demag_done: ; Decode temperature protection limit mov Temp1, #Pgm_Enable_Temp_Prot mov A, @Temp1 mov Temp1, A jz decode_temp_done mov A, #(TEMP_LIMIT-TEMP_LIMIT_STEP) decode_temp_step: add A, #TEMP_LIMIT_STEP djnz Temp1, decode_temp_step decode_temp_done: mov Temp_Prot_Limit, A ; Decode throttle cal mov Temp1, #Pgm_Min_Throttle ; Throttle cal is in 4us units mov A, @Temp1 call scale_throttle_cal mov Min_Throttle_L, Temp1 mov Min_Throttle_H, Temp2 mov Temp1, #Pgm_Center_Throttle ; Throttle cal is in 4us units mov A, @Temp1 call scale_throttle_cal mov Center_Throttle_L, Temp1 mov Center_Throttle_H, Temp2 mov Temp1, #Pgm_Max_Throttle ; Throttle cal is in 4us units mov A, @Temp1 call scale_throttle_cal mov Max_Throttle_L, Temp1 mov Max_Throttle_H, Temp2 call switch_power_off ; Reset DPTR ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Find throttle gains ; ; No assumptions ; ; Finds throttle gains for both directions in bidirectional mode ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** find_throttle_gains: ; Check if full range is chosen jnb Flags2.RCP_FULL_RANGE, find_throttle_gains_normal mov Temp3, #0 ; Min throttle mov Temp4, #0 mov Temp5, #255 ; Max throttle mov Temp6, #0 mov Temp7, #0 ; Deadband call find_throttle_gain mov Throttle_Gain_M, Temp4 mov Throttle_Gain, Temp3 ret find_throttle_gains_normal: ; Check if bidirectional operation jnb Flags3.PGM_BIDIR, find_throttle_gains_bidir_done mov Temp1, #Pgm_Min_Throttle mov A, @Temp1 mov Temp3, A mov Temp4, #0 mov Temp1, #Pgm_Center_Throttle mov A, @Temp1 mov Temp5, A mov Temp6, #0 clr C mov A, Temp3 ; Scale gains in bidirectional rlc A mov Temp3, A mov A, Temp4 rlc A mov Temp4, A clr C mov A, Temp5 rlc A mov Temp5, A mov A, Temp6 rlc A mov Temp6, A mov Temp7, #10 ; Compensate for deadband in bidirectional call find_throttle_gain mov Throttle_Gain_BD_Rev_M, Temp4 mov Throttle_Gain_BD_Rev, Temp3 find_throttle_gains_bidir_done: mov Temp1, #Pgm_Min_Throttle jnb Flags3.PGM_BIDIR, ($+5) mov Temp1, #Pgm_Center_Throttle mov A, @Temp1 mov Temp3, A mov Temp4, #0 mov Temp1, #Pgm_Max_Throttle mov A, @Temp1 mov Temp5, A mov Temp6, #0 mov Temp7, #0 ; No deadband jnb Flags3.PGM_BIDIR, find_throttle_gain_fwd clr C mov A, Temp3 ; Scale gains in bidirectional rlc A mov Temp3, A mov A, Temp4 rlc A mov Temp4, A clr C mov A, Temp5 rlc A mov Temp5, A mov A, Temp6 rlc A mov Temp6, A mov Temp7, #10 ; Compensate for deadband in bidirectional find_throttle_gain_fwd: call find_throttle_gain mov Throttle_Gain_M, Temp4 mov Throttle_Gain, Temp3 ret ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Find throttle gain ; ; The difference between max and min throttle must be more than 140us (a Pgm_xxx_Throttle difference of 35) ; Temp4/3 holds min throttle, Temp6/5 holds max throttle, Temp7 holds deadband, Temp4/Temp3 gives resulting gain ; ; Finds throttle gain from throttle calibration values ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** find_throttle_gain: ; Subtract deadband from max clr C mov A, Temp5 subb A, Temp7 mov Temp5, A mov A, Temp6 subb A, #0 mov Temp6, A ; Calculate difference clr C mov A, Temp5 subb A, Temp3 mov Temp5, A mov A, Temp6 subb A, Temp4 mov Temp6, A ; Check that difference is minimum 35 clr C mov A, Temp5 subb A, #35 mov A, Temp6 subb A, #0 jnc ($+6) mov Temp5, #35 mov Temp6, #0 ; Check that difference is maximum 511 clr C mov A, Temp5 subb A, #255 mov A, Temp6 subb A, #1 jc ($+6) mov Temp5, #255 mov Temp6, #1 ; Find gain mov Temp4, #0FFh find_throttle_gain_loop: inc Temp4 mov Temp3, #0 test_throttle_gain: inc Temp3 mov A, Temp3 jnz test_throttle_gain_mult clr C mov A, Temp5 ; Set multiplier x2 and range /2 rlc A mov Temp5, A mov A, Temp6 rlc A mov Temp6, A ajmp find_throttle_gain_loop test_throttle_gain_mult: mov A, Temp5 ; A has difference, B has gain mov B, Temp3 mul AB mov Temp7, B mov A, Temp6 mov B, Temp3 mul AB add A, Temp7 subb A, #124 jc test_throttle_gain mov A, Temp3 cpl A jz find_throttle_gain_loop ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Average throttle ; ; Outputs result in Temp8 ; ; Averages throttle calibration readings ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** average_throttle: setb Flags2.RCP_FULL_RANGE ; Set range to 1000-2020us call find_throttle_gains ; Set throttle gains call wait30ms call wait30ms mov Temp3, #0 mov Temp4, #0 mov Temp5, #16 ; Average 16 measurments average_throttle_meas: call wait3ms ; Wait for new RC pulse value mov A, New_Rcp ; Get new RC pulse value add A, Temp3 mov Temp3, A mov A, #0 addc A, Temp4 mov Temp4, A djnz Temp5, average_throttle_meas mov Temp5, #4 ; Shift 4 times average_throttle_div: clr C mov A, Temp4 ; Shift right rrc A mov Temp4, A mov A, Temp3 rrc A mov Temp3, A djnz Temp5, average_throttle_div mov Temp8, A ; Copy to Temp8 mov A, Temp4 jz ($+4) mov Temp8, #0FFh clr Flags2.RCP_FULL_RANGE call find_throttle_gains ; Set throttle gains ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; LED control ; ; No assumptions ; ; Controls LEDs ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** led_control: mov Temp1, #Pgm_LED_Control mov A, @Temp1 mov Temp2, A anl A, #03h Set_LED_0 jnz led_0_done Clear_LED_0 led_0_done: mov A, Temp2 anl A, #0Ch Set_LED_1 jnz led_1_done Clear_LED_1 led_1_done: mov A, Temp2 anl A, #030h Set_LED_2 jnz led_2_done Clear_LED_2 led_2_done: mov A, Temp2 anl A, #0C0h Set_LED_3 jnz led_3_done Clear_LED_3 led_3_done: ret ;**** **** **** **** **** **** **** **** **** **** **** **** **** ;**** **** **** **** **** **** **** **** **** **** **** **** **** ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Main program start ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** ;**** **** **** **** **** **** **** **** **** **** **** **** **** ;**** **** **** **** **** **** **** **** **** **** **** **** **** pgm_start: ; Initialize flash keys to invalid values mov Flash_Key_1, #0 mov Flash_Key_2, #0 ; Disable the WDT. mov WDTCN, #0DEh ; Disable watchdog mov WDTCN, #0ADh ; Initialize stack mov SP, #0c0h ; Stack = 64 upper bytes of RAM ; Initialize VDD monitor orl VDM0CN, #080h ; Enable the VDD monitor IF ONE_S_CAPABLE == 0 mov RSTSRC, #06h ; Set missing clock and VDD monitor as a reset source if not 1S capable ELSE mov RSTSRC, #04h ; Do not set VDD monitor as a reset source for 1S ESCSs, in order to avoid resets due to it ENDIF ; Set clock frequency mov CLKSEL, #00h ; Set clock divider to 1 ; Switch power off call switch_power_off ; Ports initialization mov P0, #P0_INIT mov P0MDIN, #P0_DIGITAL mov P0MDOUT, #P0_PUSHPULL mov P0, #P0_INIT mov P0SKIP, #P0_SKIP mov P1, #P1_INIT mov P1MDIN, #P1_DIGITAL mov P1MDOUT, #P1_PUSHPULL mov P1, #P1_INIT mov P1SKIP, #P1_SKIP mov P2MDOUT, #P2_PUSHPULL ; Initialize the XBAR and related functionality Initialize_Xbar ; Switch power off again, after initializing ports call switch_power_off ; Clear RAM clr A ; Clear accumulator mov Temp1, A ; Clear Temp1 clear_ram: mov @Temp1, A ; Clear RAM djnz Temp1, clear_ram ; Is A not zero? - jump ; Set default programmed parameters call set_default_parameters ; Read all programmed parameters call read_all_eeprom_parameters ; Set beep strength mov Temp1, #Pgm_Beep_Strength mov Beep_Strength, @Temp1 ; Set initial arm variable mov Initial_Arm, #1 ; Initializing beep clr IE_EA ; Disable interrupts explicitly call wait200ms call beep_f1 call wait30ms call beep_f2 call wait30ms call beep_f3 call wait30ms call led_control ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; No signal entry point ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** init_no_signal: ; Disable interrupts explicitly clr IE_EA ; Initialize flash keys to invalid values mov Flash_Key_1, #0 mov Flash_Key_2, #0 ; Check if input signal is high for more than 15ms mov Temp1, #250 input_high_check_1: mov Temp2, #250 input_high_check_2: jnb RTX_PORT.RTX_PIN, bootloader_done ; Look for low djnz Temp2, input_high_check_2 djnz Temp1, input_high_check_1 ljmp 1C00h ; Jump to bootloader bootloader_done: ; Decode settings call decode_settings ; Find throttle gain from stored min and max settings call find_throttle_gains ; Set beep strength mov Temp1, #Pgm_Beep_Strength mov Beep_Strength, @Temp1 ; Switch power off call switch_power_off ; Set clock frequency IF MCU_48MHZ == 1 Set_MCU_Clk_24MHz ENDIF mov IT01CF, #RTX_PIN ; Route RCP input to INT0 mov TCON, #11h ; Timer0 run and INT0 edge triggered mov CKCON0, #04h ; Timer0 clock is system clock mov TMOD, #09h ; Timer0 set to 16bits and gated by INT0 mov TMR2CN0, #04h ; Timer2 enabled mov TMR3CN0, #04h ; Timer3 enabled Initialize_PCA ; Initialize PCA Set_Pwm_Polarity ; Set pwm polarity Enable_Power_Pwm_Module ; Enable power pwm module Enable_Damp_Pwm_Module ; Enable damping pwm module ; Enable interrupts IF MCU_48MHZ == 0 mov IE, #21h ; Enable timer2 interrupts and INT0 interrupts ELSE mov IE, #23h ; Enable timer0, timer2 interrupts and INT0 interrupts ENDIF mov EIE1, #90h ; Enable timer3 and PCA0 interrupts mov EIP1, #10h ; High priority to PCA0 interrupts ; Initialize comparator mov CMP0CN0, #80h ; Comparator enabled, no hysteresis mov CMP0MD, #00h ; Comparator response time 100ns ; Initialize ADC Initialize_Adc ; Initialize ADC operation call wait1ms setb IE_EA ; Enable all interrupts ; Reset stall count mov Stall_Cnt, #0 ; Initialize RC pulse clr Flags2.RCP_UPDATED ; Clear updated flag call wait200ms ; Test whether signal is OneShot125 clr Flags2.RCP_ONESHOT125 ; Clear OneShot125 flag mov Rcp_Outside_Range_Cnt, #0 ; Reset out of range counter call wait100ms ; Wait for new RC pulse clr C mov A, Rcp_Outside_Range_Cnt ; Check how many pulses were outside normal range ("745-2235us") subb A, #10 jc validate_rcp_start setb Flags2.RCP_ONESHOT125 ; Set OneShot125 flag ; Test whether signal is OneShot42 clr Flags2.RCP_ONESHOT42 ; Clear OneShot42 flag mov Rcp_Outside_Range_Cnt, #0 ; Reset out of range counter call wait100ms ; Wait for new RC pulse clr C mov A, Rcp_Outside_Range_Cnt ; Check how many pulses were outside normal range ("745-2235us") subb A, #10 jc validate_rcp_start setb Flags2.RCP_ONESHOT42 ; Set OneShot42 flag ; Test whether signal is Multishot clr Flags2.RCP_MULTISHOT ; Clear Multishot flag mov Rcp_Outside_Range_Cnt, #0 ; Reset out of range counter call wait100ms ; Wait for new RC pulse clr C mov A, Rcp_Outside_Range_Cnt ; Check how many pulses were outside normal range ("745-2235us") subb A, #10 jc validate_rcp_start setb Flags2.RCP_MULTISHOT ; Set OneShot42 flag ; Validate RC pulse validate_rcp_start: call wait3ms ; Wait for new RC pulse jb Flags2.RCP_UPDATED, ($+6) ; Is there an updated RC pulse available - proceed ljmp init_no_signal ; Go back to detect input signal ; Beep arm sequence start signal clr IE_EA ; Disable all interrupts call beep_f1 ; Signal that RC pulse is ready call beep_f1 call beep_f1 setb IE_EA ; Enable all interrupts call wait200ms ; Arming sequence start arming_start: jnb Flags3.PGM_BIDIR, ($+5) ajmp program_by_tx_checked ; Disable tx programming if bidirectional operation call wait3ms mov Temp1, #Pgm_Enable_TX_Program; Start programming mode entry if enabled mov A, @Temp1 clr C subb A, #1 ; Is TX programming enabled? jnc arming_initial_arm_check ; Yes - proceed jmp program_by_tx_checked ; No - branch arming_initial_arm_check: mov A, Initial_Arm ; Yes - check if it is initial arm sequence clr C subb A, #1 ; Is it the initial arm sequence? jnc arming_check ; Yes - proceed jmp program_by_tx_checked ; No - branch arming_check: ; Initialize flash keys to valid values mov Flash_Key_1, #0A5h mov Flash_Key_2, #0F1h ; Throttle calibration and tx program entry mov Temp8, #2 ; Set 1 seconds wait time throttle_high_cal: setb Flags2.RCP_FULL_RANGE ; Set range to 1000-2020us call find_throttle_gains ; Set throttle gains call wait100ms ; Wait for new throttle value clr IE_EA ; Disable interrupts (freeze New_Rcp value) clr Flags2.RCP_FULL_RANGE ; Set programmed range call find_throttle_gains ; Set throttle gains clr C mov A, New_Rcp ; Load new RC pulse value subb A, #(255/2) ; Is RC pulse above midstick? setb IE_EA ; Enable interrupts jc program_by_tx_checked ; No - branch call wait1ms clr IE_EA ; Disable all interrupts call beep_f4 setb IE_EA ; Enable all interrupts djnz Temp8, throttle_high_cal ; Continue to wait call average_throttle clr C mov A, Temp8 mov Temp1, #Pgm_Max_Throttle ; Store mov @Temp1, A call wait200ms call success_beep throttle_low_cal_start: mov Temp8, #10 ; Set 3 seconds wait time throttle_low_cal: setb Flags2.RCP_FULL_RANGE ; Set range to 1000-2020us call find_throttle_gains ; Set throttle gains call wait100ms clr IE_EA ; Disable interrupts (freeze New_Rcp value) clr Flags2.RCP_FULL_RANGE ; Set programmed range call find_throttle_gains ; Set throttle gains clr C mov A, New_Rcp ; Load new RC pulse value subb A, #(255/2) ; Below midstick? setb IE_EA ; Enable interrupts jnc throttle_low_cal_start ; No - start over call wait1ms clr IE_EA ; Disable all interrupts call beep_f1 call wait10ms call beep_f1 setb IE_EA ; Enable all interrupts djnz Temp8, throttle_low_cal ; Continue to wait call average_throttle mov A, Temp8 add A, #3 ; Add about 1% mov Temp1, #Pgm_Min_Throttle ; Store mov @Temp1, A mov Temp1, A ; Min throttle in Temp1 mov Temp2, #Pgm_Max_Throttle mov A, @Temp2 clr C subb A, #35 ; Subtract 35 (140us) from max throttle jc program_by_tx_entry_limit subb A, Temp1 ; Subtract min from max jnc program_by_tx_entry_store program_by_tx_entry_limit: mov A, Temp1 ; Load min add A, #35 ; Make max 140us higher than min mov Temp1, #Pgm_Max_Throttle ; Store new max mov @Temp1, A program_by_tx_entry_store: call wait200ms call erase_and_store_all_in_eeprom call success_beep_inverted program_by_tx_entry_wait: call wait100ms call find_throttle_gains ; Set throttle gains ajmp init_no_signal ; Go back program_by_tx_checked: ; Initialize flash keys to invalid values mov Flash_Key_1, #0 mov Flash_Key_2, #0 call wait100ms ; Wait for new throttle value mov Temp1, #1 ; Default stop value jnb Flags3.PGM_BIDIR, ($+5) ; No - branch mov Temp1, #5 ; Higher stop value for bidirectional clr C mov A, New_Rcp ; Load new RC pulse value subb A, Temp1 ; Below stop? jc arm_end_beep ; Yes - proceed jmp arming_start ; No - start over arm_end_beep: ; Beep arm sequence end signal clr IE_EA ; Disable all interrupts call beep_f4 ; Signal that rcpulse is ready call beep_f4 call beep_f4 setb IE_EA ; Enable all interrupts call wait200ms ; Clear initial arm variable mov Initial_Arm, #0 ; Armed and waiting for power on wait_for_power_on: clr A mov Power_On_Wait_Cnt_L, A ; Clear wait counter mov Power_On_Wait_Cnt_H, A wait_for_power_on_loop: inc Power_On_Wait_Cnt_L ; Increment low wait counter mov A, Power_On_Wait_Cnt_L cpl A jnz wait_for_power_on_no_beep; Counter wrapping (about 3 sec) inc Power_On_Wait_Cnt_H ; Increment high wait counter mov Temp1, #Pgm_Beacon_Delay mov A, @Temp1 mov Temp1, #25 ; Approximately 1 min dec A jz beep_delay_set mov Temp1, #50 ; Approximately 2 min dec A jz beep_delay_set mov Temp1, #125 ; Approximately 5 min dec A jz beep_delay_set mov Temp1, #250 ; Approximately 10 min dec A jz beep_delay_set mov Power_On_Wait_Cnt_H, #0 ; Reset counter for infinite delay beep_delay_set: clr C mov A, Power_On_Wait_Cnt_H subb A, Temp1 ; Check against chosen delay jc wait_for_power_on_no_beep; Has delay elapsed? call switch_power_off ; Switch power off in case braking is set call wait1ms dec Power_On_Wait_Cnt_H ; Decrement high wait counter mov Power_On_Wait_Cnt_L, #0 ; Set low wait counter mov Temp1, #Pgm_Beacon_Strength mov Beep_Strength, @Temp1 clr IE_EA ; Disable all interrupts call beep_f4 ; Signal that there is no signal setb IE_EA ; Enable all interrupts mov Temp1, #Pgm_Beep_Strength mov Beep_Strength, @Temp1 call wait100ms ; Wait for new RC pulse to be measured wait_for_power_on_no_beep: call wait10ms mov A, Rcp_Timeout_Cntd ; Load RC pulse timeout counter value jnz wait_for_power_on_not_missing ; If it is not zero - proceed jmp init_no_signal ; If pulses missing - go back to detect input signal wait_for_power_on_not_missing: mov Temp1, #1 clr C mov A, New_Rcp ; Load new RC pulse value subb A, Temp1 ; Higher than stop (plus some hysteresis)? jc wait_for_power_on_loop ; No - start over lcall wait100ms ; Wait to see if start pulse was only a glitch mov A, Rcp_Timeout_Cntd ; Load RC pulse timeout counter value jnz ($+5) ; If it is not zero - proceed ljmp init_no_signal ; If it is zero (pulses missing) - go back to detect input signal ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Start entry point ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** init_start: clr IE_EA call switch_power_off clr A setb IE_EA clr A mov Adc_Conversion_Cnt, A mov Flags0, A ; Clear flags0 mov Flags1, A ; Clear flags1 mov Demag_Detected_Metric, A ; Clear demag metric ;**** **** **** **** **** ; Motor start beginning ;**** **** **** **** **** mov Adc_Conversion_Cnt, #8 ; Make sure a temp reading is done call wait1ms call start_adc_conversion read_initial_temp: jnb ADC0CN0_ADINT, read_initial_temp Read_Adc_Result ; Read initial temperature mov A, Temp2 jnz ($+3) ; Is reading below 256? mov Temp1, A ; Yes - set average temperature value to zero mov Current_Average_Temp, Temp1 ; Set initial average temperature call check_temp_voltage_and_limit_power mov Adc_Conversion_Cnt, #8 ; Make sure a temp reading is done next time ; Set up start operating conditions clr IE_EA ; Disable interrupts mov Pwm_Limit, #0FFh ; Set pwm limit to max call set_startup_pwm mov Pwm_Limit, Pwm_Limit_Beg mov Pwm_Limit_By_Rpm, Pwm_Limit_Beg setb IE_EA ; Begin startup sequence IF MCU_48MHZ == 1 Set_MCU_Clk_48MHz ENDIF jnb Flags3.PGM_BIDIR, init_start_bidir_done ; Check if bidirectional operation clr Flags3.PGM_DIR_REV ; Set spinning direction. Default fwd jnb Flags2.RCP_DIR_REV, ($+5) ; Check force direction setb Flags3.PGM_DIR_REV ; Set spinning direction init_start_bidir_done: setb Flags1.STARTUP_PHASE ; Set startup phase flag mov Startup_Cnt, #0 ; Reset counter call comm5comm6 ; Initialize commutation call comm6comm1 call initialize_timing ; Initialize timing call calc_next_comm_timing ; Set virtual commutation point call initialize_timing ; Initialize timing call calc_next_comm_timing call initialize_timing ; Initialize timing ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; ; Run entry point ; ;**** **** **** **** **** **** **** **** **** **** **** **** **** ; Run 1 = B(p-on) + C(n-pwm) - comparator A evaluated ; Out_cA changes from low to high run1: call wait_for_comp_out_high ; Wait for high ; setup_comm_wait ; Setup wait time from zero cross to commutation ; evaluate_comparator_integrity ; Check whether comparator reading has been normal call wait_for_comm ; Wait from zero cross to commutation call comm1comm2 ; Commutate call calc_next_comm_timing ; Calculate next timing and wait advance timing wait ; wait_advance_timing ; Wait advance timing and start zero cross wait ; calc_new_wait_times ; wait_before_zc_scan ; Wait zero cross wait and start zero cross timeout ; Run 2 = A(p-on) + C(n-pwm) - comparator B evaluated ; Out_cB changes from high to low run2: call wait_for_comp_out_low ; setup_comm_wait ; evaluate_comparator_integrity jb Flags1.HIGH_RPM, ($+6) ; Skip if high rpm lcall set_pwm_limit_low_rpm jnb Flags1.HIGH_RPM, ($+6) ; Do if high rpm lcall set_pwm_limit_high_rpm call wait_for_comm call comm2comm3 call calc_next_comm_timing ; wait_advance_timing ; calc_new_wait_times ; wait_before_zc_scan ; Run 3 = A(p-on) + B(n-pwm) - comparator C evaluated ; Out_cC changes from low to high run3: call wait_for_comp_out_high ; setup_comm_wait ; evaluate_comparator_integrity call wait_for_comm call comm3comm4 call calc_next_comm_timing ; wait_advance_timing ; calc_new_wait_times ; wait_before_zc_scan ; Run 4 = C(p-on) + B(n-pwm) - comparator A evaluated ; Out_cA changes from high to low run4: call wait_for_comp_out_low ; setup_comm_wait ; evaluate_comparator_integrity call wait_for_comm call comm4comm5 call calc_next_comm_timing ; wait_advance_timing ; calc_new_wait_times ; wait_before_zc_scan ; Run 5 = C(p-on) + A(n-pwm) - comparator B evaluated ; Out_cB changes from low to high run5: call wait_for_comp_out_high ; setup_comm_wait ; evaluate_comparator_integrity call wait_for_comm call comm5comm6 call calc_next_comm_timing ; wait_advance_timing ; calc_new_wait_times ; wait_before_zc_scan ; Run 6 = B(p-on) + A(n-pwm) - comparator C evaluated ; Out_cC changes from high to low run6: call start_adc_conversion call wait_for_comp_out_low ; setup_comm_wait ; evaluate_comparator_integrity call wait_for_comm call comm6comm1 call check_temp_voltage_and_limit_power call calc_next_comm_timing ; wait_advance_timing ; calc_new_wait_times ; wait_before_zc_scan ; Check if it is direct startup jnb Flags1.STARTUP_PHASE, normal_run_checks ; Set spoolup power variables mov Pwm_Limit, Pwm_Limit_Beg ; Set initial max power ; Check startup counter mov Temp2, #24 ; Set nominal startup parameters mov Temp3, #12 clr C mov A, Startup_Cnt ; Load counter subb A, Temp2 ; Is counter above requirement? jc direct_start_check_rcp ; No - proceed clr Flags1.STARTUP_PHASE ; Clear startup phase flag setb Flags1.INITIAL_RUN_PHASE ; Set initial run phase flag mov Initial_Run_Rot_Cntd, Temp3 ; Set initial run rotation count mov Pwm_Limit, Pwm_Limit_Beg mov Pwm_Limit_By_Rpm, Pwm_Limit_Beg jmp normal_run_checks direct_start_check_rcp: clr C mov A, New_Rcp ; Load new pulse value subb A, #1 ; Check if pulse is below stop value jc ($+5) ljmp run1 ; Continue to run jmp run_to_wait_for_power_on normal_run_checks: ; Check if it is initial run phase jnb Flags1.INITIAL_RUN_PHASE, initial_run_phase_done ; If not initial run phase - branch jb Flags1.DIR_CHANGE_BRAKE, initial_run_phase_done ; If a direction change - branch ; Decrement startup rotaton count mov A, Initial_Run_Rot_Cntd dec A ; Check number of initial rotations jnz normal_run_check_startup_rot ; Branch if counter is not zero clr Flags1.INITIAL_RUN_PHASE ; Clear initial run phase flag setb Flags1.MOTOR_STARTED ; Set motor started jmp run1 ; Continue with normal run normal_run_check_startup_rot: mov Initial_Run_Rot_Cntd, A ; Not zero - store counter clr C mov A, New_Rcp ; Load new pulse value subb A, #1 ; Check if pulse is below stop value jc ($+5) ljmp run1 ; Continue to run jmp run_to_wait_for_power_on initial_run_phase_done: ; Reset stall count mov Stall_Cnt, #0 ; Exit run loop after a given time mov Temp1, #250 mov Temp2, #Pgm_Brake_On_Stop mov A, @Temp2 jz ($+4) mov Temp1, #3 ; About 100ms before stopping when brake is set clr C mov A, Rcp_Stop_Cnt ; Load stop RC pulse counter low byte value subb A, Temp1 ; Is number of stop RC pulses above limit? jnc run_to_wait_for_power_on ; Yes, go back to wait for poweron mov A, Rcp_Timeout_Cntd ; Load RC pulse timeout counter value jz run_to_wait_for_power_on ; If it is zero - go back to wait for poweron run6_check_dir: jnb Flags3.PGM_BIDIR, run6_check_speed ; Check if bidirectional operation jb Flags3.PGM_DIR_REV, run6_check_dir_rev ; Check if actual rotation direction jb Flags2.RCP_DIR_REV, run6_check_dir_change ; Matches force direction jmp run6_check_speed run6_check_dir_rev: jnb Flags2.RCP_DIR_REV, run6_check_dir_change jmp run6_check_speed run6_check_dir_change: jb Flags1.DIR_CHANGE_BRAKE, run6_check_speed setb Flags1.DIR_CHANGE_BRAKE ; Set brake flag mov Pwm_Limit, Pwm_Limit_Beg ; Set max power while braking jmp run4 ; Go back to run 4, thereby changing force direction run6_check_speed: mov Temp1, #0F0h ; Default minimum speed jnb Flags1.DIR_CHANGE_BRAKE, run6_brake_done; Is it a direction change? mov Pwm_Limit, Pwm_Limit_Beg ; Set max power while braking mov Temp1, #20h ; Bidirectional braking termination speed run6_brake_done: clr C mov A, Comm_Period4x_H ; Is Comm_Period4x more than 32ms (~1220 eRPM)? subb A, Temp1 jnc ($+5) ; Yes - stop or turn direction ljmp run1 ; No - go back to run 1 jnb Flags1.DIR_CHANGE_BRAKE, run_to_wait_for_power_on ; If it is not a direction change - stop clr Flags1.DIR_CHANGE_BRAKE ; Clear brake flag clr Flags3.PGM_DIR_REV ; Set spinning direction. Default fwd jnb Flags2.RCP_DIR_REV, ($+5) ; Check force direction setb Flags3.PGM_DIR_REV ; Set spinning direction setb Flags1.INITIAL_RUN_PHASE mov Initial_Run_Rot_Cntd, #18 mov Pwm_Limit, Pwm_Limit_Beg ; Set initial max power jmp run1 ; Go back to run 1 run_to_wait_for_power_on_fail: inc Stall_Cnt ; Increment stall count mov A, New_Rcp ; Check if RCP is zero, then it is a normal stop jz run_to_wait_for_power_on ajmp run_to_wait_for_power_on_stall_done run_to_wait_for_power_on: mov Stall_Cnt, #0 run_to_wait_for_power_on_stall_done: clr IE_EA call switch_power_off mov Flags0, #0 ; Clear flags0 mov Flags1, #0 ; Clear flags1 IF MCU_48MHZ == 1 Set_MCU_Clk_24MHz ENDIF setb IE_EA call wait100ms ; Wait for pwm to be stopped call switch_power_off mov Temp1, #Pgm_Brake_On_Stop mov A, @Temp1 jz run_to_wait_for_power_on_brake_done AcomFET_on BcomFET_on CcomFET_on run_to_wait_for_power_on_brake_done: clr C mov A, Stall_Cnt subb A, #4 jc jmp_wait_for_power_on jmp init_no_signal jmp_wait_for_power_on: jmp wait_for_power_on ; Go back to wait for power on ;**** **** **** **** **** **** **** **** **** **** **** **** **** $include (BLHeliPgm.inc) ; Include source code for programming the ESC $include (BLHeliBootLoad.inc) ; Include source code for bootloader ;**** **** **** **** **** **** **** **** **** **** **** **** **** CSEG AT 19FDh reset: ljmp pgm_start END