Fork from bluejay at github and modified for my custom ESC. I need to modify it because some mistake design on my ESC hardware.
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.

4136 lines
111 KiB

  1. $NOMOD51
  2. ;**** **** **** **** ****
  3. ;
  4. ; Bluejay digital ESC firmware for controlling brushless motors in multirotors
  5. ;
  6. ; Copyright 2020 Mathias Rasmussen
  7. ; Copyright 2011, 2012 Steffen Skaug
  8. ;
  9. ; This file is part of Bluejay.
  10. ;
  11. ; Bluejay 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. ; Bluejay 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 Bluejay. If not, see <http://www.gnu.org/licenses/>.
  23. ;
  24. ;**** **** **** **** ****
  25. ;
  26. ; Bluejay is a fork of BLHeli_S <https://github.com/bitdump/BLHeli> by Steffen Skaug.
  27. ;
  28. ; The input signal can be DShot with rates: DShot150, DShot300 and DShot600.
  29. ;
  30. ; This file is best viewed with tab width set to 5.
  31. ;
  32. ;**** **** **** **** ****
  33. ; Master clock is internal 24MHz oscillator (or 48MHz, for which the times below are halved)
  34. ; Although 24/48 are used in the code, the exact clock frequencies are 24.5MHz or 49.0 MHz
  35. ; Timer 0 (41.67ns counts) always counts up and is used for
  36. ; - RC pulse measurement
  37. ; - DShot telemetry pulse timing
  38. ; Timer 1 (41.67ns counts) always counts up and is used for
  39. ; - DShot frame sync detection
  40. ; Timer 2 (500ns counts) always counts up and is used for
  41. ; - RC pulse timeout counts and commutation times
  42. ; Timer 3 (500ns counts) always counts up and is used for
  43. ; - Commutation timeouts
  44. ; PCA0 (41.67ns counts) always counts up and is used for
  45. ; - Hardware PWM generation
  46. ;
  47. ;**** **** **** **** ****
  48. ; Motor control:
  49. ; - Brushless motor control with 6 states for each electrical 360 degrees
  50. ; - An advance timing of 0deg has zero cross 30deg after one commutation and 30deg before the next
  51. ; - Timing advance in this implementation is set to 15deg nominally
  52. ; - Motor pwm is always damped light (aka complementary pwm, regenerative braking)
  53. ; Motor sequence starting from zero crossing:
  54. ; - Timer wait: Wt_Comm 15deg ; Time to wait from zero cross to actual commutation
  55. ; - Timer wait: Wt_Advance 15deg ; Time to wait for timing advance. Nominal commutation point is after this
  56. ; - Timer wait: Wt_Zc_Scan 7.5deg ; Time to wait before looking for zero cross
  57. ; - Scan for zero cross 22.5deg ; Nominal, with some motor variations
  58. ;
  59. ; Motor startup:
  60. ; There is a startup phase and an initial run phase, before normal bemf commutation run begins.
  61. ;
  62. ;**** **** **** **** ****
  63. ; List of enumerated supported ESCs
  64. A_ EQU 1 ; X X RC X MC MB MA CC X X Cc Cp Bc Bp Ac Ap
  65. B_ EQU 2 ; X X RC X MC MB MA CC X X Ap Ac Bp Bc Cp Cc
  66. C_ EQU 3 ; Ac Ap MC MB MA CC X RC X X X X Cc Cp Bc Bp
  67. D_ EQU 4 ; X X RC X CC MA MC MB X X Cc Cp Bc Bp Ac Ap Com fets inverted
  68. E_ EQU 5 ; L1 L0 RC X MC MB MA CC X L2 Cc Cp Bc Bp Ac Ap A with LEDs
  69. F_ EQU 6 ; X X RC X MA MB MC CC X X Cc Cp Bc Bp Ac Ap
  70. G_ EQU 7 ; X X RC X CC MA MC MB X X Cc Cp Bc Bp Ac Ap Like D, but non-inverted com fets
  71. H_ EQU 8 ; RC X X X MA MB CC MC X Ap Bp Cp X Ac Bc Cc
  72. I_ EQU 9 ; X X RC X MC MB MA CC X X Ac Bc Cc Ap Bp Cp
  73. J_ EQU 10 ; L2 L1 L0 RC CC MB MC MA X X Cc Bc Ac Cp Bp Ap LEDs
  74. K_ EQU 11 ; X X MC X MB CC MA RC X X Ap Bp Cp Cc Bc Ac Com fets inverted
  75. L_ EQU 12 ; X X RC X CC MA MB MC X X Ac Bc Cc Ap Bp Cp
  76. M_ EQU 13 ; MA MC CC MB RC L0 X X X Cc Bc Ac Cp Bp Ap X LED
  77. N_ EQU 14 ; X X RC X MC MB MA CC X X Cp Cc Bp Bc Ap Ac
  78. O_ EQU 15 ; X X RC X CC MA MC MB X X Cc Cp Bc Bp Ac Ap Like D, but low side pwm
  79. P_ EQU 16 ; X X RC MA CC MB MC X X Cc Bc Ac Cp Bp Ap X
  80. Q_ EQU 17 ; Cp Bp Ap L1 L0 X RC X X MA MB MC CC Cc Bc Ac LEDs
  81. R_ EQU 18 ; X X RC X MC MB MA CC X X Ac Bc Cc Ap Bp Cp
  82. S_ EQU 19 ; X X RC X CC MA MC MB X X Cc Cp Bc Bp Ac Ap Like O, but com fets inverted
  83. T_ EQU 20 ; RC X MA X MB CC MC X X X Cp Bp Ap Ac Bc Cc
  84. U_ EQU 21 ; MA MC CC MB RC L0 L1 L2 X Cc Bc Ac Cp Bp Ap X Like M, but with 3 LEDs
  85. V_ EQU 22 ; Cc X RC X MC CC MB MA X Ap Ac Bp X X Bc Cp
  86. W_ EQU 23 ; RC MC MB X CC MA X X X Ap Bp Cp X X X X Tristate gate driver
  87. ;**** **** **** **** ****
  88. ; Select the port mapping to use (or unselect all for use with external batch compile file)
  89. ;ESCNO EQU A_
  90. ;**** **** **** **** ****
  91. ; Select the MCU type (or unselect for use with external batch compile file)
  92. ;MCU_48MHZ EQU 0
  93. ;**** **** **** **** ****
  94. ; Select the fet dead time (or unselect for use with external batch compile file)
  95. ;FETON_DELAY EQU 15 ; 20.4ns per step
  96. ;**** **** **** **** ****
  97. ; Select the pwm frequency (or unselect for use with external batch compile file)
  98. ;PWM_FREQ EQU 0 ; 0=24, 1=48, 2=96 kHz
  99. PWM_CENTERED EQU FETON_DELAY > 0 ; Use center aligned pwm on ESCs with dead time
  100. IF MCU_48MHZ < 2 AND PWM_FREQ < 3
  101. ; Number of bits in pwm high byte
  102. PWM_BITS_H EQU (2 + MCU_48MHZ - PWM_CENTERED - PWM_FREQ)
  103. ENDIF
  104. $include (Common.inc) ; Include common source code for EFM8BBx based ESCs
  105. ;**** **** **** **** ****
  106. ; Programming defaults
  107. DEFAULT_PGM_RPM_POWER_SLOPE EQU 9 ; 0=Off, 1..13 (Power limit factor in relation to rpm)
  108. DEFAULT_PGM_COMM_TIMING EQU 4 ; 1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=High
  109. DEFAULT_PGM_DEMAG_COMP EQU 2 ; 1=Disabled 2=Low 3=High
  110. DEFAULT_PGM_DIRECTION EQU 1 ; 1=Normal 2=Reversed 3=Bidir 4=Bidir rev
  111. DEFAULT_PGM_BEEP_STRENGTH EQU 40 ; 0..255 (BLHeli_S is 1..255)
  112. DEFAULT_PGM_BEACON_STRENGTH EQU 80 ; 0..255
  113. DEFAULT_PGM_BEACON_DELAY EQU 4 ; 1=1m 2=2m 3=5m 4=10m 5=Infinite
  114. DEFAULT_PGM_ENABLE_TEMP_PROT EQU 7 ; 0=Disabled 1=80C 2=90C 3=100C 4=110C 5=120C 6=130C 7=140C
  115. DEFAULT_PGM_BRAKE_ON_STOP EQU 0 ; 1=Enabled 0=Disabled
  116. DEFAULT_PGM_LED_CONTROL EQU 0 ; Byte for LED control. 2bits per LED, 0=Off, 1=On
  117. DEFAULT_PGM_STARTUP_POWER_MIN EQU 51 ; 0..255 => (1000..1125 Throttle): value * (1000 / 2047) + 1000
  118. DEFAULT_PGM_STARTUP_BEEP EQU 1 ; 0=Short beep, 1=Melody
  119. DEFAULT_PGM_DITHERING EQU 1 ; 0=Disabled, 1=Enabled
  120. DEFAULT_PGM_STARTUP_POWER_MAX EQU 25 ; 0..255 => (1000..2000 Throttle): Maximum startup power
  121. ;**** **** **** **** ****
  122. ; Temporary register definitions
  123. Temp1 EQU R0
  124. Temp2 EQU R1
  125. Temp3 EQU R2
  126. Temp4 EQU R3
  127. Temp5 EQU R4
  128. Temp6 EQU R5
  129. Temp7 EQU R6
  130. Temp8 EQU R7
  131. ;**** **** **** **** ****
  132. ; RAM definitions
  133. ; Bit-addressable data segment
  134. DSEG AT 20h
  135. Bit_Access: DS 1 ; MUST BE AT THIS ADDRESS. Variable at bit accessible address (for non interrupt routines)
  136. Bit_Access_Int: DS 1 ; Variable at bit accessible address (for interrupts)
  137. Flags_Startup: DS 1 ; State flags. Reset upon init_start
  138. Flag_Startup_Phase BIT Flags_Startup.0 ; Set when in startup phase
  139. Flag_Initial_Run_Phase BIT Flags_Startup.1 ; Set when in initial run phase, before synchronized run is achieved
  140. ; Note: Remaining bits must be cleared
  141. Flags1: DS 1 ; State flags. Reset upon init_start
  142. Flag_Timer3_Pending BIT Flags1.0 ; Timer 3 pending flag
  143. Flag_Demag_Detected BIT Flags1.1 ; Set when excessive demag time is detected
  144. Flag_Comp_Timed_Out BIT Flags1.2 ; Set when comparator reading timed out
  145. Flag_Motor_Running BIT Flags1.3
  146. Flag_Motor_Started BIT Flags1.4 ; Set when motor is started
  147. Flag_Dir_Change_Brake BIT Flags1.5 ; Set when braking before direction change
  148. Flag_High_Rpm BIT Flags1.6 ; Set when motor rpm is high (Comm_Period4x_H less than 2)
  149. Flag_Low_Pwm_Power BIT Flags1.7 ; Set when pwm duty cycle is below 50%
  150. Flags2: DS 1 ; State flags. NOT reset upon init_start
  151. Flag_Pgm_Dir_Rev BIT Flags2.0 ; Programmed direction. 0=normal, 1=reversed
  152. Flag_Pgm_Bidir_Rev BIT Flags2.1 ; Programmed bidirectional direction. 0=normal, 1=reversed
  153. Flag_Pgm_Bidir BIT Flags2.2 ; Programmed bidirectional operation. 0=normal, 1=bidirectional
  154. Flag_Skip_Timer2_Int BIT Flags2.3 ; Set for 48MHz MCUs when timer 2 interrupt shall be ignored
  155. Flag_Clock_At_48MHz BIT Flags2.4 ; Set if 48MHz MCUs run at 48MHz
  156. Flag_Rcp_Stop BIT Flags2.5 ; Set if the RC pulse value is zero
  157. Flag_Rcp_Dir_Rev BIT Flags2.6 ; RC pulse direction in bidirectional mode
  158. Flag_Rcp_DShot_Inverted BIT Flags2.7 ; DShot RC pulse input is inverted (and supports telemetry)
  159. Flags3: DS 1 ; State flags. NOT reset upon init_start
  160. Flag_Telemetry_Pending BIT Flags3.0 ; DShot telemetry data packet is ready to be sent
  161. Flag_Dithering BIT Flags3.1 ; Dithering enabled
  162. Flag_Had_Signal BIT Flags3.2 ; Used to detect reset after having had a valid signal
  163. Tlm_Data_L: DS 1 ; DShot telemetry data (lo byte)
  164. Tlm_Data_H: DS 1 ; DShot telemetry data (hi byte)
  165. ;**** **** **** **** ****
  166. ; Direct addressing data segment
  167. DSEG AT 30h
  168. Rcp_Outside_Range_Cnt: DS 1 ; RC pulse outside range counter (incrementing)
  169. Rcp_Timeout_Cntd: DS 1 ; RC pulse timeout counter (decrementing)
  170. Rcp_Stop_Cnt: DS 1 ; Counter for RC pulses below stop value
  171. Power_On_Wait_Cnt_L: DS 1 ; Power on wait counter (lo byte)
  172. Power_On_Wait_Cnt_H: DS 1 ; Power on wait counter (hi byte)
  173. Startup_Cnt: DS 1 ; Startup phase commutations counter (incrementing)
  174. Startup_Zc_Timeout_Cntd: DS 1 ; Startup zero cross timeout counter (decrementing)
  175. Initial_Run_Rot_Cntd: DS 1 ; Initial run rotations counter (decrementing)
  176. Startup_Stall_Cnt: DS 1 ; Counts start/run attempts that resulted in stall. Reset upon a proper stop
  177. Demag_Detected_Metric: DS 1 ; Metric used to gauge demag event frequency
  178. Demag_Pwr_Off_Thresh: DS 1 ; Metric threshold above which power is cut
  179. Low_Rpm_Pwr_Slope: DS 1 ; Sets the slope of power increase for low rpm
  180. Timer2_X: DS 1 ; Timer 2 extended byte
  181. Prev_Comm_L: DS 1 ; Previous commutation timer 2 timestamp (lo byte)
  182. Prev_Comm_H: DS 1 ; Previous commutation timer 2 timestamp (hi byte)
  183. Prev_Comm_X: DS 1 ; Previous commutation timer 2 timestamp (ext byte)
  184. Prev_Prev_Comm_L: DS 1 ; Pre-previous commutation timer 2 timestamp (lo byte)
  185. Prev_Prev_Comm_H: DS 1 ; Pre-previous commutation timer 2 timestamp (hi byte)
  186. Comm_Period4x_L: DS 1 ; Timer 2 counts between the last 4 commutations (lo byte)
  187. Comm_Period4x_H: DS 1 ; Timer 2 counts between the last 4 commutations (hi byte)
  188. Comparator_Read_Cnt: DS 1 ; Number of comparator reads done
  189. Wt_Adv_Start_L: DS 1 ; Timer 3 start point for commutation advance timing (lo byte)
  190. Wt_Adv_Start_H: DS 1 ; Timer 3 start point for commutation advance timing (hi byte)
  191. Wt_Zc_Scan_Start_L: DS 1 ; Timer 3 start point from commutation to zero cross scan (lo byte)
  192. Wt_Zc_Scan_Start_H: DS 1 ; Timer 3 start point from commutation to zero cross scan (hi byte)
  193. Wt_Zc_Tout_Start_L: DS 1 ; Timer 3 start point for zero cross scan timeout (lo byte)
  194. Wt_Zc_Tout_Start_H: DS 1 ; Timer 3 start point for zero cross scan timeout (hi byte)
  195. Wt_Comm_Start_L: DS 1 ; Timer 3 start point from zero cross to commutation (lo byte)
  196. Wt_Comm_Start_H: DS 1 ; Timer 3 start point from zero cross to commutation (hi byte)
  197. Power_Pwm_Reg_L: DS 1 ; Power pwm register setting (lo byte)
  198. Power_Pwm_Reg_H: DS 1 ; Power pwm register setting (hi byte)
  199. Damp_Pwm_Reg_L: DS 1 ; Damping pwm register setting (lo byte)
  200. Damp_Pwm_Reg_H: DS 1 ; Damping pwm register setting (hi byte)
  201. Pwm_Limit: DS 1 ; Maximum allowed pwm (8-bit)
  202. Pwm_Limit_By_Rpm: DS 1 ; Maximum allowed pwm for low or high rpm (8-bit)
  203. Pwm_Limit_Beg: DS 1 ; Initial pwm limit (8-bit)
  204. Adc_Conversion_Cnt: DS 1 ; Adc conversion counter
  205. Current_Average_Temp: DS 1 ; Current average temperature (lo byte ADC reading, assuming hi byte is 1)
  206. Temp_Prot_Limit: DS 1 ; Temperature protection limit
  207. Beep_Strength: DS 1 ; Strength of beeps
  208. Flash_Key_1: DS 1 ; Flash key one
  209. Flash_Key_2: DS 1 ; Flash key two
  210. DShot_Pwm_Thr: DS 1 ; DShot pulse width threshold value (timer 0 ticks)
  211. DShot_Timer_Preset: DS 1 ; DShot timer preset for frame sync detection (timer 1 lo byte)
  212. DShot_Frame_Start_L: DS 1 ; DShot frame start timestamp (timer 2 lo byte)
  213. DShot_Frame_Start_H: DS 1 ; DShot frame start timestamp (timer 2 hi byte)
  214. DShot_Frame_Length_Thr: DS 1 ; DShot frame length criteria (timer 2 ticks)
  215. DShot_Cmd: DS 1 ; DShot command
  216. DShot_Cmd_Cnt: DS 1 ; DShot command count
  217. ; Pulse durations for GCR encoding DShot telemetry data
  218. DShot_GCR_Pulse_Time_1: DS 1 ; Encodes binary: 1
  219. DShot_GCR_Pulse_Time_2: DS 1 ; Encodes binary: 01
  220. DShot_GCR_Pulse_Time_3: DS 1 ; Encodes binary: 001
  221. DShot_GCR_Pulse_Time_1_Tmp: DS 1
  222. DShot_GCR_Pulse_Time_2_Tmp: DS 1
  223. DShot_GCR_Pulse_Time_3_Tmp: DS 1
  224. DShot_GCR_Start_Delay: DS 1
  225. ;**** **** **** **** ****
  226. ; Indirect addressing data segments
  227. ISEG AT 080h ; The variables below must be in this sequence
  228. _Pgm_Gov_P_Gain: DS 1 ;
  229. Pgm_Startup_Power_Min: DS 1 ; Minimum power during startup phase
  230. Pgm_Startup_Beep: DS 1 ; Startup beep melody on/off
  231. Pgm_Dithering: DS 1 ; Enable dithering
  232. Pgm_Startup_Power_Max: DS 1 ; Maximum power (limit) during startup (and starting initial run phase)
  233. _Pgm_Rampup_Slope: DS 1 ;
  234. Pgm_Rpm_Power_Slope: DS 1 ; Low RPM power protection slope (factor)
  235. _Pgm_Pwm_Freq: DS 1 ; PWM frequency
  236. Pgm_Direction: DS 1 ; Rotation direction
  237. _Pgm_Input_Pol: DS 1 ; Input PWM polarity
  238. Initialized_L_Dummy: DS 1 ; Place holder
  239. Initialized_H_Dummy: DS 1 ; Place holder
  240. _Pgm_Enable_TX_Program: DS 1 ; Enable/disable value for TX programming
  241. _Pgm_Main_Rearm_Start: DS 1 ; Enable/disable re-arming main every start
  242. _Pgm_Gov_Setup_Target: DS 1 ; Main governor setup target
  243. _Pgm_Startup_Rpm: DS 1 ; Startup RPM
  244. _Pgm_Startup_Accel: DS 1 ; Startup acceleration
  245. _Pgm_Volt_Comp: DS 1 ; Voltage comp
  246. Pgm_Comm_Timing: DS 1 ; Commutation timing
  247. _Pgm_Damping_Force: DS 1 ; Damping force
  248. _Pgm_Gov_Range: DS 1 ; Governor range
  249. _Pgm_Startup_Method: DS 1 ; Startup method
  250. _Pgm_Min_Throttle: DS 1 ; Minimum throttle
  251. _Pgm_Max_Throttle: DS 1 ; Maximum throttle
  252. Pgm_Beep_Strength: DS 1 ; Beep strength
  253. Pgm_Beacon_Strength: DS 1 ; Beacon strength
  254. Pgm_Beacon_Delay: DS 1 ; Beacon delay
  255. _Pgm_Throttle_Rate: DS 1 ; Throttle rate
  256. Pgm_Demag_Comp: DS 1 ; Demag compensation
  257. _Pgm_BEC_Voltage_High: DS 1 ; BEC voltage
  258. _Pgm_Center_Throttle: DS 1 ; Center throttle (in bidirectional mode)
  259. _Pgm_Main_Spoolup_Time: DS 1 ; Main spoolup time
  260. Pgm_Enable_Temp_Prot: DS 1 ; Temperature protection enable
  261. _Pgm_Enable_Power_Prot: DS 1 ; Low RPM power protection enable
  262. _Pgm_Enable_Pwm_Input: DS 1 ; Enable PWM input signal
  263. _Pgm_Pwm_Dither: DS 1 ; Output PWM dither
  264. Pgm_Brake_On_Stop: DS 1 ; Braking when throttle is zero
  265. Pgm_LED_Control: DS 1 ; LED control
  266. ISEG AT 0B0h
  267. Stack: DS 16 ; Reserved stack space
  268. ISEG AT 0C0h
  269. Dithering_Patterns: DS 16 ; Bit patterns for pwm dithering
  270. ISEG AT 0D0h
  271. Temp_Storage: DS 48 ; Temporary storage
  272. ;**** **** **** **** ****
  273. ; EEPROM code segments
  274. ; A segment of the flash is used as "EEPROM", which is not available in SiLabs MCUs
  275. CSEG AT 1A00h
  276. EEPROM_FW_MAIN_REVISION EQU 0 ; Main revision of the firmware
  277. EEPROM_FW_SUB_REVISION EQU 9 ; Sub revision of the firmware
  278. EEPROM_LAYOUT_REVISION EQU 201 ; Revision of the EEPROM layout
  279. Eep_FW_Main_Revision: DB EEPROM_FW_MAIN_REVISION ; EEPROM firmware main revision number
  280. Eep_FW_Sub_Revision: DB EEPROM_FW_SUB_REVISION ; EEPROM firmware sub revision number
  281. Eep_Layout_Revision: DB EEPROM_LAYOUT_REVISION ; EEPROM layout revision number
  282. _Eep_Pgm_Gov_P_Gain: DB 0FFh
  283. Eep_Pgm_Startup_Power_Min: DB DEFAULT_PGM_STARTUP_POWER_MIN
  284. Eep_Pgm_Startup_Beep: DB DEFAULT_PGM_STARTUP_BEEP
  285. Eep_Pgm_Dithering: DB DEFAULT_PGM_DITHERING
  286. Eep_Pgm_Startup_Power_Max: DB DEFAULT_PGM_STARTUP_POWER_MAX
  287. _Eep_Pgm_Rampup_Slope: DB 0FFh
  288. Eep_Pgm_Rpm_Power_Slope: DB DEFAULT_PGM_RPM_POWER_SLOPE ; EEPROM copy of programmed startup power
  289. _Eep_Pgm_Pwm_Freq: DB 0FFh
  290. Eep_Pgm_Direction: DB DEFAULT_PGM_DIRECTION ; EEPROM copy of programmed rotation direction
  291. _Eep__Pgm_Input_Pol: DB 0FFh
  292. Eep_Initialized_L: DB 055h ; EEPROM initialized signature (lo byte)
  293. Eep_Initialized_H: DB 0AAh ; EEPROM initialized signature (hi byte)
  294. _Eep_Enable_TX_Program: DB 0FFh ; EEPROM TX programming enable
  295. _Eep_Main_Rearm_Start: DB 0FFh
  296. _Eep_Pgm_Gov_Setup_Target: DB 0FFh
  297. _Eep_Pgm_Startup_Rpm: DB 0FFh
  298. _Eep_Pgm_Startup_Accel: DB 0FFh
  299. _Eep_Pgm_Volt_Comp: DB 0FFh
  300. Eep_Pgm_Comm_Timing: DB DEFAULT_PGM_COMM_TIMING ; EEPROM copy of programmed commutation timing
  301. _Eep_Pgm_Damping_Force: DB 0FFh
  302. _Eep_Pgm_Gov_Range: DB 0FFh
  303. _Eep_Pgm_Startup_Method: DB 0FFh
  304. _Eep_Pgm_Min_Throttle: DB 0FFh ; EEPROM copy of programmed minimum throttle
  305. _Eep_Pgm_Max_Throttle: DB 0FFh ; EEPROM copy of programmed minimum throttle
  306. Eep_Pgm_Beep_Strength: DB DEFAULT_PGM_BEEP_STRENGTH ; EEPROM copy of programmed beep strength
  307. Eep_Pgm_Beacon_Strength: DB DEFAULT_PGM_BEACON_STRENGTH ; EEPROM copy of programmed beacon strength
  308. Eep_Pgm_Beacon_Delay: DB DEFAULT_PGM_BEACON_DELAY ; EEPROM copy of programmed beacon delay
  309. _Eep_Pgm_Throttle_Rate: DB 0FFh
  310. Eep_Pgm_Demag_Comp: DB DEFAULT_PGM_DEMAG_COMP ; EEPROM copy of programmed demag compensation
  311. _Eep_Pgm_BEC_Voltage_High: DB 0FFh
  312. _Eep_Pgm_Center_Throttle: DB 0FFh ; EEPROM copy of programmed center throttle
  313. _Eep_Pgm_Main_Spoolup_Time: DB 0FFh
  314. Eep_Pgm_Temp_Prot_Enable: DB DEFAULT_PGM_ENABLE_TEMP_PROT ; EEPROM copy of programmed temperature protection enable
  315. _Eep_Pgm_Enable_Power_Prot: DB 0FFh ; EEPROM copy of programmed low rpm power protection enable
  316. _Eep_Pgm_Enable_Pwm_Input: DB 0FFh
  317. _Eep_Pgm_Pwm_Dither: DB 0FFh
  318. Eep_Pgm_Brake_On_Stop: DB DEFAULT_PGM_BRAKE_ON_STOP ; EEPROM copy of programmed braking when throttle is zero
  319. Eep_Pgm_LED_Control: DB DEFAULT_PGM_LED_CONTROL ; EEPROM copy of programmed LED control
  320. Eep_Dummy: DB 0FFh ; EEPROM address for safety reason
  321. CSEG AT 1A60h
  322. Eep_Name: DB "Bluejay (BETA) " ; Name tag (16 Bytes)
  323. ;**** **** **** **** ****
  324. Interrupt_Table_Definition ; SiLabs interrupts
  325. CSEG AT 80h ; Code segment after interrupt vectors
  326. ;**** **** **** **** ****
  327. ; Table definitions
  328. ; Rampup pwm power (8-bit)
  329. ;STARTUP_POWER_TABLE: DB 1, 2, 3, 4, 6, 9, 12, 18, 25, 37, 50, 62, 75
  330. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  331. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  332. ;
  333. ; Macros
  334. ;
  335. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  336. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  337. DSHOT_TLM_CLOCK EQU 24500000 ; 24.5MHz
  338. DSHOT_TLM_START_DELAY EQU -(5 * 25 / 4) ; Start telemetry after 5 us (~30 us after receiving DShot cmd)
  339. DSHOT_TLM_PREDELAY EQU 7 ; 7 timer 0 ticks inherent delay
  340. IF MCU_48MHZ == 1
  341. DSHOT_TLM_CLOCK_48 EQU 49000000 ; 49MHz
  342. DSHOT_TLM_START_DELAY_48 EQU -(16 * 49 / 4) ; Start telemetry after 16 us (~30 us after receiving DShot cmd)
  343. DSHOT_TLM_PREDELAY_48 EQU 11 ; 11 timer 0 ticks inherent delay
  344. ENDIF
  345. Set_DShot_Tlm_Bitrate MACRO rate
  346. mov DShot_GCR_Pulse_Time_1, #(DSHOT_TLM_PREDELAY - (1 * DSHOT_TLM_CLOCK / 4 / rate))
  347. mov DShot_GCR_Pulse_Time_2, #(DSHOT_TLM_PREDELAY - (2 * DSHOT_TLM_CLOCK / 4 / rate))
  348. mov DShot_GCR_Pulse_Time_3, #(DSHOT_TLM_PREDELAY - (3 * DSHOT_TLM_CLOCK / 4 / rate))
  349. mov DShot_GCR_Start_Delay, #DSHOT_TLM_START_DELAY
  350. IF MCU_48MHZ == 1
  351. mov DShot_GCR_Pulse_Time_1_Tmp, #(DSHOT_TLM_PREDELAY_48 - (1 * DSHOT_TLM_CLOCK_48 / 4 / rate))
  352. mov DShot_GCR_Pulse_Time_2_Tmp, #(DSHOT_TLM_PREDELAY_48 - (2 * DSHOT_TLM_CLOCK_48 / 4 / rate))
  353. mov DShot_GCR_Pulse_Time_3_Tmp, #(DSHOT_TLM_PREDELAY_48 - (3 * DSHOT_TLM_CLOCK_48 / 4 / rate))
  354. ENDIF
  355. ENDM
  356. ; DShot GCR encoding, adjust time by adding to previous item
  357. GCR_Add_Time MACRO reg
  358. mov B, @reg
  359. mov A, DShot_GCR_Pulse_Time_2
  360. cjne A, B, ($+5)
  361. mov A, DShot_GCR_Pulse_Time_3
  362. mov @reg, A
  363. ENDM
  364. ; Prepare telemetry packet while waiting for timer 3 to wrap
  365. Wait_For_Timer3 MACRO
  366. LOCAL wait_for_t3 done_waiting
  367. jb Flag_Telemetry_Pending, wait_for_t3
  368. jnb Flag_Timer3_Pending, done_waiting
  369. call dshot_tlm_create_packet
  370. wait_for_t3:
  371. jnb Flag_Timer3_Pending, done_waiting
  372. sjmp wait_for_t3
  373. done_waiting:
  374. ENDM
  375. ; Used for subdividing the DShot telemetry routine into chunks,
  376. ; that will return if timer 3 has wrapped
  377. Early_Return_Packet_Stage MACRO num
  378. Early_Return_Packet_Stage_ num, %(num + 1)
  379. ENDM
  380. Early_Return_Packet_Stage_ MACRO num next
  381. IF num > 0
  382. inc Temp5 ;; Increment current packet stage
  383. jb Flag_Timer3_Pending, dshot_packet_stage_&num ;; Return early if timer 3 has wrapped
  384. pop PSW
  385. ret
  386. dshot_packet_stage_&num:
  387. ENDIF
  388. IF num < 5
  389. cjne Temp5, #(num), dshot_packet_stage_&next ;; If this is not current stage, skip to next
  390. ENDIF
  391. ENDM
  392. Decode_DShot_2Bit MACRO dest, decode_fail
  393. movx A, @Temp1
  394. mov Temp7, A
  395. clr C
  396. subb A, Temp6 ;; Subtract previous timestamp
  397. clr C
  398. subb A, Temp2
  399. jc decode_fail ;; Check that bit is longer than minimum
  400. subb A, Temp2 ;; Check if bit is zero or one
  401. rlca dest ;; Shift bit into data byte
  402. inc Temp1 ;; Next bit
  403. movx A, @Temp1
  404. mov Temp6, A
  405. clr C
  406. subb A, Temp7
  407. clr C
  408. subb A, Temp2
  409. jc decode_fail
  410. subb A, Temp2
  411. rlca dest
  412. inc Temp1
  413. ENDM
  414. ;**** **** **** **** ****
  415. ; Compound instructions for convenience
  416. xcha MACRO var1, var2 ;; Exchange via accumulator
  417. mov A, var1
  418. xch A, var2
  419. mov var1, A
  420. ENDM
  421. rrca MACRO var ;; Rotate right through carry via accumulator
  422. mov A, var
  423. rrc A
  424. mov var, A
  425. ENDM
  426. rlca MACRO var ;; Rotate left through carry via accumulator
  427. mov A, var
  428. rlc A
  429. mov var, A
  430. ENDM
  431. rla MACRO var ;; Rotate left via accumulator
  432. mov A, var
  433. rl A
  434. mov var, A
  435. ENDM
  436. ljc MACRO label ;; Long jump if carry set
  437. LOCAL skip
  438. jnc skip
  439. jmp label
  440. skip:
  441. ENDM
  442. imov MACRO reg, val ;; Increment pointer register and move
  443. inc reg
  444. mov @reg, val ;; Write value to memory address pointed to by register
  445. ENDM
  446. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  447. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  448. ;
  449. ; Interrupt handlers
  450. ;
  451. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  452. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  453. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  454. ;
  455. ; Timer 0 interrupt routine (High priority)
  456. ;
  457. ; Generate DShot telemetry signal
  458. ;
  459. ; Requirements:
  460. ; - Must NOT be called while Flag_Telemetry_Pending is cleared
  461. ; - Must NOT write to Temp5, Temp8
  462. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  463. t0_int:
  464. push PSW
  465. mov PSW, #10h ; Select register bank 2 for this interrupt
  466. dec Temp1
  467. cjne Temp1, #(Temp_Storage - 1), t0_int_dshot_tlm_transition
  468. inc Temp1 ; Set pointer to uncritical position
  469. ; If last pulse is high, telemetry is finished,
  470. ; otherwise wait for it to return to high
  471. jb RTX_PORT.RTX_PIN, t0_int_dshot_tlm_finish
  472. t0_int_dshot_tlm_transition:
  473. cpl RTX_PORT.RTX_PIN ; Invert signal level
  474. mov TL0, @Temp1 ; Schedule next update
  475. pop PSW
  476. reti
  477. t0_int_dshot_tlm_finish:
  478. ; Configure RTX_PIN for digital input
  479. anl RTX_MDOUT, #(NOT (1 SHL RTX_PIN)) ; Set RTX_PIN output mode to open-drain
  480. setb RTX_PORT.RTX_PIN ; Float high
  481. clr IE_ET0 ; Disable timer 0 interrupts
  482. mov CKCON0, Temp8 ; Restore regular DShot timer 0/1 clock settings
  483. mov TMOD, #0AAh ; Timer 0/1 gated by INT0/1
  484. clr TCON_IE0 ; Clear int0 pending flag
  485. clr TCON_IE1 ; Clear int1 pending flag
  486. mov TL0, #0 ; Reset timer 0 count
  487. setb IE_EX0 ; Enable int0 interrupts
  488. setb IE_EX1 ; Enable int1 interrupts
  489. Enable_PCA_Interrupt ; Enable pca interrupts
  490. clr Flag_Telemetry_Pending ; Mark that new telemetry packet may be created
  491. pop PSW
  492. reti
  493. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  494. ;
  495. ; Timer 1 interrupt routine
  496. ;
  497. ; Decode DShot frame
  498. ; Process new throttle value and update pwm registers
  499. ; Schedule DShot telemetry
  500. ;
  501. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  502. t1_int:
  503. clr IE_EX0 ; Disable int0 interrupts
  504. clr TCON_TR1 ; Stop timer 1
  505. mov TL1, DShot_Timer_Preset ; Reset sync timer
  506. push PSW
  507. mov PSW, #8h ; Select register bank 1 for this interrupt
  508. push ACC
  509. push B
  510. clr TMR2CN0_TR2 ; Timer 2 disabled
  511. mov Temp2, TMR2L ; Read timer value
  512. mov Temp3, TMR2H
  513. setb TMR2CN0_TR2 ; Timer 2 enabled
  514. ; Check frame time length
  515. clr C
  516. mov A, Temp2
  517. subb A, DShot_Frame_Start_L
  518. mov Temp2, A
  519. mov A, Temp3
  520. subb A, DShot_Frame_Start_H
  521. jnz t1_int_frame_fail ; Frame too long
  522. clr C
  523. mov A, Temp2
  524. subb A, DShot_Frame_Length_Thr
  525. jc t1_int_frame_fail ; Frame too short
  526. subb A, DShot_Frame_Length_Thr
  527. jnc t1_int_frame_fail ; Frame too long
  528. ; Check that correct number of pulses is received
  529. cjne Temp1, #16, t1_int_frame_fail ; Read current pointer
  530. ; Decode transmitted data
  531. mov Temp1, #0 ; Set pointer
  532. mov Temp2, DShot_Pwm_Thr ; DShot pulse width criteria
  533. mov Temp6, #0 ; Reset timestamp
  534. ; Decode DShot data Msb. Use more code space to save time (by not using loop)
  535. Decode_DShot_2Bit Temp5, t1_int_frame_fail
  536. Decode_DShot_2Bit Temp5, t1_int_frame_fail
  537. sjmp t1_int_decode_lsb
  538. t1_int_frame_fail:
  539. sjmp t1_int_outside_range
  540. t1_int_decode_lsb:
  541. ; Decode DShot data Lsb
  542. Decode_DShot_2Bit Temp4, t1_int_outside_range
  543. Decode_DShot_2Bit Temp4, t1_int_outside_range
  544. Decode_DShot_2Bit Temp4, t1_int_outside_range
  545. Decode_DShot_2Bit Temp4, t1_int_outside_range
  546. sjmp t1_int_decode_checksum
  547. t1_int_outside_range:
  548. inc Rcp_Outside_Range_Cnt
  549. mov A, Rcp_Outside_Range_Cnt
  550. jnz ($+4)
  551. dec Rcp_Outside_Range_Cnt
  552. clr C
  553. mov A, Rcp_Outside_Range_Cnt
  554. subb A, #50 ; Allow a given number of outside pulses
  555. jc t1_int_exit_timeout ; If outside limits - ignore first pulses
  556. setb Flag_Rcp_Stop ; Set pulse length to zero
  557. clr A
  558. mov DShot_Cmd, A ; Clear DShot command
  559. mov DShot_Cmd_Cnt, A ; Clear DShot command count
  560. ajmp t1_int_exit_no_tlm ; Exit without resetting timeout
  561. t1_int_exit_timeout:
  562. mov Rcp_Timeout_Cntd, #10 ; Set timeout count
  563. ajmp t1_int_exit_no_tlm
  564. t1_int_decode_checksum:
  565. ; Decode DShot data checksum
  566. Decode_DShot_2Bit Temp3, t1_int_outside_range
  567. Decode_DShot_2Bit Temp3, t1_int_outside_range
  568. ; XOR check (in inverted data, which is ok), only low nibble is considered
  569. mov A, Temp4
  570. swap A
  571. xrl A, Temp4
  572. xrl A, Temp5
  573. xrl A, Temp3
  574. jnb Flag_Rcp_DShot_Inverted, ($+4)
  575. cpl A ; Invert checksum if using inverted DShot
  576. anl A, #0Fh
  577. jnz t1_int_outside_range ; XOR check
  578. ; Invert DShot data and subtract 96 (still 12 bits)
  579. clr C
  580. mov A, Temp4
  581. cpl A
  582. mov Temp3, A
  583. subb A, #96
  584. mov Temp4, A
  585. mov A, Temp5
  586. cpl A
  587. anl A, #0Fh
  588. subb A, #0
  589. mov Temp5, A
  590. jnc t1_normal_range
  591. mov A, Temp3 ; Check for 0 or DShot command
  592. mov Temp5, #0
  593. mov Temp4, #0
  594. jz t1_normal_range
  595. mov Temp3, #0
  596. clr C ; We are in the special DShot range
  597. rrc A ; Divide by 2
  598. jnc t1_dshot_set_cmd ; Check for tlm bit set (if not telemetry, Temp3 will be zero and result in invalid command)
  599. mov Temp3, A
  600. cjne A, DShot_Cmd, t1_dshot_set_cmd
  601. inc DShot_Cmd_Cnt
  602. sjmp t1_normal_range
  603. t1_dshot_set_cmd:
  604. mov DShot_Cmd, Temp3
  605. mov DShot_Cmd_Cnt, #0
  606. t1_normal_range:
  607. ; Check for bidirectional operation (0=stop, 96-2095->fwd, 2096-4095->rev)
  608. jnb Flag_Pgm_Bidir, t1_int_not_bidir ; If not bidirectional operation - branch
  609. ; Subtract 2000 (still 12 bits)
  610. clr C
  611. mov A, Temp4
  612. subb A, #0D0h
  613. mov B, A
  614. mov A, Temp5
  615. subb A, #07h
  616. clr Flag_Rcp_Dir_Rev
  617. jc t1_int_bidir_rev_chk ; If result is negative - branch
  618. mov Temp4, B
  619. mov Temp5, A
  620. setb Flag_Rcp_Dir_Rev
  621. t1_int_bidir_rev_chk:
  622. jb Flag_Pgm_Bidir_Rev, ($+5)
  623. cpl Flag_Rcp_Dir_Rev
  624. clr C ; Multiply throttle value by 2
  625. rlca Temp4
  626. rlca Temp5
  627. t1_int_not_bidir:
  628. ; From here Temp5/Temp4 should be at most 3999 (4095-96)
  629. mov A, Temp4 ; Divide by 16 (12 to 8-bit)
  630. anl A, #0F0h
  631. orl A, Temp5 ; Note: Assumes Temp5 to be 4-bit
  632. swap A
  633. mov B, #5 ; Divide by 5 (80 in total)
  634. div AB
  635. mov Temp3, A
  636. ; Align to 11 bits
  637. ;clr C ; Note: Cleared by div
  638. rrca Temp5
  639. mov A, Temp4
  640. rrc A
  641. ; Scale from 2000 to 2048
  642. add A, Temp3
  643. mov Temp4, A
  644. mov A, Temp5
  645. addc A, #0
  646. mov Temp5, A
  647. jnb ACC.3, ($+7) ; Limit to 11-bit maximum
  648. mov Temp4, #0FFh
  649. mov Temp5, #07h
  650. ; Do not boost when changing direction in bidirectional mode
  651. jb Flag_Motor_Started, t1_int_startup_boosted
  652. mov A, Flags_Startup ; Boost pwm during direct start
  653. jz t1_int_startup_boosted
  654. mov A, Temp5
  655. jnz t1_int_stall_boost ; Already more power than minimum at startup
  656. mov Temp2, #Pgm_Startup_Power_Min ; Read minimum startup power setting
  657. mov B, @Temp2
  658. clr C ; Set power to at least be minimum startup power
  659. mov A, Temp4
  660. subb A, B
  661. jnc t1_int_stall_boost
  662. mov Temp4, B
  663. t1_int_stall_boost:
  664. mov A, Startup_Stall_Cnt ; Check stall count
  665. jz t1_int_startup_boosted
  666. mov B, #40 ; Note: Stall count should be less than 6
  667. mul AB
  668. add A, Temp4 ; Add more power when failing to start motor (stalling)
  669. mov Temp4, A
  670. mov A, Temp5
  671. addc A, #0
  672. mov Temp5, A
  673. jnb ACC.3, ($+7) ; Limit to 11-bit maximum
  674. mov Temp4, #0FFh
  675. mov Temp5, #07h
  676. t1_int_startup_boosted:
  677. ; Set 8-bit value
  678. mov A, Temp4
  679. anl A, #0F8h
  680. orl A, Temp5 ; Assumes Temp5 to be 3-bit (11-bit rcp)
  681. swap A
  682. rl A
  683. mov Temp2, A
  684. jnz t1_int_rcp_not_zero
  685. mov A, Temp4 ; Only set Rcp_Stop if all all 11 bits are zero
  686. jnz t1_int_rcp_not_zero
  687. setb Flag_Rcp_Stop
  688. sjmp t1_int_zero_rcp_checked
  689. t1_int_rcp_not_zero:
  690. mov Rcp_Stop_Cnt, #0 ; Reset rcp stop counter
  691. clr Flag_Rcp_Stop ; Pulse ready
  692. t1_int_zero_rcp_checked:
  693. ; Decrement outside range counter
  694. mov A, Rcp_Outside_Range_Cnt
  695. jz ($+4)
  696. dec Rcp_Outside_Range_Cnt
  697. ; Set pwm limit
  698. clr C
  699. mov A, Pwm_Limit ; Limit to the smallest
  700. mov Temp6, A ; Store limit in Temp6
  701. subb A, Pwm_Limit_By_Rpm
  702. jc ($+4)
  703. mov Temp6, Pwm_Limit_By_Rpm
  704. ; Check against limit
  705. clr C
  706. mov A, Temp6
  707. subb A, Temp2 ; 8-bit rc pulse
  708. jnc t1_int_scale_pwm_resolution
  709. IF PWM_BITS_H == 0 ; 8-bit pwm
  710. mov A, Temp6
  711. mov Temp2, A
  712. ELSE
  713. mov A, Temp6 ; Multiply limit by 8 for 11-bit pwm
  714. mov B, #8
  715. mul AB
  716. mov Temp4, A
  717. mov Temp5, B
  718. ENDIF
  719. t1_int_scale_pwm_resolution:
  720. ; Scale pwm resolution and invert (duty cycle is defined inversely)
  721. IF PWM_BITS_H == 3 ; 11-bit pwm
  722. mov A, Temp5
  723. cpl A
  724. anl A, #7
  725. mov Temp3, A
  726. mov A, Temp4
  727. cpl A
  728. mov Temp2, A
  729. ELSEIF PWM_BITS_H == 2 ; 10-bit pwm
  730. clr C
  731. mov A, Temp5
  732. rrc A
  733. cpl A
  734. anl A, #3
  735. mov Temp3, A
  736. mov A, Temp4
  737. rrc A
  738. cpl A
  739. mov Temp2, A
  740. ELSEIF PWM_BITS_H == 1 ; 9-bit pwm
  741. mov B, Temp5
  742. mov A, Temp4
  743. mov C, B.0
  744. rrc A
  745. mov C, B.1
  746. rrc A
  747. cpl A
  748. mov Temp2, A
  749. mov A, Temp5
  750. rr A
  751. rr A
  752. cpl A
  753. anl A, #1
  754. mov Temp3, A
  755. ELSEIF PWM_BITS_H == 0 ; 8-bit pwm
  756. mov A, Temp2 ; Temp2 already 8-bit
  757. cpl A
  758. mov Temp2, A
  759. mov Temp3, #0
  760. ENDIF
  761. ; 11-bit effective dithering of 8/9/10-bit pwm
  762. IF PWM_BITS_H < 3
  763. jnb Flag_Dithering, t1_int_set_pwm
  764. mov A, Temp4 ; 11-bit low byte
  765. cpl A
  766. anl A, #((1 SHL (3 - PWM_BITS_H)) - 1); Get index into dithering pattern table
  767. add A, #Dithering_Patterns
  768. mov Temp1, A ; Reuse DShot pwm pointer since it is not currently in use.
  769. mov A, @Temp1 ; Retrieve pattern
  770. rl A ; Rotate pattern
  771. mov @Temp1, A ; Store pattern
  772. jnb ACC.0, t1_int_set_pwm ; Increment if bit is set
  773. mov A, Temp2
  774. add A, #1
  775. mov Temp2, A
  776. jnz t1_int_set_pwm
  777. IF PWM_BITS_H != 0
  778. mov A, Temp3
  779. addc A, #0
  780. mov Temp3, A
  781. jnb ACC.PWM_BITS_H, t1_int_set_pwm
  782. dec Temp3 ; Reset on overflow
  783. ENDIF
  784. dec Temp2
  785. ENDIF
  786. t1_int_set_pwm:
  787. ; Set pwm registers
  788. IF FETON_DELAY != 0
  789. clr C
  790. mov A, Temp2 ; Skew damping fet timing
  791. IF MCU_48MHZ == 0
  792. subb A, #((FETON_DELAY + 1) SHR 1)
  793. ELSE
  794. subb A, #(FETON_DELAY)
  795. ENDIF
  796. mov Temp4, A
  797. mov A, Temp3
  798. subb A, #0
  799. mov Temp5, A
  800. jnc t1_int_set_pwm_damp_set
  801. clr A ; Set to minimum value
  802. mov Temp4, A
  803. mov Temp5, A
  804. t1_int_set_pwm_damp_set:
  805. ENDIF
  806. mov Power_Pwm_Reg_L, Temp2
  807. mov Power_Pwm_Reg_H, Temp3
  808. IF FETON_DELAY != 0
  809. mov Damp_Pwm_Reg_L, Temp4
  810. mov Damp_Pwm_Reg_H, Temp5
  811. ENDIF
  812. mov Rcp_Timeout_Cntd, #10 ; Set timeout count
  813. ; Prepare DShot telemetry
  814. jnb Flag_Rcp_DShot_Inverted, t1_int_exit_no_tlm ; Only send telemetry for inverted DShot
  815. jnb Flag_Telemetry_Pending, t1_int_exit_no_tlm ; Check if telemetry packet is ready
  816. ; Prepare timer 0 for sending telemetry data
  817. mov CKCON0, #01h ; Timer 0 is system clock divided by 4
  818. mov TMOD, #0A2h ; Timer 0 runs free not gated by INT0
  819. ; Configure RTX_PIN for digital output
  820. setb RTX_PORT.RTX_PIN ; Default to high level
  821. orl RTX_MDOUT, #(1 SHL RTX_PIN) ; Set output mode to push-pull
  822. mov Temp1, #0 ; Set pointer to start
  823. ; Note: Delay must be large enough to ensure port is ready for output
  824. mov TL0, DShot_GCR_Start_Delay ; Telemetry will begin after this delay
  825. clr TCON_TF0 ; Clear timer 0 overflow flag
  826. setb IE_ET0 ; Enable timer 0 interrupts
  827. sjmp t1_int_exit_no_int
  828. t1_int_exit_no_tlm:
  829. mov Temp1, #0 ; Set pointer to start
  830. mov TL0, #0 ; Reset timer 0
  831. setb IE_EX0 ; Enable int0 interrupts
  832. setb IE_EX1 ; Enable int1 interrupts
  833. Enable_PCA_Interrupt ; Enable pca interrupts
  834. t1_int_exit_no_int:
  835. pop B ; Restore preserved registers
  836. pop ACC
  837. pop PSW
  838. reti
  839. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  840. ;
  841. ; Timer 2 interrupt routine
  842. ;
  843. ; Update RC pulse timeout and stop counters
  844. ; Happens every 32ms
  845. ; Requirements: Temp variables can NOT be used since PSW.x is not set
  846. ;
  847. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  848. t2_int:
  849. push ACC
  850. clr TMR2CN0_TF2H ; Clear interrupt flag
  851. inc Timer2_X
  852. IF MCU_48MHZ == 1
  853. jnb Flag_Clock_At_48MHz, t2_int_start ; Always run if clock is 24MHz
  854. jbc Flag_Skip_Timer2_Int, t2_int_exit ; Flag set? - Skip interrupt and clear flag
  855. t2_int_start:
  856. setb Flag_Skip_Timer2_Int ; Skip next interrupt
  857. ENDIF
  858. ; Update RC pulse timeout counter
  859. mov A, Rcp_Timeout_Cntd ; RC pulse timeout count zero?
  860. jz ($+4) ; Yes - do not decrement
  861. dec Rcp_Timeout_Cntd ; No - decrement
  862. jnb Flag_Rcp_Stop, t2_int_exit ; Exit if pulse is above stop value
  863. ; Update RC pulse stop counter
  864. inc Rcp_Stop_Cnt ; Increment stop counter
  865. mov A, Rcp_Stop_Cnt
  866. jnz ($+4) ; Branch if counter has not wrapped
  867. dec Rcp_Stop_Cnt ; Set stop counter back to max
  868. t2_int_exit:
  869. pop ACC ; Restore preserved registers
  870. reti
  871. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  872. ;
  873. ; Timer 3 interrupt routine
  874. ;
  875. ; Used for commutation timing
  876. ; Requirements: Temp variables can NOT be used since PSW.x is not set
  877. ; ACC can not be used, as it is not pushed to stack
  878. ;
  879. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  880. t3_int:
  881. clr IE_EA ; Disable all interrupts
  882. anl EIE1, #7Fh ; Disable timer 3 interrupts
  883. anl TMR3CN0, #07Fh ; Clear timer 3 interrupt flag
  884. mov TMR3RLL, #0FAh ; Set a short delay before next interrupt
  885. mov TMR3RLH, #0FFh
  886. clr Flag_Timer3_Pending ; Flag that timer has wrapped
  887. setb IE_EA ; Enable all interrupts
  888. reti
  889. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  890. ;
  891. ; Int0 interrupt routine (High priority)
  892. ;
  893. ; Read and store DShot pwm signal for decoding
  894. ;
  895. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  896. int0_int:
  897. push ACC
  898. mov A, TL0 ; Read pwm for DShot immediately
  899. mov TL1, DShot_Timer_Preset ; Reset sync timer
  900. ; Temp1 in register bank 1 points to pwm timings
  901. push PSW
  902. mov PSW, #8h
  903. movx @Temp1, A ; Store pwm
  904. inc Temp1
  905. pop PSW
  906. pop ACC
  907. reti
  908. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  909. ;
  910. ; Int1 interrupt routine
  911. ;
  912. ; Used for RC pulse timing
  913. ;
  914. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  915. int1_int:
  916. clr IE_EX1 ; Disable int1 interrupts
  917. setb TCON_TR1 ; Start timer 1
  918. clr TMR2CN0_TR2 ; Timer 2 disabled
  919. mov DShot_Frame_Start_L, TMR2L ; Read timer value
  920. mov DShot_Frame_Start_H, TMR2H
  921. setb TMR2CN0_TR2 ; Timer 2 enabled
  922. reti
  923. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  924. ;
  925. ; PCA interrupt routine
  926. ;
  927. ; Update pwm registers according to PCA clock signal
  928. ;
  929. ; Requirements: Temp variables can NOT be used since PSW.x is not set
  930. ;
  931. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  932. pca_int:
  933. clr IE_EA ; Disable all interrupts
  934. push ACC
  935. IF FETON_DELAY != 0 ; HI/LO enable style drivers
  936. mov A, PCA0L ; Read low byte first, to transfer high byte to holding register
  937. mov A, PCA0H
  938. jnb Flag_Low_Pwm_Power, pca_int_hi_pwm
  939. ; Power below 50%, update pca in the 0x00-0x0F range
  940. jb ACC.PWM_BITS_H, pca_int_exit ; PWM edge selection bit (continue if up edge)
  941. sjmp pca_int_set_pwm
  942. pca_int_hi_pwm:
  943. ; Power above 50%, update pca in the 0x20-0x2F range
  944. jnb ACC.PWM_BITS_H, pca_int_exit ; PWM edge selection bit (continue if down edge)
  945. pca_int_set_pwm:
  946. IF PWM_BITS_H != 0
  947. jb ACC.(PWM_BITS_H-1), pca_int_exit
  948. ELSE
  949. mov A, PCA0L
  950. jb ACC.7, pca_int_exit
  951. ENDIF
  952. ENDIF
  953. ; Set power pwm auto-reload registers
  954. IF PWM_BITS_H != 0
  955. mov PCA0_POWER_L, Power_Pwm_Reg_L
  956. mov PCA0_POWER_H, Power_Pwm_Reg_H
  957. ELSE
  958. mov PCA0_POWER_H, Power_Pwm_Reg_L
  959. ENDIF
  960. IF FETON_DELAY != 0
  961. ; Set damp pwm auto-reload registers
  962. IF PWM_BITS_H != 0
  963. mov PCA0_DAMP_L, Damp_Pwm_Reg_L
  964. mov PCA0_DAMP_H, Damp_Pwm_Reg_H
  965. ELSE
  966. mov PCA0_DAMP_H, Damp_Pwm_Reg_L
  967. ENDIF
  968. ENDIF
  969. setb Flag_Low_Pwm_Power
  970. IF PWM_BITS_H != 0
  971. mov A, Power_Pwm_Reg_H
  972. jb ACC.(PWM_BITS_H - 1), ($+5)
  973. ELSE
  974. mov A, Power_Pwm_Reg_L
  975. jb ACC.7, ($+5)
  976. ENDIF
  977. clr Flag_Low_Pwm_Power
  978. Disable_COVF_Interrupt
  979. IF FETON_DELAY == 0 ; EN/PWM style drivers
  980. Disable_CCF_Interrupt
  981. ENDIF
  982. anl EIE1, #0EFh ; Pwm updated, disable pca interrupts
  983. pca_int_exit:
  984. Clear_COVF_Interrupt
  985. IF FETON_DELAY == 0
  986. Clear_CCF_Interrupt
  987. ENDIF
  988. pop ACC ; Restore preserved registers
  989. setb IE_EA ; Enable all interrupts
  990. reti
  991. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  992. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  993. ;
  994. ; Misc utility functions
  995. ;
  996. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  997. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  998. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  999. ;
  1000. ; Wait a number of milliseconds (Multiple entry points)
  1001. ;
  1002. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1003. wait1ms:
  1004. mov Temp2, #1
  1005. sjmp wait_ms_o
  1006. wait5ms:
  1007. mov Temp2, #5
  1008. sjmp wait_ms_o
  1009. wait10ms:
  1010. mov Temp2, #10
  1011. sjmp wait_ms_o
  1012. wait100ms:
  1013. mov Temp2, #100
  1014. sjmp wait_ms_o
  1015. wait200ms:
  1016. mov Temp2, #200
  1017. sjmp wait_ms_o
  1018. wait250ms:
  1019. mov Temp2, #250
  1020. sjmp wait_ms_o
  1021. wait_ms_o: ; Outer loop
  1022. mov Temp1, #23
  1023. wait_ms_m: ; Middle loop
  1024. clr A
  1025. djnz ACC, $ ; Inner loop (41.8us - 1024 cycles)
  1026. djnz Temp1, wait_ms_m
  1027. djnz Temp2, wait_ms_o
  1028. ret
  1029. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1030. ;
  1031. ; Beeper routines (Multiple entry points)
  1032. ;
  1033. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1034. beep_f1:
  1035. mov Temp3, #66 ; Off wait loop length (Tone)
  1036. mov Temp4, #(3500 / 66) ; Number of beep pulses (Duration)
  1037. sjmp beep
  1038. beep_f2:
  1039. mov Temp3, #45
  1040. mov Temp4, #(3500 / 45)
  1041. sjmp beep
  1042. beep_f3:
  1043. mov Temp3, #38
  1044. mov Temp4, #(3500 / 38)
  1045. sjmp beep
  1046. beep_f4:
  1047. mov Temp3, #25
  1048. mov Temp4, #(3500 / 25)
  1049. sjmp beep
  1050. beep_f5:
  1051. mov Temp3, #20
  1052. mov Temp4, #(3500 / 20)
  1053. sjmp beep
  1054. beep_f1_short:
  1055. mov Temp3, #66
  1056. mov Temp4, #(2000 / 66)
  1057. sjmp beep
  1058. beep_f2_short:
  1059. mov Temp3, #45
  1060. mov Temp4, #(2000 / 45)
  1061. sjmp beep
  1062. beep:
  1063. mov A, Beep_Strength
  1064. jnz beep_start ; Start if beep strength is not 0
  1065. ret
  1066. beep_start:
  1067. mov Temp2, #2
  1068. beep_onoff:
  1069. clr A
  1070. BcomFET_off ; BcomFET off
  1071. djnz ACC, $ ; Allow some time after comfet is turned off
  1072. BpwmFET_on ; BpwmFET on (in order to charge the driver of the BcomFET)
  1073. djnz ACC, $ ; Let the pwmfet be turned on a while
  1074. BpwmFET_off ; BpwmFET off again
  1075. djnz ACC, $ ; Allow some time after pwmfet is turned off
  1076. BcomFET_on ; BcomFET on
  1077. djnz ACC, $ ; Allow some time after comfet is turned on
  1078. mov A, Temp2 ; Turn on pwmfet
  1079. jb ACC.0, beep_apwmfet_on
  1080. ApwmFET_on
  1081. beep_apwmfet_on:
  1082. jnb ACC.0, beep_cpwmfet_on
  1083. CpwmFET_on
  1084. beep_cpwmfet_on:
  1085. mov A, Beep_Strength ; On time according to beep strength
  1086. djnz ACC, $
  1087. mov A, Temp2 ; Turn off pwmfet
  1088. jb ACC.0, beep_apwmfet_off
  1089. ApwmFET_off
  1090. beep_apwmfet_off:
  1091. jnb ACC.0, beep_cpwmfet_off
  1092. CpwmFET_off
  1093. beep_cpwmfet_off:
  1094. mov A, #150 ; Off for 25 us
  1095. djnz ACC, $
  1096. djnz Temp2, beep_onoff ; Toggle next pwmfet
  1097. mov A, Temp3
  1098. beep_off: ; Fets off loop
  1099. mov Temp1, #200
  1100. djnz Temp1, $
  1101. djnz ACC, beep_off ; Off time according to beep frequency
  1102. djnz Temp4, beep_start ; Number of beep pulses (duration)
  1103. BcomFET_off
  1104. ret
  1105. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1106. ;
  1107. ; LED control
  1108. ;
  1109. ; Controls LEDs
  1110. ;
  1111. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1112. led_control:
  1113. mov Temp1, #Pgm_LED_Control
  1114. mov A, @Temp1
  1115. mov Temp2, A
  1116. anl A, #03h
  1117. Set_LED_0
  1118. jnz led_0_done
  1119. Clear_LED_0
  1120. led_0_done:
  1121. mov A, Temp2
  1122. anl A, #0Ch
  1123. Set_LED_1
  1124. jnz led_1_done
  1125. Clear_LED_1
  1126. led_1_done:
  1127. mov A, Temp2
  1128. anl A, #030h
  1129. Set_LED_2
  1130. jnz led_2_done
  1131. Clear_LED_2
  1132. led_2_done:
  1133. mov A, Temp2
  1134. anl A, #0C0h
  1135. Set_LED_3
  1136. jnz led_3_done
  1137. Clear_LED_3
  1138. led_3_done:
  1139. ret
  1140. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1141. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1142. ;
  1143. ; Power and temperature control
  1144. ;
  1145. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1146. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1147. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1148. ;
  1149. ; Switch power off routine
  1150. ;
  1151. ; Switches all fets off
  1152. ;
  1153. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1154. switch_power_off:
  1155. All_pwmFETs_Off ; Turn off all pwm fets
  1156. All_comFETs_Off ; Turn off all commutation fets
  1157. Set_Pwms_Off
  1158. ret
  1159. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1160. ;
  1161. ; Set pwm limit low rpm
  1162. ;
  1163. ; Sets power limit for low rpm
  1164. ;
  1165. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1166. set_pwm_limit:
  1167. jb Flag_High_Rpm, set_pwm_limit_high_rpm ; If high rpm, limit pwm by rpm instead
  1168. ;set_pwm_limit_low_rpm:
  1169. ; Set pwm limit
  1170. mov Temp1, #0FFh ; Default full power
  1171. jb Flag_Startup_Phase, set_pwm_limit_low_rpm_exit ; Exit if startup phase set
  1172. mov A, Low_Rpm_Pwr_Slope ; Check if low RPM power protection is enabled
  1173. jz set_pwm_limit_low_rpm_exit ; Exit if disabled (zero)
  1174. mov A, Comm_Period4x_H
  1175. jz set_pwm_limit_low_rpm_exit ; Avoid divide by zero
  1176. mov A, #255 ; Divide 255 by Comm_Period4x_H
  1177. jnb Flag_Initial_Run_Phase, ($+5) ; More protection for initial run phase
  1178. mov A, #127
  1179. mov B, Comm_Period4x_H
  1180. div AB
  1181. mov B, Low_Rpm_Pwr_Slope ; Multiply by slope
  1182. mul AB
  1183. mov Temp1, A ; Set new limit
  1184. xch A, B
  1185. jz ($+4) ; Limit to max
  1186. mov Temp1, #0FFh
  1187. clr C
  1188. mov A, Temp1 ; Limit to min
  1189. subb A, Pwm_Limit_Beg
  1190. jnc set_pwm_limit_low_rpm_exit
  1191. mov Temp1, Pwm_Limit_Beg
  1192. set_pwm_limit_low_rpm_exit:
  1193. mov Pwm_Limit_By_Rpm, Temp1
  1194. ret
  1195. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1196. ;
  1197. ; Set pwm limit high rpm
  1198. ;
  1199. ; Sets power limit for high rpm
  1200. ;
  1201. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1202. set_pwm_limit_high_rpm:
  1203. clr C
  1204. mov A, Comm_Period4x_L
  1205. IF MCU_48MHZ == 1
  1206. subb A, #0A0h ; Limit Comm_Period to 160, which is 500k erpm
  1207. ELSE
  1208. subb A, #0E4h ; Limit Comm_Period to 228, which is 350k erpm
  1209. ENDIF
  1210. mov A, Comm_Period4x_H
  1211. subb A, #00h
  1212. mov A, Pwm_Limit_By_Rpm
  1213. jnc set_pwm_limit_high_rpm_inc_limit
  1214. dec A
  1215. sjmp set_pwm_limit_high_rpm_store
  1216. set_pwm_limit_high_rpm_inc_limit:
  1217. inc A
  1218. set_pwm_limit_high_rpm_store:
  1219. jz ($+4)
  1220. mov Pwm_Limit_By_Rpm, A
  1221. ret
  1222. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1223. ;
  1224. ; Check motor temperature and limit power
  1225. ;
  1226. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1227. check_temp_and_limit_power:
  1228. inc Adc_Conversion_Cnt ; Increment conversion counter
  1229. clr C
  1230. mov A, Adc_Conversion_Cnt ; Is conversion count equal to temp rate?
  1231. subb A, #8
  1232. jc temp_increase_pwm_limit ; No - increase pwm limit
  1233. ; Wait for ADC conversion to complete
  1234. jnb ADC0CN0_ADINT, check_temp_and_limit_power
  1235. ; Read ADC result
  1236. Read_Adc_Result
  1237. ; Stop ADC
  1238. Stop_Adc
  1239. mov Adc_Conversion_Cnt, #0 ; Yes - temperature check. Reset counter
  1240. mov A, Temp2 ; Move ADC MSB to Temp3
  1241. mov Temp3, A
  1242. mov Temp2, #Pgm_Enable_Temp_Prot ; Is temp protection enabled?
  1243. mov A, @Temp2
  1244. jz temp_check_exit ; No - branch
  1245. mov A, Temp3 ; Is temperature reading below 256?
  1246. jnz temp_average_inc_dec ; No - proceed
  1247. mov A, Current_Average_Temp ; Yes - decrement average
  1248. jz temp_average_updated ; Already zero - no change
  1249. sjmp temp_average_dec ; Decrement
  1250. temp_average_inc_dec:
  1251. clr C
  1252. mov A, Temp1 ; Check if current temperature is above or below average
  1253. subb A, Current_Average_Temp
  1254. jz temp_average_updated_load_acc ; Equal - no change
  1255. mov A, Current_Average_Temp ; Above - increment average
  1256. jnc temp_average_inc
  1257. jz temp_average_updated ; Below - decrement average if average is not already zero
  1258. temp_average_dec:
  1259. dec A ; Decrement average
  1260. sjmp temp_average_updated
  1261. temp_average_inc:
  1262. inc A ; Increment average
  1263. jz temp_average_dec
  1264. sjmp temp_average_updated
  1265. temp_average_updated_load_acc:
  1266. mov A, Current_Average_Temp
  1267. temp_average_updated:
  1268. mov Current_Average_Temp, A
  1269. clr C
  1270. subb A, Temp_Prot_Limit ; Is temperature below first limit?
  1271. jc temp_check_exit ; Yes - exit
  1272. mov Pwm_Limit, #192 ; No - limit pwm
  1273. clr C
  1274. subb A, #(TEMP_LIMIT_STEP / 2) ; Is temperature below second limit
  1275. jc temp_check_exit ; Yes - exit
  1276. mov Pwm_Limit, #128 ; No - limit pwm
  1277. clr C
  1278. subb A, #(TEMP_LIMIT_STEP / 2) ; Is temperature below third limit
  1279. jc temp_check_exit ; Yes - exit
  1280. mov Pwm_Limit, #64 ; No - limit pwm
  1281. clr C
  1282. subb A, #(TEMP_LIMIT_STEP / 2) ; Is temperature below final limit
  1283. jc temp_check_exit ; Yes - exit
  1284. mov Pwm_Limit, #0 ; No - limit pwm
  1285. temp_check_exit:
  1286. ret
  1287. temp_increase_pwm_limit:
  1288. mov A, Pwm_Limit
  1289. add A, #16 ; Increase pwm limit
  1290. jnc ($+4) ; Check if above maximum
  1291. mov A, #255 ; Set maximum value
  1292. mov Pwm_Limit, A ; Set new pwm limit
  1293. ret
  1294. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1295. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1296. ;
  1297. ; Commutation and timing
  1298. ;
  1299. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1300. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1301. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1302. ;
  1303. ; Initialize timing routine
  1304. ;
  1305. ; Part of initialization before motor start
  1306. ;
  1307. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1308. initialize_timing:
  1309. mov Comm_Period4x_L, #00h ; Set commutation period registers
  1310. mov Comm_Period4x_H, #0F0h
  1311. ret
  1312. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1313. ;
  1314. ; Calculate next commutation timing routine
  1315. ;
  1316. ; Called immediately after each commutation
  1317. ; Also sets up timer 3 to wait advance timing
  1318. ; Two entry points are used
  1319. ;
  1320. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1321. calc_next_comm_timing: ; Entry point for run phase
  1322. ; Read commutation time
  1323. clr IE_EA
  1324. clr TMR2CN0_TR2 ; Timer 2 disabled
  1325. mov Temp1, TMR2L ; Load timer 2 value
  1326. mov Temp2, TMR2H
  1327. mov Temp3, Timer2_X
  1328. jnb TMR2CN0_TF2H, ($+4) ; Check if interrupt is pending
  1329. inc Temp3 ; If it is pending, then timer has already wrapped
  1330. setb TMR2CN0_TR2 ; Timer 2 enabled
  1331. setb IE_EA
  1332. IF MCU_48MHZ == 1
  1333. clr C
  1334. rrca Temp3
  1335. rrca Temp2
  1336. rrca Temp1
  1337. ENDIF
  1338. ; Calculate this commutation time
  1339. mov Temp4, Prev_Comm_L
  1340. mov Temp5, Prev_Comm_H
  1341. mov Prev_Comm_L, Temp1 ; Store timestamp as previous commutation
  1342. mov Prev_Comm_H, Temp2
  1343. clr C
  1344. mov A, Temp1
  1345. subb A, Temp4 ; Calculate the new commutation time
  1346. mov Temp1, A
  1347. mov A, Temp2
  1348. subb A, Temp5
  1349. jb Flag_Startup_Phase, calc_next_comm_startup
  1350. IF MCU_48MHZ == 1
  1351. anl A, #7Fh
  1352. ENDIF
  1353. mov Temp2, A
  1354. jnb Flag_High_Rpm, calc_next_comm_normal ; Branch normal rpm
  1355. ajmp calc_next_comm_timing_fast ; Branch high rpm
  1356. calc_next_comm_startup:
  1357. mov Temp6, Prev_Comm_X
  1358. mov Prev_Comm_X, Temp3 ; Store extended timestamp as previous commutation
  1359. mov Temp2, A
  1360. mov A, Temp3
  1361. subb A, Temp6 ; Calculate the new extended commutation time
  1362. IF MCU_48MHZ == 1
  1363. anl A, #7Fh
  1364. ENDIF
  1365. mov Temp3, A
  1366. jz calc_next_comm_startup_no_X
  1367. mov Temp1, #0FFh
  1368. mov Temp2, #0FFh
  1369. sjmp calc_next_comm_startup_average
  1370. calc_next_comm_startup_no_X:
  1371. mov Temp7, Prev_Prev_Comm_L
  1372. mov Temp8, Prev_Prev_Comm_H
  1373. mov Prev_Prev_Comm_L, Temp4
  1374. mov Prev_Prev_Comm_H, Temp5
  1375. mov Temp1, Prev_Comm_L ; Reload this commutation time
  1376. mov Temp2, Prev_Comm_H
  1377. ; Calculate the new commutation time based upon the two last commutations (to reduce sensitivity to offset)
  1378. clr C
  1379. mov A, Temp1
  1380. subb A, Temp7
  1381. mov Temp1, A
  1382. mov A, Temp2
  1383. subb A, Temp8
  1384. mov Temp2, A
  1385. calc_next_comm_startup_average:
  1386. clr C
  1387. mov A, Comm_Period4x_H ; Average with previous and save
  1388. rrc A
  1389. mov Temp4, A
  1390. mov A, Comm_Period4x_L
  1391. rrc A
  1392. mov Temp3, A
  1393. mov A, Temp1
  1394. add A, Temp3
  1395. mov Comm_Period4x_L, A
  1396. mov A, Temp2
  1397. addc A, Temp4
  1398. mov Comm_Period4x_H, A
  1399. jnc ($+8)
  1400. mov Comm_Period4x_L, #0FFh
  1401. mov Comm_Period4x_H, #0FFh
  1402. sjmp calc_new_wait_times_setup
  1403. calc_next_comm_normal:
  1404. ; Calculate new commutation time
  1405. mov Temp3, Comm_Period4x_L ; Comm_Period4x(-l-h) holds the time of 4 commutations
  1406. mov Temp4, Comm_Period4x_H
  1407. mov Temp5, Comm_Period4x_L ; Copy variables
  1408. mov Temp6, Comm_Period4x_H
  1409. mov Temp7, #4 ; Divide Comm_Period4x 4 times as default
  1410. mov Temp8, #2 ; Divide new commutation time 2 times as default
  1411. clr C
  1412. mov A, Temp4
  1413. subb A, #04h
  1414. jc calc_next_comm_avg_period_div
  1415. dec Temp7 ; Reduce averaging time constant for low speeds
  1416. dec Temp8
  1417. clr C
  1418. mov A, Temp4
  1419. subb A, #08h
  1420. jc calc_next_comm_avg_period_div
  1421. jb Flag_Initial_Run_Phase, calc_next_comm_avg_period_div ; Do not average very fast during initial run
  1422. dec Temp7 ; Reduce averaging time constant more for even lower speeds
  1423. dec Temp8
  1424. calc_next_comm_avg_period_div:
  1425. clr C
  1426. rrca Temp6 ; Divide by 2
  1427. rrca Temp5
  1428. djnz Temp7, calc_next_comm_avg_period_div
  1429. clr C
  1430. mov A, Temp3
  1431. subb A, Temp5 ; Subtract a fraction
  1432. mov Temp3, A
  1433. mov A, Temp4
  1434. subb A, Temp6
  1435. mov Temp4, A
  1436. mov A, Temp8 ; Divide new time
  1437. jz calc_next_comm_new_period_div_done
  1438. calc_next_comm_new_period_div:
  1439. clr C
  1440. rrca Temp2 ; Divide by 2
  1441. rrca Temp1
  1442. djnz Temp8, calc_next_comm_new_period_div
  1443. calc_next_comm_new_period_div_done:
  1444. mov A, Temp3
  1445. add A, Temp1 ; Add the divided new time
  1446. mov Temp3, A
  1447. mov A, Temp4
  1448. addc A, Temp2
  1449. mov Temp4, A
  1450. mov Comm_Period4x_L, Temp3 ; Store Comm_Period4x_X
  1451. mov Comm_Period4x_H, Temp4
  1452. jnc calc_new_wait_times_setup ; If period larger than 0xffff - go to slow case
  1453. mov Temp4, #0FFh
  1454. mov Comm_Period4x_L, Temp4 ; Set commutation period registers to very slow timing (0xffff)
  1455. mov Comm_Period4x_H, Temp4
  1456. calc_new_wait_times_setup:
  1457. ; Set high rpm bit (if above 156k erpm)
  1458. clr C
  1459. mov A, Temp4
  1460. subb A, #2
  1461. jnc ($+4)
  1462. setb Flag_High_Rpm ; Set high rpm bit
  1463. ; Load programmed commutation timing
  1464. jnb Flag_Startup_Phase, calc_new_wait_per_startup_done ; Set dedicated timing during startup
  1465. mov Temp8, #3
  1466. sjmp calc_new_wait_per_demag_done
  1467. calc_new_wait_per_startup_done:
  1468. mov Temp1, #Pgm_Comm_Timing ; Load timing setting
  1469. mov A, @Temp1
  1470. mov Temp8, A ; Store in Temp8
  1471. clr C
  1472. mov A, Demag_Detected_Metric ; Check demag metric
  1473. subb A, #130
  1474. jc calc_new_wait_per_demag_done
  1475. inc Temp8 ; Increase timing
  1476. clr C
  1477. mov A, Demag_Detected_Metric
  1478. subb A, #160
  1479. jc ($+3)
  1480. inc Temp8 ; Increase timing again
  1481. clr C
  1482. mov A, Temp8 ; Limit timing to max
  1483. subb A, #6
  1484. jc ($+4)
  1485. mov Temp8, #5 ; Set timing to max
  1486. calc_new_wait_per_demag_done:
  1487. ; Set timing reduction
  1488. mov Temp7, #2
  1489. ; Load current commutation timing
  1490. mov A, Comm_Period4x_H ; Divide 4 times
  1491. swap A
  1492. anl A, #00Fh
  1493. mov Temp2, A
  1494. mov A, Comm_Period4x_H
  1495. swap A
  1496. anl A, #0F0h
  1497. mov Temp1, A
  1498. mov A, Comm_Period4x_L
  1499. swap A
  1500. anl A, #00Fh
  1501. add A, Temp1
  1502. mov Temp1, A
  1503. clr C
  1504. mov A, Temp1
  1505. subb A, Temp7
  1506. mov Temp3, A
  1507. mov A, Temp2
  1508. subb A, #0
  1509. mov Temp4, A
  1510. jc load_min_time ; Check that result is still positive
  1511. jnz calc_next_comm_timing_exit ; Check that result is still above minimum
  1512. mov A, Temp3
  1513. jnz calc_next_comm_timing_exit
  1514. load_min_time:
  1515. mov Temp3, #1 ; Set minimum time
  1516. mov Temp4, #0
  1517. sjmp calc_next_comm_timing_exit
  1518. ;**** **** **** **** ****
  1519. ; Calculate next commutation timing fast routine
  1520. ; Fast calculation (Comm_Period4x_H less than 2)
  1521. calc_next_comm_timing_fast:
  1522. ; Calculate new commutation time
  1523. mov Temp3, Comm_Period4x_L ; Comm_Period4x(-l-h) holds the time of 4 commutations
  1524. mov Temp4, Comm_Period4x_H
  1525. mov A, Temp4 ; Divide by 2 4 times
  1526. swap A
  1527. mov Temp7, A
  1528. mov A, Temp3
  1529. swap A
  1530. anl A, #0Fh
  1531. orl A, Temp7
  1532. mov Temp5, A
  1533. clr C
  1534. mov A, Temp3 ; Subtract a fraction
  1535. subb A, Temp5
  1536. mov Temp3, A
  1537. mov A, Temp4
  1538. subb A, #0
  1539. mov Temp4, A
  1540. clr C
  1541. mov A, Temp1
  1542. rrc A ; Divide by 2 2 times
  1543. clr C
  1544. rrc A
  1545. mov Temp1, A
  1546. mov A, Temp3 ; Add the divided new time
  1547. add A, Temp1
  1548. mov Temp3, A
  1549. mov A, Temp4
  1550. addc A, #0
  1551. mov Temp4, A
  1552. mov Comm_Period4x_L, Temp3 ; Store Comm_Period4x_X
  1553. mov Comm_Period4x_H, Temp4
  1554. clr C
  1555. subb A, #2 ; If erpm below 156k - go to normal case
  1556. jc ($+4)
  1557. clr Flag_High_Rpm ; Clear high rpm bit
  1558. ; Set timing reduction
  1559. mov Temp1, #2
  1560. mov A, Temp4 ; Divide by 2 4 times
  1561. swap A
  1562. mov Temp7, A
  1563. mov Temp4, #0
  1564. mov A, Temp3
  1565. swap A
  1566. anl A, #0Fh
  1567. orl A, Temp7
  1568. mov Temp3, A
  1569. clr C
  1570. subb A, Temp1
  1571. mov Temp3, A
  1572. jc load_min_time_fast ; Check that result is still positive
  1573. jnz calc_new_wait_times_fast_done ; Check that result is still above minimum
  1574. load_min_time_fast:
  1575. mov Temp3, #1 ; Set minimum time
  1576. calc_new_wait_times_fast_done:
  1577. mov Temp1, #Pgm_Comm_Timing ; Load timing setting
  1578. mov A, @Temp1
  1579. mov Temp8, A ; Store in Temp8
  1580. calc_next_comm_timing_exit:
  1581. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1582. ;
  1583. ; Wait advance timing routine
  1584. ; NOTE: Be VERY careful if using temp registers. They are passed over this routine
  1585. ;
  1586. ; Waits for the advance timing to elapse and sets up the next zero cross wait
  1587. ;
  1588. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1589. wait_advance_timing:
  1590. Wait_For_Timer3
  1591. ; Setup next wait time
  1592. mov TMR3RLL, Wt_ZC_Tout_Start_L
  1593. mov TMR3RLH, Wt_ZC_Tout_Start_H
  1594. setb Flag_Timer3_Pending
  1595. orl EIE1, #80h ; Enable timer 3 interrupts
  1596. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1597. ;
  1598. ; Calculate new wait times routine
  1599. ;
  1600. ; Calculates new wait times
  1601. ;
  1602. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1603. calc_new_wait_times:
  1604. clr C
  1605. clr A
  1606. subb A, Temp3 ; Negate
  1607. mov Temp1, A
  1608. clr A
  1609. subb A, Temp4
  1610. mov Temp2, A
  1611. IF MCU_48MHZ == 1
  1612. clr C
  1613. rlca Temp1 ; Multiply by 2
  1614. rlca Temp2
  1615. ENDIF
  1616. jb Flag_High_Rpm, calc_new_wait_times_fast ; Branch if high rpm
  1617. mov A, Temp1 ; Copy values
  1618. mov Temp3, A
  1619. mov A, Temp2
  1620. mov Temp4, A
  1621. setb C ; Negative numbers - set carry
  1622. mov A, Temp2
  1623. rrc A ; Divide by 2
  1624. mov Temp6, A
  1625. mov A, Temp1
  1626. rrc A
  1627. mov Temp5, A
  1628. mov Wt_Zc_Tout_Start_L, Temp1 ; Set 15deg time for zero cross scan timeout
  1629. mov Wt_Zc_Tout_Start_H, Temp2
  1630. clr C
  1631. mov A, Temp8 ; (Temp8 has Pgm_Comm_Timing)
  1632. subb A, #3 ; Is timing normal?
  1633. jz store_times_decrease ; Yes - branch
  1634. mov A, Temp8
  1635. jb ACC.0, adjust_timing_two_steps; If an odd number - branch
  1636. mov A, Temp1 ; Add 7.5deg and store in Temp1/2
  1637. add A, Temp5
  1638. mov Temp1, A
  1639. mov A, Temp2
  1640. addc A, Temp6
  1641. mov Temp2, A
  1642. mov A, Temp5 ; Store 7.5deg in Temp3/4
  1643. mov Temp3, A
  1644. mov A, Temp6
  1645. mov Temp4, A
  1646. sjmp store_times_up_or_down
  1647. adjust_timing_two_steps:
  1648. mov A, Temp1 ; Add 15deg and store in Temp1/2
  1649. setb C ; Add 1 to final result (Temp1/2 * 2 + 1)
  1650. addc A, Temp1
  1651. mov Temp1, A
  1652. mov A, Temp2
  1653. addc A, Temp2
  1654. mov Temp2, A
  1655. mov Temp3, #0FFh ; Store minimum time in Temp3/4
  1656. mov Temp4, #0FFh
  1657. store_times_up_or_down:
  1658. clr C
  1659. mov A, Temp8
  1660. subb A, #3 ; Is timing higher than normal?
  1661. jc store_times_decrease ; No - branch
  1662. store_times_increase:
  1663. mov Wt_Comm_Start_L, Temp3 ; Now commutation time (~60deg) divided by 4 (~15deg nominal)
  1664. mov Wt_Comm_Start_H, Temp4
  1665. mov Wt_Adv_Start_L, Temp1 ; New commutation advance time (~15deg nominal)
  1666. mov Wt_Adv_Start_H, Temp2
  1667. mov Wt_Zc_Scan_Start_L, Temp5 ; Use this value for zero cross scan delay (7.5deg)
  1668. mov Wt_Zc_Scan_Start_H, Temp6
  1669. sjmp calc_new_wait_times_exit
  1670. store_times_decrease:
  1671. mov Wt_Comm_Start_L, Temp1 ; Now commutation time (~60deg) divided by 4 (~15deg nominal)
  1672. mov Wt_Comm_Start_H, Temp2
  1673. mov Wt_Adv_Start_L, Temp3 ; New commutation advance time (~15deg nominal)
  1674. mov Wt_Adv_Start_H, Temp4
  1675. mov Wt_Zc_Scan_Start_L, Temp5 ; Use this value for zero cross scan delay (7.5deg)
  1676. mov Wt_Zc_Scan_Start_H, Temp6
  1677. ; Set very short delays for all but advance time during startup, in order to widen zero cross capture range
  1678. jnb Flag_Startup_Phase, calc_new_wait_times_exit
  1679. mov Wt_Comm_Start_L, #0F0h
  1680. mov Wt_Comm_Start_H, #0FFh
  1681. mov Wt_Zc_Scan_Start_L, #0F0h
  1682. mov Wt_Zc_Scan_Start_H, #0FFh
  1683. mov Wt_Zc_Tout_Start_L, #0F0h
  1684. mov Wt_Zc_Tout_Start_H, #0FFh
  1685. sjmp calc_new_wait_times_exit
  1686. ;**** **** **** **** ****
  1687. ; Calculate new wait times fast routine
  1688. calc_new_wait_times_fast:
  1689. mov A, Temp1 ; Copy values
  1690. mov Temp3, A
  1691. setb C ; Negative numbers - set carry
  1692. rrc A ; Divide by 2
  1693. mov Temp5, A
  1694. mov Wt_Zc_Tout_Start_L, Temp1 ; Set 15deg time for zero cross scan timeout
  1695. clr C
  1696. mov A, Temp8 ; (Temp8 has Pgm_Comm_Timing)
  1697. subb A, #3 ; Is timing normal?
  1698. jz store_times_decrease_fast ; Yes - branch
  1699. mov A, Temp8
  1700. jb ACC.0, adjust_timing_two_steps_fast ; If an odd number - branch
  1701. mov A, Temp1 ; Add 7.5deg and store in Temp1
  1702. add A, Temp5
  1703. mov Temp1, A
  1704. mov A, Temp5 ; Store 7.5deg in Temp3
  1705. mov Temp3, A
  1706. sjmp store_times_up_or_down_fast
  1707. adjust_timing_two_steps_fast:
  1708. mov A, Temp1 ; Add 15deg and store in Temp1
  1709. add A, Temp1
  1710. add A, #1
  1711. mov Temp1, A
  1712. mov Temp3, #0FFh ; Store minimum time in Temp3
  1713. store_times_up_or_down_fast:
  1714. clr C
  1715. mov A, Temp8
  1716. subb A, #3 ; Is timing higher than normal?
  1717. jc store_times_decrease_fast ; No - branch
  1718. store_times_increase_fast:
  1719. mov Wt_Comm_Start_L, Temp3 ; Now commutation time (~60deg) divided by 4 (~15deg nominal)
  1720. mov Wt_Adv_Start_L, Temp1 ; New commutation advance time (~15deg nominal)
  1721. mov Wt_Zc_Scan_Start_L, Temp5 ; Use this value for zero cross scan delay (7.5deg)
  1722. sjmp calc_new_wait_times_exit
  1723. store_times_decrease_fast:
  1724. mov Wt_Comm_Start_L, Temp1 ; Now commutation time (~60deg) divided by 4 (~15deg nominal)
  1725. mov Wt_Adv_Start_L, Temp3 ; New commutation advance time (~15deg nominal)
  1726. mov Wt_Zc_Scan_Start_L, Temp5 ; Use this value for zero cross scan delay (7.5deg)
  1727. calc_new_wait_times_exit:
  1728. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1729. ;
  1730. ; Wait before zero cross scan routine
  1731. ;
  1732. ; Waits for the zero cross scan wait time to elapse
  1733. ; Also sets up timer 3 for the zero cross scan timeout time
  1734. ;
  1735. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1736. wait_before_zc_scan:
  1737. Wait_For_Timer3
  1738. mov Startup_Zc_Timeout_Cntd, #2
  1739. setup_zc_scan_timeout:
  1740. setb Flag_Timer3_Pending
  1741. orl EIE1, #80h ; Enable timer 3 interrupts
  1742. mov A, Flags_Startup
  1743. jz wait_before_zc_scan_exit
  1744. mov Temp1, Comm_Period4x_L ; Set long timeout when starting
  1745. mov Temp2, Comm_Period4x_H
  1746. clr C
  1747. rrca Temp2
  1748. rrca Temp1
  1749. IF MCU_48MHZ == 0
  1750. clr C
  1751. rrca Temp2
  1752. rrca Temp1
  1753. ENDIF
  1754. jnb Flag_Startup_Phase, setup_zc_scan_timeout_startup_done
  1755. mov A, Temp2
  1756. add A, #40h ; Increase timeout somewhat to avoid false wind up
  1757. mov Temp2, A
  1758. setup_zc_scan_timeout_startup_done:
  1759. clr IE_EA
  1760. anl EIE1, #7Fh ; Disable timer 3 interrupts
  1761. mov TMR3CN0, #00h ; Timer 3 disabled and interrupt flag cleared
  1762. clr C
  1763. clr A
  1764. subb A, Temp1 ; Set timeout
  1765. mov TMR3L, A
  1766. clr A
  1767. subb A, Temp2
  1768. mov TMR3H, A
  1769. mov TMR3CN0, #04h ; Timer 3 enabled and interrupt flag cleared
  1770. setb Flag_Timer3_Pending
  1771. orl EIE1, #80h ; Enable timer 3 interrupts
  1772. setb IE_EA
  1773. wait_before_zc_scan_exit:
  1774. ret
  1775. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1776. ;
  1777. ; Wait for comparator to go low/high routines
  1778. ;
  1779. ; Waits for the zero cross scan wait time to elapse
  1780. ; Then scans for comparator going low/high
  1781. ;
  1782. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1783. wait_for_comp_out_low:
  1784. mov B, #00h ; Desired comparator output
  1785. jnb Flag_Dir_Change_Brake, comp_init
  1786. mov B, #40h
  1787. sjmp comp_init
  1788. wait_for_comp_out_high:
  1789. mov B, #40h ; Desired comparator output
  1790. jnb Flag_Dir_Change_Brake, comp_init
  1791. mov B, #00h
  1792. comp_init:
  1793. setb Flag_Demag_Detected ; Set demag detected flag as default
  1794. mov Comparator_Read_Cnt, #0 ; Reset number of comparator reads
  1795. comp_start:
  1796. ; Set number of comparator readings
  1797. mov Temp3, #(1 SHL MCU_48MHZ) ; Number of OK readings required
  1798. mov Temp4, #(1 SHL MCU_48MHZ) ; Max number of readings required
  1799. jb Flag_High_Rpm, comp_check_timeout ; Branch if high rpm
  1800. mov A, Flags_Startup ; Clear demag detected flag if start phases
  1801. jz ($+4)
  1802. clr Flag_Demag_Detected
  1803. jnb Flag_Startup_Phase, comp_not_startup
  1804. mov Temp3, #(27 SHL MCU_48MHZ) ; Set many samples during startup, approximately one pwm period
  1805. mov Temp4, #(27 SHL MCU_48MHZ)
  1806. sjmp comp_check_timeout
  1807. comp_not_startup:
  1808. ; Too low value (~<15) causes rough running at pwm harmonics.
  1809. ; Too high a value (~>35) causes the RCT4215 630 to run rough on full throttle
  1810. mov Temp4, #(20 SHL MCU_48MHZ)
  1811. mov A, Comm_Period4x_H ; Set number of readings higher for lower speeds
  1812. IF MCU_48MHZ == 0
  1813. clr C
  1814. rrc A
  1815. ENDIF
  1816. jnz ($+3)
  1817. inc A ; Minimum 1
  1818. mov Temp3, A
  1819. clr C
  1820. subb A, #(20 SHL MCU_48MHZ)
  1821. jc ($+4)
  1822. mov Temp3, #(20 SHL MCU_48MHZ) ; Maximum 20
  1823. comp_check_timeout:
  1824. jb Flag_Timer3_Pending, comp_check_timeout_not_timed_out ; Has zero cross scan timeout elapsed?
  1825. mov A, Comparator_Read_Cnt ; Check that comparator has been read
  1826. jz comp_check_timeout_not_timed_out ; If not read - branch
  1827. jnb Flag_Startup_Phase, comp_check_timeout_timeout_extended ; Extend timeout during startup
  1828. djnz Startup_Zc_Timeout_Cntd, comp_check_timeout_extend_timeout
  1829. comp_check_timeout_timeout_extended:
  1830. setb Flag_Comp_Timed_Out
  1831. sjmp comp_exit
  1832. comp_check_timeout_extend_timeout:
  1833. call setup_zc_scan_timeout
  1834. comp_check_timeout_not_timed_out:
  1835. inc Comparator_Read_Cnt ; Increment comparator read count
  1836. Read_Comp_Out ; Read comparator output
  1837. anl A, #40h
  1838. cjne A, B, comp_read_wrong
  1839. ; Comp read ok
  1840. mov A, Startup_Cnt ; Force a timeout for the first commutation
  1841. jz comp_start
  1842. jb Flag_Demag_Detected, comp_start ; Do not accept correct comparator output if it is demag
  1843. djnz Temp3, comp_check_timeout ; Decrement readings counter - repeat comparator reading if not zero
  1844. clr Flag_Comp_Timed_Out
  1845. sjmp comp_exit
  1846. comp_read_wrong:
  1847. jb Flag_Startup_Phase, comp_read_wrong_startup
  1848. jb Flag_Demag_Detected, comp_read_wrong_extend_timeout
  1849. inc Temp3 ; Increment number of OK readings required
  1850. clr C
  1851. mov A, Temp3
  1852. subb A, Temp4
  1853. jc comp_check_timeout ; If below initial requirement - take another reading
  1854. sjmp comp_start ; Otherwise - go back and restart
  1855. comp_read_wrong_startup:
  1856. inc Temp3 ; Increment number of OK readings required
  1857. clr C
  1858. mov A, Temp3
  1859. subb A, Temp4 ; If above initial requirement - do not increment further
  1860. jc ($+3)
  1861. dec Temp3
  1862. sjmp comp_check_timeout ; Continue to look for good ones
  1863. comp_read_wrong_extend_timeout:
  1864. clr Flag_Demag_Detected ; Clear demag detected flag
  1865. anl EIE1, #7Fh ; Disable timer 3 interrupts
  1866. mov TMR3CN0, #00h ; Timer 3 disabled and interrupt flag cleared
  1867. jnb Flag_High_Rpm, comp_read_wrong_low_rpm ; Branch if not high rpm
  1868. mov TMR3L, #00h ; Set timeout to ~1ms
  1869. mov TMR3H, #-(8 SHL MCU_48MHZ)
  1870. comp_read_wrong_timeout_set:
  1871. mov TMR3CN0, #04h ; Timer 3 enabled and interrupt flag cleared
  1872. setb Flag_Timer3_Pending
  1873. orl EIE1, #80h ; Enable timer 3 interrupts
  1874. jmp comp_start ; If comparator output is not correct - go back and restart
  1875. comp_read_wrong_low_rpm:
  1876. mov A, Comm_Period4x_H ; Set timeout to ~4x comm period 4x value
  1877. mov Temp7, #0FFh ; Default to long
  1878. IF MCU_48MHZ == 1
  1879. clr C
  1880. rlc A
  1881. jc comp_read_wrong_load_timeout
  1882. ENDIF
  1883. clr C
  1884. rlc A
  1885. jc comp_read_wrong_load_timeout
  1886. clr C
  1887. rlc A
  1888. jc comp_read_wrong_load_timeout
  1889. mov Temp7, A
  1890. comp_read_wrong_load_timeout:
  1891. clr C
  1892. clr A
  1893. subb A, Temp7
  1894. mov TMR3L, #0
  1895. mov TMR3H, A
  1896. sjmp comp_read_wrong_timeout_set
  1897. comp_exit:
  1898. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1899. ;
  1900. ; Setup commutation timing routine
  1901. ;
  1902. ; Sets up and starts wait from commutation to zero cross
  1903. ;
  1904. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1905. setup_comm_wait:
  1906. clr IE_EA
  1907. anl EIE1, #7Fh ; Disable timer 3 interrupts
  1908. mov TMR3CN0, #00h ; Timer 3 disabled and interrupt flag cleared
  1909. mov TMR3L, Wt_Comm_Start_L
  1910. mov TMR3H, Wt_Comm_Start_H
  1911. mov TMR3CN0, #04h ; Timer 3 enabled and interrupt flag cleared
  1912. ; Setup next wait time
  1913. mov TMR3RLL, Wt_Adv_Start_L
  1914. mov TMR3RLH, Wt_Adv_Start_H
  1915. setb Flag_Timer3_Pending
  1916. orl EIE1, #80h ; Enable timer 3 interrupts
  1917. setb IE_EA ; Enable interrupts again
  1918. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1919. ;
  1920. ; Evaluate comparator integrity
  1921. ;
  1922. ; Checks comparator signal behavior versus expected behavior
  1923. ;
  1924. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1925. evaluate_comparator_integrity:
  1926. jb Flag_Startup_Phase, eval_comp_startup ; Do not exit run mode during startup phases
  1927. jnb Flag_Comp_Timed_Out, eval_comp_exit ; Has timeout elapsed?
  1928. jb Flag_Initial_Run_Phase, eval_comp_exit ; Do not exit run mode if initial run phase
  1929. jb Flag_Dir_Change_Brake, eval_comp_exit ; Do not exit run mode if braking
  1930. jb Flag_Demag_Detected, eval_comp_exit ; Do not exit run mode if it is a demag situation
  1931. dec SP ; Routine exit without "ret" command
  1932. dec SP
  1933. ljmp run_to_wait_for_power_on_fail ; Exit run mode if timeout has elapsed
  1934. eval_comp_startup:
  1935. inc Startup_Cnt ; Increment startup counter
  1936. eval_comp_exit:
  1937. ret
  1938. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1939. ;
  1940. ; Wait for commutation routine
  1941. ;
  1942. ; Waits from zero cross to commutation
  1943. ;
  1944. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1945. wait_for_comm:
  1946. ; Update demag metric
  1947. mov A, Demag_Detected_Metric ; Sliding average of 8, 256 when demag and 0 when not. Limited to minimum 120
  1948. mov B, #7
  1949. mul AB ; Multiply by 7
  1950. jnb Flag_Demag_Detected, ($+4) ; Add new value for current demag status
  1951. inc B
  1952. mov C, B.0 ; Divide by 8
  1953. rrc A
  1954. mov C, B.1
  1955. rrc A
  1956. mov C, B.2
  1957. rrc A
  1958. mov Demag_Detected_Metric, A
  1959. clr C
  1960. subb A, #120 ; Limit to minimum 120
  1961. jnc ($+5)
  1962. mov Demag_Detected_Metric, #120
  1963. clr C
  1964. mov A, Demag_Detected_Metric ; Check demag metric
  1965. subb A, Demag_Pwr_Off_Thresh
  1966. jc wait_for_comm_wait ; Cut power if many consecutive demags. This will help retain sync during hard accelerations
  1967. All_pwmFETs_off
  1968. Set_Pwms_Off
  1969. wait_for_comm_wait:
  1970. Wait_For_Timer3
  1971. ; Setup next wait time
  1972. mov TMR3RLL, Wt_Zc_Scan_Start_L
  1973. mov TMR3RLH, Wt_Zc_Scan_Start_H
  1974. setb Flag_Timer3_Pending
  1975. orl EIE1, #80h ; Enable timer 3 interrupts
  1976. ret
  1977. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1978. ;
  1979. ; Commutation routines
  1980. ;
  1981. ; Performs commutation switching
  1982. ;
  1983. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1984. ; Comm phase 1 to comm phase 2
  1985. comm1_comm2: ; C->A
  1986. Set_RPM_Out
  1987. jb Flag_Pgm_Dir_Rev, comm1_comm2_rev
  1988. clr IE_EA ; Disable all interrupts
  1989. BcomFET_off ; Turn off comfet
  1990. AcomFET_on ; Turn on comfet
  1991. Set_Pwm_C ; To reapply power after a demag cut
  1992. setb IE_EA
  1993. Set_Comp_Phase_B ; Set comparator phase
  1994. ret
  1995. comm1_comm2_rev: ; A->C
  1996. clr IE_EA ; Disable all interrupts
  1997. BcomFET_off ; Turn off comfet
  1998. CcomFET_on ; Turn on comfet (reverse)
  1999. Set_Pwm_A ; To reapply power after a demag cut
  2000. setb IE_EA
  2001. Set_Comp_Phase_B ; Set comparator phase
  2002. ret
  2003. ; Comm phase 2 to comm phase 3
  2004. comm2_comm3: ; B->A
  2005. Clear_RPM_Out
  2006. jb Flag_Pgm_Dir_Rev, comm2_comm3_rev
  2007. clr IE_EA ; Disable all interrupts
  2008. CpwmFET_off ; Turn off pwmfet
  2009. Set_Pwm_B ; To reapply power after a demag cut
  2010. AcomFET_on
  2011. setb IE_EA
  2012. Set_Comp_Phase_C ; Set comparator phase
  2013. ret
  2014. comm2_comm3_rev: ; B->C
  2015. clr IE_EA ; Disable all interrupts
  2016. ApwmFET_off ; Turn off pwmfet (reverse)
  2017. Set_Pwm_B ; To reapply power after a demag cut
  2018. CcomFET_on
  2019. setb IE_EA
  2020. Set_Comp_Phase_A ; Set comparator phase (reverse)
  2021. ret
  2022. ; Comm phase 3 to comm phase 4
  2023. comm3_comm4: ; B->C
  2024. Set_RPM_Out
  2025. jb Flag_Pgm_Dir_Rev, comm3_comm4_rev
  2026. clr IE_EA ; Disable all interrupts
  2027. AcomFET_off ; Turn off comfet
  2028. CcomFET_on ; Turn on comfet
  2029. Set_Pwm_B ; To reapply power after a demag cut
  2030. setb IE_EA
  2031. Set_Comp_Phase_A ; Set comparator phase
  2032. ret
  2033. comm3_comm4_rev: ; B->A
  2034. clr IE_EA ; Disable all interrupts
  2035. CcomFET_off ; Turn off comfet (reverse)
  2036. AcomFET_on ; Turn on comfet (reverse)
  2037. Set_Pwm_B ; To reapply power after a demag cut
  2038. setb IE_EA
  2039. Set_Comp_Phase_C ; Set comparator phase (reverse)
  2040. ret
  2041. ; Comm phase 4 to comm phase 5
  2042. comm4_comm5: ; A->C
  2043. Clear_RPM_Out
  2044. jb Flag_Pgm_Dir_Rev, comm4_comm5_rev
  2045. clr IE_EA ; Disable all interrupts
  2046. BpwmFET_off ; Turn off pwmfet
  2047. Set_Pwm_A ; To reapply power after a demag cut
  2048. CcomFET_on
  2049. setb IE_EA
  2050. Set_Comp_Phase_B ; Set comparator phase
  2051. ret
  2052. comm4_comm5_rev: ; C->A
  2053. clr IE_EA ; Disable all interrupts
  2054. BpwmFET_off ; Turn off pwmfet
  2055. Set_Pwm_C
  2056. AcomFET_on ; To reapply power after a demag cut
  2057. setb IE_EA
  2058. Set_Comp_Phase_B ; Set comparator phase
  2059. ret
  2060. ; Comm phase 5 to comm phase 6
  2061. comm5_comm6: ; A->B
  2062. Set_RPM_Out
  2063. jb Flag_Pgm_Dir_Rev, comm5_comm6_rev
  2064. clr IE_EA ; Disable all interrupts
  2065. CcomFET_off ; Turn off comfet
  2066. BcomFET_on ; Turn on comfet
  2067. Set_Pwm_A ; To reapply power after a demag cut
  2068. setb IE_EA
  2069. Set_Comp_Phase_C ; Set comparator phase
  2070. ret
  2071. comm5_comm6_rev: ; C->B
  2072. clr IE_EA ; Disable all interrupts
  2073. AcomFET_off ; Turn off comfet (reverse)
  2074. BcomFET_on ; Turn on comfet
  2075. Set_Pwm_C ; To reapply power after a demag cut
  2076. setb IE_EA
  2077. Set_Comp_Phase_A ; Set comparator phase (reverse)
  2078. ret
  2079. ; Comm phase 6 to comm phase 1
  2080. comm6_comm1: ; C->B
  2081. Clear_RPM_Out
  2082. jb Flag_Pgm_Dir_Rev, comm6_comm1_rev
  2083. clr IE_EA ; Disable all interrupts
  2084. ApwmFET_off ; Turn off pwmfet
  2085. Set_Pwm_C
  2086. BcomFET_on ; To reapply power after a demag cut
  2087. setb IE_EA
  2088. Set_Comp_Phase_A ; Set comparator phase
  2089. ret
  2090. comm6_comm1_rev: ; A->B
  2091. clr IE_EA ; Disable all interrupts
  2092. CpwmFET_off ; Turn off pwmfet (reverse)
  2093. Set_Pwm_A
  2094. BcomFET_on ; To reapply power after a demag cut
  2095. setb IE_EA
  2096. Set_Comp_Phase_C ; Set comparator phase (reverse)
  2097. ret
  2098. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2099. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2100. ;
  2101. ; DShot
  2102. ;
  2103. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2104. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2105. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2106. ;
  2107. ; Detect DShot RCP level
  2108. ;
  2109. ; Determine if RCP signal level is normal or inverted DShot
  2110. ;
  2111. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2112. detect_rcp_level:
  2113. mov A, #50 ; Must detect the same level 50 times (25 us)
  2114. mov C, RTX_PORT.RTX_PIN
  2115. detect_rcp_level_read:
  2116. jc ($+5)
  2117. jb RTX_PORT.RTX_PIN, detect_rcp_level ; Level changed from low to high - start over
  2118. jnc ($+5)
  2119. jnb RTX_PORT.RTX_PIN, detect_rcp_level ; Level changed from high to low - start over
  2120. djnz ACC, detect_rcp_level_read
  2121. mov Flag_Rcp_DShot_Inverted, C
  2122. ret
  2123. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2124. ;
  2125. ; Check DShot command
  2126. ;
  2127. ; Determine received DShot command and perform action
  2128. ;
  2129. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2130. dshot_cmd_check:
  2131. mov A, DShot_Cmd
  2132. jz dshot_cmd_exit_no_clear
  2133. mov Temp1, A
  2134. clr C
  2135. subb A, #6 ; Beacon beeps for command 1-5
  2136. jnc dshot_cmd_direction_1
  2137. call beacon_beep
  2138. call wait200ms
  2139. sjmp dshot_cmd_exit
  2140. dshot_cmd_direction_1:
  2141. ; Change programmed motor direction to normal
  2142. cjne Temp1, #7, dshot_cmd_direction_2
  2143. clr C
  2144. mov A, DShot_Cmd_Cnt
  2145. subb A, #6 ; Needs to receive it 6 times in a row
  2146. jc dshot_cmd_exit_no_clear
  2147. mov A, #1
  2148. jnb Flag_Pgm_Bidir, ($+5)
  2149. mov A, #3
  2150. mov Temp1, #Pgm_Direction
  2151. mov @Temp1, A
  2152. clr Flag_Pgm_Dir_Rev
  2153. clr Flag_Pgm_Bidir_Rev
  2154. sjmp dshot_cmd_exit
  2155. dshot_cmd_direction_2:
  2156. ; Change programmed motor direction to reversed
  2157. cjne Temp1, #8, dshot_cmd_direction_bidir_off
  2158. clr C
  2159. mov A, DShot_Cmd_Cnt
  2160. subb A, #6 ; Needs to receive it 6 times in a row
  2161. jc dshot_cmd_exit_no_clear
  2162. mov A, #2
  2163. jnb Flag_Pgm_Bidir, ($+5)
  2164. mov A, #4
  2165. mov Temp1, #Pgm_Direction
  2166. mov @Temp1, A
  2167. setb Flag_Pgm_Dir_Rev
  2168. setb Flag_Pgm_Bidir_Rev
  2169. sjmp dshot_cmd_exit
  2170. dshot_cmd_direction_bidir_off:
  2171. ; Change programmed motor mode to normal (not bidirectional)
  2172. cjne Temp1, #9, dshot_cmd_direction_bidir_on
  2173. clr C
  2174. mov A, DShot_Cmd_Cnt
  2175. subb A, #6 ; Needs to receive it 6 times in a row
  2176. jc dshot_cmd_exit_no_clear
  2177. jnb Flag_Pgm_Bidir, dshot_cmd_exit
  2178. clr C
  2179. mov Temp1, #Pgm_Direction
  2180. mov A, @Temp1
  2181. dec A
  2182. clr ACC.1
  2183. inc A
  2184. mov @Temp1, A
  2185. clr Flag_Pgm_Bidir
  2186. sjmp dshot_cmd_exit
  2187. dshot_cmd_direction_bidir_on:
  2188. ; Change programmed motor mode to bidirectional
  2189. cjne Temp1, #10, dshot_cmd_direction_normal
  2190. clr C
  2191. mov A, DShot_Cmd_Cnt
  2192. subb A, #6 ; Needs to receive it 6 times in a row
  2193. jc dshot_cmd_exit_no_clear
  2194. jb Flag_Pgm_Bidir, dshot_cmd_exit
  2195. mov Temp1, #Pgm_Direction
  2196. mov A, @Temp1
  2197. dec A
  2198. setb ACC.1
  2199. inc A
  2200. mov @Temp1, A
  2201. setb Flag_Pgm_Bidir
  2202. dshot_cmd_exit:
  2203. mov DShot_Cmd, #0
  2204. mov DShot_Cmd_Cnt, #0
  2205. dshot_cmd_exit_no_clear:
  2206. ret
  2207. dshot_cmd_direction_normal:
  2208. ; Change programmed motor direction to that stored in eeprom
  2209. cjne Temp1, #20, dshot_cmd_direction_reverse
  2210. clr C
  2211. mov A, DShot_Cmd_Cnt
  2212. subb A, #6 ; Needs to receive it 6 times in a row
  2213. jc dshot_cmd_exit_no_clear
  2214. clr IE_EA ; DPTR used in interrupts
  2215. mov DPTR, #Eep_Pgm_Direction ; Read from flash
  2216. mov A, #0
  2217. movc A, @A+DPTR
  2218. setb IE_EA
  2219. mov Temp1, #Pgm_Direction
  2220. mov @Temp1, A
  2221. rrc A ; Lsb to carry
  2222. cpl C
  2223. mov Flag_Pgm_Dir_Rev, C
  2224. mov Flag_Pgm_Bidir_Rev, C
  2225. sjmp dshot_cmd_exit
  2226. dshot_cmd_direction_reverse: ; Temporary reverse
  2227. ; Change programmed motor direction to the reverse of what is stored in eeprom
  2228. cjne Temp1, #21, dshot_cmd_save_settings
  2229. clr C
  2230. mov A, DShot_Cmd_Cnt
  2231. subb A, #6 ; Needs to receive it 6 times in a row
  2232. jc dshot_cmd_exit_no_clear
  2233. clr IE_EA ; DPTR used in interrupts
  2234. mov DPTR, #Eep_Pgm_Direction ; Read from flash
  2235. mov A, #0
  2236. movc A, @A+DPTR
  2237. setb IE_EA
  2238. dec A
  2239. cpl ACC.0
  2240. inc A
  2241. mov Temp1, #Pgm_Direction
  2242. mov @Temp1, A
  2243. rrc A ; Lsb to carry
  2244. cpl C
  2245. mov Flag_Pgm_Dir_Rev, C
  2246. mov Flag_Pgm_Bidir_Rev, C
  2247. sjmp dshot_cmd_exit
  2248. dshot_cmd_save_settings:
  2249. cjne Temp1, #12, dshot_cmd_exit
  2250. clr C
  2251. mov A, DShot_Cmd_Cnt
  2252. subb A, #6 ; Needs to receive it 6 times in a row
  2253. jc dshot_cmd_exit_no_clear
  2254. mov Flash_Key_1, #0A5h ; Initialize flash keys to valid values
  2255. mov Flash_Key_2, #0F1h
  2256. call erase_and_store_all_in_eeprom
  2257. mov Flash_Key_1, #0 ; Initialize flash keys to invalid values
  2258. mov Flash_Key_2, #0
  2259. setb IE_EA
  2260. sjmp dshot_cmd_exit
  2261. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2262. ;
  2263. ; DShot beacon beep
  2264. ;
  2265. ; Beep with beacon strength
  2266. ; Beep type 1-5 in Temp1
  2267. ;
  2268. ; Note: This routine switches off power
  2269. ;
  2270. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2271. beacon_beep:
  2272. clr IE_EA ; Disable all interrupts
  2273. call switch_power_off ; Switch power off in case braking is set
  2274. mov Temp2, #Pgm_Beacon_Strength ; Set beacon beep strength
  2275. mov Beep_Strength, @Temp2
  2276. cjne Temp1, #1, beacon_beep2
  2277. call beep_f1
  2278. sjmp beacon_beep_exit
  2279. beacon_beep2:
  2280. cjne Temp1, #2, beacon_beep3
  2281. call beep_f2
  2282. sjmp beacon_beep_exit
  2283. beacon_beep3:
  2284. cjne Temp1, #3, beacon_beep4
  2285. call beep_f3
  2286. sjmp beacon_beep_exit
  2287. beacon_beep4:
  2288. cjne Temp1, #4, beacon_beep5
  2289. call beep_f4
  2290. sjmp beacon_beep_exit
  2291. beacon_beep5:
  2292. call beep_f5
  2293. beacon_beep_exit:
  2294. mov Temp2, #Pgm_Beep_Strength ; Set normal beep strength
  2295. mov Beep_Strength, @Temp2
  2296. setb IE_EA ; Enable all interrupts
  2297. ret
  2298. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2299. ;
  2300. ; DShot telemetry create packet
  2301. ;
  2302. ; Create DShot telemetry packet and prepare it for being sent
  2303. ; The routine is divided into 6 sections that can return early
  2304. ; in order to reduce commutation interference
  2305. ;
  2306. ; Requirements: Must NOT be called while Flag_Telemetry_Pending is set
  2307. ;
  2308. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2309. dshot_tlm_create_packet:
  2310. push PSW
  2311. mov PSW, #10h ; Select register bank 2
  2312. Early_Return_Packet_Stage 0
  2313. ; Read commutation period
  2314. clr IE_EA
  2315. mov Tlm_Data_L, Comm_Period4x_L
  2316. mov Tlm_Data_H, Comm_Period4x_H
  2317. setb IE_EA
  2318. ; Multiply period by 3/4 (1/2 + 1/4)
  2319. mov A, Tlm_Data_L
  2320. mov C, Tlm_Data_H.0
  2321. rrc A
  2322. mov Temp2, A
  2323. mov C, Tlm_Data_H.1
  2324. rrc A
  2325. add A, Temp2
  2326. mov Tlm_Data_L, A
  2327. mov A, Tlm_Data_H
  2328. rr A
  2329. clr ACC.7
  2330. mov Temp2, A
  2331. rr A
  2332. clr ACC.7
  2333. addc A, Temp2
  2334. mov Tlm_Data_H, A
  2335. Early_Return_Packet_Stage 1
  2336. mov A, Tlm_Data_H
  2337. ; 12-bit encode telemetry data
  2338. jnz dshot_12bit_encode
  2339. mov A, Tlm_Data_L ; Already 12-bit
  2340. jnz dshot_tlm_12bit_encoded
  2341. ; If period is zero then reset to FFFFh (FFFh for 12-bit)
  2342. mov Tlm_Data_H, #0Fh
  2343. mov Tlm_Data_L, #0FFh
  2344. dshot_tlm_12bit_encoded:
  2345. Early_Return_Packet_Stage 2
  2346. mov A, Tlm_Data_L
  2347. ; Compute inverted xor checksum (4-bit)
  2348. swap A
  2349. xrl A, Tlm_Data_L
  2350. xrl A, Tlm_Data_H
  2351. cpl A
  2352. ; GCR encode the telemetry data (16-bit)
  2353. mov Temp1, #Temp_Storage ; Store pulse timings in Temp_Storage
  2354. mov @Temp1, DShot_GCR_Pulse_Time_1; Final transition time
  2355. call dshot_gcr_encode ; GCR encode lowest 4-bit of A (store through Temp1)
  2356. Early_Return_Packet_Stage 3
  2357. mov A, Tlm_Data_L
  2358. call dshot_gcr_encode
  2359. Early_Return_Packet_Stage 4
  2360. mov A, Tlm_Data_L
  2361. swap A
  2362. call dshot_gcr_encode
  2363. Early_Return_Packet_Stage 5
  2364. mov A, Tlm_Data_H
  2365. call dshot_gcr_encode
  2366. inc Temp1
  2367. mov Temp5, #0 ; Reset current packet stage
  2368. pop PSW
  2369. setb Flag_Telemetry_Pending ; Mark that packet is ready to be sent
  2370. ret
  2371. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2372. ;
  2373. ; DShot 12-bit encode
  2374. ;
  2375. ; Encodes 16-bit e-period as a 12-bit value of the form:
  2376. ; <e e e m m m m m m m m m> where M SHL E ~ e-period [us]
  2377. ;
  2378. ; Note: Not callable to improve performance
  2379. ;
  2380. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2381. dshot_12bit_encode:
  2382. ; Encode 16-bit e-period as a 12-bit value
  2383. jb ACC.7, dshot_12bit_7 ; ACC = Tlm_Data_H
  2384. jb ACC.6, dshot_12bit_6
  2385. jb ACC.5, dshot_12bit_5
  2386. jb ACC.4, dshot_12bit_4
  2387. jb ACC.3, dshot_12bit_3
  2388. jb ACC.2, dshot_12bit_2
  2389. jb ACC.1, dshot_12bit_1
  2390. mov A, Tlm_Data_L ; Already 12-bit (E=0)
  2391. ajmp dshot_tlm_12bit_encoded
  2392. dshot_12bit_7:
  2393. ;mov A, Tlm_Data_H
  2394. mov C, Tlm_Data_L.7
  2395. rlc A
  2396. mov Tlm_Data_L, A
  2397. mov Tlm_Data_H, #0fh
  2398. ajmp dshot_tlm_12bit_encoded
  2399. dshot_12bit_6:
  2400. ;mov A, Tlm_Data_H
  2401. mov C, Tlm_Data_L.7
  2402. rlc A
  2403. mov C, Tlm_Data_L.6
  2404. rlc A
  2405. mov Tlm_Data_L, A
  2406. mov Tlm_Data_H, #0dh
  2407. ajmp dshot_tlm_12bit_encoded
  2408. dshot_12bit_5:
  2409. ;mov A, Tlm_Data_H
  2410. mov C, Tlm_Data_L.7
  2411. rlc A
  2412. mov C, Tlm_Data_L.6
  2413. rlc A
  2414. mov C, Tlm_Data_L.5
  2415. rlc A
  2416. mov Tlm_Data_L, A
  2417. mov Tlm_Data_H, #0bh
  2418. ajmp dshot_tlm_12bit_encoded
  2419. dshot_12bit_4:
  2420. mov A, Tlm_Data_L
  2421. anl A, #0f0h
  2422. clr Tlm_Data_H.4
  2423. orl A, Tlm_Data_H
  2424. swap A
  2425. mov Tlm_Data_L, A
  2426. mov Tlm_Data_H, #09h
  2427. ajmp dshot_tlm_12bit_encoded
  2428. dshot_12bit_3:
  2429. mov A, Tlm_Data_L
  2430. mov C, Tlm_Data_H.0
  2431. rrc A
  2432. mov C, Tlm_Data_H.1
  2433. rrc A
  2434. mov C, Tlm_Data_H.2
  2435. rrc A
  2436. mov Tlm_Data_L, A
  2437. mov Tlm_Data_H, #07h
  2438. ajmp dshot_tlm_12bit_encoded
  2439. dshot_12bit_2:
  2440. mov A, Tlm_Data_L
  2441. mov C, Tlm_Data_H.0
  2442. rrc A
  2443. mov C, Tlm_Data_H.1
  2444. rrc A
  2445. mov Tlm_Data_L, A
  2446. mov Tlm_Data_H, #05h
  2447. ajmp dshot_tlm_12bit_encoded
  2448. dshot_12bit_1:
  2449. mov A, Tlm_Data_L
  2450. mov C, Tlm_Data_H.0
  2451. rrc A
  2452. mov Tlm_Data_L, A
  2453. mov Tlm_Data_H, #03h
  2454. ajmp dshot_tlm_12bit_encoded
  2455. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2456. ;
  2457. ; DShot GCR encode
  2458. ;
  2459. ; GCR encode e-period data for DShot telemetry
  2460. ;
  2461. ; Input
  2462. ; - Temp1: Data pointer for storing pulse timings
  2463. ; - A: 4-bit value to GCR encode
  2464. ; - B: Time that must be added to transition
  2465. ; Output
  2466. ; - B: Time remaining to be added to next transition
  2467. ;
  2468. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2469. dshot_gcr_encode:
  2470. anl A, #0Fh
  2471. rl A ; Multiply by 2 to match jump offsets
  2472. mov DPTR, #dshot_gcr_encode_jump_table
  2473. jmp @A+DPTR
  2474. dshot_gcr_encode_jump_table:
  2475. ajmp dshot_gcr_encode_0_11001
  2476. ajmp dshot_gcr_encode_1_11011
  2477. ajmp dshot_gcr_encode_2_10010
  2478. ajmp dshot_gcr_encode_3_10011
  2479. ajmp dshot_gcr_encode_4_11101
  2480. ajmp dshot_gcr_encode_5_10101
  2481. ajmp dshot_gcr_encode_6_10110
  2482. ajmp dshot_gcr_encode_7_10111
  2483. ajmp dshot_gcr_encode_8_11010
  2484. ajmp dshot_gcr_encode_9_01001
  2485. ajmp dshot_gcr_encode_A_01010
  2486. ajmp dshot_gcr_encode_B_01011
  2487. ajmp dshot_gcr_encode_C_11110
  2488. ajmp dshot_gcr_encode_D_01101
  2489. ajmp dshot_gcr_encode_E_01110
  2490. ajmp dshot_gcr_encode_F_01111
  2491. ; GCR encoding is ordered by least significant bit first,
  2492. ; and represented as pulse durations.
  2493. dshot_gcr_encode_0_11001:
  2494. imov Temp1, DShot_GCR_Pulse_Time_3
  2495. imov Temp1, DShot_GCR_Pulse_Time_1
  2496. imov Temp1, DShot_GCR_Pulse_Time_1
  2497. ret
  2498. dshot_gcr_encode_1_11011:
  2499. imov Temp1, DShot_GCR_Pulse_Time_1
  2500. imov Temp1, DShot_GCR_Pulse_Time_2
  2501. imov Temp1, DShot_GCR_Pulse_Time_1
  2502. imov Temp1, DShot_GCR_Pulse_Time_1
  2503. ret
  2504. dshot_gcr_encode_2_10010:
  2505. GCR_Add_Time Temp1
  2506. imov Temp1, DShot_GCR_Pulse_Time_3
  2507. imov Temp1, DShot_GCR_Pulse_Time_1
  2508. ret
  2509. dshot_gcr_encode_3_10011:
  2510. imov Temp1, DShot_GCR_Pulse_Time_1
  2511. imov Temp1, DShot_GCR_Pulse_Time_3
  2512. imov Temp1, DShot_GCR_Pulse_Time_1
  2513. ret
  2514. dshot_gcr_encode_4_11101:
  2515. imov Temp1, DShot_GCR_Pulse_Time_2
  2516. imov Temp1, DShot_GCR_Pulse_Time_1
  2517. imov Temp1, DShot_GCR_Pulse_Time_1
  2518. imov Temp1, DShot_GCR_Pulse_Time_1
  2519. ret
  2520. dshot_gcr_encode_5_10101:
  2521. imov Temp1, DShot_GCR_Pulse_Time_2
  2522. imov Temp1, DShot_GCR_Pulse_Time_2
  2523. imov Temp1, DShot_GCR_Pulse_Time_1
  2524. ret
  2525. dshot_gcr_encode_6_10110:
  2526. GCR_Add_Time Temp1
  2527. imov Temp1, DShot_GCR_Pulse_Time_1
  2528. imov Temp1, DShot_GCR_Pulse_Time_2
  2529. imov Temp1, DShot_GCR_Pulse_Time_1
  2530. ret
  2531. dshot_gcr_encode_7_10111:
  2532. imov Temp1, DShot_GCR_Pulse_Time_1
  2533. imov Temp1, DShot_GCR_Pulse_Time_1
  2534. imov Temp1, DShot_GCR_Pulse_Time_2
  2535. imov Temp1, DShot_GCR_Pulse_Time_1
  2536. ret
  2537. dshot_gcr_encode_8_11010:
  2538. GCR_Add_Time Temp1
  2539. imov Temp1, DShot_GCR_Pulse_Time_2
  2540. imov Temp1, DShot_GCR_Pulse_Time_1
  2541. imov Temp1, DShot_GCR_Pulse_Time_1
  2542. ret
  2543. dshot_gcr_encode_9_01001:
  2544. imov Temp1, DShot_GCR_Pulse_Time_3
  2545. imov Temp1, DShot_GCR_Pulse_Time_2
  2546. ret
  2547. dshot_gcr_encode_A_01010:
  2548. GCR_Add_Time Temp1
  2549. imov Temp1, DShot_GCR_Pulse_Time_2
  2550. imov Temp1, DShot_GCR_Pulse_Time_2
  2551. ret
  2552. dshot_gcr_encode_B_01011:
  2553. imov Temp1, DShot_GCR_Pulse_Time_1
  2554. imov Temp1, DShot_GCR_Pulse_Time_2
  2555. imov Temp1, DShot_GCR_Pulse_Time_2
  2556. ret
  2557. dshot_gcr_encode_C_11110:
  2558. GCR_Add_Time Temp1
  2559. imov Temp1, DShot_GCR_Pulse_Time_1
  2560. imov Temp1, DShot_GCR_Pulse_Time_1
  2561. imov Temp1, DShot_GCR_Pulse_Time_1
  2562. imov Temp1, DShot_GCR_Pulse_Time_1
  2563. ret
  2564. dshot_gcr_encode_D_01101:
  2565. imov Temp1, DShot_GCR_Pulse_Time_2
  2566. imov Temp1, DShot_GCR_Pulse_Time_1
  2567. imov Temp1, DShot_GCR_Pulse_Time_2
  2568. ret
  2569. dshot_gcr_encode_E_01110:
  2570. GCR_Add_Time Temp1
  2571. imov Temp1, DShot_GCR_Pulse_Time_1
  2572. imov Temp1, DShot_GCR_Pulse_Time_1
  2573. imov Temp1, DShot_GCR_Pulse_Time_2
  2574. ret
  2575. dshot_gcr_encode_F_01111:
  2576. imov Temp1, DShot_GCR_Pulse_Time_1
  2577. imov Temp1, DShot_GCR_Pulse_Time_1
  2578. imov Temp1, DShot_GCR_Pulse_Time_1
  2579. imov Temp1, DShot_GCR_Pulse_Time_2
  2580. ret
  2581. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2582. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2583. ;
  2584. ; ESC programming (EEPROM emulation)
  2585. ;
  2586. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2587. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2588. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2589. ;
  2590. ; Read all eeprom parameters routine
  2591. ;
  2592. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2593. read_all_eeprom_parameters:
  2594. ; Check initialized signature
  2595. mov DPTR, #Eep_Initialized_L
  2596. mov Temp1, #Bit_Access
  2597. call read_eeprom_byte
  2598. mov A, Bit_Access
  2599. cjne A, #055h, read_eeprom_store_defaults
  2600. inc DPTR ; Now Eep_Initialized_H
  2601. call read_eeprom_byte
  2602. mov A, Bit_Access
  2603. cjne A, #0AAh, read_eeprom_store_defaults
  2604. jmp read_eeprom_read
  2605. read_eeprom_store_defaults:
  2606. mov Flash_Key_1, #0A5h
  2607. mov Flash_Key_2, #0F1h
  2608. call set_default_parameters
  2609. call erase_and_store_all_in_eeprom
  2610. mov Flash_Key_1, #0
  2611. mov Flash_Key_2, #0
  2612. jmp read_eeprom_exit
  2613. read_eeprom_read:
  2614. ; Read eeprom
  2615. mov DPTR, #_Eep_Pgm_Gov_P_Gain
  2616. mov Temp1, #_Pgm_Gov_P_Gain
  2617. mov Temp4, #10 ; 10 parameters
  2618. read_eeprom_block1:
  2619. call read_eeprom_byte
  2620. inc DPTR
  2621. inc Temp1
  2622. djnz Temp4, read_eeprom_block1
  2623. mov DPTR, #_Eep_Enable_TX_Program
  2624. mov Temp1, #_Pgm_Enable_TX_Program
  2625. mov Temp4, #26 ; 26 parameters
  2626. read_eeprom_block2:
  2627. call read_eeprom_byte
  2628. inc DPTR
  2629. inc Temp1
  2630. djnz Temp4, read_eeprom_block2
  2631. mov DPTR, #Eep_Dummy ; Set pointer to uncritical area
  2632. read_eeprom_exit:
  2633. ret
  2634. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2635. ;
  2636. ; Erase flash and store all parameter value in EEPROM routine
  2637. ;
  2638. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2639. erase_and_store_all_in_eeprom:
  2640. clr IE_EA ; Disable interrupts
  2641. call read_tags
  2642. call erase_flash ; Erase flash
  2643. mov DPTR, #Eep_FW_Main_Revision ; Store firmware main revision
  2644. mov A, #EEPROM_FW_MAIN_REVISION
  2645. call write_eeprom_byte_from_acc
  2646. inc DPTR ; Now firmware sub revision
  2647. mov A, #EEPROM_FW_SUB_REVISION
  2648. call write_eeprom_byte_from_acc
  2649. inc DPTR ; Now layout revision
  2650. mov A, #EEPROM_LAYOUT_REVISION
  2651. call write_eeprom_byte_from_acc
  2652. ; Write eeprom
  2653. mov DPTR, #_Eep_Pgm_Gov_P_Gain
  2654. mov Temp1, #_Pgm_Gov_P_Gain
  2655. mov Temp4, #10 ; 10 parameters
  2656. write_eeprom_block1:
  2657. call write_eeprom_byte
  2658. inc DPTR
  2659. inc Temp1
  2660. djnz Temp4, write_eeprom_block1
  2661. mov DPTR, #_Eep_Enable_TX_Program
  2662. mov Temp1, #_Pgm_Enable_TX_Program
  2663. mov Temp4, #26 ; 26 parameters
  2664. write_eeprom_block2:
  2665. call write_eeprom_byte
  2666. inc DPTR
  2667. inc Temp1
  2668. djnz Temp4, write_eeprom_block2
  2669. call write_tags
  2670. call write_eeprom_signature
  2671. mov DPTR, #Eep_Dummy ; Set pointer to uncritical area
  2672. ret
  2673. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2674. ;
  2675. ; Read eeprom byte routine
  2676. ;
  2677. ; Gives data in A and in address given by Temp1
  2678. ; Assumes address in DPTR
  2679. ;
  2680. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2681. read_eeprom_byte:
  2682. clr A
  2683. movc A, @A+DPTR ; Read from flash
  2684. mov @Temp1, A
  2685. ret
  2686. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2687. ;
  2688. ; Write eeprom byte routine
  2689. ;
  2690. ; Assumes data in address given by Temp1, or in accumulator
  2691. ; Assumes address in DPTR
  2692. ;
  2693. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2694. write_eeprom_byte:
  2695. mov A, @Temp1
  2696. write_eeprom_byte_from_acc:
  2697. orl PSCTL, #01h ; Set the PSWE bit
  2698. anl PSCTL, #0FDh ; Clear the PSEE bit
  2699. mov Temp8, A
  2700. clr C
  2701. mov A, DPH ; Check that address is not in bootloader area
  2702. subb A, #1Ch
  2703. jc ($+3)
  2704. ret
  2705. mov A, Temp8
  2706. mov FLKEY, Flash_Key_1 ; First key code
  2707. mov FLKEY, Flash_Key_2 ; Second key code
  2708. movx @DPTR, A ; Write to flash
  2709. anl PSCTL, #0FEh ; Clear the PSWE bit
  2710. ret
  2711. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2712. ;
  2713. ; Erase flash routine (erases the flash segment used for "eeprom" variables)
  2714. ;
  2715. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2716. erase_flash:
  2717. orl PSCTL, #02h ; Set the PSEE bit
  2718. orl PSCTL, #01h ; Set the PSWE bit
  2719. mov FLKEY, Flash_Key_1 ; First key code
  2720. mov FLKEY, Flash_Key_2 ; Second key code
  2721. mov DPTR, #Eep_Initialized_L
  2722. movx @DPTR, A
  2723. anl PSCTL, #0FCh ; Clear the PSEE and PSWE bits
  2724. ret
  2725. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2726. ;
  2727. ; Write eeprom signature routine
  2728. ;
  2729. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2730. write_eeprom_signature:
  2731. mov DPTR, #Eep_Initialized_L
  2732. mov A, #055h
  2733. call write_eeprom_byte_from_acc
  2734. mov DPTR, #Eep_Initialized_H
  2735. mov A, #0AAh
  2736. call write_eeprom_byte_from_acc
  2737. ret
  2738. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2739. ;
  2740. ; Read all tags from flash and store in temporary storage
  2741. ;
  2742. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2743. read_tags:
  2744. mov Temp3, #48 ; Number of tags
  2745. mov Temp2, #Temp_Storage ; Set RAM address
  2746. mov Temp1, #Bit_Access
  2747. mov DPTR, #Eep_ESC_Layout ; Set flash address
  2748. read_tag:
  2749. call read_eeprom_byte
  2750. mov A, Bit_Access
  2751. mov @Temp2, A ; Write to RAM
  2752. inc Temp2
  2753. inc DPTR
  2754. djnz Temp3, read_tag
  2755. ret
  2756. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2757. ;
  2758. ; Write all tags from temporary storage and store in flash
  2759. ;
  2760. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2761. write_tags:
  2762. mov Temp3, #48 ; Number of tags
  2763. mov Temp2, #Temp_Storage ; Set RAM address
  2764. mov DPTR, #Eep_ESC_Layout ; Set flash address
  2765. write_tag:
  2766. mov A, @Temp2 ; Read from RAM
  2767. call write_eeprom_byte_from_acc
  2768. inc Temp2
  2769. inc DPTR
  2770. djnz Temp3, write_tag
  2771. ret
  2772. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2773. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2774. ;
  2775. ; Settings
  2776. ;
  2777. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2778. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2779. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2780. ;
  2781. ; Set default parameters
  2782. ;
  2783. ; Sets default programming parameters
  2784. ;
  2785. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2786. set_default_parameters:
  2787. mov Temp1, #_Pgm_Gov_P_Gain
  2788. mov @Temp1, #0FFh ; _Pgm_Gov_P_Gain
  2789. imov Temp1, #DEFAULT_PGM_STARTUP_POWER_MIN ; Pgm_Startup_Power_Min
  2790. imov Temp1, #DEFAULT_PGM_STARTUP_BEEP ; Pgm_Startup_Beep
  2791. imov Temp1, #DEFAULT_PGM_DITHERING ; Pgm_Dithering
  2792. imov Temp1, #DEFAULT_PGM_STARTUP_POWER_MAX ; Pgm_Startup_Power_Max
  2793. imov Temp1, #0FFh ; _Pgm_Rampup_Slope
  2794. imov Temp1, #DEFAULT_PGM_RPM_POWER_SLOPE ; Pgm_Rpm_Power_Slope
  2795. imov Temp1, #0FFh ; _Pgm_Pwm_Freq
  2796. imov Temp1, #DEFAULT_PGM_DIRECTION ; Pgm_Direction
  2797. imov Temp1, #0FFh ; _Pgm_Input_Pol
  2798. inc Temp1 ; Skip Initialized_L_Dummy
  2799. inc Temp1 ; Skip Initialized_H_Dummy
  2800. imov Temp1, #0FFh ; _Pgm_Enable_TX_Program
  2801. imov Temp1, #0FFh ; _Pgm_Main_Rearm_Start
  2802. imov Temp1, #0FFh ; _Pgm_Gov_Setup_Target
  2803. imov Temp1, #0FFh ; _Pgm_Startup_Rpm
  2804. imov Temp1, #0FFh ; _Pgm_Startup_Accel
  2805. imov Temp1, #0FFh ; _Pgm_Volt_Comp
  2806. imov Temp1, #DEFAULT_PGM_COMM_TIMING ; Pgm_Comm_Timing
  2807. imov Temp1, #0FFh ; _Pgm_Damping_Force
  2808. imov Temp1, #0FFh ; _Pgm_Gov_Range
  2809. imov Temp1, #0FFh ; _Pgm_Startup_Method
  2810. imov Temp1, #0FFh ; _Pgm_Min_Throttle
  2811. imov Temp1, #0FFh ; _Pgm_Max_Throttle
  2812. imov Temp1, #DEFAULT_PGM_BEEP_STRENGTH ; Pgm_Beep_Strength
  2813. imov Temp1, #DEFAULT_PGM_BEACON_STRENGTH ; Pgm_Beacon_Strength
  2814. imov Temp1, #DEFAULT_PGM_BEACON_DELAY ; Pgm_Beacon_Delay
  2815. imov Temp1, #0FFh ; _Pgm_Throttle_Rate
  2816. imov Temp1, #DEFAULT_PGM_DEMAG_COMP ; Pgm_Demag_Comp
  2817. imov Temp1, #0FFh ; _Pgm_BEC_Voltage_High
  2818. imov Temp1, #0FFh ; _Pgm_Center_Throttle
  2819. imov Temp1, #0FFh ; _Pgm_Main_Spoolup_Time
  2820. imov Temp1, #DEFAULT_PGM_ENABLE_TEMP_PROT ; Pgm_Enable_Temp_Prot
  2821. imov Temp1, #0FFh ; _Pgm_Enable_Power_Prot
  2822. imov Temp1, #0FFh ; _Pgm_Enable_Pwm_Input
  2823. imov Temp1, #0FFh ; _Pgm_Pwm_Dither
  2824. imov Temp1, #DEFAULT_PGM_BRAKE_ON_STOP ; Pgm_Brake_On_Stop
  2825. imov Temp1, #DEFAULT_PGM_LED_CONTROL ; Pgm_LED_Control
  2826. ret
  2827. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2828. ;
  2829. ; Decode settings
  2830. ;
  2831. ; Decodes various settings
  2832. ;
  2833. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2834. decode_settings:
  2835. mov Temp1, #Pgm_Direction ; Load programmed direction
  2836. mov A, @Temp1
  2837. dec A
  2838. mov C, ACC.1 ; Set bidirectional mode
  2839. mov Flag_Pgm_Bidir, C
  2840. mov C, ACC.0 ; Set direction (Normal / Reversed)
  2841. mov Flag_Pgm_Dir_Rev, C
  2842. mov Flag_Pgm_Bidir_Rev, C
  2843. ; Check startup power
  2844. mov Temp1, #Pgm_Startup_Power_Max
  2845. mov A, #80 ; Limit to at most 80
  2846. subb A, @Temp1
  2847. jnc ($+4)
  2848. mov @Temp1, #80
  2849. ; Check low rpm power slope
  2850. mov Temp1, #Pgm_Rpm_Power_Slope
  2851. mov A, #13 ; Limit to at most 13
  2852. subb A, @Temp1
  2853. jnc ($+4)
  2854. mov @Temp1, #13
  2855. mov Low_Rpm_Pwr_Slope, @Temp1
  2856. ; Decode demag compensation
  2857. mov Temp1, #Pgm_Demag_Comp
  2858. mov A, @Temp1
  2859. mov Demag_Pwr_Off_Thresh, #255 ; Set default
  2860. cjne A, #2, decode_demag_high
  2861. mov Demag_Pwr_Off_Thresh, #160 ; Settings for demag comp low
  2862. decode_demag_high:
  2863. cjne A, #3, decode_demag_done
  2864. mov Demag_Pwr_Off_Thresh, #130 ; Settings for demag comp high
  2865. decode_demag_done:
  2866. ; Decode temperature protection limit
  2867. mov Temp1, #Pgm_Enable_Temp_Prot
  2868. mov A, @Temp1
  2869. mov Temp1, A
  2870. jz decode_temp_done
  2871. mov A, #(TEMP_LIMIT-TEMP_LIMIT_STEP)
  2872. decode_temp_step:
  2873. add A, #TEMP_LIMIT_STEP
  2874. djnz Temp1, decode_temp_step
  2875. decode_temp_done:
  2876. mov Temp_Prot_Limit, A
  2877. mov Temp1, #Pgm_Beep_Strength ; Set beep strength
  2878. mov Beep_Strength, @Temp1
  2879. mov Temp1, #Pgm_Dithering ; Read programmed dithering setting
  2880. mov A, @Temp1
  2881. clr Flag_Dithering
  2882. jz decode_dithering
  2883. setb Flag_Dithering
  2884. decode_dithering:
  2885. IF PWM_BITS_H == 2 ; Initialize pwm dithering bit patterns
  2886. mov Temp1, #Dithering_Patterns
  2887. mov @Temp1, #00h
  2888. imov Temp1, #55h
  2889. ELSEIF PWM_BITS_H == 1
  2890. mov Temp1, #Dithering_Patterns
  2891. mov @Temp1, #00h
  2892. imov Temp1, #11h
  2893. imov Temp1, #55h
  2894. imov Temp1, #77h
  2895. ELSEIF PWM_BITS_H == 0
  2896. mov Temp1, #Dithering_Patterns
  2897. mov @Temp1, #00h
  2898. imov Temp1, #01h
  2899. imov Temp1, #11h
  2900. imov Temp1, #25h
  2901. imov Temp1, #55h
  2902. imov Temp1, #5Bh
  2903. imov Temp1, #77h
  2904. imov Temp1, #7fh
  2905. ENDIF
  2906. ret
  2907. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2908. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2909. ;
  2910. ; Main program
  2911. ;
  2912. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2913. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2914. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2915. ;
  2916. ; Main program entry point
  2917. ;
  2918. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2919. pgm_start:
  2920. mov Flash_Key_1, #0 ; Initialize flash keys to invalid values
  2921. mov Flash_Key_2, #0
  2922. mov WDTCN, #0DEh ; Disable watchdog (WDT)
  2923. mov WDTCN, #0ADh
  2924. mov SP, #Stack ; Initialize stack (16 bytes of indirect RAM)
  2925. orl VDM0CN, #080h ; Enable the VDD monitor
  2926. mov RSTSRC, #06h ; Set missing clock and VDD monitor as a reset source if not 1S capable
  2927. mov CLKSEL, #00h ; Set clock divider to 1 (Oscillator 0 at 24MHz)
  2928. call switch_power_off
  2929. ; Ports initialization
  2930. mov P0, #P0_INIT
  2931. mov P0MDIN, #P0_DIGITAL
  2932. mov P0MDOUT, #P0_PUSHPULL
  2933. mov P0, #P0_INIT
  2934. mov P0SKIP, #P0_SKIP
  2935. mov P1, #P1_INIT
  2936. mov P1MDIN, #P1_DIGITAL
  2937. mov P1MDOUT, #P1_PUSHPULL
  2938. mov P1, #P1_INIT
  2939. mov P1SKIP, #P1_SKIP
  2940. mov P2MDOUT, #P2_PUSHPULL
  2941. Initialize_Xbar ; Initialize the XBAR and related functionality
  2942. call switch_power_off ; Switch power off again, after initializing ports
  2943. ; Clear RAM
  2944. clr A ; Clear accumulator
  2945. mov Temp1, A ; Clear Temp1
  2946. clear_ram:
  2947. mov @Temp1, A ; Clear RAM address
  2948. djnz Temp1, clear_ram ; Decrement address and repeat
  2949. call set_default_parameters ; Set default programmed parameters
  2950. call read_all_eeprom_parameters ; Read all programmed parameters
  2951. mov Temp1, #Pgm_Beep_Strength ; Read programmed beep strength
  2952. mov Beep_Strength, @Temp1 ; Set beep strength
  2953. ; Initializing beeps
  2954. clr IE_EA ; Disable interrupts explicitly
  2955. call wait100ms ; Wait a bit to avoid audible resets if not properly powered
  2956. mov Temp1, #Pgm_Startup_Beep ; Read programmed startup beep setting
  2957. mov A, @Temp1
  2958. jnz startup_beep_melody
  2959. call beep_f2_short ; Short startup beep
  2960. call wait250ms
  2961. call wait250ms
  2962. call wait250ms
  2963. call wait250ms
  2964. sjmp startup_beep_done
  2965. startup_beep_melody:
  2966. call beep_f1 ; Normal startup melody
  2967. call wait5ms
  2968. call beep_f2
  2969. call wait5ms
  2970. call beep_f1
  2971. call wait5ms
  2972. call beep_f3
  2973. call wait200ms
  2974. call beep_f2
  2975. call beep_f4
  2976. call beep_f4
  2977. startup_beep_done:
  2978. call led_control ; Set LEDs to programmed values
  2979. call wait250ms ; Wait for flight controller to get ready
  2980. call wait250ms
  2981. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2982. ;
  2983. ; No signal entry point
  2984. ;
  2985. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2986. init_no_signal:
  2987. clr IE_EA ; Disable interrupts explicitly
  2988. mov Flash_Key_1, #0 ; Initialize flash keys to invalid values
  2989. mov Flash_Key_2, #0
  2990. mov Temp1, #250 ; Check if input signal is high for more than 15ms
  2991. input_high_check_1:
  2992. mov Temp2, #250
  2993. input_high_check_2:
  2994. jnb RTX_PORT.RTX_PIN, bootloader_done ; Look for low
  2995. djnz Temp2, input_high_check_2
  2996. djnz Temp1, input_high_check_1
  2997. ljmp 1C00h ; Jump to bootloader
  2998. bootloader_done:
  2999. call decode_settings
  3000. call switch_power_off
  3001. IF MCU_48MHZ == 1
  3002. Set_MCU_Clk_24MHz ; Set clock frequency
  3003. ENDIF
  3004. jnb Flag_Had_Signal, setup_dshot
  3005. call beep_f2 ; Beep on signal loss or stall
  3006. call beep_f2
  3007. call beep_f2
  3008. call beep_f2
  3009. call beep_f2
  3010. call wait200ms
  3011. clr Flag_Had_Signal
  3012. setup_dshot:
  3013. ; Setup timers for DShot
  3014. mov TCON, #51h ; Timer 0/1 run and INT0 edge triggered
  3015. mov CKCON0, #01h ; Timer 0/1 clock is system clock divided by 4 (for DShot150)
  3016. mov TMOD, #0AAh ; Timer 0/1 set to 8bits auto reload and gated by INT0/1
  3017. mov TH0, #0 ; Auto reload value zero
  3018. mov TH1, #0
  3019. mov TMR2CN0, #04h ; Timer 2 enabled (system clock divided by 12)
  3020. mov TMR3CN0, #04h ; Timer 3 enabled (system clock divided by 12)
  3021. Initialize_PCA ; Initialize PCA
  3022. Set_Pwm_Polarity ; Set pwm polarity
  3023. Enable_Power_Pwm_Module ; Enable power pwm module
  3024. Enable_Damp_Pwm_Module ; Enable damping pwm module
  3025. Initialize_Comparator ; Initialize comparator
  3026. Initialize_Adc ; Initialize ADC operation
  3027. call wait1ms
  3028. mov Startup_Stall_Cnt, #0 ; Reset stall count
  3029. mov DShot_Cmd, #0 ; Clear DShot command
  3030. mov DShot_Cmd_Cnt, #0 ; Clear DShot command count
  3031. clr Flag_Telemetry_Pending ; Clear DShot telemetry
  3032. call detect_rcp_level ; Detect RCP level (normal or inverted DShot)
  3033. ; Route RCP according to detected DShot signal (normal or inverted)
  3034. mov IT01CF, #(80h + (RTX_PIN SHL 4) + RTX_PIN) ; Route RCP input to INT0/1, with INT1 inverted
  3035. jnb Flag_Rcp_DShot_Inverted, ($+6)
  3036. mov IT01CF, #(08h + (RTX_PIN SHL 4) + RTX_PIN) ; Route RCP input to INT0/1, with INT0 inverted
  3037. ; Setup interrupts for DShot
  3038. mov IE, #2Dh ; Enable timer 1/2 interrupts and INT0/1 interrupts
  3039. mov EIE1, #90h ; Enable timer 3 and PCA0 interrupts
  3040. mov IP, #03h ; High priority to timer 0 and INT0 interrupts
  3041. setb IE_EA ; Enable all interrupts
  3042. ; Setup variables for DShot150 (Only on 24MHz because frame length threshold cannot be scaled up)
  3043. IF MCU_48MHZ == 0
  3044. mov DShot_Timer_Preset, #-64 ; Load DShot sync timer preset (for DShot150)
  3045. mov DShot_Pwm_Thr, #8 ; Load DShot qualification pwm threshold (for DShot150)
  3046. mov DShot_Frame_Length_Thr, #160 ; Load DShot frame length criteria
  3047. Set_DShot_Tlm_Bitrate 187500 ; = 5/4 * 150000
  3048. ; Test whether signal is DShot150
  3049. mov Rcp_Outside_Range_Cnt, #10 ; Set out of range counter
  3050. call wait100ms ; Wait for new RC pulse
  3051. mov A, Rcp_Outside_Range_Cnt ; Check if pulses were accepted
  3052. mov DShot_Cmd, #0
  3053. mov DShot_Cmd_Cnt, #0
  3054. jz arming_begin
  3055. ENDIF
  3056. mov CKCON0, #0Ch ; Timer 0/1 clock is system clock (for DShot300/600)
  3057. ; Setup variables for DShot300
  3058. mov DShot_Timer_Preset, #-128 ; Load DShot sync timer preset (for DShot300)
  3059. mov DShot_Pwm_Thr, #16 ; Load DShot pwm threshold (for DShot300)
  3060. mov DShot_Frame_Length_Thr, #80 ; Load DShot frame length criteria
  3061. Set_DShot_Tlm_Bitrate 375000 ; = 5/4 * 300000
  3062. ; Test whether signal is DShot300
  3063. mov Rcp_Outside_Range_Cnt, #10 ; Set out of range counter
  3064. call wait100ms ; Wait for new RC pulse
  3065. mov A, Rcp_Outside_Range_Cnt ; Check if pulses were accepted
  3066. mov DShot_Cmd, #0
  3067. mov DShot_Cmd_Cnt, #0
  3068. jz arming_begin
  3069. ; Setup variables for DShot600
  3070. mov DShot_Timer_Preset, #-64 ; Load DShot sync timer preset (for DShot600)
  3071. mov DShot_Pwm_Thr, #8 ; Load DShot pwm threshold (for DShot600)
  3072. mov DShot_Frame_Length_Thr, #40 ; Load DShot frame length criteria
  3073. Set_DShot_Tlm_Bitrate 750000 ; = 5/4 * 600000
  3074. ; Test whether signal is DShot600
  3075. mov Rcp_Outside_Range_Cnt, #10 ; Set out of range counter
  3076. call wait100ms ; Wait for new RC pulse
  3077. mov A, Rcp_Outside_Range_Cnt ; Check if pulses were accepted
  3078. mov DShot_Cmd, #0
  3079. mov DShot_Cmd_Cnt, #0
  3080. jz arming_begin
  3081. ljmp init_no_signal
  3082. arming_begin:
  3083. push PSW
  3084. mov PSW, #10h ; Temp8 in register bank 2 holds value
  3085. mov Temp8, CKCON0 ; Save DShot clock settings for telemetry
  3086. pop PSW
  3087. setb Flag_Had_Signal ; Mark that a signal has been detected
  3088. clr IE_EA
  3089. call beep_f1_short ; Beep signal that RC pulse is ready
  3090. setb IE_EA
  3091. arming_wait:
  3092. call wait100ms
  3093. jnb Flag_Rcp_Stop, arming_wait ; Wait until throttle is zero
  3094. clr IE_EA
  3095. call beep_f2_short ; Beep signal that ESC is armed
  3096. setb IE_EA
  3097. wait_for_power_on: ; Armed and waiting for power on
  3098. clr A
  3099. mov Comm_Period4x_L, A ; Reset commutation period for telemetry
  3100. mov Comm_Period4x_H, A
  3101. mov Power_On_Wait_Cnt_L, A ; Clear beacon wait counter
  3102. mov Power_On_Wait_Cnt_H, A
  3103. wait_for_power_on_loop:
  3104. inc Power_On_Wait_Cnt_L ; Increment low wait counter
  3105. mov A, Power_On_Wait_Cnt_L
  3106. cpl A
  3107. jnz wait_for_power_on_no_beep ; Counter wrapping (about 3 sec)
  3108. inc Power_On_Wait_Cnt_H ; Increment high wait counter
  3109. mov Temp1, #Pgm_Beacon_Delay
  3110. mov A, @Temp1
  3111. mov Temp1, #20 ; 1 min
  3112. dec A
  3113. jz beep_delay_set
  3114. mov Temp1, #40 ; 2 min
  3115. dec A
  3116. jz beep_delay_set
  3117. mov Temp1, #100 ; 5 min
  3118. dec A
  3119. jz beep_delay_set
  3120. mov Temp1, #200 ; 10 min
  3121. dec A
  3122. jz beep_delay_set
  3123. mov Power_On_Wait_Cnt_H, #0 ; Reset counter for infinite delay
  3124. beep_delay_set:
  3125. clr C
  3126. mov A, Power_On_Wait_Cnt_H
  3127. subb A, Temp1 ; Check against chosen delay
  3128. jc wait_for_power_on_no_beep ; Has delay elapsed?
  3129. dec Power_On_Wait_Cnt_H ; Decrement high wait counter for continued beeping
  3130. mov Temp1, #4 ; Beep tone 4
  3131. call beacon_beep
  3132. wait_for_power_on_no_beep:
  3133. jb Flag_Telemetry_Pending, wait_for_power_telemetry_done
  3134. setb Flag_Timer3_Pending ; Set flag to avoid early return
  3135. call dshot_tlm_create_packet ; Create telemetry packet (0 rpm)
  3136. wait_for_power_telemetry_done:
  3137. call wait10ms
  3138. mov A, Rcp_Timeout_Cntd ; Load RC pulse timeout counter value
  3139. jnz wait_for_power_on_not_missing ; If it is not zero - proceed
  3140. ljmp init_no_signal ; If pulses missing - go back to detect input signal
  3141. wait_for_power_on_not_missing:
  3142. jnb Flag_Rcp_Stop, wait_for_power_on_nonzero ; Higher than stop, Yes - proceed
  3143. mov A, DShot_Cmd
  3144. jz wait_for_power_on_loop ; Check DShot command if not zero, otherwise wait for power
  3145. call dshot_cmd_check
  3146. sjmp wait_for_power_on_not_missing ; Check DShot command again, in case it needs to be received multiple times
  3147. wait_for_power_on_nonzero:
  3148. call wait100ms ; Wait to see if start pulse was only a glitch
  3149. mov DShot_Cmd, #0 ; Reset DShot command
  3150. mov DShot_Cmd_Cnt, #0
  3151. mov A, Rcp_Timeout_Cntd ; Load RC pulse timeout counter value
  3152. jnz init_start ; If it is not zero - proceed
  3153. ljmp init_no_signal ; If it is zero (pulses missing) - go back to detect input signal
  3154. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3155. ;
  3156. ; Motor start entry point
  3157. ;
  3158. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3159. init_start:
  3160. clr IE_EA ; Disable interrupts
  3161. call switch_power_off
  3162. setb IE_EA ; Enable interrupts
  3163. clr A
  3164. mov Flags1, A ; Clear flags1
  3165. mov Flags_Startup, A ; Clear startup flags
  3166. mov Demag_Detected_Metric, A ; Clear demag metric
  3167. call wait1ms
  3168. ; Read initial average temperature
  3169. Start_Adc ; Start adc conversion
  3170. jnb ADC0CN0_ADINT, $ ; Wait for adc conversion to complete
  3171. Read_Adc_Result ; Read initial temperature
  3172. mov A, Temp2
  3173. jnz ($+3) ; Is reading below 256?
  3174. mov Temp1, A ; Yes - set average temperature value to zero
  3175. mov Current_Average_Temp, Temp1 ; Set initial average temperature
  3176. mov Adc_Conversion_Cnt, #8 ; Make sure a temp reading is done
  3177. call check_temp_and_limit_power
  3178. mov Adc_Conversion_Cnt, #8 ; Make sure a temp reading is done next time
  3179. ; Set up start operating conditions
  3180. clr IE_EA ; Disable interrupts
  3181. mov Temp2, #Pgm_Startup_Power_Max
  3182. mov Pwm_Limit_Beg, @Temp2 ; Set initial pwm limit
  3183. mov Pwm_Limit, Pwm_Limit_Beg
  3184. mov Pwm_Limit_By_Rpm, Pwm_Limit_Beg
  3185. setb IE_EA ; Enable interrupts
  3186. ; Begin startup sequence
  3187. IF MCU_48MHZ == 1
  3188. Set_MCU_Clk_48MHz
  3189. ; Scale DShot criteria for 48MHz
  3190. clr C
  3191. rlca DShot_Timer_Preset ; Scale sync timer preset
  3192. clr C
  3193. rlca DShot_Frame_Length_Thr ; Scale frame length criteria
  3194. clr C
  3195. rlca DShot_Pwm_Thr ; Scale pulse width criteria
  3196. ; Scale DShot telemetry for 24MHz
  3197. xcha DShot_GCR_Pulse_Time_1, DShot_GCR_Pulse_Time_1_Tmp
  3198. xcha DShot_GCR_Pulse_Time_2, DShot_GCR_Pulse_Time_2_Tmp
  3199. xcha DShot_GCR_Pulse_Time_3, DShot_GCR_Pulse_Time_3_Tmp
  3200. mov DShot_GCR_Start_Delay, #DSHOT_TLM_START_DELAY_48
  3201. ENDIF
  3202. jnb Flag_Pgm_Bidir, init_start_bidir_done ; Check if bidirectional operation
  3203. mov C, Flag_Rcp_Dir_Rev ; Read force direction
  3204. mov Flag_Pgm_Dir_Rev, C ; Set spinning direction
  3205. ;**** **** **** **** ****
  3206. ; Motor start beginning
  3207. init_start_bidir_done:
  3208. setb Flag_Startup_Phase ; Set startup phase flag
  3209. mov Startup_Cnt, #0 ; Reset counter
  3210. call comm5_comm6 ; Initialize commutation
  3211. call comm6_comm1
  3212. call initialize_timing ; Initialize timing
  3213. call calc_next_comm_timing ; Set virtual commutation point
  3214. call initialize_timing ; Initialize timing
  3215. call calc_next_comm_timing
  3216. call initialize_timing ; Initialize timing
  3217. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3218. ;
  3219. ; Run entry point
  3220. ;
  3221. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3222. ; Run 1 = B(p-on) + C(n-pwm) - comparator A evaluated
  3223. ; Out_cA changes from low to high
  3224. run1:
  3225. call wait_for_comp_out_high ; Wait for high
  3226. ; setup_comm_wait ; Setup wait time from zero cross to commutation
  3227. ; evaluate_comparator_integrity ; Check whether comparator reading has been normal
  3228. call wait_for_comm ; Wait from zero cross to commutation
  3229. call comm1_comm2 ; Commutate
  3230. call calc_next_comm_timing ; Calculate next timing and wait advance timing wait
  3231. ; wait_advance_timing ; Wait advance timing and start zero cross wait
  3232. ; calc_new_wait_times
  3233. ; wait_before_zc_scan ; Wait zero cross wait and start zero cross timeout
  3234. ; Run 2 = A(p-on) + C(n-pwm) - comparator B evaluated
  3235. ; Out_cB changes from high to low
  3236. run2:
  3237. call wait_for_comp_out_low
  3238. ; setup_comm_wait
  3239. ; evaluate_comparator_integrity
  3240. call set_pwm_limit ; Set pwm power limit for low or high rpm
  3241. call wait_for_comm
  3242. call comm2_comm3
  3243. call calc_next_comm_timing
  3244. ; wait_advance_timing
  3245. ; calc_new_wait_times
  3246. ; wait_before_zc_scan
  3247. ; Run 3 = A(p-on) + B(n-pwm) - comparator C evaluated
  3248. ; Out_cC changes from low to high
  3249. run3:
  3250. call wait_for_comp_out_high
  3251. ; setup_comm_wait
  3252. ; evaluate_comparator_integrity
  3253. call wait_for_comm
  3254. call comm3_comm4
  3255. call calc_next_comm_timing
  3256. ; wait_advance_timing
  3257. ; calc_new_wait_times
  3258. ; wait_before_zc_scan
  3259. ; Run 4 = C(p-on) + B(n-pwm) - comparator A evaluated
  3260. ; Out_cA changes from high to low
  3261. run4:
  3262. call wait_for_comp_out_low
  3263. ; setup_comm_wait
  3264. ; evaluate_comparator_integrity
  3265. call wait_for_comm
  3266. call comm4_comm5
  3267. call calc_next_comm_timing
  3268. ; wait_advance_timing
  3269. ; calc_new_wait_times
  3270. ; wait_before_zc_scan
  3271. ; Run 5 = C(p-on) + A(n-pwm) - comparator B evaluated
  3272. ; Out_cB changes from low to high
  3273. run5:
  3274. call wait_for_comp_out_high
  3275. ; setup_comm_wait
  3276. ; evaluate_comparator_integrity
  3277. call wait_for_comm
  3278. call comm5_comm6
  3279. call calc_next_comm_timing
  3280. ; wait_advance_timing
  3281. ; calc_new_wait_times
  3282. ; wait_before_zc_scan
  3283. ; Run 6 = B(p-on) + A(n-pwm) - comparator C evaluated
  3284. ; Out_cC changes from high to low
  3285. run6:
  3286. Start_Adc ; Start adc conversion
  3287. call wait_for_comp_out_low
  3288. ; setup_comm_wait
  3289. ; evaluate_comparator_integrity
  3290. call wait_for_comm
  3291. call comm6_comm1
  3292. call check_temp_and_limit_power
  3293. call calc_next_comm_timing
  3294. ; wait_advance_timing
  3295. ; calc_new_wait_times
  3296. ; wait_before_zc_scan
  3297. ; Check if it is direct startup
  3298. jnb Flag_Startup_Phase, normal_run_checks
  3299. mov Pwm_Limit, Pwm_Limit_Beg ; Set initial max power
  3300. clr C
  3301. mov A, Startup_Cnt ; Load startup counter
  3302. subb A, #24 ; Is counter above requirement?
  3303. jnc startup_phase_done
  3304. jnb Flag_Rcp_Stop, run1 ; If pulse is above stop value - Continue to run
  3305. ajmp run_to_wait_for_power_on
  3306. startup_phase_done:
  3307. clr Flag_Startup_Phase ; Clear startup phase flag
  3308. setb Flag_Initial_Run_Phase ; Set initial run phase flag
  3309. mov Initial_Run_Rot_Cntd, #12 ; Set initial run rotation count
  3310. mov Pwm_Limit, Pwm_Limit_Beg
  3311. mov Pwm_Limit_By_Rpm, Pwm_Limit_Beg
  3312. normal_run_checks:
  3313. ; Check if it is initial run phase
  3314. jnb Flag_Initial_Run_Phase, initial_run_phase_done ; If not initial run phase - branch
  3315. jb Flag_Dir_Change_Brake, initial_run_phase_done ; If a direction change - branch
  3316. ; Decrement startup rotation count
  3317. mov A, Initial_Run_Rot_Cntd
  3318. dec A
  3319. ; Check number of initial rotations
  3320. jnz initial_run_check_startup_rot ; Branch if counter is not zero
  3321. clr Flag_Initial_Run_Phase ; Clear initial run phase flag
  3322. setb Flag_Motor_Started ; Set motor started
  3323. jmp run1 ; Continue with normal run
  3324. initial_run_check_startup_rot:
  3325. mov Initial_Run_Rot_Cntd, A ; Not zero - store counter
  3326. jb Flag_Pgm_Bidir, initial_run_continue_run ; Check if bidirectional operation
  3327. jb Flag_Rcp_Stop, run_to_wait_for_power_on ; Check if pulse is below stop value
  3328. initial_run_continue_run:
  3329. jmp run1 ; Continue to run
  3330. initial_run_phase_done:
  3331. ; Reset stall count
  3332. mov Startup_Stall_Cnt, #0
  3333. setb Flag_Motor_Running
  3334. ; Exit run loop after a given time
  3335. jb Flag_Pgm_Bidir, run6_check_timeout ; Check if bidirectional operation
  3336. mov Temp1, #250
  3337. mov Temp2, #Pgm_Brake_On_Stop
  3338. mov A, @Temp2
  3339. jz ($+4)
  3340. mov Temp1, #3 ; About 100ms before stopping when brake is set
  3341. clr C
  3342. mov A, Rcp_Stop_Cnt ; Load stop RC pulse counter low byte value
  3343. subb A, Temp1 ; Is number of stop RC pulses above limit?
  3344. jnc run_to_wait_for_power_on ; Yes, go back to wait for power on
  3345. run6_check_timeout:
  3346. mov A, Rcp_Timeout_Cntd ; Load RC pulse timeout counter value
  3347. jz run_to_wait_for_power_on ; If it is zero - go back to wait for power on
  3348. run6_check_dir:
  3349. jnb Flag_Pgm_Bidir, run6_check_speed ; Check if bidirectional operation
  3350. jb Flag_Pgm_Dir_Rev, run6_check_dir_rev ; Check if actual rotation direction
  3351. jb Flag_Rcp_Dir_Rev, run6_check_dir_change ; Matches force direction
  3352. sjmp run6_check_speed
  3353. run6_check_dir_rev:
  3354. jnb Flag_Rcp_Dir_Rev, run6_check_dir_change
  3355. sjmp run6_check_speed
  3356. run6_check_dir_change:
  3357. jb Flag_Dir_Change_Brake, run6_check_speed
  3358. setb Flag_Dir_Change_Brake ; Set brake flag
  3359. mov Pwm_Limit, Pwm_Limit_Beg ; Set max power while braking
  3360. jmp run4 ; Go back to run 4, thereby changing force direction
  3361. run6_check_speed:
  3362. mov Temp1, #0F0h ; Default minimum speed
  3363. jnb Flag_Dir_Change_Brake, run6_brake_done; Is it a direction change?
  3364. mov Pwm_Limit, Pwm_Limit_Beg ; Set max power while braking
  3365. mov Temp1, #20h ; Bidirectional braking termination speed
  3366. run6_brake_done:
  3367. clr C
  3368. mov A, Comm_Period4x_H ; Is Comm_Period4x more than 32ms (~1220 eRPM)?
  3369. subb A, Temp1
  3370. ljc run1 ; No - go back to run 1
  3371. jnb Flag_Dir_Change_Brake, run_to_wait_for_power_on ; If it is not a direction change - stop
  3372. ; Turn spinning direction
  3373. clr Flag_Dir_Change_Brake ; Clear brake
  3374. mov C, Flag_Rcp_Dir_Rev ; Read force direction
  3375. mov Flag_Pgm_Dir_Rev, C ; Set spinning direction
  3376. setb Flag_Initial_Run_Phase
  3377. mov Initial_Run_Rot_Cntd, #18
  3378. mov Pwm_Limit, Pwm_Limit_Beg ; Set initial max power
  3379. jmp run1 ; Go back to run 1
  3380. run_to_wait_for_power_on_fail:
  3381. jb Flag_Motor_Running, run_to_wait_for_power_on
  3382. inc Startup_Stall_Cnt ; Increment stall count if motors did not properly start
  3383. run_to_wait_for_power_on:
  3384. clr IE_EA ; Disable all interrupts
  3385. call switch_power_off
  3386. mov Flags1, #0 ; Clear flags1
  3387. mov Flags_Startup, #0 ; Clear startup flags
  3388. IF MCU_48MHZ == 1
  3389. Set_MCU_Clk_24MHz
  3390. ; Scale DShot criteria for 24MHz
  3391. setb C
  3392. rrca DShot_Timer_Preset ; Scale sync timer preset
  3393. clr C
  3394. rrca DShot_Frame_Length_Thr ; Scale frame length criteria
  3395. clr C
  3396. rrca DShot_Pwm_Thr ; Scale pulse width criteria
  3397. ; Scale DShot telemetry for 24MHz
  3398. xcha DShot_GCR_Pulse_Time_1, DShot_GCR_Pulse_Time_1_Tmp
  3399. xcha DShot_GCR_Pulse_Time_2, DShot_GCR_Pulse_Time_2_Tmp
  3400. xcha DShot_GCR_Pulse_Time_3, DShot_GCR_Pulse_Time_3_Tmp
  3401. mov DShot_GCR_Start_Delay, #DSHOT_TLM_START_DELAY
  3402. ENDIF
  3403. setb IE_EA ; Enable all interrupts
  3404. call wait100ms ; Wait for pwm to be stopped
  3405. call switch_power_off
  3406. mov Temp1, #Pgm_Brake_On_Stop ; Check if using brake on stop
  3407. mov A, @Temp1
  3408. jz run_to_wait_for_power_on_brake_done
  3409. AcomFET_on
  3410. BcomFET_on
  3411. CcomFET_on
  3412. run_to_wait_for_power_on_brake_done:
  3413. jnb Flag_Rcp_Stop, ($+6) ; Check if RCP is zero, then it is a normal stop
  3414. mov Startup_Stall_Cnt, #0
  3415. clr C
  3416. mov A, Startup_Stall_Cnt
  3417. subb A, #4 ; Maximum consecutive stalls before stopping
  3418. ljc wait_for_power_on ; Go back to wait for power on
  3419. ljmp init_no_signal ; Stalled too many times
  3420. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3421. $include (BLHeliBootLoad.inc) ; Include source code for bootloader
  3422. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3423. CSEG AT 19FDh
  3424. reset:
  3425. ljmp pgm_start
  3426. END