You can not select more than 25 topics Topics must start with a letter or number, can include dashes ('-') and can be up to 35 characters long.

3734 lines
97 KiB

  1. $NOMOD51
  2. ;**** **** **** **** ****
  3. ;
  4. ; BLHeli program for controlling brushless motors in multirotors
  5. ;
  6. ; Copyright 2011, 2012 Steffen Skaug
  7. ; This program is distributed under the terms of the GNU General Public License
  8. ;
  9. ; This file is part of BLHeli.
  10. ;
  11. ; BLHeli is free software: you can redistribute it and/or modify
  12. ; it under the terms of the GNU General Public License as published by
  13. ; the Free Software Foundation, either version 3 of the License, or
  14. ; (at your option) any later version.
  15. ;
  16. ; BLHeli is distributed in the hope that it will be useful,
  17. ; but WITHOUT ANY WARRANTY; without even the implied warranty of
  18. ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  19. ; GNU General Public License for more details.
  20. ;
  21. ; You should have received a copy of the GNU General Public License
  22. ; along with BLHeli. If not, see <http://www.gnu.org/licenses/>.
  23. ;
  24. ;**** **** **** **** ****
  25. ;
  26. ; This software was initially designed for use with Eflite mCP X, but is now adapted to copters/planes in general
  27. ;
  28. ; The software was inspired by and started from from Bernard Konze's BLMC: http://home.versanet.de/~bkonze/blc_6a/blc_6a.htm
  29. ; And also Simon Kirby's TGY: https://github.com/sim-/tgy
  30. ;
  31. ; This file is best viewed with tab width set to 5
  32. ;
  33. ; The code is designed for multirotor applications, running damped light mode
  34. ;
  35. ; 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.
  36. ; The code autodetects normal, OneShot125, Oneshot42 or Multishot.
  37. ;
  38. ; The first lines of the software must be modified according to the chosen environment:
  39. ; ESCNO EQU "ESC"
  40. ; MCU_48MHZ EQU "N"
  41. ; FETON_DELAY EQU "N"
  42. ;
  43. ;**** **** **** **** ****
  44. ; Revision history:
  45. ; - Rev16.0 Started. Built upon rev 14.5 of base code
  46. ; Using hardware pwm for very smooth throttle response, silent running and support of very high rpms
  47. ; Implemented reverse bidirectional mode
  48. ; Implemented separate throttle gains fwd and rev in bidirectional mode
  49. ; Implemented support for Oneshot42 and Multishot
  50. ; - Rev16.1 Made low rpm power limiting programmable through the startup power parameter
  51. ; - Rev16.2 Fixed bug that prevented temperature protection
  52. ; Improved robustness to very high input signal rates
  53. ; Beeps can be turned off by programming beep strength to 1
  54. ; Throttle cal difference is checked to be above required minimum before storing. Throttle cal max is not stored until successful min throttle cal
  55. ; - Rev16.3 Implemented programmable temperature protection
  56. ; Improved protection of bootloader and generally reduced risk of flash corruption
  57. ; Some small changes for improved sync hold
  58. ;
  59. ;
  60. ;**** **** **** **** ****
  61. ; Minimum 8K Bytes of In-System Self-Programmable Flash
  62. ; Minimum 512 Bytes Internal SRAM
  63. ;
  64. ;**** **** **** **** ****
  65. ; Master clock is internal 24MHz oscillator (or 48MHz, for which the times below are halved)
  66. ; Although 24/48 are used in the code, the exact clock frequencies are 24.5MHz or 49.0 MHz
  67. ; Timer 0 (41.67ns counts) always counts up and is used for
  68. ; - RC pulse measurement
  69. ; Timer 2 (500ns counts) always counts up and is used for
  70. ; - RC pulse timeout counts and commutation times
  71. ; Timer 3 (500ns counts) always counts up and is used for
  72. ; - Commutation timeouts
  73. ; PCA0 (41.67ns counts) always counts up and is used for
  74. ; - Hardware PWM generation
  75. ;
  76. ;**** **** **** **** ****
  77. ; Interrupt handling
  78. ; The C8051 does not disable interrupts when entering an interrupt routine.
  79. ; Also some interrupt flags need to be cleared by software
  80. ; The code disables interrupts in some interrupt routines
  81. ; - Interrupts are disabled during beeps, to avoid audible interference from interrupts
  82. ;
  83. ;**** **** **** **** ****
  84. ; Motor control:
  85. ; - Brushless motor control with 6 states for each electrical 360 degrees
  86. ; - An advance timing of 0deg has zero cross 30deg after one commutation and 30deg before the next
  87. ; - Timing advance in this implementation is set to 15deg nominally
  88. ; - Motor pwm is always damped light (aka complementary pwm, regenerative braking)
  89. ; Motor sequence starting from zero crossing:
  90. ; - Timer wait: Wt_Comm 15deg ; Time to wait from zero cross to actual commutation
  91. ; - Timer wait: Wt_Advance 15deg ; Time to wait for timing advance. Nominal commutation point is after this
  92. ; - Timer wait: Wt_Zc_Scan 7.5deg ; Time to wait before looking for zero cross
  93. ; - Scan for zero cross 22.5deg ; Nominal, with some motor variations
  94. ;
  95. ; Motor startup:
  96. ; There is a startup phase and an initial run phase, before normal bemf commutation run begins.
  97. ;
  98. ;**** **** **** **** ****
  99. ; List of enumerated supported ESCs
  100. A_ EQU 1 ; X X RC X MC MB MA CC X X Cc Cp Bc Bp Ac Ap
  101. B_ EQU 2 ; X X RC X MC MB MA CC X X Ap Ac Bp Bc Cp Cc
  102. C_ EQU 3 ; Ac Ap MC MB MA CC X RC X X X X Cc Cp Bc Bp
  103. D_ EQU 4 ; X X RC X CC MA MC MB X X Cc Cp Bc Bp Ac Ap Com fets inverted
  104. E_ EQU 5 ; L1 L0 RC X MC MB MA CC X L2 Cc Cp Bc Bp Ac Ap A with LEDs
  105. F_ EQU 6 ; X X RC X MA MB MC CC X X Cc Cp Bc Bp Ac Ap
  106. 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
  107. H_ EQU 8 ; RC X X X MA MB CC MC X Ap Bp Cp X Ac Bc Cc
  108. I_ EQU 9 ; X X RC X MC MB MA CC X X Ac Bc Cc Ap Bp Cp
  109. J_ EQU 10 ; L2 L1 L0 RC CC MB MC MA X X Cc Bc Ac Cp Bp Ap
  110. K_ EQU 11 ; X X MC X MB CC MA RC X X Ap Bp Cp Cc Bc Ac Com fets inverted
  111. L_ EQU 12 ; X X RC X CC MA MB MC X X Ac Bc Cc Ap Bp Cp
  112. ;**** **** **** **** ****
  113. ; Select the port mapping to use (or unselect all for use with external batch compile file)
  114. ;ESCNO EQU A_
  115. ;ESCNO EQU B_
  116. ;ESCNO EQU C_
  117. ;ESCNO EQU D_
  118. ;ESCNO EQU E_
  119. ;ESCNO EQU F_
  120. ;ESCNO EQU G_
  121. ;ESCNO EQU H_
  122. ;ESCNO EQU I_
  123. ;ESCNO EQU J_
  124. ;ESCNO EQU K_
  125. ;ESCNO EQU L_
  126. ;**** **** **** **** ****
  127. ; Select the MCU type (or unselect for use with external batch compile file)
  128. ;MCU_48MHZ EQU 1
  129. ;**** **** **** **** ****
  130. ; Select the fet deadtime (or unselect for use with external batch compile file)
  131. ;FETON_DELAY EQU 50 ; 20.4ns per step
  132. ;**** **** **** **** ****
  133. ; ESC selection statements
  134. IF ESCNO == A_
  135. $include (A.inc) ; Select pinout A
  136. ENDIF
  137. IF ESCNO == B_
  138. $include (B.inc) ; Select pinout B
  139. ENDIF
  140. IF ESCNO == C_
  141. $include (C.inc) ; Select pinout C
  142. ENDIF
  143. IF ESCNO == D_
  144. $include (D.inc) ; Select pinout D
  145. ENDIF
  146. IF ESCNO == E_
  147. $include (E.inc) ; Select pinout E
  148. ENDIF
  149. IF ESCNO == F_
  150. $include (F.inc) ; Select pinout F
  151. ENDIF
  152. IF ESCNO == G_
  153. $include (G.inc) ; Select pinout G
  154. ENDIF
  155. IF ESCNO == H_
  156. $include (H.inc) ; Select pinout H
  157. ENDIF
  158. IF ESCNO == I_
  159. $include (I.inc) ; Select pinout I
  160. ENDIF
  161. IF ESCNO == J_
  162. $include (J.inc) ; Select pinout J
  163. ENDIF
  164. IF ESCNO == K_
  165. $include (K.inc) ; Select pinout K
  166. ENDIF
  167. IF ESCNO == L_
  168. $include (L.inc) ; Select pinout L
  169. ENDIF
  170. ;**** **** **** **** ****
  171. ; Programming defaults
  172. ;
  173. 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
  174. DEFAULT_PGM_COMM_TIMING EQU 3 ; 1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=High
  175. DEFAULT_PGM_DEMAG_COMP EQU 2 ; 1=Disabled 2=Low 3=High
  176. DEFAULT_PGM_DIRECTION EQU 1 ; 1=Normal 2=Reversed 3=Bidir 4=Bidir rev
  177. DEFAULT_PGM_BEEP_STRENGTH EQU 40 ; Beep strength
  178. DEFAULT_PGM_BEACON_STRENGTH EQU 80 ; Beacon strength
  179. DEFAULT_PGM_BEACON_DELAY EQU 4 ; 1=1m 2=2m 3=5m 4=10m 5=Infinite
  180. ; COMMON
  181. DEFAULT_PGM_ENABLE_TX_PROGRAM EQU 1 ; 1=Enabled 0=Disabled
  182. DEFAULT_PGM_MIN_THROTTLE EQU 37 ; 4*37+1000=1148
  183. DEFAULT_PGM_MAX_THROTTLE EQU 208 ; 4*208+1000=1832
  184. DEFAULT_PGM_CENTER_THROTTLE EQU 122 ; 4*122+1000=1488 (used in bidirectional mode)
  185. DEFAULT_PGM_ENABLE_TEMP_PROT EQU 7 ; 0=Disabled 1=80C 2=90C 3=100C 4=110C 5=120C 6=130C 7=140C
  186. DEFAULT_PGM_ENABLE_POWER_PROT EQU 1 ; 1=Enabled 0=Disabled
  187. DEFAULT_PGM_BRAKE_ON_STOP EQU 0 ; 1=Enabled 0=Disabled
  188. DEFAULT_PGM_LED_CONTROL EQU 0 ; Byte for LED control. 2bits per LED, 0=Off, 1=On
  189. ;**** **** **** **** ****
  190. ; Temporary register definitions
  191. Temp1 EQU R0
  192. Temp2 EQU R1
  193. Temp3 EQU R2
  194. Temp4 EQU R3
  195. Temp5 EQU R4
  196. Temp6 EQU R5
  197. Temp7 EQU R6
  198. Temp8 EQU R7
  199. ;**** **** **** **** ****
  200. ; Register definitions
  201. DSEG AT 20h ; Variables segment
  202. Bit_Access: DS 1 ; MUST BE AT THIS ADDRESS. Variable at bit accessible address (for non interrupt routines)
  203. Bit_Access_Int: DS 1 ; Variable at bit accessible address (for interrupts)
  204. Rcp_Outside_Range_Cnt: DS 1 ; RC pulse outside range counter (incrementing)
  205. Rcp_Timeout_Cntd: DS 1 ; RC pulse timeout counter (decrementing)
  206. Flags0: DS 1 ; State flags. Reset upon init_start
  207. T3_PENDING EQU 0 ; Timer3 pending flag
  208. DEMAG_DETECTED EQU 1 ; Set when excessive demag time is detected
  209. DEMAG_CUT_POWER EQU 2 ; Set when demag compensation cuts power
  210. COMP_TIMED_OUT EQU 3 ; Set when comparator reading timed out
  211. ; EQU 4
  212. ; EQU 5
  213. ; EQU 6
  214. ; EQU 7
  215. Flags1: DS 1 ; State flags. Reset upon init_start
  216. STARTUP_PHASE EQU 0 ; Set when in startup phase
  217. INITIAL_RUN_PHASE EQU 1 ; Set when in initial run phase, before synchronized run is achieved
  218. MOTOR_STARTED EQU 2 ; Set when motor is started
  219. DIR_CHANGE_BRAKE EQU 3 ; Set when braking before direction change
  220. HIGH_RPM EQU 4 ; Set when motor rpm is high (Comm_Period4x_H less than 2)
  221. ; EQU 5
  222. ; EQU 6
  223. ; EQU 7
  224. Flags2: DS 1 ; State flags. NOT reset upon init_start
  225. RCP_UPDATED EQU 0 ; New RC pulse length value available
  226. RCP_ONESHOT125 EQU 1 ; RC pulse input is OneShot125 (125-250us)
  227. RCP_ONESHOT42 EQU 2 ; RC pulse input is OneShot42 (41.67-83us)
  228. RCP_MULTISHOT EQU 3 ; RC pulse input is Multishot (5-25us)
  229. RCP_DIR_REV EQU 4 ; RC pulse direction in bidirectional mode
  230. RCP_FULL_RANGE EQU 5 ; When set full input signal range is used (1000-2000us) and stored calibration values are ignored
  231. ; EQU 6
  232. ; EQU 7
  233. Flags3: DS 1 ; State flags. NOT reset upon init_start
  234. PGM_DIR_REV EQU 0 ; Programmed direction. 0=normal, 1=reversed
  235. PGM_BIDIR_REV EQU 1 ; Programmed bidirectional direction. 0=normal, 1=reversed
  236. PGM_BIDIR EQU 2 ; Programmed bidirectional operation. 0=normal, 1=bidirectional
  237. ; EQU 3
  238. ; EQU 4
  239. ; EQU 5
  240. ; EQU 6
  241. ; EQU 7
  242. ;**** **** **** **** ****
  243. ; RAM definitions
  244. DSEG AT 30h ; Ram data segment, direct addressing
  245. Initial_Arm: DS 1 ; Variable that is set during the first arm sequence after power on
  246. Min_Throttle_L: DS 1 ; Minimum throttle scaled (lo byte)
  247. Min_Throttle_H: DS 1 ; Minimum throttle scaled (hi byte)
  248. Center_Throttle_L: DS 1 ; Center throttle scaled (lo byte)
  249. Center_Throttle_H: DS 1 ; Center throttle scaled (hi byte)
  250. Max_Throttle_L: DS 1 ; Maximum throttle scaled (lo byte)
  251. Max_Throttle_H: DS 1 ; Maximum throttle scaled (hi byte)
  252. Power_On_Wait_Cnt_L: DS 1 ; Power on wait counter (lo byte)
  253. Power_On_Wait_Cnt_H: DS 1 ; Power on wait counter (hi byte)
  254. Startup_Cnt: DS 1 ; Startup phase commutations counter (incrementing)
  255. Startup_Zc_Timeout_Cntd: DS 1 ; Startup zero cross timeout counter (decrementing)
  256. Initial_Run_Rot_Cntd: DS 1 ; Initial run rotations counter (decrementing)
  257. Stall_Cnt: DS 1 ; Counts start/run attempts that resulted in stall. Reset upon a proper stop
  258. Demag_Detected_Metric: DS 1 ; Metric used to gauge demag event frequency
  259. Demag_Pwr_Off_Thresh: DS 1 ; Metric threshold above which power is cut
  260. Low_Rpm_Pwr_Slope: DS 1 ; Sets the slope of power increase for low rpms
  261. Timer0_X: DS 1 ; Timer 0 extended byte
  262. Timer2_X: DS 1 ; Timer 2 extended byte
  263. Prev_Comm_L: DS 1 ; Previous commutation timer3 timestamp (lo byte)
  264. Prev_Comm_H: DS 1 ; Previous commutation timer3 timestamp (hi byte)
  265. Prev_Comm_X: DS 1 ; Previous commutation timer3 timestamp (ext byte)
  266. Prev_Prev_Comm_L: DS 1 ; Pre-previous commutation timer3 timestamp (lo byte)
  267. Prev_Prev_Comm_H: DS 1 ; Pre-previous commutation timer3 timestamp (hi byte)
  268. Comm_Period4x_L: DS 1 ; Timer3 counts between the last 4 commutations (lo byte)
  269. Comm_Period4x_H: DS 1 ; Timer3 counts between the last 4 commutations (hi byte)
  270. Comparator_Read_Cnt: DS 1 ; Number of comparator reads done
  271. Wt_Adv_Start_L: DS 1 ; Timer3 start point for commutation advance timing (lo byte)
  272. Wt_Adv_Start_H: DS 1 ; Timer3 start point for commutation advance timing (hi byte)
  273. Wt_Zc_Scan_Start_L: DS 1 ; Timer3 start point from commutation to zero cross scan (lo byte)
  274. Wt_Zc_Scan_Start_H: DS 1 ; Timer3 start point from commutation to zero cross scan (hi byte)
  275. Wt_Zc_Tout_Start_L: DS 1 ; Timer3 start point for zero cross scan timeout (lo byte)
  276. Wt_Zc_Tout_Start_H: DS 1 ; Timer3 start point for zero cross scan timeout (hi byte)
  277. Wt_Comm_Start_L: DS 1 ; Timer3 start point from zero cross to commutation (lo byte)
  278. Wt_Comm_Start_H: DS 1 ; Timer3 start point from zero cross to commutation (hi byte)
  279. New_Rcp: DS 1 ; New RC pulse value in pca counts
  280. Rcp_Stop_Cnt: DS 1 ; Counter for RC pulses below stop value
  281. Power_Pwm_Reg_L: DS 1 ; Power pwm register setting (lo byte)
  282. Power_Pwm_Reg_H: DS 1 ; Power pwm register setting (hi byte). 0x3F is minimum power
  283. Damp_Pwm_Reg_L: DS 1 ; Damping pwm register setting (lo byte)
  284. Damp_Pwm_Reg_H: DS 1 ; Damping pwm register setting (hi byte)
  285. Current_Power_Pwm_Reg_H: DS 1 ; Current power pwm register setting that is loaded in the PCA register (hi byte)
  286. Pwm_Limit: DS 1 ; Maximum allowed pwm
  287. Pwm_Limit_By_Rpm: DS 1 ; Maximum allowed pwm for low or high rpms
  288. Pwm_Limit_Beg: DS 1 ; Initial pwm limit
  289. Adc_Conversion_Cnt: DS 1 ; Adc conversion counter
  290. Current_Average_Temp: DS 1 ; Current average temperature (lo byte ADC reading, assuming hi byte is 1)
  291. Throttle_Gain: DS 1 ; Gain to be applied to RCP value
  292. Throttle_Gain_M: DS 1 ; Gain to be applied to RCP value (multiplier 0=1x, 1=2x, 2=4x etc))
  293. Throttle_Gain_BD_Rev: DS 1 ; Gain to be applied to RCP value for reverse direction in bidirectional mode
  294. 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)
  295. Beep_Strength: DS 1 ; Strength of beeps
  296. Skip_T2_Int: DS 1 ; Set for 48MHz MCUs when timer 2 interrupt shall be ignored
  297. Clock_Set_At_48MHz: DS 1 ; Variable set if 48MHz MCUs run at 48MHz
  298. Flash_Key_1: DS 1 ; Flash key one
  299. Flash_Key_2: DS 1 ; Flash key two
  300. Temp_Prot_Limit: DS 1 ; Temperature protection limit
  301. ; Indirect addressing data segment. The variables below must be in this sequence
  302. ISEG AT 080h
  303. _Pgm_Gov_P_Gain: DS 1 ; Programmed governor P gain
  304. _Pgm_Gov_I_Gain: DS 1 ; Programmed governor I gain
  305. _Pgm_Gov_Mode: DS 1 ; Programmed governor mode
  306. _Pgm_Low_Voltage_Lim: DS 1 ; Programmed low voltage limit
  307. _Pgm_Motor_Gain: DS 1 ; Programmed motor gain
  308. _Pgm_Motor_Idle: DS 1 ; Programmed motor idle speed
  309. Pgm_Startup_Pwr: DS 1 ; Programmed startup power
  310. _Pgm_Pwm_Freq: DS 1 ; Programmed pwm frequency
  311. Pgm_Direction: DS 1 ; Programmed rotation direction
  312. Pgm_Input_Pol: DS 1 ; Programmed input pwm polarity
  313. Initialized_L_Dummy: DS 1 ; Place holder
  314. Initialized_H_Dummy: DS 1 ; Place holder
  315. Pgm_Enable_TX_Program: DS 1 ; Programmed enable/disable value for TX programming
  316. _Pgm_Main_Rearm_Start: DS 1 ; Programmed enable/disable re-arming main every start
  317. _Pgm_Gov_Setup_Target: DS 1 ; Programmed main governor setup target
  318. _Pgm_Startup_Rpm: DS 1 ; Programmed startup rpm (unused - place holder)
  319. _Pgm_Startup_Accel: DS 1 ; Programmed startup acceleration (unused - place holder)
  320. _Pgm_Volt_Comp: DS 1 ; Place holder
  321. Pgm_Comm_Timing: DS 1 ; Programmed commutation timing
  322. _Pgm_Damping_Force: DS 1 ; Programmed damping force (unused - place holder)
  323. _Pgm_Gov_Range: DS 1 ; Programmed governor range
  324. _Pgm_Startup_Method: DS 1 ; Programmed startup method (unused - place holder)
  325. Pgm_Min_Throttle: DS 1 ; Programmed throttle minimum
  326. Pgm_Max_Throttle: DS 1 ; Programmed throttle maximum
  327. Pgm_Beep_Strength: DS 1 ; Programmed beep strength
  328. Pgm_Beacon_Strength: DS 1 ; Programmed beacon strength
  329. Pgm_Beacon_Delay: DS 1 ; Programmed beacon delay
  330. _Pgm_Throttle_Rate: DS 1 ; Programmed throttle rate (unused - place holder)
  331. Pgm_Demag_Comp: DS 1 ; Programmed demag compensation
  332. _Pgm_BEC_Voltage_High: DS 1 ; Programmed BEC voltage
  333. Pgm_Center_Throttle: DS 1 ; Programmed throttle center (in bidirectional mode)
  334. _Pgm_Main_Spoolup_Time: DS 1 ; Programmed main spoolup time
  335. Pgm_Enable_Temp_Prot: DS 1 ; Programmed temperature protection enable
  336. Pgm_Enable_Power_Prot: DS 1 ; Programmed low rpm power protection enable
  337. _Pgm_Enable_Pwm_Input: DS 1 ; Programmed PWM input signal enable
  338. _Pgm_Pwm_Dither: DS 1 ; Programmed output PWM dither
  339. Pgm_Brake_On_Stop: DS 1 ; Programmed braking when throttle is zero
  340. Pgm_LED_Control: DS 1 ; Programmed LED control
  341. ; The sequence of the variables below is no longer of importance
  342. Pgm_Startup_Pwr_Decoded: DS 1 ; Programmed startup power decoded
  343. ; Indirect addressing data segment
  344. ISEG AT 0D0h
  345. Tag_Temporary_Storage: DS 48 ; Temporary storage for tags when updating "Eeprom"
  346. ;**** **** **** **** ****
  347. CSEG AT 1A00h ; "Eeprom" segment
  348. EEPROM_FW_MAIN_REVISION EQU 16 ; Main revision of the firmware
  349. EEPROM_FW_SUB_REVISION EQU 3 ; Sub revision of the firmware
  350. EEPROM_LAYOUT_REVISION EQU 33 ; Revision of the EEPROM layout
  351. Eep_FW_Main_Revision: DB EEPROM_FW_MAIN_REVISION ; EEPROM firmware main revision number
  352. Eep_FW_Sub_Revision: DB EEPROM_FW_SUB_REVISION ; EEPROM firmware sub revision number
  353. Eep_Layout_Revision: DB EEPROM_LAYOUT_REVISION ; EEPROM layout revision number
  354. _Eep_Pgm_Gov_P_Gain: DB 0FFh
  355. _Eep_Pgm_Gov_I_Gain: DB 0FFh
  356. _Eep_Pgm_Gov_Mode: DB 0FFh
  357. _Eep_Pgm_Low_Voltage_Lim: DB 0FFh
  358. _Eep_Pgm_Motor_Gain: DB 0FFh
  359. _Eep_Pgm_Motor_Idle: DB 0FFh
  360. Eep_Pgm_Startup_Pwr: DB DEFAULT_PGM_STARTUP_PWR ; EEPROM copy of programmed startup power
  361. _Eep_Pgm_Pwm_Freq: DB 0FFh
  362. Eep_Pgm_Direction: DB DEFAULT_PGM_DIRECTION ; EEPROM copy of programmed rotation direction
  363. _Eep_Pgm_Input_Pol: DB 0FFh
  364. Eep_Initialized_L: DB 055h ; EEPROM initialized signature low byte
  365. Eep_Initialized_H: DB 0AAh ; EEPROM initialized signature high byte
  366. Eep_Enable_TX_Program: DB DEFAULT_PGM_ENABLE_TX_PROGRAM ; EEPROM TX programming enable
  367. _Eep_Main_Rearm_Start: DB 0FFh
  368. _Eep_Pgm_Gov_Setup_Target: DB 0FFh
  369. _Eep_Pgm_Startup_Rpm: DB 0FFh
  370. _Eep_Pgm_Startup_Accel: DB 0FFh
  371. _Eep_Pgm_Volt_Comp: DB 0FFh
  372. Eep_Pgm_Comm_Timing: DB DEFAULT_PGM_COMM_TIMING ; EEPROM copy of programmed commutation timing
  373. _Eep_Pgm_Damping_Force: DB 0FFh
  374. _Eep_Pgm_Gov_Range: DB 0FFh
  375. _Eep_Pgm_Startup_Method: DB 0FFh
  376. Eep_Pgm_Min_Throttle: DB DEFAULT_PGM_MIN_THROTTLE ; EEPROM copy of programmed minimum throttle
  377. Eep_Pgm_Max_Throttle: DB DEFAULT_PGM_MAX_THROTTLE ; EEPROM copy of programmed minimum throttle
  378. Eep_Pgm_Beep_Strength: DB DEFAULT_PGM_BEEP_STRENGTH ; EEPROM copy of programmed beep strength
  379. Eep_Pgm_Beacon_Strength: DB DEFAULT_PGM_BEACON_STRENGTH ; EEPROM copy of programmed beacon strength
  380. Eep_Pgm_Beacon_Delay: DB DEFAULT_PGM_BEACON_DELAY ; EEPROM copy of programmed beacon delay
  381. _Eep_Pgm_Throttle_Rate: DB 0FFh
  382. Eep_Pgm_Demag_Comp: DB DEFAULT_PGM_DEMAG_COMP ; EEPROM copy of programmed demag compensation
  383. _Eep_Pgm_BEC_Voltage_High: DB 0FFh
  384. Eep_Pgm_Center_Throttle: DB DEFAULT_PGM_CENTER_THROTTLE ; EEPROM copy of programmed center throttle
  385. _Eep_Pgm_Main_Spoolup_Time: DB 0FFh
  386. Eep_Pgm_Temp_Prot_Enable: DB DEFAULT_PGM_ENABLE_TEMP_PROT ; EEPROM copy of programmed temperature protection enable
  387. Eep_Pgm_Enable_Power_Prot: DB DEFAULT_PGM_ENABLE_POWER_PROT ; EEPROM copy of programmed low rpm power protection enable
  388. _Eep_Pgm_Enable_Pwm_Input: DB 0FFh
  389. _Eep_Pgm_Pwm_Dither: DB 0FFh
  390. Eep_Pgm_Brake_On_Stop: DB DEFAULT_PGM_BRAKE_ON_STOP ; EEPROM copy of programmed braking when throttle is zero
  391. Eep_Pgm_LED_Control: DB DEFAULT_PGM_LED_CONTROL ; EEPROM copy of programmed LED control
  392. Eep_Dummy: DB 0FFh ; EEPROM address for safety reason
  393. CSEG AT 1A60h
  394. Eep_Name: DB " " ; Name tag (16 Bytes)
  395. ;**** **** **** **** ****
  396. Interrupt_Table_Definition ; SiLabs interrupts
  397. CSEG AT 80h ; Code segment after interrupt vectors
  398. ;**** **** **** **** ****
  399. ; Table definitions
  400. STARTUP_POWER_TABLE: DB 04h, 06h, 08h, 0Ch, 10h, 18h, 20h, 30h, 40h, 60h, 80h, 0A0h, 0C0h
  401. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  402. ;
  403. ; Timer0 interrupt routine
  404. ;
  405. ; No assumptions
  406. ;
  407. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  408. IF MCU_48MHZ == 1
  409. t0_int:
  410. clr TCON_TF0 ; Clear interrupt flag
  411. inc Timer0_X
  412. reti
  413. ENDIF
  414. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  415. ;
  416. ; Timer2 interrupt routine
  417. ;
  418. ; No assumptions
  419. ; Requirements: Temp variables can NOT be used since PSW.x is not set
  420. ;
  421. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  422. t2_int: ; Happens every 32ms
  423. push PSW ; Preserve registers through interrupt
  424. push ACC
  425. clr TMR2CN0_TF2H ; Clear interrupt flag
  426. inc Timer2_X
  427. IF MCU_48MHZ == 1
  428. mov A, Clock_Set_At_48MHz
  429. jz t2_int_start
  430. ; Check skip variable
  431. mov A, Skip_T2_Int
  432. jz t2_int_start ; Execute this interrupt
  433. mov Skip_T2_Int, #0
  434. ajmp t2_int_exit
  435. t2_int_start:
  436. mov Skip_T2_Int, #1 ; Skip next interrupt
  437. ENDIF
  438. ; Update RC pulse timeout counter
  439. mov A, Rcp_Timeout_Cntd ; RC pulse timeout count zero?
  440. jz t2_int_exit ; Yes - do not decrement
  441. dec Rcp_Timeout_Cntd ; No decrement
  442. t2_int_exit:
  443. pop ACC ; Restore preserved registers
  444. pop PSW
  445. reti
  446. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  447. ;
  448. ; Timer3 interrupt routine
  449. ;
  450. ; No assumptions
  451. ; Requirements: Temp variables can NOT be used since PSW.x is not set
  452. ; ACC can not be used, as it is not pushed to stack
  453. ;
  454. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  455. t3_int: ; Used for commutation timing
  456. clr IE_EA ; Disable all interrupts
  457. anl EIE1, #7Fh ; Disable timer3 interrupts
  458. mov TMR3RLL, #0FAh ; Set a short delay before next interrupt
  459. mov TMR3RLH, #0FFh
  460. clr Flags0.T3_PENDING ; Flag that timer has wrapped
  461. anl TMR3CN0, #07Fh ; Timer3 interrupt flag cleared
  462. setb IE_EA ; Enable all interrupts
  463. reti
  464. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  465. ;
  466. ; Int0 interrupt routine
  467. ;
  468. ; No assumptions
  469. ;
  470. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  471. int0_int: ; Used for RC pulse timing
  472. clr IE_EA
  473. anl EIE1, #0EFh ; Disable pca interrupts
  474. push PSW ; Preserve registers through interrupt
  475. push ACC
  476. push B
  477. setb PSW.3 ; Select register bank 1 for this interrupt
  478. setb IE_EA
  479. ; Get the counter values
  480. Get_Rcp_Capture_Values
  481. ; Scale down to 10 bits (for 24MHz, and 11 bits for 48MHz)
  482. jnb Flags2.RCP_MULTISHOT, int0_int_fall_not_multishot
  483. ; Multishot - Multiply by 2 and add 1/16 and 1/32
  484. mov A, Temp1 ; Divide by 16
  485. swap A
  486. anl A, #0Fh
  487. mov Temp3, A
  488. mov A, Temp2
  489. swap A
  490. anl A, #0F0h
  491. orl A, Temp3
  492. mov Temp3, A
  493. clr C ; Make divided by 32
  494. rrc A
  495. add A, Temp3 ; Add 1/16 to 1/32
  496. mov Temp3, A
  497. clr C ; Multiply by 2
  498. mov A, Temp1
  499. rlc A
  500. mov Temp1, A
  501. mov A, Temp2
  502. rlc A
  503. mov Temp2, A
  504. mov A, Temp1 ; Add 1/16 and 1/32
  505. add A, Temp3
  506. mov Temp3, A
  507. mov A, Temp2
  508. IF MCU_48MHZ == 0
  509. addc A, #03h ; Add to low end, to make signal look like 20-40us
  510. ELSE
  511. addc A, #06h
  512. ENDIF
  513. mov Temp4, A
  514. ajmp int0_int_fall_gain_done
  515. int0_int_fall_not_multishot:
  516. jnb Flags2.RCP_ONESHOT42, int0_int_fall_not_oneshot_42
  517. ; Oneshot42 - Add 2/256
  518. clr C
  519. mov A, Temp1
  520. rlc A
  521. mov A, Temp2
  522. rlc A
  523. mov Temp3, A
  524. mov A, Temp1
  525. add A, Temp3
  526. mov Temp3, A
  527. mov A, Temp2
  528. addc A, #0
  529. mov Temp4, A
  530. ajmp int0_int_fall_gain_done
  531. int0_int_fall_not_oneshot_42:
  532. jnb Flags2.RCP_ONESHOT125, int0_int_fall_not_oneshot_125
  533. ; Oneshot125 - multiply by 86/256
  534. mov A, Temp1 ; Multiply by 86 and divide by 256
  535. mov B, #56h
  536. mul AB
  537. mov Temp3, B
  538. mov A, Temp2
  539. mov B, #56h
  540. mul AB
  541. add A, Temp3
  542. mov Temp3, A
  543. xch A, B
  544. addc A, #0
  545. mov Temp4, A
  546. ajmp int0_int_fall_gain_done
  547. int0_int_fall_not_oneshot_125:
  548. ; Regular signal - multiply by 43/1024
  549. IF MCU_48MHZ == 1
  550. clr C
  551. mov A, Temp3 ; Divide by 2
  552. rrc A
  553. mov Temp3, A
  554. mov A, Temp2
  555. rrc A
  556. mov Temp2, A
  557. mov A, Temp1
  558. rrc A
  559. mov Temp1, A
  560. ENDIF
  561. mov A, Temp1 ; Multiply by 43 and divide by 1024
  562. IF MCU_48MHZ == 0
  563. mov B, #2Bh
  564. ELSE
  565. mov B, #56h ; Multiply by 86
  566. ENDIF
  567. mul AB
  568. mov Temp3, B
  569. mov A, Temp2
  570. IF MCU_48MHZ == 0
  571. mov B, #2Bh
  572. ELSE
  573. mov B, #56h ; Multiply by 86
  574. ENDIF
  575. mul AB
  576. add A, Temp3
  577. mov Temp3, A
  578. xch A, B
  579. addc A, #0
  580. clr C
  581. rrc A ; Divide by 2 for total 512
  582. mov Temp4, A
  583. mov A, Temp3
  584. rrc A
  585. mov Temp3, A
  586. clr C
  587. mov A, Temp4 ; Divide by 2 for total 1024
  588. rrc A
  589. mov Temp4, A
  590. mov A, Temp3
  591. rrc A
  592. mov Temp3, A
  593. int0_int_fall_gain_done:
  594. ; Check if 2235us or above (in order to ignore false pulses)
  595. clr C
  596. mov A, Temp4 ; Is pulse 2235us or higher?
  597. IF MCU_48MHZ == 0
  598. subb A, #09h
  599. ELSE
  600. subb A, #12h
  601. ENDIF
  602. jnc int0_int_outside_range ; Yes - ignore pulse
  603. ; Check if below 745us (in order to ignore false pulses)
  604. clr C
  605. mov A, Temp4 ; Is pulse below 745us?
  606. IF MCU_48MHZ == 0
  607. subb A, #03h
  608. ELSE
  609. subb A, #06h
  610. ENDIF
  611. jnc int0_int_check_full_range ; No - proceed
  612. int0_int_outside_range:
  613. inc Rcp_Outside_Range_Cnt
  614. mov A, Rcp_Outside_Range_Cnt
  615. jnz ($+4)
  616. dec Rcp_Outside_Range_Cnt
  617. clr C
  618. mov A, Rcp_Outside_Range_Cnt
  619. subb A, #10 ; Allow a given number of outside pulses
  620. jnc ($+4)
  621. ajmp int0_int_set_timeout ; If outside limits - ignore pulse
  622. mov New_Rcp, #0 ; Set pulse length to zero
  623. mov Temp3, #0
  624. mov Temp4, #0
  625. ajmp int0_int_set_pwm_registers
  626. int0_int_check_full_range:
  627. ; Decrement outside range counter
  628. mov A, Rcp_Outside_Range_Cnt
  629. jz ($+4)
  630. dec Rcp_Outside_Range_Cnt
  631. ; Calculate "1000us" plus throttle minimum
  632. jnb Flags2.RCP_FULL_RANGE, int0_int_set_min ; Check if full range is chosen
  633. mov Temp5, #0 ; Set 1000us as default minimum
  634. IF MCU_48MHZ == 0
  635. mov Temp6, #4
  636. ELSE
  637. mov Temp6, #8
  638. ENDIF
  639. ajmp int0_int_calculate
  640. int0_int_set_min:
  641. mov Temp5, Min_Throttle_L ; Min throttle value scaled
  642. mov Temp6, Min_Throttle_H
  643. jnb Flags3.PGM_BIDIR, ($+7)
  644. mov Temp5, Center_Throttle_L ; Center throttle value scaled
  645. mov Temp6, Center_Throttle_H
  646. int0_int_calculate:
  647. clr C
  648. mov A, Temp3 ; Subtract minimum
  649. subb A, Temp5
  650. mov Temp3, A
  651. mov A, Temp4
  652. subb A, Temp6
  653. mov Temp4, A
  654. mov Bit_Access_Int.0, C
  655. mov Temp7, Throttle_Gain ; Load Temp7/Temp8 with throttle gain
  656. mov Temp8, Throttle_Gain_M
  657. jnb Flags3.PGM_BIDIR, int0_int_not_bidir ; If not bidirectional operation - branch
  658. jnc int0_int_bidir_fwd ; If result is positive - branch
  659. int0_int_bidir_rev:
  660. jb Flags2.RCP_DIR_REV, int0_int_bidir_rev_chk ; If same direction - branch
  661. setb Flags2.RCP_DIR_REV
  662. ajmp int0_int_bidir_rev_chk
  663. int0_int_bidir_fwd:
  664. jnb Flags2.RCP_DIR_REV, int0_int_bidir_rev_chk ; If same direction - branch
  665. clr Flags2.RCP_DIR_REV
  666. int0_int_bidir_rev_chk:
  667. jnb Flags2.RCP_DIR_REV, ($+7)
  668. mov Temp7, Throttle_Gain_BD_Rev ; Load Temp7/Temp8 with throttle gain for bidirectional reverse
  669. mov Temp8, Throttle_Gain_BD_Rev_M
  670. jb Flags3.PGM_BIDIR_REV, ($+5)
  671. cpl Flags2.RCP_DIR_REV
  672. clr C ; Multiply throttle value by 2
  673. mov A, Temp3
  674. rlc A
  675. mov Temp3, A
  676. mov A, Temp4
  677. rlc A
  678. mov Temp4, A
  679. mov C, Bit_Access_Int.0
  680. jnc int0_int_bidir_do_deadband ; If result is positive - branch
  681. mov A, Temp3 ; Change sign
  682. cpl A
  683. add A, #1
  684. mov Temp3, A
  685. mov A, Temp4
  686. cpl A
  687. addc A, #0
  688. mov Temp4, A
  689. int0_int_bidir_do_deadband:
  690. clr C ; Subtract deadband
  691. mov A, Temp3
  692. IF MCU_48MHZ == 0
  693. subb A, #40
  694. ELSE
  695. subb A, #80
  696. ENDIF
  697. mov Temp3, A
  698. mov A, Temp4
  699. subb A, #0
  700. mov Temp4, A
  701. jnc int0_int_do_throttle_gain
  702. mov Temp1, #0
  703. mov Temp3, #0
  704. mov Temp4, #0
  705. ajmp int0_int_do_throttle_gain
  706. int0_int_not_bidir:
  707. mov C, Bit_Access_Int.0
  708. jnc int0_int_do_throttle_gain ; If result is positive - branch
  709. int0_int_unidir_neg:
  710. mov Temp1, #0 ; Yes - set to minimum
  711. mov Temp3, #0
  712. mov Temp4, #0
  713. ajmp int0_int_pulse_ready
  714. int0_int_do_throttle_gain:
  715. ; Boost pwm during direct start
  716. mov A, Flags1
  717. anl A, #((1 SHL STARTUP_PHASE)+(1 SHL INITIAL_RUN_PHASE))
  718. jz int0_int_startup_boosted
  719. jb Flags1.MOTOR_STARTED, int0_int_startup_boosted ; Do not boost when changing direction in bidirectional mode
  720. mov A, Pwm_Limit_Beg ; Set 25% of max startup power as minimum power
  721. IF MCU_48MHZ == 1
  722. rlc A
  723. ENDIF
  724. mov Temp2, A
  725. mov A, Temp4
  726. jnz int0_int_startup_boost_stall
  727. clr C
  728. mov A, Temp2
  729. subb A, Temp3
  730. jc int0_int_startup_boost_stall
  731. mov A, Temp2
  732. mov Temp3, A
  733. int0_int_startup_boost_stall:
  734. mov A, Stall_Cnt ; Add an extra power boost during start
  735. swap A
  736. IF MCU_48MHZ == 1
  737. rlc A
  738. ENDIF
  739. add A, Temp3
  740. mov Temp3, A
  741. mov A, Temp4
  742. addc A, #0
  743. mov Temp4, A
  744. int0_int_startup_boosted:
  745. mov A, Temp3 ; Multiply throttle value by throttle gain
  746. mov B, Temp7 ; Temp7 has Throttle_Gain
  747. mul AB
  748. mov Temp2, A
  749. mov Temp3, B
  750. mov A, Temp4
  751. mov B, Temp7 ; Temp7 has Throttle_Gain
  752. mul AB
  753. add A, Temp3
  754. mov Temp3, A
  755. xch A, B
  756. addc A, #0
  757. mov Temp4, A
  758. clr C ; Generate 8bit number
  759. mov A, Temp4
  760. rrc A
  761. mov Temp6, A
  762. mov A, Temp3
  763. rrc A
  764. mov Temp1, A
  765. IF MCU_48MHZ == 1
  766. clr C
  767. mov A, Temp6
  768. rrc A
  769. mov Temp6, A
  770. mov A, Temp1
  771. rrc A
  772. mov Temp1, A
  773. ENDIF
  774. inc Temp8 ; Temp8 has Throttle_Gain_M
  775. int0_int_gain_loop:
  776. mov A, Temp8
  777. dec A
  778. jz int0_int_gain_rcp_done ; Skip one multiply by 2 of New_Rcp
  779. clr C
  780. mov A, Temp1 ; Multiply New_Rcp by 2
  781. rlc A
  782. mov Temp1, A
  783. int0_int_gain_rcp_done:
  784. clr C
  785. mov A, Temp2 ; Multiply pwm by 2
  786. rlc A
  787. mov A, Temp3
  788. rlc A
  789. mov Temp3, A
  790. mov A, Temp4
  791. rlc A
  792. mov Temp4, A
  793. djnz Temp8, int0_int_gain_loop
  794. mov A, Temp4
  795. IF MCU_48MHZ == 0
  796. jnb ACC.2, int0_int_pulse_ready ; Check that RC pulse is within legal range
  797. ELSE
  798. jnb ACC.3, int0_int_pulse_ready
  799. ENDIF
  800. mov Temp1, #0FFh
  801. mov Temp3, #0FFh
  802. IF MCU_48MHZ == 0
  803. mov Temp4, #3
  804. ELSE
  805. mov Temp4, #7
  806. ENDIF
  807. int0_int_pulse_ready:
  808. mov New_Rcp, Temp1 ; Store new pulse length
  809. setb Flags2.RCP_UPDATED ; Set updated flag
  810. ; Set pwm limit
  811. clr C
  812. mov A, Pwm_Limit ; Limit to the smallest
  813. mov Temp5, A ; Store limit in Temp5
  814. subb A, Pwm_Limit_By_Rpm
  815. jc ($+4)
  816. mov Temp5, Pwm_Limit_By_Rpm
  817. ; Check against limit
  818. clr C
  819. mov A, Temp5
  820. subb A, New_Rcp
  821. jnc int0_int_set_pwm_registers
  822. mov A, Temp5 ; Multiply limit by 4 (8 for 48MHz MCUs)
  823. IF MCU_48MHZ == 0
  824. mov B, #4
  825. ELSE
  826. mov B, #8
  827. ENDIF
  828. mul AB
  829. mov Temp3, A
  830. mov Temp4, B
  831. int0_int_set_pwm_registers:
  832. mov A, Temp3
  833. cpl A
  834. mov Temp1, A
  835. mov A, Temp4
  836. cpl A
  837. IF MCU_48MHZ == 0
  838. anl A, #3
  839. ELSE
  840. anl A, #7
  841. ENDIF
  842. mov Temp2, A
  843. IF FETON_DELAY != 0
  844. clr C
  845. mov A, Temp1 ; Skew damping fet timing
  846. IF MCU_48MHZ == 0
  847. subb A, #FETON_DELAY
  848. ELSE
  849. subb A, #(FETON_DELAY SHL 1)
  850. ENDIF
  851. mov Temp3, A
  852. mov A, Temp2
  853. subb A, #0
  854. mov Temp4, A
  855. jnc int0_int_set_pwm_damp_set
  856. mov Temp3, #0
  857. mov Temp4, #0
  858. int0_int_set_pwm_damp_set:
  859. ENDIF
  860. mov Power_Pwm_Reg_L, Temp1
  861. mov Power_Pwm_Reg_H, Temp2
  862. IF FETON_DELAY != 0
  863. mov Damp_Pwm_Reg_L, Temp3
  864. mov Damp_Pwm_Reg_H, Temp4
  865. ENDIF
  866. mov Rcp_Timeout_Cntd, #10 ; Set timeout count
  867. IF FETON_DELAY != 0
  868. pop B ; Restore preserved registers
  869. pop ACC
  870. pop PSW
  871. Clear_COVF_Interrupt
  872. Enable_COVF_Interrupt ; Generate a pca interrupt
  873. orl EIE1, #10h ; Enable pca interrupts
  874. reti
  875. ELSE
  876. mov A, Current_Power_Pwm_Reg_H
  877. IF MCU_48MHZ == 0
  878. jnb ACC.1, int0_int_set_pca_int_hi_pwm
  879. ELSE
  880. jnb ACC.2, int0_int_set_pca_int_hi_pwm
  881. ENDIF
  882. pop B ; Restore preserved registers
  883. pop ACC
  884. pop PSW
  885. Clear_COVF_Interrupt
  886. Enable_COVF_Interrupt ; Generate a pca interrupt
  887. orl EIE1, #10h ; Enable pca interrupts
  888. reti
  889. int0_int_set_pca_int_hi_pwm:
  890. pop B ; Restore preserved registers
  891. pop ACC
  892. pop PSW
  893. Clear_CCF_Interrupt
  894. Enable_CCF_Interrupt ; Generate pca interrupt
  895. orl EIE1, #10h ; Enable pca interrupts
  896. reti
  897. ENDIF
  898. int0_int_set_timeout:
  899. mov Rcp_Timeout_Cntd, #10 ; Set timeout count
  900. pop B ; Restore preserved registers
  901. pop ACC
  902. pop PSW
  903. orl EIE1, #10h ; Enable pca interrupts
  904. reti
  905. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  906. ;
  907. ; PCA interrupt routine
  908. ;
  909. ; No assumptions
  910. ;
  911. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  912. pca_int: ; Used for setting pwm registers
  913. clr IE_EA
  914. anl IE, #0FEh ; Disable int0 interrupts
  915. push PSW ; Preserve registers through interrupt
  916. push ACC
  917. setb PSW.3 ; Select register bank 1 for this interrupt
  918. IF FETON_DELAY != 0 ; HI/LO enable style drivers
  919. mov Temp1, PCA0L ; Read low byte, to transfer high byte to holding register
  920. mov A, Current_Power_Pwm_Reg_H
  921. IF MCU_48MHZ == 0
  922. jnb ACC.1, pca_int_hi_pwm
  923. ELSE
  924. jnb ACC.2, pca_int_hi_pwm
  925. ENDIF
  926. mov A, PCA0H
  927. IF MCU_48MHZ == 0
  928. jb ACC.1, pca_int_exit ; Power below 50%, update pca in the 0x00-0x0F range
  929. jb ACC.0, pca_int_exit
  930. ELSE
  931. jb ACC.2, pca_int_exit
  932. jb ACC.1, pca_int_exit
  933. ENDIF
  934. ajmp pca_int_set_pwm
  935. pca_int_hi_pwm:
  936. mov A, PCA0H
  937. IF MCU_48MHZ == 0
  938. jnb ACC.1, pca_int_exit ; Power above 50%, update pca in the 0x20-0x2F range
  939. jb ACC.0, pca_int_exit
  940. ELSE
  941. jnb ACC.2, pca_int_exit
  942. jb ACC.1, pca_int_exit
  943. ENDIF
  944. pca_int_set_pwm:
  945. Set_Power_Pwm_Regs
  946. Set_Damp_Pwm_Regs
  947. mov Current_Power_Pwm_Reg_H, Power_Pwm_Reg_H
  948. Disable_COVF_Interrupt
  949. ELSE ; EN/PWM style drivers
  950. Set_Power_Pwm_Regs
  951. mov Current_Power_Pwm_Reg_H, Power_Pwm_Reg_H
  952. Disable_COVF_Interrupt
  953. Disable_CCF_Interrupt
  954. ENDIF
  955. ; Check RC pulse against stop value
  956. clr C
  957. mov A, New_Rcp ; Load new pulse value
  958. subb A, #1 ; Check if pulse is below stop value
  959. jc pca_int_rcp_stop
  960. ; RC pulse higher than stop value, reset stop counter
  961. mov Rcp_Stop_Cnt, #0 ; Reset rcp stop counter
  962. ajmp pca_int_exit
  963. pca_int_rcp_stop:
  964. ; RC pulse less than stop value
  965. mov A, Rcp_Stop_Cnt ; Increment stop counter
  966. add A, #1
  967. mov Rcp_Stop_Cnt, A
  968. jnc ($+5) ; Branch if counter has not wrapped
  969. mov Rcp_Stop_Cnt, #0FFh ; Set stop counter to max
  970. pca_int_exit:
  971. Clear_COVF_Interrupt
  972. IF FETON_DELAY == 0
  973. Clear_CCF_Interrupt
  974. ENDIF
  975. pop ACC ; Restore preserved registers
  976. pop PSW
  977. orl IE, #01h ; Enable int0 interrupts
  978. setb IE_EA
  979. reti
  980. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  981. ;
  982. ; Wait xms ~(x*4*250) (Different entry points)
  983. ;
  984. ; No assumptions
  985. ;
  986. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  987. wait1ms:
  988. mov Temp2, #1
  989. jmp waitxms_o
  990. wait3ms:
  991. mov Temp2, #3
  992. jmp waitxms_o
  993. wait10ms:
  994. mov Temp2, #10
  995. jmp waitxms_o
  996. wait30ms:
  997. mov Temp2, #30
  998. jmp waitxms_o
  999. wait100ms:
  1000. mov Temp2, #100
  1001. jmp waitxms_o
  1002. wait200ms:
  1003. mov Temp2, #200
  1004. jmp waitxms_o
  1005. waitxms_o: ; Outer loop
  1006. mov Temp1, #23
  1007. waitxms_m: ; Middle loop
  1008. clr A
  1009. djnz ACC, $ ; Inner loop (42.7us - 1024 cycles)
  1010. djnz Temp1, waitxms_m
  1011. djnz Temp2, waitxms_o
  1012. ret
  1013. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1014. ;
  1015. ; Beeper routines (4 different entry points)
  1016. ;
  1017. ; No assumptions
  1018. ;
  1019. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1020. beep_f1: ; Entry point 1, load beeper frequency 1 settings
  1021. mov Temp3, #20 ; Off wait loop length
  1022. mov Temp4, #120 ; Number of beep pulses
  1023. jmp beep
  1024. beep_f2: ; Entry point 2, load beeper frequency 2 settings
  1025. mov Temp3, #16
  1026. mov Temp4, #140
  1027. jmp beep
  1028. beep_f3: ; Entry point 3, load beeper frequency 3 settings
  1029. mov Temp3, #13
  1030. mov Temp4, #180
  1031. jmp beep
  1032. beep_f4: ; Entry point 4, load beeper frequency 4 settings
  1033. mov Temp3, #11
  1034. mov Temp4, #200
  1035. jmp beep
  1036. beep: ; Beep loop start
  1037. mov A, Beep_Strength
  1038. djnz ACC, beep_start
  1039. ret
  1040. beep_start:
  1041. mov Temp2, #2
  1042. beep_onoff:
  1043. clr A
  1044. BcomFET_off ; BcomFET off
  1045. djnz ACC, $ ; Allow some time after comfet is turned off
  1046. BpwmFET_on ; BpwmFET on (in order to charge the driver of the BcomFET)
  1047. djnz ACC, $ ; Let the pwmfet be turned on a while
  1048. BpwmFET_off ; BpwmFET off again
  1049. djnz ACC, $ ; Allow some time after pwmfet is turned off
  1050. BcomFET_on ; BcomFET on
  1051. djnz ACC, $ ; Allow some time after comfet is turned on
  1052. ; Turn on pwmfet
  1053. mov A, Temp2
  1054. jb ACC.0, beep_apwmfet_on
  1055. ApwmFET_on ; ApwmFET on
  1056. beep_apwmfet_on:
  1057. jnb ACC.0, beep_cpwmfet_on
  1058. CpwmFET_on ; CpwmFET on
  1059. beep_cpwmfet_on:
  1060. mov A, Beep_Strength
  1061. djnz ACC, $
  1062. ; Turn off pwmfet
  1063. mov A, Temp2
  1064. jb ACC.0, beep_apwmfet_off
  1065. ApwmFET_off ; ApwmFET off
  1066. beep_apwmfet_off:
  1067. jnb ACC.0, beep_cpwmfet_off
  1068. CpwmFET_off ; CpwmFET off
  1069. beep_cpwmfet_off:
  1070. mov A, #150 ; 25�s off
  1071. djnz ACC, $
  1072. djnz Temp2, beep_onoff
  1073. ; Copy variable
  1074. mov A, Temp3
  1075. mov Temp1, A
  1076. beep_off: ; Fets off loop
  1077. djnz ACC, $
  1078. djnz Temp1, beep_off
  1079. djnz Temp4, beep
  1080. BcomFET_off ; BcomFET off
  1081. ret
  1082. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1083. ;
  1084. ; Set pwm limit low rpm
  1085. ;
  1086. ; No assumptions
  1087. ;
  1088. ; Sets power limit for low rpms and disables demag for low rpms
  1089. ;
  1090. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1091. set_pwm_limit_low_rpm:
  1092. ; Set pwm limit and demag disable for low rpms
  1093. mov Temp1, #0FFh ; Default full power
  1094. jb Flags1.STARTUP_PHASE, set_pwm_limit_low_rpm_exit ; Exit if startup phase set
  1095. mov Temp2, #Pgm_Enable_Power_Prot ; Check if low RPM power protection is enabled
  1096. mov A, @Temp2
  1097. jz set_pwm_limit_low_rpm_exit ; Exit if disabled
  1098. mov A, Comm_Period4x_H
  1099. jz set_pwm_limit_low_rpm_exit ; Avoid divide by zero
  1100. mov A, #255 ; Divide 255 by Comm_Period4x_H
  1101. mov B, Comm_Period4x_H
  1102. div AB
  1103. mov B, Low_Rpm_Pwr_Slope ; Multiply by slope
  1104. jnb Flags1.INITIAL_RUN_PHASE, ($+6) ; More protection for initial run phase
  1105. mov B, #5
  1106. mul AB
  1107. mov Temp1, A ; Set new limit
  1108. xch A, B
  1109. jz ($+4) ; Limit to max
  1110. mov Temp1, #0FFh
  1111. clr C
  1112. mov A, Temp1 ; Limit to min
  1113. subb A, Pwm_Limit_Beg
  1114. jnc set_pwm_limit_low_rpm_exit
  1115. mov Temp1, Pwm_Limit_Beg
  1116. set_pwm_limit_low_rpm_exit:
  1117. mov Pwm_Limit_By_Rpm, Temp1
  1118. ret
  1119. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1120. ;
  1121. ; Set pwm limit high rpm
  1122. ;
  1123. ; No assumptions
  1124. ;
  1125. ; Sets power limit for high rpms
  1126. ;
  1127. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1128. set_pwm_limit_high_rpm:
  1129. IF MCU_48MHZ == 1
  1130. clr C
  1131. mov A, Comm_Period4x_L
  1132. subb A, #0A0h ; Limit Comm_Period to 160, which is 500k erpm
  1133. mov A, Comm_Period4x_H
  1134. subb A, #00h
  1135. ELSE
  1136. clr C
  1137. mov A, Comm_Period4x_L
  1138. subb A, #0E4h ; Limit Comm_Period to 228, which is 350k erpm
  1139. mov A, Comm_Period4x_H
  1140. subb A, #00h
  1141. ENDIF
  1142. mov A, Pwm_Limit_By_Rpm
  1143. jnc set_pwm_limit_high_rpm_inc_limit
  1144. dec A
  1145. ajmp set_pwm_limit_high_rpm_store
  1146. set_pwm_limit_high_rpm_inc_limit:
  1147. inc A
  1148. set_pwm_limit_high_rpm_store:
  1149. jz ($+4)
  1150. mov Pwm_Limit_By_Rpm, A
  1151. ret
  1152. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1153. ;
  1154. ; Start ADC conversion
  1155. ;
  1156. ; No assumptions
  1157. ;
  1158. ; Start conversion used for measuring power supply voltage
  1159. ;
  1160. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1161. start_adc_conversion:
  1162. ; Start adc
  1163. Start_Adc
  1164. ret
  1165. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1166. ;
  1167. ; Check temperature, power supply voltage and limit power
  1168. ;
  1169. ; No assumptions
  1170. ;
  1171. ; Used to limit main motor power in order to maintain the required voltage
  1172. ;
  1173. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1174. check_temp_voltage_and_limit_power:
  1175. inc Adc_Conversion_Cnt ; Increment conversion counter
  1176. clr C
  1177. mov A, Adc_Conversion_Cnt ; Is conversion count equal to temp rate?
  1178. subb A, #8
  1179. jc check_voltage_start ; No - check voltage
  1180. ; Wait for ADC conversion to complete
  1181. jnb ADC0CN0_ADINT, check_temp_voltage_and_limit_power
  1182. ; Read ADC result
  1183. Read_Adc_Result
  1184. ; Stop ADC
  1185. Stop_Adc
  1186. mov Adc_Conversion_Cnt, #0 ; Yes - temperature check. Reset counter
  1187. mov A, Temp2 ; Move ADC MSB to Temp3
  1188. mov Temp3, A
  1189. mov Temp2, #Pgm_Enable_Temp_Prot ; Is temp protection enabled?
  1190. mov A, @Temp2
  1191. jz temp_check_exit ; No - branch
  1192. mov A, Temp3 ; Is temperature reading below 256?
  1193. jnz temp_average_inc_dec ; No - proceed
  1194. mov A, Current_Average_Temp ; Yes - decrement average
  1195. jz temp_average_updated ; Already zero - no change
  1196. jmp temp_average_dec ; Decrement
  1197. temp_average_inc_dec:
  1198. clr C
  1199. mov A, Temp1 ; Check if current temperature is above or below average
  1200. subb A, Current_Average_Temp
  1201. jz temp_average_updated_load_acc ; Equal - no change
  1202. mov A, Current_Average_Temp ; Above - increment average
  1203. jnc temp_average_inc
  1204. jz temp_average_updated ; Below - decrement average if average is not already zero
  1205. temp_average_dec:
  1206. dec A ; Decrement average
  1207. jmp temp_average_updated
  1208. temp_average_inc:
  1209. inc A ; Increment average
  1210. jz temp_average_dec
  1211. jmp temp_average_updated
  1212. temp_average_updated_load_acc:
  1213. mov A, Current_Average_Temp
  1214. temp_average_updated:
  1215. mov Current_Average_Temp, A
  1216. clr C
  1217. subb A, Temp_Prot_Limit ; Is temperature below first limit?
  1218. jc temp_check_exit ; Yes - exit
  1219. mov Pwm_Limit, #192 ; No - limit pwm
  1220. clr C
  1221. subb A, #(TEMP_LIMIT_STEP/2) ; Is temperature below second limit
  1222. jc temp_check_exit ; Yes - exit
  1223. mov Pwm_Limit, #128 ; No - limit pwm
  1224. clr C
  1225. subb A, #(TEMP_LIMIT_STEP/2) ; Is temperature below third limit
  1226. jc temp_check_exit ; Yes - exit
  1227. mov Pwm_Limit, #64 ; No - limit pwm
  1228. clr C
  1229. subb A, #(TEMP_LIMIT_STEP/2) ; Is temperature below final limit
  1230. jc temp_check_exit ; Yes - exit
  1231. mov Pwm_Limit, #0 ; No - limit pwm
  1232. temp_check_exit:
  1233. ret
  1234. check_voltage_start:
  1235. ; Increase pwm limit
  1236. mov A, Pwm_Limit
  1237. add A, #16
  1238. jnc ($+4) ; If not max - branch
  1239. mov A, #255
  1240. mov Pwm_Limit, A ; Increment limit
  1241. ret
  1242. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1243. ;
  1244. ; Set startup PWM routine
  1245. ;
  1246. ; Either the SETTLE_PHASE or the STEPPER_PHASE flag must be set
  1247. ;
  1248. ; Used for pwm control during startup
  1249. ;
  1250. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1251. set_startup_pwm:
  1252. ; Adjust startup power
  1253. mov A, #50 ; Set power
  1254. mov Temp2, #Pgm_Startup_Pwr_Decoded
  1255. mov B, @Temp2
  1256. mul AB
  1257. xch A, B
  1258. mov C, B.7 ; Multiply result by 2 (unity gain is 128)
  1259. rlc A
  1260. mov Temp1, A ; Transfer to Temp1
  1261. clr C
  1262. mov A, Temp1 ; Check against limit
  1263. subb A, Pwm_Limit
  1264. jc startup_pwm_set_pwm ; If pwm below limit - branch
  1265. mov Temp1, Pwm_Limit ; Limit pwm
  1266. startup_pwm_set_pwm:
  1267. mov Pwm_Limit_Beg, Temp1 ; Set initial pwm limit
  1268. ret
  1269. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1270. ;
  1271. ; Initialize timing routine
  1272. ;
  1273. ; No assumptions
  1274. ;
  1275. ; Part of initialization before motor start
  1276. ;
  1277. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1278. initialize_timing:
  1279. mov Comm_Period4x_L, #00h ; Set commutation period registers
  1280. mov Comm_Period4x_H, #0F0h
  1281. ret
  1282. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1283. ;
  1284. ; Calculate next commutation timing routine
  1285. ;
  1286. ; No assumptions
  1287. ;
  1288. ; Called immediately after each commutation
  1289. ; Also sets up timer 3 to wait advance timing
  1290. ; Two entry points are used
  1291. ;
  1292. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1293. calc_next_comm_timing: ; Entry point for run phase
  1294. ; Read commutation time
  1295. clr IE_EA
  1296. mov TMR2CN0, #00h ; Timer2 disabled
  1297. mov Temp1, TMR2L ; Load timer value
  1298. mov Temp2, TMR2H
  1299. mov Temp3, Timer2_X
  1300. jnb TMR2CN0_TF2H, ($+4) ; Check if interrupt is pending
  1301. inc Temp3 ; If it is pending, then timer has already wrapped
  1302. mov TMR2CN0, #04h ; Timer2 enabled
  1303. setb IE_EA
  1304. IF MCU_48MHZ == 1
  1305. clr C
  1306. mov A, Temp3
  1307. rrc A
  1308. mov Temp3, A
  1309. mov A, Temp2
  1310. rrc A
  1311. mov Temp2, A
  1312. mov A, Temp1
  1313. rrc A
  1314. mov Temp1, A
  1315. ENDIF
  1316. ; Calculate this commutation time
  1317. mov Temp4, Prev_Comm_L
  1318. mov Temp5, Prev_Comm_H
  1319. mov Prev_Comm_L, Temp1 ; Store timestamp as previous commutation
  1320. mov Prev_Comm_H, Temp2
  1321. clr C
  1322. mov A, Temp1
  1323. subb A, Temp4 ; Calculate the new commutation time
  1324. mov Temp1, A
  1325. mov A, Temp2
  1326. subb A, Temp5
  1327. jb Flags1.STARTUP_PHASE, calc_next_comm_startup
  1328. IF MCU_48MHZ == 1
  1329. anl A, #7Fh
  1330. ENDIF
  1331. mov Temp2, A
  1332. jnb Flags1.HIGH_RPM, ($+5) ; Branch if high rpm
  1333. ajmp calc_next_comm_timing_fast
  1334. ajmp calc_next_comm_normal
  1335. calc_next_comm_startup:
  1336. mov Temp6, Prev_Comm_X
  1337. mov Prev_Comm_X, Temp3 ; Store extended timestamp as previous commutation
  1338. mov Temp2, A
  1339. mov A, Temp3
  1340. subb A, Temp6 ; Calculate the new extended commutation time
  1341. IF MCU_48MHZ == 1
  1342. anl A, #7Fh
  1343. ENDIF
  1344. mov Temp3, A
  1345. jz ($+6)
  1346. mov Temp1, #0FFh
  1347. mov Temp2, #0FFh
  1348. mov Temp7, Prev_Prev_Comm_L
  1349. mov Temp8, Prev_Prev_Comm_H
  1350. mov Prev_Prev_Comm_L, Temp4
  1351. mov Prev_Prev_Comm_H, Temp5
  1352. mov Temp1, Prev_Comm_L ; Reload this commutation time
  1353. mov Temp2, Prev_Comm_H
  1354. clr C
  1355. mov A, Temp1
  1356. subb A, Temp7 ; Calculate the new commutation time based upon the two last commutations (to reduce sensitivity to offset)
  1357. mov Temp1, A
  1358. mov A, Temp2
  1359. subb A, Temp8
  1360. mov Temp2, A
  1361. clr C
  1362. mov A, Comm_Period4x_H ; Average with previous and save
  1363. rrc A
  1364. mov Temp4, A
  1365. mov A, Comm_Period4x_L
  1366. rrc A
  1367. mov Temp3, A
  1368. mov A, Temp1
  1369. add A, Temp3
  1370. mov Comm_Period4x_L, A
  1371. mov A, Temp2
  1372. addc A, Temp4
  1373. mov Comm_Period4x_H, A
  1374. jnc ($+8)
  1375. mov Comm_Period4x_L, #0FFh
  1376. mov Comm_Period4x_H, #0FFh
  1377. ajmp calc_new_wait_times_setup
  1378. calc_next_comm_normal:
  1379. ; Calculate new commutation time
  1380. mov Temp3, Comm_Period4x_L ; Comm_Period4x(-l-h) holds the time of 4 commutations
  1381. mov Temp4, Comm_Period4x_H
  1382. mov Temp5, Comm_Period4x_L ; Copy variables
  1383. mov Temp6, Comm_Period4x_H
  1384. mov Temp7, #4 ; Divide Comm_Period4x 4 times as default
  1385. mov Temp8, #2 ; Divide new commutation time 2 times as default
  1386. clr C
  1387. mov A, Temp4
  1388. subb A, #04h
  1389. jc calc_next_comm_avg_period_div
  1390. dec Temp7 ; Reduce averaging time constant for low speeds
  1391. dec Temp8
  1392. clr C
  1393. mov A, Temp4
  1394. subb A, #08h
  1395. jc calc_next_comm_avg_period_div
  1396. jb Flags1.INITIAL_RUN_PHASE, calc_next_comm_avg_period_div ; Do not average very fast during initial run
  1397. dec Temp7 ; Reduce averaging time constant more for even lower speeds
  1398. dec Temp8
  1399. calc_next_comm_avg_period_div:
  1400. clr C
  1401. mov A, Temp6
  1402. rrc A ; Divide by 2
  1403. mov Temp6, A
  1404. mov A, Temp5
  1405. rrc A
  1406. mov Temp5, A
  1407. djnz Temp7, calc_next_comm_avg_period_div
  1408. clr C
  1409. mov A, Temp3
  1410. subb A, Temp5 ; Subtract a fraction
  1411. mov Temp3, A
  1412. mov A, Temp4
  1413. subb A, Temp6
  1414. mov Temp4, A
  1415. mov A, Temp8 ; Divide new time
  1416. jz calc_next_comm_new_period_div_done
  1417. calc_next_comm_new_period_div:
  1418. clr C
  1419. mov A, Temp2
  1420. rrc A ; Divide by 2
  1421. mov Temp2, A
  1422. mov A, Temp1
  1423. rrc A
  1424. mov Temp1, A
  1425. djnz Temp8, calc_next_comm_new_period_div
  1426. calc_next_comm_new_period_div_done:
  1427. mov A, Temp3
  1428. add A, Temp1 ; Add the divided new time
  1429. mov Temp3, A
  1430. mov A, Temp4
  1431. addc A, Temp2
  1432. mov Temp4, A
  1433. mov Comm_Period4x_L, Temp3 ; Store Comm_Period4x_X
  1434. mov Comm_Period4x_H, Temp4
  1435. jnc calc_new_wait_times_setup; If period larger than 0xffff - go to slow case
  1436. mov Temp4, #0FFh
  1437. mov Comm_Period4x_L, Temp4 ; Set commutation period registers to very slow timing (0xffff)
  1438. mov Comm_Period4x_H, Temp4
  1439. calc_new_wait_times_setup:
  1440. ; Set high rpm bit (if above 156k erpm)
  1441. clr C
  1442. mov A, Temp4
  1443. subb A, #2
  1444. jnc ($+4)
  1445. setb Flags1.HIGH_RPM ; Set high rpm bit
  1446. ; Load programmed commutation timing
  1447. jnb Flags1.STARTUP_PHASE, calc_new_wait_per_startup_done ; Set dedicated timing during startup
  1448. mov Temp8, #3
  1449. ajmp calc_new_wait_per_demag_done
  1450. calc_new_wait_per_startup_done:
  1451. mov Temp1, #Pgm_Comm_Timing ; Load timing setting
  1452. mov A, @Temp1
  1453. mov Temp8, A ; Store in Temp8
  1454. clr C
  1455. mov A, Demag_Detected_Metric ; Check demag metric
  1456. subb A, #130
  1457. jc calc_new_wait_per_demag_done
  1458. inc Temp8 ; Increase timing
  1459. clr C
  1460. mov A, Demag_Detected_Metric
  1461. subb A, #160
  1462. jc ($+3)
  1463. inc Temp8 ; Increase timing again
  1464. clr C
  1465. mov A, Temp8 ; Limit timing to max
  1466. subb A, #6
  1467. jc ($+4)
  1468. mov Temp8, #5 ; Set timing to max
  1469. calc_new_wait_per_demag_done:
  1470. ; Set timing reduction
  1471. mov Temp7, #2
  1472. ; Load current commutation timing
  1473. mov A, Comm_Period4x_H ; Divide 4 times
  1474. swap A
  1475. anl A, #00Fh
  1476. mov Temp2, A
  1477. mov A, Comm_Period4x_H
  1478. swap A
  1479. anl A, #0F0h
  1480. mov Temp1, A
  1481. mov A, Comm_Period4x_L
  1482. swap A
  1483. anl A, #00Fh
  1484. add A, Temp1
  1485. mov Temp1, A
  1486. clr C
  1487. mov A, Temp1
  1488. subb A, Temp7
  1489. mov Temp3, A
  1490. mov A, Temp2
  1491. subb A, #0
  1492. mov Temp4, A
  1493. jc load_min_time ; Check that result is still positive
  1494. clr C
  1495. mov A, Temp3
  1496. subb A, #1
  1497. mov A, Temp4
  1498. subb A, #0
  1499. jnc calc_new_wait_times_exit ; Check that result is still above minumum
  1500. load_min_time:
  1501. mov Temp3, #1
  1502. clr A
  1503. mov Temp4, A
  1504. calc_new_wait_times_exit:
  1505. ajmp wait_advance_timing
  1506. ; Fast calculation (Comm_Period4x_H less than 2)
  1507. calc_next_comm_timing_fast:
  1508. ; Calculate new commutation time
  1509. mov Temp3, Comm_Period4x_L ; Comm_Period4x(-l-h) holds the time of 4 commutations
  1510. mov Temp4, Comm_Period4x_H
  1511. mov A, Temp4 ; Divide by 2 4 times
  1512. swap A
  1513. mov Temp7, A
  1514. mov A, Temp3
  1515. swap A
  1516. anl A, #0Fh
  1517. orl A, Temp7
  1518. mov Temp5, A
  1519. clr C
  1520. mov A, Temp3 ; Subtract a fraction
  1521. subb A, Temp5
  1522. mov Temp3, A
  1523. mov A, Temp4
  1524. subb A, #0
  1525. mov Temp4, A
  1526. clr C
  1527. mov A, Temp1
  1528. rrc A ; Divide by 2 2 times
  1529. clr C
  1530. rrc A
  1531. mov Temp1, A
  1532. mov A, Temp3 ; Add the divided new time
  1533. add A, Temp1
  1534. mov Temp3, A
  1535. mov A, Temp4
  1536. addc A, #0
  1537. mov Temp4, A
  1538. mov Comm_Period4x_L, Temp3 ; Store Comm_Period4x_X
  1539. mov Comm_Period4x_H, Temp4
  1540. clr C
  1541. mov A, Temp4 ; If erpm below 156k - go to normal case
  1542. subb A, #2
  1543. jc ($+4)
  1544. clr Flags1.HIGH_RPM ; Clear high rpm bit
  1545. ; Set timing reduction
  1546. mov Temp1, #2
  1547. mov A, Temp4 ; Divide by 2 4 times
  1548. swap A
  1549. mov Temp7, A
  1550. mov Temp4, #0
  1551. mov A, Temp3
  1552. swap A
  1553. anl A, #0Fh
  1554. orl A, Temp7
  1555. mov Temp3, A
  1556. clr C
  1557. mov A, Temp3
  1558. subb A, Temp1
  1559. mov Temp3, A
  1560. jc load_min_time_fast ; Check that result is still positive
  1561. clr C
  1562. subb A, #1
  1563. jnc calc_new_wait_times_fast_done ; Check that result is still above minumum
  1564. load_min_time_fast:
  1565. mov Temp3, #1
  1566. calc_new_wait_times_fast_done:
  1567. mov Temp1, #Pgm_Comm_Timing ; Load timing setting
  1568. mov A, @Temp1
  1569. mov Temp8, A ; Store in Temp8
  1570. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1571. ;
  1572. ; Wait advance timing routine
  1573. ;
  1574. ; No assumptions
  1575. ; NOTE: Be VERY careful if using temp registers. They are passed over this routine
  1576. ;
  1577. ; Waits for the advance timing to elapse and sets up the next zero cross wait
  1578. ;
  1579. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1580. wait_advance_timing:
  1581. jnb Flags0.T3_PENDING, ($+5)
  1582. ajmp wait_advance_timing
  1583. ; Setup next wait time
  1584. mov TMR3RLL, Wt_ZC_Tout_Start_L
  1585. mov TMR3RLH, Wt_ZC_Tout_Start_H
  1586. setb Flags0.T3_PENDING
  1587. orl EIE1, #80h ; Enable timer3 interrupts
  1588. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1589. ;
  1590. ; Calculate new wait times routine
  1591. ;
  1592. ; No assumptions
  1593. ;
  1594. ; Calculates new wait times
  1595. ;
  1596. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1597. calc_new_wait_times:
  1598. clr C
  1599. clr A
  1600. subb A, Temp3 ; Negate
  1601. mov Temp1, A
  1602. clr A
  1603. subb A, Temp4
  1604. mov Temp2, A
  1605. IF MCU_48MHZ == 1
  1606. clr C
  1607. mov A, Temp1 ; Multiply by 2
  1608. rlc A
  1609. mov Temp1, A
  1610. mov A, Temp2
  1611. rlc A
  1612. mov Temp2, A
  1613. ENDIF
  1614. jnb Flags1.HIGH_RPM, ($+5) ; Branch if high rpm
  1615. ajmp calc_new_wait_times_fast
  1616. mov A, Temp1 ; Copy values
  1617. mov Temp3, A
  1618. mov A, Temp2
  1619. mov Temp4, A
  1620. setb C ; Negative numbers - set carry
  1621. mov A, Temp2
  1622. rrc A ; Divide by 2
  1623. mov Temp6, A
  1624. mov A, Temp1
  1625. rrc A
  1626. mov Temp5, A
  1627. mov Wt_Zc_Tout_Start_L, Temp1; Set 15deg time for zero cross scan timeout
  1628. mov Wt_Zc_Tout_Start_H, Temp2
  1629. clr C
  1630. mov A, Temp8 ; (Temp8 has Pgm_Comm_Timing)
  1631. subb A, #3 ; Is timing normal?
  1632. jz store_times_decrease ; Yes - branch
  1633. mov A, Temp8
  1634. jb ACC.0, adjust_timing_two_steps ; If an odd number - branch
  1635. mov A, Temp1 ; Add 7.5deg and store in Temp1/2
  1636. add A, Temp5
  1637. mov Temp1, A
  1638. mov A, Temp2
  1639. addc A, Temp6
  1640. mov Temp2, A
  1641. mov A, Temp5 ; Store 7.5deg in Temp3/4
  1642. mov Temp3, A
  1643. mov A, Temp6
  1644. mov Temp4, A
  1645. jmp store_times_up_or_down
  1646. adjust_timing_two_steps:
  1647. mov A, Temp1 ; Add 15deg and store in Temp1/2
  1648. add A, Temp1
  1649. mov Temp1, A
  1650. mov A, Temp2
  1651. addc A, Temp2
  1652. mov Temp2, A
  1653. clr C
  1654. mov A, Temp1
  1655. add A, #1
  1656. mov Temp1, A
  1657. mov A, Temp2
  1658. addc A, #0
  1659. mov Temp2, A
  1660. mov Temp3, #-1 ; Store minimum time in Temp3/4
  1661. mov Temp4, #0FFh
  1662. store_times_up_or_down:
  1663. clr C
  1664. mov A, Temp8
  1665. subb A, #3 ; Is timing higher than normal?
  1666. jc store_times_decrease ; No - branch
  1667. store_times_increase:
  1668. mov Wt_Comm_Start_L, Temp3 ; Now commutation time (~60deg) divided by 4 (~15deg nominal)
  1669. mov Wt_Comm_Start_H, Temp4
  1670. mov Wt_Adv_Start_L, Temp1 ; New commutation advance time (~15deg nominal)
  1671. mov Wt_Adv_Start_H, Temp2
  1672. mov Wt_Zc_Scan_Start_L, Temp5 ; Use this value for zero cross scan delay (7.5deg)
  1673. mov Wt_Zc_Scan_Start_H, Temp6
  1674. ajmp wait_before_zc_scan
  1675. store_times_decrease:
  1676. mov Wt_Comm_Start_L, Temp1 ; Now commutation time (~60deg) divided by 4 (~15deg nominal)
  1677. mov Wt_Comm_Start_H, Temp2
  1678. mov Wt_Adv_Start_L, Temp3 ; New commutation advance time (~15deg nominal)
  1679. mov Wt_Adv_Start_H, Temp4
  1680. mov Wt_Zc_Scan_Start_L, Temp5 ; Use this value for zero cross scan delay (7.5deg)
  1681. mov Wt_Zc_Scan_Start_H, Temp6
  1682. jnb Flags1.STARTUP_PHASE, store_times_exit
  1683. 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
  1684. mov Wt_Comm_Start_H, #0FFh
  1685. mov Wt_Zc_Scan_Start_L, #0F0h
  1686. mov Wt_Zc_Scan_Start_H, #0FFh
  1687. mov Wt_Zc_Tout_Start_L, #0F0h
  1688. mov Wt_Zc_Tout_Start_H, #0FFh
  1689. store_times_exit:
  1690. ajmp wait_before_zc_scan
  1691. calc_new_wait_times_fast:
  1692. mov A, Temp1 ; Copy values
  1693. mov Temp3, A
  1694. setb C ; Negative numbers - set carry
  1695. mov A, Temp1 ; Divide by 2
  1696. rrc A
  1697. mov Temp5, A
  1698. mov Wt_Zc_Tout_Start_L, Temp1; Set 15deg time for zero cross scan timeout
  1699. clr C
  1700. mov A, Temp8 ; (Temp8 has Pgm_Comm_Timing)
  1701. subb A, #3 ; Is timing normal?
  1702. jz store_times_decrease_fast; Yes - branch
  1703. mov A, Temp8
  1704. jb ACC.0, adjust_timing_two_steps_fast ; If an odd number - branch
  1705. mov A, Temp1 ; Add 7.5deg and store in Temp1
  1706. add A, Temp5
  1707. mov Temp1, A
  1708. mov A, Temp5 ; Store 7.5deg in Temp3
  1709. mov Temp3, A
  1710. ajmp store_times_up_or_down_fast
  1711. adjust_timing_two_steps_fast:
  1712. mov A, Temp1 ; Add 15deg and store in Temp1
  1713. add A, Temp1
  1714. add A, #1
  1715. mov Temp1, A
  1716. mov Temp3, #-1 ; Store minimum time in Temp3
  1717. store_times_up_or_down_fast:
  1718. clr C
  1719. mov A, Temp8
  1720. subb A, #3 ; Is timing higher than normal?
  1721. jc store_times_decrease_fast; No - branch
  1722. store_times_increase_fast:
  1723. mov Wt_Comm_Start_L, Temp3 ; Now commutation time (~60deg) divided by 4 (~15deg nominal)
  1724. mov Wt_Adv_Start_L, Temp1 ; New commutation advance time (~15deg nominal)
  1725. mov Wt_Zc_Scan_Start_L, Temp5 ; Use this value for zero cross scan delay (7.5deg)
  1726. ajmp wait_before_zc_scan
  1727. store_times_decrease_fast:
  1728. mov Wt_Comm_Start_L, Temp1 ; Now commutation time (~60deg) divided by 4 (~15deg nominal)
  1729. mov Wt_Adv_Start_L, Temp3 ; New commutation advance time (~15deg nominal)
  1730. mov Wt_Zc_Scan_Start_L, Temp5 ; Use this value for zero cross scan delay (7.5deg)
  1731. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1732. ;
  1733. ; Wait before zero cross scan routine
  1734. ;
  1735. ; No assumptions
  1736. ;
  1737. ; Waits for the zero cross scan wait time to elapse
  1738. ; Also sets up timer 3 for the zero cross scan timeout time
  1739. ;
  1740. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1741. wait_before_zc_scan:
  1742. jnb Flags0.T3_PENDING, ($+5)
  1743. ajmp wait_before_zc_scan
  1744. mov Startup_Zc_Timeout_Cntd, #2
  1745. setup_zc_scan_timeout:
  1746. setb Flags0.T3_PENDING
  1747. orl EIE1, #80h ; Enable timer3 interrupts
  1748. mov A, Flags1
  1749. anl A, #((1 SHL STARTUP_PHASE)+(1 SHL INITIAL_RUN_PHASE))
  1750. jz wait_before_zc_scan_exit
  1751. mov Temp1, Comm_Period4x_L ; Set long timeout when starting
  1752. mov Temp2, Comm_Period4x_H
  1753. clr C
  1754. mov A, Temp2
  1755. rrc A
  1756. mov Temp2, A
  1757. mov A, Temp1
  1758. rrc A
  1759. mov Temp1, A
  1760. IF MCU_48MHZ == 0
  1761. clr C
  1762. mov A, Temp2
  1763. rrc A
  1764. mov Temp2, A
  1765. mov A, Temp1
  1766. rrc A
  1767. mov Temp1, A
  1768. ENDIF
  1769. jnb Flags1.STARTUP_PHASE, setup_zc_scan_timeout_startup_done
  1770. mov A, Temp2
  1771. add A, #40h ; Increase timeout somewhat to avoid false wind up
  1772. mov Temp2, A
  1773. setup_zc_scan_timeout_startup_done:
  1774. clr IE_EA
  1775. anl EIE1, #7Fh ; Disable timer3 interrupts
  1776. mov TMR3CN0, #00h ; Timer3 disabled and interrupt flag cleared
  1777. clr C
  1778. clr A
  1779. subb A, Temp1 ; Set timeout
  1780. mov TMR3L, A
  1781. clr A
  1782. subb A, Temp2
  1783. mov TMR3H, A
  1784. mov TMR3CN0, #04h ; Timer3 enabled and interrupt flag cleared
  1785. setb Flags0.T3_PENDING
  1786. orl EIE1, #80h ; Enable timer3 interrupts
  1787. setb IE_EA
  1788. wait_before_zc_scan_exit:
  1789. ret
  1790. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1791. ;
  1792. ; Wait for comparator to go low/high routines
  1793. ;
  1794. ; No assumptions
  1795. ;
  1796. ; Waits for the zero cross scan wait time to elapse
  1797. ; Then scans for comparator going low/high
  1798. ;
  1799. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1800. wait_for_comp_out_low:
  1801. setb Flags0.DEMAG_DETECTED ; Set demag detected flag as default
  1802. mov Comparator_Read_Cnt, #0 ; Reset number of comparator reads
  1803. mov Bit_Access, #00h ; Desired comparator output
  1804. jnb Flags1.DIR_CHANGE_BRAKE, ($+6)
  1805. mov Bit_Access, #40h
  1806. jmp wait_for_comp_out_start
  1807. wait_for_comp_out_high:
  1808. setb Flags0.DEMAG_DETECTED ; Set demag detected flag as default
  1809. mov Comparator_Read_Cnt, #0 ; Reset number of comparator reads
  1810. mov Bit_Access, #40h ; Desired comparator output
  1811. jnb Flags1.DIR_CHANGE_BRAKE, ($+6)
  1812. mov Bit_Access, #00h
  1813. wait_for_comp_out_start:
  1814. ; Set number of comparator readings
  1815. mov Temp1, #1 ; Number of OK readings required
  1816. mov Temp2, #1 ; Max number of readings required
  1817. jb Flags1.HIGH_RPM, comp_scale_samples ; Branch if high rpm
  1818. mov A, Flags1 ; Clear demag detected flag if start phases
  1819. anl A, #((1 SHL STARTUP_PHASE)+(1 SHL INITIAL_RUN_PHASE))
  1820. jz ($+4)
  1821. clr Flags0.DEMAG_DETECTED
  1822. 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
  1823. mov A, Comm_Period4x_H ; Set number of readings higher for lower speeds
  1824. clr C
  1825. rrc A
  1826. jnz ($+3)
  1827. inc A
  1828. mov Temp1, A
  1829. clr C
  1830. subb A, #20
  1831. jc ($+4)
  1832. mov Temp1, #20
  1833. jnb Flags1.STARTUP_PHASE, comp_scale_samples
  1834. mov Temp1, #27 ; Set many samples during startup, approximately one pwm period
  1835. mov Temp2, #27
  1836. comp_scale_samples:
  1837. IF MCU_48MHZ == 1
  1838. clr C
  1839. mov A, Temp1
  1840. rlc A
  1841. mov Temp1, A
  1842. clr C
  1843. mov A, Temp2
  1844. rlc A
  1845. mov Temp2, A
  1846. ENDIF
  1847. comp_check_timeout:
  1848. jb Flags0.T3_PENDING, comp_check_timeout_not_timed_out ; Has zero cross scan timeout elapsed?
  1849. mov A, Comparator_Read_Cnt ; Check that comparator has been read
  1850. jz comp_check_timeout_not_timed_out ; If not read - branch
  1851. jnb Flags1.STARTUP_PHASE, comp_check_timeout_timeout_extended ; Extend timeout during startup
  1852. djnz Startup_Zc_Timeout_Cntd, comp_check_timeout_extend_timeout
  1853. comp_check_timeout_timeout_extended:
  1854. setb Flags0.COMP_TIMED_OUT
  1855. ajmp setup_comm_wait
  1856. comp_check_timeout_extend_timeout:
  1857. call setup_zc_scan_timeout
  1858. comp_check_timeout_not_timed_out:
  1859. inc Comparator_Read_Cnt ; Increment comparator read count
  1860. Read_Comp_Out ; Read comparator output
  1861. anl A, #40h
  1862. cjne A, Bit_Access, comp_read_wrong
  1863. ajmp comp_read_ok
  1864. comp_read_wrong:
  1865. jnb Flags1.STARTUP_PHASE, comp_read_wrong_not_startup
  1866. inc Temp1 ; Increment number of OK readings required
  1867. clr C
  1868. mov A, Temp1
  1869. subb A, Temp2 ; If above initial requirement - go back and restart
  1870. jc ($+3)
  1871. dec Temp1
  1872. ajmp comp_check_timeout ; Continue to look for good ones
  1873. comp_read_wrong_not_startup:
  1874. jb Flags0.DEMAG_DETECTED, comp_read_wrong_extend_timeout
  1875. inc Temp1 ; Increment number of OK readings required
  1876. clr C
  1877. mov A, Temp1
  1878. subb A, Temp2
  1879. jc ($+4)
  1880. ajmp wait_for_comp_out_start ; If above initial requirement - go back and restart
  1881. ajmp comp_check_timeout ; Otherwise - take another reading
  1882. comp_read_wrong_extend_timeout:
  1883. clr Flags0.DEMAG_DETECTED ; Clear demag detected flag
  1884. clr IE_EA
  1885. anl EIE1, #7Fh ; Disable timer3 interrupts
  1886. mov TMR3CN0, #00h ; Timer3 disabled and interrupt flag cleared
  1887. jnb Flags1.HIGH_RPM, comp_read_wrong_low_rpm ; Branch if not high rpm
  1888. mov TMR3L, #00h ; Set timeout to ~1ms
  1889. IF MCU_48MHZ == 1
  1890. mov TMR3H, #0F0h
  1891. ELSE
  1892. mov TMR3H, #0F8h
  1893. ENDIF
  1894. comp_read_wrong_timeout_set:
  1895. mov TMR3CN0, #04h ; Timer3 enabled and interrupt flag cleared
  1896. setb Flags0.T3_PENDING
  1897. orl EIE1, #80h ; Enable timer3 interrupts
  1898. setb IE_EA
  1899. ajmp wait_for_comp_out_start ; If comparator output is not correct - go back and restart
  1900. comp_read_wrong_low_rpm:
  1901. mov A, Comm_Period4x_H ; Set timeout to ~4x comm period 4x value
  1902. mov Temp7, #0FFh ; Default to long
  1903. IF MCU_48MHZ == 1
  1904. clr C
  1905. rlc A
  1906. jc comp_read_wrong_load_timeout
  1907. ENDIF
  1908. clr C
  1909. rlc A
  1910. jc comp_read_wrong_load_timeout
  1911. clr C
  1912. rlc A
  1913. jc comp_read_wrong_load_timeout
  1914. mov Temp7, A
  1915. comp_read_wrong_load_timeout:
  1916. clr C
  1917. clr A
  1918. subb A, Temp7
  1919. mov TMR3L, #0
  1920. mov TMR3H, A
  1921. ajmp comp_read_wrong_timeout_set
  1922. comp_read_ok:
  1923. clr C
  1924. mov A, Startup_Cnt ; Force a timeout for the first commutation
  1925. subb A, #1
  1926. jnc ($+4)
  1927. ajmp wait_for_comp_out_start
  1928. jnb Flags0.DEMAG_DETECTED, ($+5) ; Do not accept correct comparator output if it is demag
  1929. ajmp wait_for_comp_out_start
  1930. djnz Temp1, comp_read_ok_jmp ; Decrement readings counter - repeat comparator reading if not zero
  1931. ajmp ($+4)
  1932. comp_read_ok_jmp:
  1933. ajmp comp_check_timeout
  1934. clr Flags0.COMP_TIMED_OUT
  1935. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1936. ;
  1937. ; Setup commutation timing routine
  1938. ;
  1939. ; No assumptions
  1940. ;
  1941. ; Sets up and starts wait from commutation to zero cross
  1942. ;
  1943. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1944. setup_comm_wait:
  1945. clr IE_EA
  1946. anl EIE1, #7Fh ; Disable timer3 interrupts
  1947. mov TMR3CN0, #00h ; Timer3 disabled and interrupt flag cleared
  1948. mov TMR3L, Wt_Comm_Start_L
  1949. mov TMR3H, Wt_Comm_Start_H
  1950. mov TMR3CN0, #04h ; Timer3 enabled and interrupt flag cleared
  1951. ; Setup next wait time
  1952. mov TMR3RLL, Wt_Adv_Start_L
  1953. mov TMR3RLH, Wt_Adv_Start_H
  1954. setb Flags0.T3_PENDING
  1955. orl EIE1, #80h ; Enable timer3 interrupts
  1956. setb IE_EA ; Enable interrupts again
  1957. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1958. ;
  1959. ; Evaluate comparator integrity
  1960. ;
  1961. ; No assumptions
  1962. ;
  1963. ; Checks comparator signal behaviour versus expected behaviour
  1964. ;
  1965. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1966. evaluate_comparator_integrity:
  1967. mov A, Flags1
  1968. anl A, #((1 SHL STARTUP_PHASE)+(1 SHL INITIAL_RUN_PHASE))
  1969. jz eval_comp_check_timeout
  1970. jb Flags1.INITIAL_RUN_PHASE, ($+5) ; Do not increment beyond startup phase
  1971. inc Startup_Cnt ; Increment counter
  1972. jmp eval_comp_exit
  1973. eval_comp_check_timeout:
  1974. jnb Flags0.COMP_TIMED_OUT, eval_comp_exit ; Has timeout elapsed?
  1975. jb Flags1.DIR_CHANGE_BRAKE, eval_comp_exit ; Do not exit run mode if it is braking
  1976. jb Flags0.DEMAG_DETECTED, eval_comp_exit ; Do not exit run mode if it is a demag situation
  1977. dec SP ; Routine exit without "ret" command
  1978. dec SP
  1979. ljmp run_to_wait_for_power_on_fail ; Yes - exit run mode
  1980. eval_comp_exit:
  1981. ret
  1982. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1983. ;
  1984. ; Wait for commutation routine
  1985. ;
  1986. ; No assumptions
  1987. ;
  1988. ; Waits from zero cross to commutation
  1989. ;
  1990. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1991. wait_for_comm:
  1992. ; Update demag metric
  1993. mov Temp1, #0
  1994. jnb Flags0.DEMAG_DETECTED, ($+5)
  1995. mov Temp1, #1
  1996. mov A, Demag_Detected_Metric ; Sliding average of 8, 256 when demag and 0 when not. Limited to minimum 120
  1997. mov B, #7
  1998. mul AB ; Multiply by 7
  1999. mov Temp2, A
  2000. mov A, B ; Add new value for current demag status
  2001. add A, Temp1
  2002. mov B, A
  2003. mov A, Temp2
  2004. mov C, B.0 ; Divide by 8
  2005. rrc A
  2006. mov C, B.1
  2007. rrc A
  2008. mov C, B.2
  2009. rrc A
  2010. mov Demag_Detected_Metric, A
  2011. clr C
  2012. subb A, #120 ; Limit to minimum 120
  2013. jnc ($+5)
  2014. mov Demag_Detected_Metric, #120
  2015. clr C
  2016. mov A, Demag_Detected_Metric ; Check demag metric
  2017. subb A, Demag_Pwr_Off_Thresh
  2018. jc wait_for_comm_wait ; Cut power if many consecutive demags. This will help retain sync during hard accelerations
  2019. setb Flags0.DEMAG_CUT_POWER ; Set demag power cut flag
  2020. All_pwmFETs_off
  2021. Set_Pwms_Off
  2022. wait_for_comm_wait:
  2023. jnb Flags0.T3_PENDING, ($+5)
  2024. ajmp wait_for_comm_wait
  2025. ; Setup next wait time
  2026. mov TMR3RLL, Wt_Zc_Scan_Start_L
  2027. mov TMR3RLH, Wt_Zc_Scan_Start_H
  2028. setb Flags0.T3_PENDING
  2029. orl EIE1, #80h ; Enable timer3 interrupts
  2030. ret
  2031. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2032. ;
  2033. ; Commutation routines
  2034. ;
  2035. ; No assumptions
  2036. ;
  2037. ; Performs commutation switching
  2038. ;
  2039. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2040. ; Comm phase 1 to comm phase 2
  2041. comm1comm2:
  2042. Set_RPM_Out
  2043. jb Flags3.PGM_DIR_REV, comm12_rev
  2044. clr IE_EA ; Disable all interrupts
  2045. BcomFET_off ; Turn off comfet
  2046. AcomFET_on ; Turn on comfet
  2047. Set_Pwm_C ; To reapply power after a demag cut
  2048. setb IE_EA
  2049. Set_Comp_Phase_B ; Set comparator phase
  2050. jmp comm_exit
  2051. comm12_rev:
  2052. clr IE_EA ; Disable all interrupts
  2053. BcomFET_off ; Turn off comfet
  2054. CcomFET_on ; Turn on comfet (reverse)
  2055. Set_Pwm_A ; To reapply power after a demag cut
  2056. setb IE_EA
  2057. Set_Comp_Phase_B ; Set comparator phase
  2058. jmp comm_exit
  2059. ; Comm phase 2 to comm phase 3
  2060. comm2comm3:
  2061. Clear_RPM_Out
  2062. jb Flags3.PGM_DIR_REV, comm23_rev
  2063. clr IE_EA ; Disable all interrupts
  2064. CpwmFET_off ; Turn off pwmfet
  2065. Set_Pwm_B
  2066. AcomFET_on ; To reapply power after a demag cut
  2067. setb IE_EA
  2068. Set_Comp_Phase_C ; Set comparator phase
  2069. ljmp comm_exit
  2070. comm23_rev:
  2071. clr IE_EA ; Disable all interrupts
  2072. ApwmFET_off ; Turn off pwmfet (reverse)
  2073. Set_Pwm_B
  2074. CcomFET_on ; To reapply power after a demag cut
  2075. setb IE_EA
  2076. Set_Comp_Phase_A ; Set comparator phase (reverse)
  2077. ljmp comm_exit
  2078. ; Comm phase 3 to comm phase 4
  2079. comm3comm4:
  2080. Set_RPM_Out
  2081. jb Flags3.PGM_DIR_REV, comm34_rev
  2082. clr IE_EA ; Disable all interrupts
  2083. AcomFET_off ; Turn off comfet
  2084. CcomFET_on ; Turn on comfet
  2085. Set_Pwm_B ; To reapply power after a demag cut
  2086. setb IE_EA
  2087. Set_Comp_Phase_A ; Set comparator phase
  2088. jmp comm_exit
  2089. comm34_rev:
  2090. clr IE_EA ; Disable all interrupts
  2091. CcomFET_off ; Turn off comfet (reverse)
  2092. AcomFET_on ; Turn on comfet (reverse)
  2093. Set_Pwm_B ; To reapply power after a demag cut
  2094. setb IE_EA
  2095. Set_Comp_Phase_C ; Set comparator phase (reverse)
  2096. jmp comm_exit
  2097. ; Comm phase 4 to comm phase 5
  2098. comm4comm5:
  2099. Clear_RPM_Out
  2100. jb Flags3.PGM_DIR_REV, comm45_rev
  2101. clr IE_EA ; Disable all interrupts
  2102. BpwmFET_off ; Turn off pwmfet
  2103. Set_Pwm_A
  2104. CcomFET_on ; To reapply power after a demag cut
  2105. setb IE_EA
  2106. Set_Comp_Phase_B ; Set comparator phase
  2107. ljmp comm_exit
  2108. comm45_rev:
  2109. clr IE_EA ; Disable all interrupts
  2110. BpwmFET_off ; Turn off pwmfet
  2111. Set_Pwm_C
  2112. AcomFET_on ; To reapply power after a demag cut
  2113. setb IE_EA
  2114. Set_Comp_Phase_B ; Set comparator phase
  2115. ljmp comm_exit
  2116. ; Comm phase 5 to comm phase 6
  2117. comm5comm6:
  2118. Set_RPM_Out
  2119. jb Flags3.PGM_DIR_REV, comm56_rev
  2120. clr IE_EA ; Disable all interrupts
  2121. CcomFET_off ; Turn off comfet
  2122. BcomFET_on ; Turn on comfet
  2123. Set_Pwm_A ; To reapply power after a demag cut
  2124. setb IE_EA
  2125. Set_Comp_Phase_C ; Set comparator phase
  2126. jmp comm_exit
  2127. comm56_rev:
  2128. clr IE_EA ; Disable all interrupts
  2129. AcomFET_off ; Turn off comfet (reverse)
  2130. BcomFET_on ; Turn on comfet
  2131. Set_Pwm_C ; To reapply power after a demag cut
  2132. setb IE_EA
  2133. Set_Comp_Phase_A ; Set comparator phase (reverse)
  2134. jmp comm_exit
  2135. ; Comm phase 6 to comm phase 1
  2136. comm6comm1:
  2137. Clear_RPM_Out
  2138. jb Flags3.PGM_DIR_REV, comm61_rev
  2139. clr IE_EA ; Disable all interrupts
  2140. ApwmFET_off ; Turn off pwmfet
  2141. Set_Pwm_C
  2142. BcomFET_on ; To reapply power after a demag cut
  2143. setb IE_EA
  2144. Set_Comp_Phase_A ; Set comparator phase
  2145. jmp comm_exit
  2146. comm61_rev:
  2147. clr IE_EA ; Disable all interrupts
  2148. CpwmFET_off ; Turn off pwmfet (reverse)
  2149. Set_Pwm_A
  2150. BcomFET_on ; To reapply power after a demag cut
  2151. setb IE_EA
  2152. Set_Comp_Phase_C ; Set comparator phase (reverse)
  2153. comm_exit:
  2154. clr Flags0.DEMAG_CUT_POWER ; Clear demag power cut flag
  2155. ret
  2156. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2157. ;
  2158. ; Switch power off routine
  2159. ;
  2160. ; No assumptions
  2161. ;
  2162. ; Switches all fets off
  2163. ;
  2164. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2165. switch_power_off:
  2166. All_pwmFETs_Off ; Turn off all pwm fets
  2167. All_comFETs_Off ; Turn off all commutation fets
  2168. Set_Pwms_Off
  2169. ret
  2170. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2171. ;
  2172. ; Set default parameters
  2173. ;
  2174. ; No assumptions
  2175. ;
  2176. ; Sets default programming parameters
  2177. ;
  2178. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2179. set_default_parameters:
  2180. mov Temp1, #_Pgm_Gov_P_Gain
  2181. mov @Temp1, #0FFh ; Governor P gain
  2182. inc Temp1
  2183. mov @Temp1, #0FFh ; Governor I gain
  2184. inc Temp1
  2185. mov @Temp1, #0FFh ; Governor mode
  2186. inc Temp1
  2187. mov @Temp1, #0FFh ; Low voltage limit
  2188. inc Temp1
  2189. mov @Temp1, #0FFh ; Multi gain
  2190. inc Temp1
  2191. mov @Temp1, #0FFh
  2192. inc Temp1
  2193. mov @Temp1, #DEFAULT_PGM_STARTUP_PWR
  2194. inc Temp1
  2195. mov @Temp1, #0FFh ; Pwm freq
  2196. inc Temp1
  2197. mov @Temp1, #DEFAULT_PGM_DIRECTION
  2198. mov Temp1, #Pgm_Enable_TX_Program
  2199. mov @Temp1, #DEFAULT_PGM_ENABLE_TX_PROGRAM
  2200. inc Temp1
  2201. mov @Temp1, #0FFh ; Main rearm start
  2202. inc Temp1
  2203. mov @Temp1, #0FFh ; Governor setup target
  2204. inc Temp1
  2205. mov @Temp1, #0FFh ; Startup rpm
  2206. inc Temp1
  2207. mov @Temp1, #0FFh ; Startup accel
  2208. inc Temp1
  2209. mov @Temp1, #0FFh ; Voltage comp
  2210. inc Temp1
  2211. mov @Temp1, #DEFAULT_PGM_COMM_TIMING
  2212. inc Temp1
  2213. mov @Temp1, #0FFh ; Damping force
  2214. inc Temp1
  2215. mov @Temp1, #0FFh ; Governor range
  2216. inc Temp1
  2217. mov @Temp1, #0FFh ; Startup method
  2218. inc Temp1
  2219. mov @Temp1, #DEFAULT_PGM_MIN_THROTTLE
  2220. inc Temp1
  2221. mov @Temp1, #DEFAULT_PGM_MAX_THROTTLE
  2222. inc Temp1
  2223. mov @Temp1, #DEFAULT_PGM_BEEP_STRENGTH
  2224. inc Temp1
  2225. mov @Temp1, #DEFAULT_PGM_BEACON_STRENGTH
  2226. inc Temp1
  2227. mov @Temp1, #DEFAULT_PGM_BEACON_DELAY
  2228. inc Temp1
  2229. mov @Temp1, #0FFh ; Throttle rate
  2230. inc Temp1
  2231. mov @Temp1, #DEFAULT_PGM_DEMAG_COMP
  2232. inc Temp1
  2233. mov @Temp1, #0FFh ; Bec voltage high
  2234. inc Temp1
  2235. mov @Temp1, #DEFAULT_PGM_CENTER_THROTTLE
  2236. inc Temp1
  2237. mov @Temp1, #0FFh
  2238. inc Temp1
  2239. mov @Temp1, #DEFAULT_PGM_ENABLE_TEMP_PROT
  2240. inc Temp1
  2241. mov @Temp1, #DEFAULT_PGM_ENABLE_POWER_PROT
  2242. inc Temp1
  2243. mov @Temp1, #0FFh ; Enable pwm input
  2244. inc Temp1
  2245. mov @Temp1, #0FFh ; Pwm dither
  2246. inc Temp1
  2247. mov @Temp1, #DEFAULT_PGM_BRAKE_ON_STOP
  2248. inc Temp1
  2249. mov @Temp1, #DEFAULT_PGM_LED_CONTROL
  2250. ret
  2251. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2252. ;
  2253. ; Scale throttle cal
  2254. ;
  2255. ; No assumptions
  2256. ;
  2257. ; Scales a throttle cal value
  2258. ; Input is ACC, output is Temp2/Temp1
  2259. ;
  2260. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2261. scale_throttle_cal:
  2262. mov Temp3, A
  2263. mov B, #0Ch ; Calculate "3%" (for going from 1000us to numerical 1024)
  2264. mul AB
  2265. mov Temp4, B
  2266. mov A, Temp3
  2267. clr C ; Shift to 9 bits
  2268. rlc A
  2269. mov Temp1, A
  2270. mov A, #1
  2271. rlc A
  2272. mov Temp2, A
  2273. mov A, Temp1 ; Shift to 10 bits
  2274. clr C
  2275. rlc A
  2276. mov Temp1, A
  2277. mov A, Temp2
  2278. rlc A
  2279. mov Temp2, A
  2280. mov A, Temp1 ; Add "3%"
  2281. clr C
  2282. add A, Temp4
  2283. mov Temp1, A
  2284. mov A, Temp2
  2285. addc A, #0
  2286. mov Temp2, A
  2287. IF MCU_48MHZ == 1
  2288. mov A, Temp1 ; Shift to 11 bits
  2289. clr C
  2290. rlc A
  2291. mov Temp1, A
  2292. mov A, Temp2
  2293. rlc A
  2294. mov Temp2, A
  2295. ENDIF
  2296. ret
  2297. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2298. ;
  2299. ; Decode settings
  2300. ;
  2301. ; No assumptions
  2302. ;
  2303. ; Decodes various settings
  2304. ;
  2305. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2306. decode_settings:
  2307. ; Load programmed direction
  2308. mov Temp1, #Pgm_Direction
  2309. mov A, @Temp1
  2310. clr C
  2311. subb A, #3
  2312. setb Flags3.PGM_BIDIR
  2313. jnc ($+4)
  2314. clr Flags3.PGM_BIDIR
  2315. clr Flags3.PGM_DIR_REV
  2316. mov A, @Temp1
  2317. jnb ACC.1, ($+5)
  2318. setb Flags3.PGM_DIR_REV
  2319. mov C, Flags3.PGM_DIR_REV
  2320. mov Flags3.PGM_BIDIR_REV, C
  2321. ; Decode startup power
  2322. mov Temp1, #Pgm_Startup_Pwr
  2323. mov A, @Temp1
  2324. dec A
  2325. mov DPTR, #STARTUP_POWER_TABLE
  2326. movc A, @A+DPTR
  2327. mov Temp1, #Pgm_Startup_Pwr_Decoded
  2328. mov @Temp1, A
  2329. ; Decode low rpm power slope
  2330. mov Temp1, #Pgm_Startup_Pwr
  2331. mov A, @Temp1
  2332. mov Low_Rpm_Pwr_Slope, A
  2333. clr C
  2334. subb A, #2
  2335. jnc ($+5)
  2336. mov Low_Rpm_Pwr_Slope, #2
  2337. ; Decode demag compensation
  2338. mov Temp1, #Pgm_Demag_Comp
  2339. mov A, @Temp1
  2340. mov Demag_Pwr_Off_Thresh, #255 ; Set default
  2341. cjne A, #2, decode_demag_high
  2342. mov Demag_Pwr_Off_Thresh, #160 ; Settings for demag comp low
  2343. decode_demag_high:
  2344. cjne A, #3, decode_demag_done
  2345. mov Demag_Pwr_Off_Thresh, #130 ; Settings for demag comp high
  2346. decode_demag_done:
  2347. ; Decode temperature protection limit
  2348. mov Temp1, #Pgm_Enable_Temp_Prot
  2349. mov A, @Temp1
  2350. mov Temp1, A
  2351. jz decode_temp_done
  2352. mov A, #(TEMP_LIMIT-TEMP_LIMIT_STEP)
  2353. decode_temp_step:
  2354. add A, #TEMP_LIMIT_STEP
  2355. djnz Temp1, decode_temp_step
  2356. decode_temp_done:
  2357. mov Temp_Prot_Limit, A
  2358. ; Decode throttle cal
  2359. mov Temp1, #Pgm_Min_Throttle ; Throttle cal is in 4us units
  2360. mov A, @Temp1
  2361. call scale_throttle_cal
  2362. mov Min_Throttle_L, Temp1
  2363. mov Min_Throttle_H, Temp2
  2364. mov Temp1, #Pgm_Center_Throttle ; Throttle cal is in 4us units
  2365. mov A, @Temp1
  2366. call scale_throttle_cal
  2367. mov Center_Throttle_L, Temp1
  2368. mov Center_Throttle_H, Temp2
  2369. mov Temp1, #Pgm_Max_Throttle ; Throttle cal is in 4us units
  2370. mov A, @Temp1
  2371. call scale_throttle_cal
  2372. mov Max_Throttle_L, Temp1
  2373. mov Max_Throttle_H, Temp2
  2374. call switch_power_off ; Reset DPTR
  2375. ret
  2376. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2377. ;
  2378. ; Find throttle gains
  2379. ;
  2380. ; No assumptions
  2381. ;
  2382. ; Finds throttle gains for both directions in bidirectional mode
  2383. ;
  2384. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2385. find_throttle_gains:
  2386. ; Check if full range is chosen
  2387. jnb Flags2.RCP_FULL_RANGE, find_throttle_gains_normal
  2388. mov Temp3, #0 ; Min throttle
  2389. mov Temp4, #0
  2390. mov Temp5, #255 ; Max throttle
  2391. mov Temp6, #0
  2392. mov Temp7, #0 ; Deadband
  2393. call find_throttle_gain
  2394. mov Throttle_Gain_M, Temp4
  2395. mov Throttle_Gain, Temp3
  2396. ret
  2397. find_throttle_gains_normal:
  2398. ; Check if bidirectional operation
  2399. jnb Flags3.PGM_BIDIR, find_throttle_gains_bidir_done
  2400. mov Temp1, #Pgm_Min_Throttle
  2401. mov A, @Temp1
  2402. mov Temp3, A
  2403. mov Temp4, #0
  2404. mov Temp1, #Pgm_Center_Throttle
  2405. mov A, @Temp1
  2406. mov Temp5, A
  2407. mov Temp6, #0
  2408. clr C
  2409. mov A, Temp3 ; Scale gains in bidirectional
  2410. rlc A
  2411. mov Temp3, A
  2412. mov A, Temp4
  2413. rlc A
  2414. mov Temp4, A
  2415. clr C
  2416. mov A, Temp5
  2417. rlc A
  2418. mov Temp5, A
  2419. mov A, Temp6
  2420. rlc A
  2421. mov Temp6, A
  2422. mov Temp7, #10 ; Compensate for deadband in bidirectional
  2423. call find_throttle_gain
  2424. mov Throttle_Gain_BD_Rev_M, Temp4
  2425. mov Throttle_Gain_BD_Rev, Temp3
  2426. find_throttle_gains_bidir_done:
  2427. mov Temp1, #Pgm_Min_Throttle
  2428. jnb Flags3.PGM_BIDIR, ($+5)
  2429. mov Temp1, #Pgm_Center_Throttle
  2430. mov A, @Temp1
  2431. mov Temp3, A
  2432. mov Temp4, #0
  2433. mov Temp1, #Pgm_Max_Throttle
  2434. mov A, @Temp1
  2435. mov Temp5, A
  2436. mov Temp6, #0
  2437. mov Temp7, #0 ; No deadband
  2438. jnb Flags3.PGM_BIDIR, find_throttle_gain_fwd
  2439. clr C
  2440. mov A, Temp3 ; Scale gains in bidirectional
  2441. rlc A
  2442. mov Temp3, A
  2443. mov A, Temp4
  2444. rlc A
  2445. mov Temp4, A
  2446. clr C
  2447. mov A, Temp5
  2448. rlc A
  2449. mov Temp5, A
  2450. mov A, Temp6
  2451. rlc A
  2452. mov Temp6, A
  2453. mov Temp7, #10 ; Compensate for deadband in bidirectional
  2454. find_throttle_gain_fwd:
  2455. call find_throttle_gain
  2456. mov Throttle_Gain_M, Temp4
  2457. mov Throttle_Gain, Temp3
  2458. ret
  2459. ret
  2460. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2461. ;
  2462. ; Find throttle gain
  2463. ;
  2464. ; The difference between max and min throttle must be more than 140us (a Pgm_xxx_Throttle difference of 35)
  2465. ; Temp4/3 holds min throttle, Temp6/5 holds max throttle, Temp7 holds deadband, Temp4/Temp3 gives resulting gain
  2466. ;
  2467. ; Finds throttle gain from throttle calibration values
  2468. ;
  2469. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2470. find_throttle_gain:
  2471. ; Subtract deadband from max
  2472. clr C
  2473. mov A, Temp5
  2474. subb A, Temp7
  2475. mov Temp5, A
  2476. mov A, Temp6
  2477. subb A, #0
  2478. mov Temp6, A
  2479. ; Calculate difference
  2480. clr C
  2481. mov A, Temp5
  2482. subb A, Temp3
  2483. mov Temp5, A
  2484. mov A, Temp6
  2485. subb A, Temp4
  2486. mov Temp6, A
  2487. ; Check that difference is minimum 35
  2488. clr C
  2489. mov A, Temp5
  2490. subb A, #35
  2491. mov A, Temp6
  2492. subb A, #0
  2493. jnc ($+6)
  2494. mov Temp5, #35
  2495. mov Temp6, #0
  2496. ; Check that difference is maximum 511
  2497. clr C
  2498. mov A, Temp5
  2499. subb A, #255
  2500. mov A, Temp6
  2501. subb A, #1
  2502. jc ($+6)
  2503. mov Temp5, #255
  2504. mov Temp6, #1
  2505. ; Find gain
  2506. mov Temp4, #0FFh
  2507. find_throttle_gain_loop:
  2508. inc Temp4
  2509. mov Temp3, #0
  2510. test_throttle_gain:
  2511. inc Temp3
  2512. mov A, Temp3
  2513. jnz test_throttle_gain_mult
  2514. clr C
  2515. mov A, Temp5 ; Set multiplier x2 and range /2
  2516. rlc A
  2517. mov Temp5, A
  2518. mov A, Temp6
  2519. rlc A
  2520. mov Temp6, A
  2521. ajmp find_throttle_gain_loop
  2522. test_throttle_gain_mult:
  2523. mov A, Temp5 ; A has difference, B has gain
  2524. mov B, Temp3
  2525. mul AB
  2526. mov Temp7, B
  2527. mov A, Temp6
  2528. mov B, Temp3
  2529. mul AB
  2530. add A, Temp7
  2531. subb A, #124
  2532. jc test_throttle_gain
  2533. mov A, Temp3
  2534. cpl A
  2535. jz find_throttle_gain_loop
  2536. ret
  2537. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2538. ;
  2539. ; Average throttle
  2540. ;
  2541. ; Outputs result in Temp8
  2542. ;
  2543. ; Averages throttle calibration readings
  2544. ;
  2545. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2546. average_throttle:
  2547. setb Flags2.RCP_FULL_RANGE ; Set range to 1000-2020us
  2548. call find_throttle_gains ; Set throttle gains
  2549. call wait30ms
  2550. call wait30ms
  2551. mov Temp3, #0
  2552. mov Temp4, #0
  2553. mov Temp5, #16 ; Average 16 measurments
  2554. average_throttle_meas:
  2555. call wait3ms ; Wait for new RC pulse value
  2556. mov A, New_Rcp ; Get new RC pulse value
  2557. add A, Temp3
  2558. mov Temp3, A
  2559. mov A, #0
  2560. addc A, Temp4
  2561. mov Temp4, A
  2562. djnz Temp5, average_throttle_meas
  2563. mov Temp5, #4 ; Shift 4 times
  2564. average_throttle_div:
  2565. clr C
  2566. mov A, Temp4 ; Shift right
  2567. rrc A
  2568. mov Temp4, A
  2569. mov A, Temp3
  2570. rrc A
  2571. mov Temp3, A
  2572. djnz Temp5, average_throttle_div
  2573. mov Temp8, A ; Copy to Temp8
  2574. mov A, Temp4
  2575. jz ($+4)
  2576. mov Temp8, #0FFh
  2577. clr Flags2.RCP_FULL_RANGE
  2578. call find_throttle_gains ; Set throttle gains
  2579. ret
  2580. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2581. ;
  2582. ; LED control
  2583. ;
  2584. ; No assumptions
  2585. ;
  2586. ; Controls LEDs
  2587. ;
  2588. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2589. led_control:
  2590. mov Temp1, #Pgm_LED_Control
  2591. mov A, @Temp1
  2592. mov Temp2, A
  2593. anl A, #03h
  2594. Set_LED_0
  2595. jnz led_0_done
  2596. Clear_LED_0
  2597. led_0_done:
  2598. mov A, Temp2
  2599. anl A, #0Ch
  2600. Set_LED_1
  2601. jnz led_1_done
  2602. Clear_LED_1
  2603. led_1_done:
  2604. mov A, Temp2
  2605. anl A, #030h
  2606. Set_LED_2
  2607. jnz led_2_done
  2608. Clear_LED_2
  2609. led_2_done:
  2610. mov A, Temp2
  2611. anl A, #0C0h
  2612. Set_LED_3
  2613. jnz led_3_done
  2614. Clear_LED_3
  2615. led_3_done:
  2616. ret
  2617. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2618. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2619. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2620. ;
  2621. ; Main program start
  2622. ;
  2623. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2624. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2625. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2626. pgm_start:
  2627. ; Initialize flash keys to invalid values
  2628. mov Flash_Key_1, #0
  2629. mov Flash_Key_2, #0
  2630. ; Disable the WDT.
  2631. mov WDTCN, #0DEh ; Disable watchdog
  2632. mov WDTCN, #0ADh
  2633. ; Initialize stack
  2634. mov SP, #0c0h ; Stack = 64 upper bytes of RAM
  2635. ; Initialize VDD monitor
  2636. orl VDM0CN, #080h ; Enable the VDD monitor
  2637. IF ONE_S_CAPABLE == 0
  2638. mov RSTSRC, #06h ; Set missing clock and VDD monitor as a reset source if not 1S capable
  2639. ELSE
  2640. mov RSTSRC, #04h ; Do not set VDD monitor as a reset source for 1S ESCSs, in order to avoid resets due to it
  2641. ENDIF
  2642. ; Set clock frequency
  2643. mov CLKSEL, #00h ; Set clock divider to 1
  2644. ; Switch power off
  2645. call switch_power_off
  2646. ; Ports initialization
  2647. mov P0, #P0_INIT
  2648. mov P0MDIN, #P0_DIGITAL
  2649. mov P0MDOUT, #P0_PUSHPULL
  2650. mov P0, #P0_INIT
  2651. mov P0SKIP, #P0_SKIP
  2652. mov P1, #P1_INIT
  2653. mov P1MDIN, #P1_DIGITAL
  2654. mov P1MDOUT, #P1_PUSHPULL
  2655. mov P1, #P1_INIT
  2656. mov P1SKIP, #P1_SKIP
  2657. mov P2MDOUT, #P2_PUSHPULL
  2658. ; Initialize the XBAR and related functionality
  2659. Initialize_Xbar
  2660. ; Switch power off again, after initializing ports
  2661. call switch_power_off
  2662. ; Clear RAM
  2663. clr A ; Clear accumulator
  2664. mov Temp1, A ; Clear Temp1
  2665. clear_ram:
  2666. mov @Temp1, A ; Clear RAM
  2667. djnz Temp1, clear_ram ; Is A not zero? - jump
  2668. ; Set default programmed parameters
  2669. call set_default_parameters
  2670. ; Read all programmed parameters
  2671. call read_all_eeprom_parameters
  2672. ; Set beep strength
  2673. mov Temp1, #Pgm_Beep_Strength
  2674. mov Beep_Strength, @Temp1
  2675. ; Set initial arm variable
  2676. mov Initial_Arm, #1
  2677. ; Initializing beep
  2678. clr IE_EA ; Disable interrupts explicitly
  2679. call wait200ms
  2680. call beep_f1
  2681. call wait30ms
  2682. call beep_f2
  2683. call wait30ms
  2684. call beep_f3
  2685. call wait30ms
  2686. call led_control
  2687. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2688. ;
  2689. ; No signal entry point
  2690. ;
  2691. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2692. init_no_signal:
  2693. ; Disable interrupts explicitly
  2694. clr IE_EA
  2695. ; Initialize flash keys to invalid values
  2696. mov Flash_Key_1, #0
  2697. mov Flash_Key_2, #0
  2698. ; Check if input signal is high for more than 15ms
  2699. mov Temp1, #250
  2700. input_high_check_1:
  2701. mov Temp2, #250
  2702. input_high_check_2:
  2703. jnb RTX_PORT.RTX_PIN, bootloader_done ; Look for low
  2704. djnz Temp2, input_high_check_2
  2705. djnz Temp1, input_high_check_1
  2706. ljmp 1C00h ; Jump to bootloader
  2707. bootloader_done:
  2708. ; Decode settings
  2709. call decode_settings
  2710. ; Find throttle gain from stored min and max settings
  2711. call find_throttle_gains
  2712. ; Set beep strength
  2713. mov Temp1, #Pgm_Beep_Strength
  2714. mov Beep_Strength, @Temp1
  2715. ; Switch power off
  2716. call switch_power_off
  2717. ; Set clock frequency
  2718. IF MCU_48MHZ == 1
  2719. Set_MCU_Clk_24MHz
  2720. ENDIF
  2721. mov IT01CF, #RTX_PIN ; Route RCP input to INT0
  2722. mov TCON, #11h ; Timer0 run and INT0 edge triggered
  2723. mov CKCON0, #04h ; Timer0 clock is system clock
  2724. mov TMOD, #09h ; Timer0 set to 16bits and gated by INT0
  2725. mov TMR2CN0, #04h ; Timer2 enabled
  2726. mov TMR3CN0, #04h ; Timer3 enabled
  2727. Initialize_PCA ; Initialize PCA
  2728. Set_Pwm_Polarity ; Set pwm polarity
  2729. Enable_Power_Pwm_Module ; Enable power pwm module
  2730. Enable_Damp_Pwm_Module ; Enable damping pwm module
  2731. ; Enable interrupts
  2732. IF MCU_48MHZ == 0
  2733. mov IE, #21h ; Enable timer2 interrupts and INT0 interrupts
  2734. ELSE
  2735. mov IE, #23h ; Enable timer0, timer2 interrupts and INT0 interrupts
  2736. ENDIF
  2737. mov EIE1, #90h ; Enable timer3 and PCA0 interrupts
  2738. mov EIP1, #10h ; High priority to PCA0 interrupts
  2739. ; Initialize comparator
  2740. mov CMP0CN0, #80h ; Comparator enabled, no hysteresis
  2741. mov CMP0MD, #00h ; Comparator response time 100ns
  2742. ; Initialize ADC
  2743. Initialize_Adc ; Initialize ADC operation
  2744. call wait1ms
  2745. setb IE_EA ; Enable all interrupts
  2746. ; Reset stall count
  2747. mov Stall_Cnt, #0
  2748. ; Initialize RC pulse
  2749. clr Flags2.RCP_UPDATED ; Clear updated flag
  2750. call wait200ms
  2751. ; Test whether signal is OneShot125
  2752. clr Flags2.RCP_ONESHOT125 ; Clear OneShot125 flag
  2753. mov Rcp_Outside_Range_Cnt, #0 ; Reset out of range counter
  2754. call wait100ms ; Wait for new RC pulse
  2755. clr C
  2756. mov A, Rcp_Outside_Range_Cnt ; Check how many pulses were outside normal range ("745-2235us")
  2757. subb A, #10
  2758. jc validate_rcp_start
  2759. setb Flags2.RCP_ONESHOT125 ; Set OneShot125 flag
  2760. ; Test whether signal is OneShot42
  2761. clr Flags2.RCP_ONESHOT42 ; Clear OneShot42 flag
  2762. mov Rcp_Outside_Range_Cnt, #0 ; Reset out of range counter
  2763. call wait100ms ; Wait for new RC pulse
  2764. clr C
  2765. mov A, Rcp_Outside_Range_Cnt ; Check how many pulses were outside normal range ("745-2235us")
  2766. subb A, #10
  2767. jc validate_rcp_start
  2768. setb Flags2.RCP_ONESHOT42 ; Set OneShot42 flag
  2769. ; Test whether signal is Multishot
  2770. clr Flags2.RCP_MULTISHOT ; Clear Multishot flag
  2771. mov Rcp_Outside_Range_Cnt, #0 ; Reset out of range counter
  2772. call wait100ms ; Wait for new RC pulse
  2773. clr C
  2774. mov A, Rcp_Outside_Range_Cnt ; Check how many pulses were outside normal range ("745-2235us")
  2775. subb A, #10
  2776. jc validate_rcp_start
  2777. setb Flags2.RCP_MULTISHOT ; Set OneShot42 flag
  2778. ; Validate RC pulse
  2779. validate_rcp_start:
  2780. call wait3ms ; Wait for new RC pulse
  2781. jb Flags2.RCP_UPDATED, ($+6) ; Is there an updated RC pulse available - proceed
  2782. ljmp init_no_signal ; Go back to detect input signal
  2783. ; Beep arm sequence start signal
  2784. clr IE_EA ; Disable all interrupts
  2785. call beep_f1 ; Signal that RC pulse is ready
  2786. call beep_f1
  2787. call beep_f1
  2788. setb IE_EA ; Enable all interrupts
  2789. call wait200ms
  2790. ; Arming sequence start
  2791. arming_start:
  2792. jnb Flags3.PGM_BIDIR, ($+5)
  2793. ajmp program_by_tx_checked ; Disable tx programming if bidirectional operation
  2794. call wait3ms
  2795. mov Temp1, #Pgm_Enable_TX_Program; Start programming mode entry if enabled
  2796. mov A, @Temp1
  2797. clr C
  2798. subb A, #1 ; Is TX programming enabled?
  2799. jnc arming_initial_arm_check ; Yes - proceed
  2800. jmp program_by_tx_checked ; No - branch
  2801. arming_initial_arm_check:
  2802. mov A, Initial_Arm ; Yes - check if it is initial arm sequence
  2803. clr C
  2804. subb A, #1 ; Is it the initial arm sequence?
  2805. jnc arming_check ; Yes - proceed
  2806. jmp program_by_tx_checked ; No - branch
  2807. arming_check:
  2808. ; Initialize flash keys to valid values
  2809. mov Flash_Key_1, #0A5h
  2810. mov Flash_Key_2, #0F1h
  2811. ; Throttle calibration and tx program entry
  2812. mov Temp8, #2 ; Set 1 seconds wait time
  2813. throttle_high_cal:
  2814. setb Flags2.RCP_FULL_RANGE ; Set range to 1000-2020us
  2815. call find_throttle_gains ; Set throttle gains
  2816. call wait100ms ; Wait for new throttle value
  2817. clr IE_EA ; Disable interrupts (freeze New_Rcp value)
  2818. clr Flags2.RCP_FULL_RANGE ; Set programmed range
  2819. call find_throttle_gains ; Set throttle gains
  2820. clr C
  2821. mov A, New_Rcp ; Load new RC pulse value
  2822. subb A, #(255/2) ; Is RC pulse above midstick?
  2823. setb IE_EA ; Enable interrupts
  2824. jc program_by_tx_checked ; No - branch
  2825. call wait1ms
  2826. clr IE_EA ; Disable all interrupts
  2827. call beep_f4
  2828. setb IE_EA ; Enable all interrupts
  2829. djnz Temp8, throttle_high_cal ; Continue to wait
  2830. call average_throttle
  2831. clr C
  2832. mov A, Temp8
  2833. mov Temp1, #Pgm_Max_Throttle ; Store
  2834. mov @Temp1, A
  2835. call wait200ms
  2836. call success_beep
  2837. throttle_low_cal_start:
  2838. mov Temp8, #10 ; Set 3 seconds wait time
  2839. throttle_low_cal:
  2840. setb Flags2.RCP_FULL_RANGE ; Set range to 1000-2020us
  2841. call find_throttle_gains ; Set throttle gains
  2842. call wait100ms
  2843. clr IE_EA ; Disable interrupts (freeze New_Rcp value)
  2844. clr Flags2.RCP_FULL_RANGE ; Set programmed range
  2845. call find_throttle_gains ; Set throttle gains
  2846. clr C
  2847. mov A, New_Rcp ; Load new RC pulse value
  2848. subb A, #(255/2) ; Below midstick?
  2849. setb IE_EA ; Enable interrupts
  2850. jnc throttle_low_cal_start ; No - start over
  2851. call wait1ms
  2852. clr IE_EA ; Disable all interrupts
  2853. call beep_f1
  2854. call wait10ms
  2855. call beep_f1
  2856. setb IE_EA ; Enable all interrupts
  2857. djnz Temp8, throttle_low_cal ; Continue to wait
  2858. call average_throttle
  2859. mov A, Temp8
  2860. add A, #3 ; Add about 1%
  2861. mov Temp1, #Pgm_Min_Throttle ; Store
  2862. mov @Temp1, A
  2863. mov Temp1, A ; Min throttle in Temp1
  2864. mov Temp2, #Pgm_Max_Throttle
  2865. mov A, @Temp2
  2866. clr C
  2867. subb A, #35 ; Subtract 35 (140us) from max throttle
  2868. jc program_by_tx_entry_limit
  2869. subb A, Temp1 ; Subtract min from max
  2870. jnc program_by_tx_entry_store
  2871. program_by_tx_entry_limit:
  2872. mov A, Temp1 ; Load min
  2873. add A, #35 ; Make max 140us higher than min
  2874. mov Temp1, #Pgm_Max_Throttle ; Store new max
  2875. mov @Temp1, A
  2876. program_by_tx_entry_store:
  2877. call wait200ms
  2878. call erase_and_store_all_in_eeprom
  2879. call success_beep_inverted
  2880. program_by_tx_entry_wait:
  2881. call wait100ms
  2882. call find_throttle_gains ; Set throttle gains
  2883. ajmp init_no_signal ; Go back
  2884. program_by_tx_checked:
  2885. ; Initialize flash keys to invalid values
  2886. mov Flash_Key_1, #0
  2887. mov Flash_Key_2, #0
  2888. call wait100ms ; Wait for new throttle value
  2889. mov Temp1, #1 ; Default stop value
  2890. jnb Flags3.PGM_BIDIR, ($+5) ; No - branch
  2891. mov Temp1, #5 ; Higher stop value for bidirectional
  2892. clr C
  2893. mov A, New_Rcp ; Load new RC pulse value
  2894. subb A, Temp1 ; Below stop?
  2895. jc arm_end_beep ; Yes - proceed
  2896. jmp arming_start ; No - start over
  2897. arm_end_beep:
  2898. ; Beep arm sequence end signal
  2899. clr IE_EA ; Disable all interrupts
  2900. call beep_f4 ; Signal that rcpulse is ready
  2901. call beep_f4
  2902. call beep_f4
  2903. setb IE_EA ; Enable all interrupts
  2904. call wait200ms
  2905. ; Clear initial arm variable
  2906. mov Initial_Arm, #0
  2907. ; Armed and waiting for power on
  2908. wait_for_power_on:
  2909. clr A
  2910. mov Power_On_Wait_Cnt_L, A ; Clear wait counter
  2911. mov Power_On_Wait_Cnt_H, A
  2912. wait_for_power_on_loop:
  2913. inc Power_On_Wait_Cnt_L ; Increment low wait counter
  2914. mov A, Power_On_Wait_Cnt_L
  2915. cpl A
  2916. jnz wait_for_power_on_no_beep; Counter wrapping (about 3 sec)
  2917. inc Power_On_Wait_Cnt_H ; Increment high wait counter
  2918. mov Temp1, #Pgm_Beacon_Delay
  2919. mov A, @Temp1
  2920. mov Temp1, #25 ; Approximately 1 min
  2921. dec A
  2922. jz beep_delay_set
  2923. mov Temp1, #50 ; Approximately 2 min
  2924. dec A
  2925. jz beep_delay_set
  2926. mov Temp1, #125 ; Approximately 5 min
  2927. dec A
  2928. jz beep_delay_set
  2929. mov Temp1, #250 ; Approximately 10 min
  2930. dec A
  2931. jz beep_delay_set
  2932. mov Power_On_Wait_Cnt_H, #0 ; Reset counter for infinite delay
  2933. beep_delay_set:
  2934. clr C
  2935. mov A, Power_On_Wait_Cnt_H
  2936. subb A, Temp1 ; Check against chosen delay
  2937. jc wait_for_power_on_no_beep; Has delay elapsed?
  2938. call switch_power_off ; Switch power off in case braking is set
  2939. call wait1ms
  2940. dec Power_On_Wait_Cnt_H ; Decrement high wait counter
  2941. mov Power_On_Wait_Cnt_L, #0 ; Set low wait counter
  2942. mov Temp1, #Pgm_Beacon_Strength
  2943. mov Beep_Strength, @Temp1
  2944. clr IE_EA ; Disable all interrupts
  2945. call beep_f4 ; Signal that there is no signal
  2946. setb IE_EA ; Enable all interrupts
  2947. mov Temp1, #Pgm_Beep_Strength
  2948. mov Beep_Strength, @Temp1
  2949. call wait100ms ; Wait for new RC pulse to be measured
  2950. wait_for_power_on_no_beep:
  2951. call wait10ms
  2952. mov A, Rcp_Timeout_Cntd ; Load RC pulse timeout counter value
  2953. jnz wait_for_power_on_not_missing ; If it is not zero - proceed
  2954. jmp init_no_signal ; If pulses missing - go back to detect input signal
  2955. wait_for_power_on_not_missing:
  2956. mov Temp1, #1
  2957. clr C
  2958. mov A, New_Rcp ; Load new RC pulse value
  2959. subb A, Temp1 ; Higher than stop (plus some hysteresis)?
  2960. jc wait_for_power_on_loop ; No - start over
  2961. lcall wait100ms ; Wait to see if start pulse was only a glitch
  2962. mov A, Rcp_Timeout_Cntd ; Load RC pulse timeout counter value
  2963. jnz ($+5) ; If it is not zero - proceed
  2964. ljmp init_no_signal ; If it is zero (pulses missing) - go back to detect input signal
  2965. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2966. ;
  2967. ; Start entry point
  2968. ;
  2969. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2970. init_start:
  2971. clr IE_EA
  2972. call switch_power_off
  2973. clr A
  2974. setb IE_EA
  2975. clr A
  2976. mov Adc_Conversion_Cnt, A
  2977. mov Flags0, A ; Clear flags0
  2978. mov Flags1, A ; Clear flags1
  2979. mov Demag_Detected_Metric, A ; Clear demag metric
  2980. ;**** **** **** **** ****
  2981. ; Motor start beginning
  2982. ;**** **** **** **** ****
  2983. mov Adc_Conversion_Cnt, #8 ; Make sure a temp reading is done
  2984. call wait1ms
  2985. call start_adc_conversion
  2986. read_initial_temp:
  2987. jnb ADC0CN0_ADINT, read_initial_temp
  2988. Read_Adc_Result ; Read initial temperature
  2989. mov A, Temp2
  2990. jnz ($+3) ; Is reading below 256?
  2991. mov Temp1, A ; Yes - set average temperature value to zero
  2992. mov Current_Average_Temp, Temp1 ; Set initial average temperature
  2993. call check_temp_voltage_and_limit_power
  2994. mov Adc_Conversion_Cnt, #8 ; Make sure a temp reading is done next time
  2995. ; Set up start operating conditions
  2996. clr IE_EA ; Disable interrupts
  2997. mov Pwm_Limit, #0FFh ; Set pwm limit to max
  2998. call set_startup_pwm
  2999. mov Pwm_Limit, Pwm_Limit_Beg
  3000. mov Pwm_Limit_By_Rpm, Pwm_Limit_Beg
  3001. setb IE_EA
  3002. ; Begin startup sequence
  3003. IF MCU_48MHZ == 1
  3004. Set_MCU_Clk_48MHz
  3005. ENDIF
  3006. jnb Flags3.PGM_BIDIR, init_start_bidir_done ; Check if bidirectional operation
  3007. clr Flags3.PGM_DIR_REV ; Set spinning direction. Default fwd
  3008. jnb Flags2.RCP_DIR_REV, ($+5) ; Check force direction
  3009. setb Flags3.PGM_DIR_REV ; Set spinning direction
  3010. init_start_bidir_done:
  3011. setb Flags1.STARTUP_PHASE ; Set startup phase flag
  3012. mov Startup_Cnt, #0 ; Reset counter
  3013. call comm5comm6 ; Initialize commutation
  3014. call comm6comm1
  3015. call initialize_timing ; Initialize timing
  3016. call calc_next_comm_timing ; Set virtual commutation point
  3017. call initialize_timing ; Initialize timing
  3018. call calc_next_comm_timing
  3019. call initialize_timing ; Initialize timing
  3020. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3021. ;
  3022. ; Run entry point
  3023. ;
  3024. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3025. ; Run 1 = B(p-on) + C(n-pwm) - comparator A evaluated
  3026. ; Out_cA changes from low to high
  3027. run1:
  3028. call wait_for_comp_out_high ; Wait for high
  3029. ; setup_comm_wait ; Setup wait time from zero cross to commutation
  3030. ; evaluate_comparator_integrity ; Check whether comparator reading has been normal
  3031. call wait_for_comm ; Wait from zero cross to commutation
  3032. call comm1comm2 ; Commutate
  3033. call calc_next_comm_timing ; Calculate next timing and wait advance timing wait
  3034. ; wait_advance_timing ; Wait advance timing and start zero cross wait
  3035. ; calc_new_wait_times
  3036. ; wait_before_zc_scan ; Wait zero cross wait and start zero cross timeout
  3037. ; Run 2 = A(p-on) + C(n-pwm) - comparator B evaluated
  3038. ; Out_cB changes from high to low
  3039. run2:
  3040. call wait_for_comp_out_low
  3041. ; setup_comm_wait
  3042. ; evaluate_comparator_integrity
  3043. jb Flags1.HIGH_RPM, ($+6) ; Skip if high rpm
  3044. lcall set_pwm_limit_low_rpm
  3045. jnb Flags1.HIGH_RPM, ($+6) ; Do if high rpm
  3046. lcall set_pwm_limit_high_rpm
  3047. call wait_for_comm
  3048. call comm2comm3
  3049. call calc_next_comm_timing
  3050. ; wait_advance_timing
  3051. ; calc_new_wait_times
  3052. ; wait_before_zc_scan
  3053. ; Run 3 = A(p-on) + B(n-pwm) - comparator C evaluated
  3054. ; Out_cC changes from low to high
  3055. run3:
  3056. call wait_for_comp_out_high
  3057. ; setup_comm_wait
  3058. ; evaluate_comparator_integrity
  3059. call wait_for_comm
  3060. call comm3comm4
  3061. call calc_next_comm_timing
  3062. ; wait_advance_timing
  3063. ; calc_new_wait_times
  3064. ; wait_before_zc_scan
  3065. ; Run 4 = C(p-on) + B(n-pwm) - comparator A evaluated
  3066. ; Out_cA changes from high to low
  3067. run4:
  3068. call wait_for_comp_out_low
  3069. ; setup_comm_wait
  3070. ; evaluate_comparator_integrity
  3071. call wait_for_comm
  3072. call comm4comm5
  3073. call calc_next_comm_timing
  3074. ; wait_advance_timing
  3075. ; calc_new_wait_times
  3076. ; wait_before_zc_scan
  3077. ; Run 5 = C(p-on) + A(n-pwm) - comparator B evaluated
  3078. ; Out_cB changes from low to high
  3079. run5:
  3080. call wait_for_comp_out_high
  3081. ; setup_comm_wait
  3082. ; evaluate_comparator_integrity
  3083. call wait_for_comm
  3084. call comm5comm6
  3085. call calc_next_comm_timing
  3086. ; wait_advance_timing
  3087. ; calc_new_wait_times
  3088. ; wait_before_zc_scan
  3089. ; Run 6 = B(p-on) + A(n-pwm) - comparator C evaluated
  3090. ; Out_cC changes from high to low
  3091. run6:
  3092. call start_adc_conversion
  3093. call wait_for_comp_out_low
  3094. ; setup_comm_wait
  3095. ; evaluate_comparator_integrity
  3096. call wait_for_comm
  3097. call comm6comm1
  3098. call check_temp_voltage_and_limit_power
  3099. call calc_next_comm_timing
  3100. ; wait_advance_timing
  3101. ; calc_new_wait_times
  3102. ; wait_before_zc_scan
  3103. ; Check if it is direct startup
  3104. jnb Flags1.STARTUP_PHASE, normal_run_checks
  3105. ; Set spoolup power variables
  3106. mov Pwm_Limit, Pwm_Limit_Beg ; Set initial max power
  3107. ; Check startup counter
  3108. mov Temp2, #24 ; Set nominal startup parameters
  3109. mov Temp3, #12
  3110. clr C
  3111. mov A, Startup_Cnt ; Load counter
  3112. subb A, Temp2 ; Is counter above requirement?
  3113. jc direct_start_check_rcp ; No - proceed
  3114. clr Flags1.STARTUP_PHASE ; Clear startup phase flag
  3115. setb Flags1.INITIAL_RUN_PHASE ; Set initial run phase flag
  3116. mov Initial_Run_Rot_Cntd, Temp3 ; Set initial run rotation count
  3117. mov Pwm_Limit, Pwm_Limit_Beg
  3118. mov Pwm_Limit_By_Rpm, Pwm_Limit_Beg
  3119. jmp normal_run_checks
  3120. direct_start_check_rcp:
  3121. clr C
  3122. mov A, New_Rcp ; Load new pulse value
  3123. subb A, #1 ; Check if pulse is below stop value
  3124. jc ($+5)
  3125. ljmp run1 ; Continue to run
  3126. jmp run_to_wait_for_power_on
  3127. normal_run_checks:
  3128. ; Check if it is initial run phase
  3129. jnb Flags1.INITIAL_RUN_PHASE, initial_run_phase_done ; If not initial run phase - branch
  3130. jb Flags1.DIR_CHANGE_BRAKE, initial_run_phase_done ; If a direction change - branch
  3131. ; Decrement startup rotaton count
  3132. mov A, Initial_Run_Rot_Cntd
  3133. dec A
  3134. ; Check number of initial rotations
  3135. jnz normal_run_check_startup_rot ; Branch if counter is not zero
  3136. clr Flags1.INITIAL_RUN_PHASE ; Clear initial run phase flag
  3137. setb Flags1.MOTOR_STARTED ; Set motor started
  3138. jmp run1 ; Continue with normal run
  3139. normal_run_check_startup_rot:
  3140. mov Initial_Run_Rot_Cntd, A ; Not zero - store counter
  3141. clr C
  3142. mov A, New_Rcp ; Load new pulse value
  3143. subb A, #1 ; Check if pulse is below stop value
  3144. jc ($+5)
  3145. ljmp run1 ; Continue to run
  3146. jmp run_to_wait_for_power_on
  3147. initial_run_phase_done:
  3148. ; Reset stall count
  3149. mov Stall_Cnt, #0
  3150. ; Exit run loop after a given time
  3151. mov Temp1, #250
  3152. mov Temp2, #Pgm_Brake_On_Stop
  3153. mov A, @Temp2
  3154. jz ($+4)
  3155. mov Temp1, #3 ; About 100ms before stopping when brake is set
  3156. clr C
  3157. mov A, Rcp_Stop_Cnt ; Load stop RC pulse counter low byte value
  3158. subb A, Temp1 ; Is number of stop RC pulses above limit?
  3159. jnc run_to_wait_for_power_on ; Yes, go back to wait for poweron
  3160. mov A, Rcp_Timeout_Cntd ; Load RC pulse timeout counter value
  3161. jz run_to_wait_for_power_on ; If it is zero - go back to wait for poweron
  3162. run6_check_dir:
  3163. jnb Flags3.PGM_BIDIR, run6_check_speed ; Check if bidirectional operation
  3164. jb Flags3.PGM_DIR_REV, run6_check_dir_rev ; Check if actual rotation direction
  3165. jb Flags2.RCP_DIR_REV, run6_check_dir_change ; Matches force direction
  3166. jmp run6_check_speed
  3167. run6_check_dir_rev:
  3168. jnb Flags2.RCP_DIR_REV, run6_check_dir_change
  3169. jmp run6_check_speed
  3170. run6_check_dir_change:
  3171. jb Flags1.DIR_CHANGE_BRAKE, run6_check_speed
  3172. setb Flags1.DIR_CHANGE_BRAKE ; Set brake flag
  3173. mov Pwm_Limit, Pwm_Limit_Beg ; Set max power while braking
  3174. jmp run4 ; Go back to run 4, thereby changing force direction
  3175. run6_check_speed:
  3176. mov Temp1, #0F0h ; Default minimum speed
  3177. jnb Flags1.DIR_CHANGE_BRAKE, run6_brake_done; Is it a direction change?
  3178. mov Pwm_Limit, Pwm_Limit_Beg ; Set max power while braking
  3179. mov Temp1, #20h ; Bidirectional braking termination speed
  3180. run6_brake_done:
  3181. clr C
  3182. mov A, Comm_Period4x_H ; Is Comm_Period4x more than 32ms (~1220 eRPM)?
  3183. subb A, Temp1
  3184. jnc ($+5) ; Yes - stop or turn direction
  3185. ljmp run1 ; No - go back to run 1
  3186. jnb Flags1.DIR_CHANGE_BRAKE, run_to_wait_for_power_on ; If it is not a direction change - stop
  3187. clr Flags1.DIR_CHANGE_BRAKE ; Clear brake flag
  3188. clr Flags3.PGM_DIR_REV ; Set spinning direction. Default fwd
  3189. jnb Flags2.RCP_DIR_REV, ($+5) ; Check force direction
  3190. setb Flags3.PGM_DIR_REV ; Set spinning direction
  3191. setb Flags1.INITIAL_RUN_PHASE
  3192. mov Initial_Run_Rot_Cntd, #18
  3193. mov Pwm_Limit, Pwm_Limit_Beg ; Set initial max power
  3194. jmp run1 ; Go back to run 1
  3195. run_to_wait_for_power_on_fail:
  3196. inc Stall_Cnt ; Increment stall count
  3197. mov A, New_Rcp ; Check if RCP is zero, then it is a normal stop
  3198. jz run_to_wait_for_power_on
  3199. ajmp run_to_wait_for_power_on_stall_done
  3200. run_to_wait_for_power_on:
  3201. mov Stall_Cnt, #0
  3202. run_to_wait_for_power_on_stall_done:
  3203. clr IE_EA
  3204. call switch_power_off
  3205. mov Flags0, #0 ; Clear flags0
  3206. mov Flags1, #0 ; Clear flags1
  3207. IF MCU_48MHZ == 1
  3208. Set_MCU_Clk_24MHz
  3209. ENDIF
  3210. setb IE_EA
  3211. call wait100ms ; Wait for pwm to be stopped
  3212. call switch_power_off
  3213. mov Temp1, #Pgm_Brake_On_Stop
  3214. mov A, @Temp1
  3215. jz run_to_wait_for_power_on_brake_done
  3216. AcomFET_on
  3217. BcomFET_on
  3218. CcomFET_on
  3219. run_to_wait_for_power_on_brake_done:
  3220. clr C
  3221. mov A, Stall_Cnt
  3222. subb A, #4
  3223. jc jmp_wait_for_power_on
  3224. jmp init_no_signal
  3225. jmp_wait_for_power_on:
  3226. jmp wait_for_power_on ; Go back to wait for power on
  3227. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3228. $include (BLHeliPgm.inc) ; Include source code for programming the ESC
  3229. $include (BLHeliBootLoad.inc) ; Include source code for bootloader
  3230. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3231. CSEG AT 19FDh
  3232. reset:
  3233. ljmp pgm_start
  3234. END