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.

6421 lines
195 KiB

11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
9 years ago
9 years ago
9 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
9 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
11 years ago
  1. ;**** **** **** **** ****
  2. ;
  3. ; BLHeli program for controlling brushless motors in helicopters and multirotors
  4. ;
  5. ; Copyright 2011, 2012 Steffen Skaug
  6. ; This program is distributed under the terms of the GNU General Public License
  7. ;
  8. ; This file is part of BLHeli.
  9. ;
  10. ; BLHeli is free software: you can redistribute it and/or modify
  11. ; it under the terms of the GNU General Public License as published by
  12. ; the Free Software Foundation, either version 3 of the License, or
  13. ; (at your option) any later version.
  14. ;
  15. ; BLHeli is distributed in the hope that it will be useful,
  16. ; but WITHOUT ANY WARRANTY; without even the implied warranty of
  17. ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
  18. ; GNU General Public License for more details.
  19. ;
  20. ; You should have received a copy of the GNU General Public License
  21. ; along with BLHeli. If not, see <http://www.gnu.org/licenses/>.
  22. ;
  23. ;**** **** **** **** ****
  24. ;
  25. ; The software was initially designed for use with Eflite mCP X, but is now adapted to copters/planes in general
  26. ;
  27. ; The software was inspired by and started from from Bernard Konze's BLMC: http://home.versanet.de/~bkonze/blc_6a/blc_6a.htm
  28. ; And also Simon Kirby's TGY: https://github.com/sim-/tgy
  29. ;
  30. ; This file is best viewed with tab width set to 5
  31. ;
  32. ; The input signal can be positive 1kHz, 2kHz, 4kHz, 8kHz or 12kHz PWM (e.g. taken from the "resistor tap" on mCPx)
  33. ; And the input signal can be PPM (1-2ms) or OneShot125 (125-250us) at rates up to several hundred Hz.
  34. ; The code autodetects the various input modes/frequencies
  35. ; The code can also be programmed to accept inverted input signal.
  36. ;
  37. ; The first lines of the software must be modified according to the chosen environment:
  38. ; Uncomment the selected ESC and main/tail/multi mode
  39. ; BESC EQU "ESC"_"mode"
  40. ;
  41. ;**** **** **** **** ****
  42. ; Revision history:
  43. ; - Rev0.0: Initial revision
  44. ; - Rev1.0: Governor functionality added
  45. ; - Rev1.1: Increased tail gain to 1.0625. Implemented for tail only
  46. ; Decreased governor proportional and integral gain by 4
  47. ; Fixed bug that caused tail power not always to be max
  48. ; - Rev1.2: Governor integral gain should be higher in order to achieve full PWM range
  49. ; Integral gain can be higher, and is increased by 2x. An integral of +-128 can now be added to requested PWM
  50. ; - Rev1.3: Governor integral extended to 24bit
  51. ; Governor proportional gain increased by 2x
  52. ; Added slow spoolup/down for governor
  53. ; Set pwm to 100% (do not turn off nFET) for high values of current pwm
  54. ; Added support for PPM input (1us to 2us pulse)
  55. ; Removed USE_COMP_STORED as it was never used
  56. ; - Rev2.0 Added measurement of pwm frequency and support for 1kHz, 2kHz, 4kHz and 8kHz
  57. ; Optimized pwm on and off routines
  58. ; Improved mosfet switching in beep routines, to reduce current draw
  59. ; Added support for ICP1 interrupt pin input
  60. ; Added ADC measurement of supply voltage, with limiting of main motor power for low voltage
  61. ; Miscellaneous other changes
  62. ; - Rev2.1 Rewritten INT0 routine to be similar to ICP
  63. ; Reduced validation threshold (RCP_VALIDATE)
  64. ; Removed requirement for RCP to go to zero again in tail arming sequence
  65. ; Removed PPM support
  66. ; - Rev2.2 Added support for HC 5A 1S ESC with Atmega48V MPU
  67. ; Increased governor proportional gain by 2x
  68. ; - Rev3.0 Added functionality for programming from TX
  69. ; Added low voltage limit scaling for 2S and 3S
  70. ; - Rev11.2 Copied over from the SiLabs version and adapted to Atmel
  71. ; Now requiring a 16MHz capable MCU for fullspec performance
  72. ; - Rev12.0 Added programmable main spoolup time
  73. ; Added programmable temperature protection enable
  74. ; Bidirectional mode stop/start improved. Motor is now stopped before starting
  75. ; Power is limited for very low rpms (when BEMF is low), in order to avoid sync loss
  76. ; Damped light mode is made more smooth and quiet, particularly at low and high rpms
  77. ; Comparator signal qualification scheme is changed
  78. ; Demag compensation scheme is significantly changed
  79. ; Increased jitter tolerance for PPM frequency measurement
  80. ; Fully damped mode removed, and damped light only supported on damped capable ESCs
  81. ; Default tail mode changed to damped light
  82. ; Miscellaneous other changes
  83. ; - Rev12.1 Fixed bug in tail code
  84. ; Improved startup for Atmel
  85. ; Added support for multiple high BEC voltages
  86. ; Added support for RPM output
  87. ; - Rev12.2 Improved running smoothness, particularly for damped light
  88. ; Avoiding lockup at full throttle when input signal is noisy
  89. ; Avoiding detection of 1-wire programming signal as valid throttle signal
  90. ; - Rev13.0 Removed throttle change rate and damping force parameters
  91. ; Temperature protection default set to off
  92. ; Added support for OneShot125
  93. ; Improved commutation timing accuracy
  94. ; - Rev13.1 Removed startup ramp for MULTI
  95. ; Improved startup for some odd ESCs
  96. ; - Rev13.2 Still tweaking startup to make it more reliable and faster for all ESC/motor combos
  97. ; Increased deadband for bidirectional operation
  98. ; Relaxed signal detection criteria
  99. ; Miscellaneous other changes
  100. ; - Rev14.0 Improved running at high RPMs and increased max RPM limit
  101. ; Improved reliability of 3D (bidirectional) mode and startup
  102. ; Avoid being locked in bootloader (implemented in Suite 13202)
  103. ; Smoother running and greatly reduced step to full power in damped light mode
  104. ; Removed low voltage limiting for MULTI
  105. ; Added pwm dither parameter
  106. ; Added setting for enable/disable of low RPM power protection
  107. ; Added setting for enable/disable of PWM input
  108. ; Better AFW and damping for some ESCs (that have a slow high side driver)
  109. ; Miscellaneous other changes
  110. ; - Rev14.1 Fixed max throttle calibration bug (for non-oneshot)
  111. ; Fixed some closed loop mode bugs
  112. ; Relaxed signal jitter requirement for looptimes below 1000
  113. ; Added skipping of damping fet switching near max power, for improved high end throttle linearity, using the concept of SimonK
  114. ; Improved sync hold at high rpms
  115. ; - Rev14.2 Improved quality of comparator reading, giving better bemf detection (for improved startup amongst others)
  116. ; Removed mechanism where some ESCs could reset upon starting motor (due to ADC being read)
  117. ; Added stalled motor shutoff after about 10 seconds (for tail and multi code with PPM input)
  118. ; Greatly increased maximum rpm limit, and added rpm limiting at 250k erpm
  119. ; Improved bidirectional operation
  120. ; - Rev14.3 Moved reset vector to be just before the settings segment, in order to better recover from partially failed flashing operation
  121. ; Shortened stall detect time to about 5sec, and prevented going into tx programming after a stall
  122. ; Optimizations of software timing and running reliability
  123. ; - Rev14.4 Improved startup, particularly for larger motors
  124. ; Improved running at very high rpms
  125. ; Made damped light default for MULTI on ESCs that support it
  126. ; Miscellaneous other changes
  127. ;
  128. ;
  129. ;
  130. ;**** **** **** **** ****
  131. ; 8K Bytes of In-System Self-Programmable Flash
  132. ; 1K Bytes Internal SRAM
  133. ; 512 Bytes Internal EEPROM
  134. ; 16MHz clock
  135. ;
  136. ;**** **** **** **** ****
  137. ; Timer 0 (500ns counts) always counts up and is used for
  138. ; - RC pulse timeout and skip counts
  139. ; Timer 1 (500ns counts) always counts up and is used for
  140. ; - RC pulse measurement (via external interrupt 0 or input capture pin)
  141. ; - Commutation timing (via output compare register A interrupt)
  142. ; Timer 2 (500ns counts) always counts up and is used for
  143. ; - PWM generation
  144. ;
  145. ;**** **** **** **** ****
  146. ; Interrupt handling
  147. ; The Atmega8 disables all interrupts when entering an interrupt routine,
  148. ; The code reenables interrupts in some interrupt routines, in order to nest pwm interrupts
  149. ; - Interrupts are disabled during beeps, to avoid audible interference from interrupts
  150. ; - RC pulse interrupts are periodically disabled in order to reduce interference with pwm interrupts.
  151. ;
  152. ;**** **** **** **** ****
  153. ; Motor control:
  154. ; - Brushless motor control with 6 states for each electrical 360 degrees
  155. ; - An advance timing of 0deg has zero cross 30deg after one commutation and 30deg before the next
  156. ; - Timing advance in this implementation is set to 15deg nominally
  157. ; - "Damped" commutation schemes are available, where more than one pfet is on when pwm is off. This will absorb energy from bemf and make step settling more damped.
  158. ; Motor sequence starting from zero crossing:
  159. ; - Timer wait: Wt_Comm 15deg ; Time to wait from zero cross to actual commutation
  160. ; - Timer wait: Wt_Advance 15deg ; Time to wait for timing advance. Nominal commutation point is after this
  161. ; - Timer wait: Wt_Zc_Scan 7.5deg ; Time to wait before looking for zero cross
  162. ; - Scan for zero cross 22.5deg , Nominal, with some motor variations
  163. ;
  164. ; Motor startup:
  165. ; There is a startup phase and an initial run phase, before normal bemf commutation run begins.
  166. ;
  167. ;**** **** **** **** ****
  168. ; Select the ESC and mode to use (or unselect all for use with external batch compile file);
  169. ;#define BLUESERIES_12A_MAIN
  170. ;#define BLUESERIES_12A_TAIL
  171. ;#define BLUESERIES_12A_MULTI
  172. ;#define BLUESERIES_20A_MAIN
  173. ;#define BLUESERIES_20A_TAIL
  174. ;#define BLUESERIES_20A_MULTI
  175. ;#define BLUESERIES_30A_MAIN
  176. ;#define BLUESERIES_30A_TAIL
  177. ;#define BLUESERIES_30A_MULTI
  178. ;#define BLUESERIES_40A_MAIN
  179. ;#define BLUESERIES_40A_TAIL
  180. ;#define BLUESERIES_40A_MULTI
  181. ;#define BLUESERIES_60A_MAIN
  182. ;#define BLUESERIES_60A_TAIL
  183. ;#define BLUESERIES_60A_MULTI
  184. ;#define BLUESERIES_70A_MAIN
  185. ;#define BLUESERIES_70A_TAIL
  186. ;#define BLUESERIES_70A_MULTI
  187. ;#define HK_UBEC_6A_MAIN
  188. ;#define HK_UBEC_6A_TAIL
  189. ;#define HK_UBEC_6A_MULTI
  190. ;#define HK_UBEC_10A_MAIN
  191. ;#define HK_UBEC_10A_TAIL
  192. ;#define HK_UBEC_10A_MULTI
  193. ;#define HK_UBEC_20A_MAIN
  194. ;#define HK_UBEC_20A_TAIL
  195. ;#define HK_UBEC_20A_MULTI
  196. ;#define HK_UBEC_30A_MAIN
  197. ;#define HK_UBEC_30A_TAIL
  198. ;#define HK_UBEC_30A_MULTI
  199. ;#define HK_UBEC_40A_MAIN
  200. ;#define HK_UBEC_40A_TAIL
  201. ;#define HK_UBEC_40A_MULTI
  202. ;#define SUPERSIMPLE_18A_MAIN
  203. ;#define SUPERSIMPLE_18A_TAIL
  204. ;#define SUPERSIMPLE_18A_MULTI
  205. ;#define SUPERSIMPLE_20A_MAIN
  206. ;#define SUPERSIMPLE_20A_TAIL
  207. ;#define SUPERSIMPLE_20A_MULTI
  208. ;#define SUPERSIMPLE_30A_MAIN
  209. ;#define SUPERSIMPLE_30A_TAIL
  210. ;#define SUPERSIMPLE_30A_MULTI
  211. ;#define SUPERSIMPLE_40A_MAIN
  212. ;#define SUPERSIMPLE_40A_TAIL
  213. ;#define SUPERSIMPLE_40A_MULTI
  214. ;#define MULTISTAR_10Av2_MAIN
  215. ;#define MULTISTAR_10Av2_TAIL
  216. ;#define MULTISTAR_10Av2_MULTI
  217. ;#define MULTISTAR_15A_MAIN ; Inverted input
  218. ;#define MULTISTAR_15A_TAIL
  219. ;#define MULTISTAR_15A_MULTI
  220. ;#define MULTISTAR_20A_MAIN ; Inverted input
  221. ;#define MULTISTAR_20A_TAIL
  222. ;#define MULTISTAR_20A_MULTI
  223. ;#define MULTISTAR_20A_NFET_MAIN ; Inverted input
  224. ;#define MULTISTAR_20A_NFET_TAIL
  225. ;#define MULTISTAR_20A_NFET_MULTI
  226. ;#define MULTISTAR_20Av2_MAIN
  227. ;#define MULTISTAR_20Av2_TAIL
  228. ;#define MULTISTAR_20Av2_MULTI
  229. ;#define MULTISTAR_30A_MAIN ; Inverted input
  230. ;#define MULTISTAR_30A_TAIL
  231. ;#define MULTISTAR_30A_MULTI
  232. ;#define MULTISTAR_40Av2_MAIN
  233. ;#define MULTISTAR_40Av2_TAIL
  234. ;#define MULTISTAR_40Av2_MULTI
  235. ;#define MULTISTAR_45A_MAIN ; Inverted input
  236. ;#define MULTISTAR_45A_TAIL
  237. ;#define MULTISTAR_45A_MULTI
  238. ;#define MYSTERY_12A_MAIN
  239. ;#define MYSTERY_12A_TAIL
  240. ;#define MYSTERY_12A_MULTI
  241. ;#define MYSTERY_30A_MAIN
  242. ;#define MYSTERY_30A_TAIL
  243. ;#define MYSTERY_30A_MULTI
  244. ;#define MYSTERY_40A_MAIN
  245. ;#define MYSTERY_40A_TAIL
  246. ;#define MYSTERY_40A_MULTI
  247. ;#define SUNRISE_HIMULTI_20A_MAIN ; Inverted input
  248. ;#define SUNRISE_HIMULTI_20A_TAIL
  249. ;#define SUNRISE_HIMULTI_20A_MULTI
  250. ;#define SUNRISE_HIMULTI_30A_MAIN ; Inverted input
  251. ;#define SUNRISE_HIMULTI_30A_TAIL
  252. ;#define SUNRISE_HIMULTI_30A_MULTI
  253. ;#define SUNRISE_HIMULTI_40A_MAIN ; Inverted input
  254. ;#define SUNRISE_HIMULTI_40A_TAIL
  255. ;#define SUNRISE_HIMULTI_40A_MULTI
  256. ;#define RCTIMER_40A_MAIN
  257. ;#define RCTIMER_40A_TAIL
  258. ;#define RCTIMER_40A_MULTI
  259. ;#define RCTIMER_NFS_30A_MAIN ; ICP1 as input
  260. ;#define RCTIMER_NFS_30A_TAIL
  261. ;#define RCTIMER_NFS_30A_MULTI
  262. ;#define YEP_7A_MAIN
  263. ;#define YEP_7A_TAIL
  264. ;#define YEP_7A_MULTI
  265. ;#define AFRO_12A_MAIN ; ICP1 as input
  266. ;#define AFRO_12A_TAIL
  267. ;#define AFRO_12A_MULTI
  268. ;#define AFRO_20A_MAIN ; ICP1 as input
  269. ;#define AFRO_20A_TAIL
  270. ;#define AFRO_20A_MULTI
  271. ;#define AFRO_20A_HV_MAIN ; ICP1 as input
  272. ;#define AFRO_20A_HV_TAIL
  273. ;#define AFRO_20A_HV_MULTI
  274. ;#define AFRO_30A_MAIN ; ICP1 as input
  275. ;#define AFRO_30A_TAIL
  276. ;#define AFRO_30A_MULTI
  277. ;#define SUNRISE_BLHELI_SLIM_20A_MAIN
  278. ;#define SUNRISE_BLHELI_SLIM_20A_TAIL
  279. ;#define SUNRISE_BLHELI_SLIM_20A_MULTI
  280. ;#define SUNRISE_BLHELI_SLIM_30A_MAIN
  281. ;#define SUNRISE_BLHELI_SLIM_30A_TAIL
  282. ;#define SUNRISE_BLHELI_SLIM_30A_MULTI
  283. ;#define DYS_SN20A_MAIN ; ICP1 as input
  284. ;#define DYS_SN20A_TAIL
  285. ;#define DYS_SN20A_MULTI
  286. ;#define TBS_CUBE_20A_MAIN ; ICP1 as input
  287. ;#define TBS_CUBE_20A_TAIL
  288. ;#define TBS_CUBE_20A_MULTI
  289. ;#define ZTW_SPIDER_LITE_18Av2_MAIN
  290. ;#define ZTW_SPIDER_LITE_18Av2_TAIL
  291. ;#define ZTW_SPIDER_LITE_18Av2_MULTI
  292. ;**** **** **** **** ****
  293. ; ESC selection statements
  294. #if defined(BLUESERIES_12A_MAIN)
  295. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  296. .INCLUDE "BlueSeries_12A.inc" ; Select BlueSeries 12A pinout
  297. #endif
  298. #if defined(BLUESERIES_12A_TAIL)
  299. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  300. .INCLUDE "BlueSeries_12A.inc" ; Select BlueSeries 12A pinout
  301. #endif
  302. #if defined(BLUESERIES_12A_MULTI)
  303. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  304. .INCLUDE "BlueSeries_12A.inc" ; Select BlueSeries 12A pinout
  305. #endif
  306. #if defined(BLUESERIES_20A_MAIN)
  307. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  308. .INCLUDE "BlueSeries_20A.inc" ; Select BlueSeries 20A pinout
  309. #endif
  310. #if defined(BLUESERIES_20A_TAIL)
  311. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  312. .INCLUDE "BlueSeries_20A.inc" ; Select BlueSeries 20A pinout
  313. #endif
  314. #if defined(BLUESERIES_20A_MULTI)
  315. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  316. .INCLUDE "BlueSeries_20A.inc" ; Select BlueSeries 20A pinout
  317. #endif
  318. #if defined(BLUESERIES_30A_MAIN)
  319. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  320. .INCLUDE "BlueSeries_30A.inc" ; Select BlueSeries 30A pinout
  321. #endif
  322. #if defined(BLUESERIES_30A_TAIL)
  323. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  324. .INCLUDE "BlueSeries_30A.inc" ; Select BlueSeries 30A pinout
  325. #endif
  326. #if defined(BLUESERIES_30A_MULTI)
  327. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  328. .INCLUDE "BlueSeries_30A.inc" ; Select BlueSeries 30A pinout
  329. #endif
  330. #if defined(BLUESERIES_40A_MAIN)
  331. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  332. .INCLUDE "BlueSeries_40A.inc" ; Select BlueSeries 40A pinout
  333. #endif
  334. #if defined(BLUESERIES_40A_TAIL)
  335. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  336. .INCLUDE "BlueSeries_40A.inc" ; Select BlueSeries 40A pinout
  337. #endif
  338. #if defined(BLUESERIES_40A_MULTI)
  339. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  340. .INCLUDE "BlueSeries_40A.inc" ; Select BlueSeries 40A pinout
  341. #endif
  342. #if defined(BLUESERIES_60A_MAIN)
  343. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  344. .INCLUDE "BlueSeries_60A.inc" ; Select BlueSeries 60A pinout
  345. #endif
  346. #if defined(BLUESERIES_60A_TAIL)
  347. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  348. .INCLUDE "BlueSeries_60A.inc" ; Select BlueSeries 60A pinout
  349. #endif
  350. #if defined(BLUESERIES_60A_MULTI)
  351. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  352. .INCLUDE "BlueSeries_60A.inc" ; Select BlueSeries 60A pinout
  353. #endif
  354. #if defined(BLUESERIES_70A_MAIN)
  355. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  356. .INCLUDE "BlueSeries_70A.inc" ; Select BlueSeries 70A pinout
  357. #endif
  358. #if defined(BLUESERIES_70A_TAIL)
  359. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  360. .INCLUDE "BlueSeries_70A.inc" ; Select BlueSeries 70A pinout
  361. #endif
  362. #if defined(BLUESERIES_70A_MULTI)
  363. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  364. .INCLUDE "BlueSeries_70A.inc" ; Select BlueSeries 70A pinout
  365. #endif
  366. #if defined(HK_UBEC_6A_MAIN)
  367. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  368. .INCLUDE "HK_UBEC_6A.inc" ; Select Hobbyking UBEC 6A pinout
  369. #endif
  370. #if defined(HK_UBEC_6A_TAIL)
  371. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  372. .INCLUDE "HK_UBEC_6A.inc" ; Select Hobbyking UBEC 6A pinout
  373. #endif
  374. #if defined(HK_UBEC_6A_MULTI)
  375. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  376. .INCLUDE "HK_UBEC_6A.inc" ; Select Hobbyking UBEC 6A pinout
  377. #endif
  378. #if defined(HK_UBEC_10A_MAIN)
  379. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  380. .INCLUDE "HK_UBEC_10A.inc" ; Select Hobbyking UBEC 10A pinout
  381. #endif
  382. #if defined(HK_UBEC_10A_TAIL)
  383. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  384. .INCLUDE "HK_UBEC_10A.inc" ; Select Hobbyking UBEC 10A pinout
  385. #endif
  386. #if defined(HK_UBEC_10A_MULTI)
  387. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  388. .INCLUDE "HK_UBEC_10A.inc" ; Select Hobbyking UBEC 10A pinout
  389. #endif
  390. #if defined(HK_UBEC_20A_MAIN)
  391. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  392. .INCLUDE "HK_UBEC_20A.inc" ; Select Hobbyking UBEC 20A pinout
  393. #endif
  394. #if defined(HK_UBEC_20A_TAIL)
  395. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  396. .INCLUDE "HK_UBEC_20A.inc" ; Select Hobbyking UBEC 20A pinout
  397. #endif
  398. #if defined(HK_UBEC_20A_MULTI)
  399. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  400. .INCLUDE "HK_UBEC_20A.inc" ; Select Hobbyking UBEC 20A pinout
  401. #endif
  402. #if defined(HK_UBEC_30A_MAIN)
  403. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  404. .INCLUDE "HK_UBEC_30A.inc" ; Select Hobbyking UBEC 30A pinout
  405. #endif
  406. #if defined(HK_UBEC_30A_TAIL)
  407. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  408. .INCLUDE "HK_UBEC_30A.inc" ; Select Hobbyking UBEC 30A pinout
  409. #endif
  410. #if defined(HK_UBEC_30A_MULTI)
  411. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  412. .INCLUDE "HK_UBEC_30A.inc" ; Select Hobbyking UBEC 30A pinout
  413. #endif
  414. #if defined(HK_UBEC_40A_MAIN)
  415. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  416. .INCLUDE "HK_UBEC_40A.inc" ; Select Hobbyking UBEC 40A pinout
  417. #endif
  418. #if defined(HK_UBEC_40A_TAIL)
  419. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  420. .INCLUDE "HK_UBEC_40A.inc" ; Select Hobbyking UBEC 40A pinout
  421. #endif
  422. #if defined(HK_UBEC_40A_MULTI)
  423. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  424. .INCLUDE "HK_UBEC_40A.inc" ; Select Hobbyking UBEC 40A pinout
  425. #endif
  426. #if defined(SUPERSIMPLE_18A_MAIN)
  427. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  428. .INCLUDE "SuperSimple_18A.inc" ; Select SuperSimple 18A pinout
  429. #endif
  430. #if defined(SUPERSIMPLE_18A_TAIL)
  431. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  432. .INCLUDE "SuperSimple_18A.inc" ; Select SuperSimple 18A pinout
  433. #endif
  434. #if defined(SUPERSIMPLE_18A_MULTI)
  435. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  436. .INCLUDE "SuperSimple_18A.inc" ; Select SuperSimple 18A pinout
  437. #endif
  438. #if defined(SUPERSIMPLE_20A_MAIN)
  439. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  440. .INCLUDE "SuperSimple_20A.inc" ; Select SuperSimple 20A pinout
  441. #endif
  442. #if defined(SUPERSIMPLE_20A_TAIL)
  443. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  444. .INCLUDE "SuperSimple_20A.inc" ; Select SuperSimple 20A pinout
  445. #endif
  446. #if defined(SUPERSIMPLE_20A_MULTI)
  447. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  448. .INCLUDE "SuperSimple_20A.inc" ; Select SuperSimple 20A pinout
  449. #endif
  450. #if defined(SUPERSIMPLE_30A_MAIN)
  451. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  452. .INCLUDE "SuperSimple_30A.inc" ; Select SuperSimple 30A pinout
  453. #endif
  454. #if defined(SUPERSIMPLE_30A_TAIL)
  455. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  456. .INCLUDE "SuperSimple_30A.inc" ; Select SuperSimple 30A pinout
  457. #endif
  458. #if defined(SUPERSIMPLE_30A_MULTI)
  459. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  460. .INCLUDE "SuperSimple_30A.inc" ; Select SuperSimple 30A pinout
  461. #endif
  462. #if defined(SUPERSIMPLE_40A_MAIN)
  463. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  464. .INCLUDE "SuperSimple_40A.inc" ; Select SuperSimple 40A pinout
  465. #endif
  466. #if defined(SUPERSIMPLE_40A_TAIL)
  467. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  468. .INCLUDE "SuperSimple_40A.inc" ; Select SuperSimple 40A pinout
  469. #endif
  470. #if defined(SUPERSIMPLE_40A_MULTI)
  471. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  472. .INCLUDE "SuperSimple_40A.inc" ; Select SuperSimple 40A pinout
  473. #endif
  474. #if defined(MULTISTAR_10Av2_MAIN)
  475. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  476. .INCLUDE "Multistar_10Av2.inc" ; Select Multistar 10A v2 pinout
  477. #endif
  478. #if defined(MULTISTAR_10Av2_TAIL)
  479. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  480. .INCLUDE "Multistar_10Av2.inc" ; Select Multistar 10A v2 pinout
  481. #endif
  482. #if defined(MULTISTAR_10Av2_MULTI)
  483. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  484. .INCLUDE "Multistar_10Av2.inc" ; Select Multistar 10A v2 pinout
  485. #endif
  486. #if defined(MULTISTAR_15A_MAIN)
  487. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  488. .INCLUDE "Multistar_15A.inc" ; Select Multistar 15A pinout
  489. #endif
  490. #if defined(MULTISTAR_15A_TAIL)
  491. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  492. .INCLUDE "Multistar_15A.inc" ; Select Multistar 15A pinout
  493. #endif
  494. #if defined(MULTISTAR_15A_MULTI)
  495. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  496. .INCLUDE "Multistar_15A.inc" ; Select Multistar 15A pinout
  497. #endif
  498. #if defined(MULTISTAR_20A_MAIN)
  499. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  500. .INCLUDE "Multistar_20A.inc" ; Select Multistar 20A pinout
  501. #endif
  502. #if defined(MULTISTAR_20A_TAIL)
  503. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  504. .INCLUDE "Multistar_20A.inc" ; Select Multistar 20A pinout
  505. #endif
  506. #if defined(MULTISTAR_20A_MULTI)
  507. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  508. .INCLUDE "Multistar_20A.inc" ; Select Multistar 20A pinout
  509. #endif
  510. #if defined(MULTISTAR_20A_NFET_MAIN)
  511. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  512. .INCLUDE "Multistar_20A_NFET.inc" ; Select Multistar 20A NFET pinout
  513. #endif
  514. #if defined(MULTISTAR_20A_NFET_TAIL)
  515. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  516. .INCLUDE "Multistar_20A_NFET.inc" ; Select Multistar 20A NFET pinout
  517. #endif
  518. #if defined(MULTISTAR_20A_NFET_MULTI)
  519. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  520. .INCLUDE "Multistar_20A_NFET.inc" ; Select Multistar 20A NFET pinout
  521. #endif
  522. #if defined(MULTISTAR_20Av2_MAIN)
  523. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  524. .INCLUDE "Multistar_20Av2.inc" ; Select Multistar 20A v2 pinout
  525. #endif
  526. #if defined(MULTISTAR_20Av2_TAIL)
  527. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  528. .INCLUDE "Multistar_20Av2.inc" ; Select Multistar 20A v2 pinout
  529. #endif
  530. #if defined(MULTISTAR_20Av2_MULTI)
  531. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  532. .INCLUDE "Multistar_20Av2.inc" ; Select Multistar 20A v2 pinout
  533. #endif
  534. #if defined(MULTISTAR_30A_MAIN)
  535. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  536. .INCLUDE "Multistar_30A.inc" ; Select Multistar 30A pinout
  537. #endif
  538. #if defined(MULTISTAR_30A_TAIL)
  539. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  540. .INCLUDE "Multistar_30A.inc" ; Select Multistar 30A pinout
  541. #endif
  542. #if defined(MULTISTAR_30A_MULTI)
  543. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  544. .INCLUDE "Multistar_30A.inc" ; Select Multistar 30A pinout
  545. #endif
  546. #if defined(MULTISTAR_40Av2_MAIN)
  547. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  548. .INCLUDE "Multistar_40Av2.inc" ; Select Multistar 40A v2 pinout
  549. #endif
  550. #if defined(MULTISTAR_40Av2_TAIL)
  551. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  552. .INCLUDE "Multistar_40Av2.inc" ; Select Multistar 40A v2 pinout
  553. #endif
  554. #if defined(MULTISTAR_40Av2_MULTI)
  555. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  556. .INCLUDE "Multistar_40Av2.inc" ; Select Multistar 40A v2 pinout
  557. #endif
  558. #if defined(MULTISTAR_45A_MAIN)
  559. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  560. .INCLUDE "Multistar_45A.inc" ; Select Multistar 45A pinout
  561. #endif
  562. #if defined(MULTISTAR_45A_TAIL)
  563. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  564. .INCLUDE "Multistar_45A.inc" ; Select Multistar 45A pinout
  565. #endif
  566. #if defined(MULTISTAR_45A_MULTI)
  567. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  568. .INCLUDE "Multistar_45A.inc" ; Select Multistar 45A pinout
  569. #endif
  570. #if defined(MYSTERY_12A_MAIN)
  571. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  572. .INCLUDE "Mystery_12A.inc" ; Select Mystery 12A pinout
  573. #endif
  574. #if defined(MYSTERY_12A_TAIL)
  575. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  576. .INCLUDE "Mystery_12A.inc" ; Select Mystery 12A pinout
  577. #endif
  578. #if defined(MYSTERY_12A_MULTI)
  579. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  580. .INCLUDE "Mystery_12A.inc" ; Select Mystery 12A pinout
  581. #endif
  582. #if defined(MYSTERY_30A_MAIN)
  583. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  584. .INCLUDE "Mystery_30A.inc" ; Select Mystery 30A pinout
  585. #endif
  586. #if defined(MYSTERY_30A_TAIL)
  587. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  588. .INCLUDE "Mystery_30A.inc" ; Select Mystery 30A pinout
  589. #endif
  590. #if defined(MYSTERY_30A_MULTI)
  591. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  592. .INCLUDE "Mystery_30A.inc" ; Select Mystery 30A pinout
  593. #endif
  594. #if defined(MYSTERY_40A_MAIN)
  595. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  596. .INCLUDE "Mystery_40A.inc" ; Select Mystery 40A pinout
  597. #endif
  598. #if defined(MYSTERY_40A_TAIL)
  599. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  600. .INCLUDE "Mystery_40A.inc" ; Select Mystery 40A pinout
  601. #endif
  602. #if defined(MYSTERY_40A_MULTI)
  603. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  604. .INCLUDE "Mystery_40A.inc" ; Select Mystery 40A pinout
  605. #endif
  606. #if defined(SUNRISE_HIMULTI_20A_MAIN)
  607. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  608. .INCLUDE "Sunrise_HiMulti_20A.inc" ; Select Sunrise HiMulti 20A pinout
  609. #endif
  610. #if defined(SUNRISE_HIMULTI_20A_TAIL)
  611. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  612. .INCLUDE "Sunrise_HiMulti_20A.inc" ; Select Sunrise HiMulti 20A pinout
  613. #endif
  614. #if defined(SUNRISE_HIMULTI_20A_MULTI)
  615. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  616. .INCLUDE "Sunrise_HiMulti_20A.inc" ; Select Sunrise HiMulti 20A pinout
  617. #endif
  618. #if defined(SUNRISE_HIMULTI_30A_MAIN)
  619. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  620. .INCLUDE "Sunrise_HiMulti_30A.inc" ; Select Sunrise HiMulti 30A pinout
  621. #endif
  622. #if defined(SUNRISE_HIMULTI_30A_TAIL)
  623. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  624. .INCLUDE "Sunrise_HiMulti_30A.inc" ; Select Sunrise HiMulti 30A pinout
  625. #endif
  626. #if defined(SUNRISE_HIMULTI_30A_MULTI)
  627. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  628. .INCLUDE "Sunrise_HiMulti_30A.inc" ; Select Sunrise HiMulti 30A pinout
  629. #endif
  630. #if defined(SUNRISE_HIMULTI_40A_MAIN)
  631. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  632. .INCLUDE "Sunrise_HiMulti_40A.inc" ; Select Sunrise HiMulti 40A pinout
  633. #endif
  634. #if defined(SUNRISE_HIMULTI_40A_TAIL)
  635. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  636. .INCLUDE "Sunrise_HiMulti_40A.inc" ; Select Sunrise HiMulti 40A pinout
  637. #endif
  638. #if defined(SUNRISE_HIMULTI_40A_MULTI)
  639. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  640. .INCLUDE "Sunrise_HiMulti_40A.inc" ; Select Sunrise HiMulti 40A pinout
  641. #endif
  642. #if defined(RCTIMER_40A_MAIN)
  643. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  644. .INCLUDE "RCTimer_40A.inc" ; Select RCTimer 40A pinout
  645. #endif
  646. #if defined(RCTIMER_40A_TAIL)
  647. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  648. .INCLUDE "RCTimer_40A.inc" ; Select RCTimer 40A pinout
  649. #endif
  650. #if defined(RCTIMER_40A_MULTI)
  651. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  652. .INCLUDE "RCTimer_40A.inc" ; Select RCTimer 40A pinout
  653. #endif
  654. #if defined(RCTIMER_NFS_30A_MAIN)
  655. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  656. .INCLUDE "RCTimer_NFS_30A.inc" ; Select RCTimer NFS 30A pinout
  657. #endif
  658. #if defined(RCTIMER_NFS_30A_TAIL)
  659. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  660. .INCLUDE "RCTimer_NFS_30A.inc" ; Select RCTimer NFS 30A pinout
  661. #endif
  662. #if defined(RCTIMER_NFS_30A_MULTI)
  663. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  664. .INCLUDE "RCTimer_NFS_30A.inc" ; Select RCTimer NFS 30A pinout
  665. #endif
  666. #if defined(YEP_7A_MAIN)
  667. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  668. .INCLUDE "YEP_7A.inc" ; Select YEP 7A pinout
  669. #endif
  670. #if defined(YEP_7A_TAIL)
  671. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  672. .INCLUDE "YEP_7A.inc" ; Select YEP 7A pinout
  673. #endif
  674. #if defined(YEP_7A_MULTI)
  675. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  676. .INCLUDE "YEP_7A.inc" ; Select YEP 7A pinout
  677. #endif
  678. #if defined(AFRO_12A_MAIN)
  679. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  680. .INCLUDE "AFRO_12A.inc" ; Select AFRO 12A pinout
  681. #endif
  682. #if defined(AFRO_12A_TAIL)
  683. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  684. .INCLUDE "AFRO_12A.inc" ; Select AFRO 12A pinout
  685. #endif
  686. #if defined(AFRO_12A_MULTI)
  687. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  688. .INCLUDE "AFRO_12A.inc" ; Select AFRO 12A pinout
  689. #endif
  690. #if defined(AFRO_20A_MAIN)
  691. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  692. .INCLUDE "AFRO_20A.inc" ; Select AFRO 20A pinout
  693. #endif
  694. #if defined(AFRO_20A_TAIL)
  695. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  696. .INCLUDE "AFRO_20A.inc" ; Select AFRO 20A pinout
  697. #endif
  698. #if defined(AFRO_20A_MULTI)
  699. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  700. .INCLUDE "AFRO_20A.inc" ; Select AFRO 20A pinout
  701. #endif
  702. #if defined(AFRO_20A_HV_MAIN)
  703. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  704. .INCLUDE "AFRO_20A_HV.inc" ; Select AFRO 20A HV pinout
  705. #endif
  706. #if defined(AFRO_20A_HV_TAIL)
  707. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  708. .INCLUDE "AFRO_20A_HV.inc" ; Select AFRO 20A HV pinout
  709. #endif
  710. #if defined(AFRO_20A_HV_MULTI)
  711. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  712. .INCLUDE "AFRO_20A_HV.inc" ; Select AFRO 20A HV pinout
  713. #endif
  714. #if defined(AFRO_30A_MAIN)
  715. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  716. .INCLUDE "AFRO_30A.inc" ; Select AFRO 30A pinout
  717. #endif
  718. #if defined(AFRO_30A_TAIL)
  719. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  720. .INCLUDE "AFRO_30A.inc" ; Select AFRO 30A pinout
  721. #endif
  722. #if defined(AFRO_30A_MULTI)
  723. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  724. .INCLUDE "AFRO_30A.inc" ; Select AFRO 30A pinout
  725. #endif
  726. #if defined(SUNRISE_BLHELI_SLIM_20A_MAIN)
  727. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  728. .INCLUDE "Sunrise_BLHeli_Slim_20A.inc" ; Select Sunrise BLHeli slim 20A pinout
  729. #endif
  730. #if defined(SUNRISE_BLHELI_SLIM_20A_TAIL)
  731. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  732. .INCLUDE "Sunrise_BLHeli_Slim_20A.inc" ; Select Sunrise BLHeli slim 20A pinout
  733. #endif
  734. #if defined(SUNRISE_BLHELI_SLIM_20A_MULTI)
  735. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  736. .INCLUDE "Sunrise_BLHeli_Slim_20A.inc" ; Select Sunrise BLHeli slim 20A pinout
  737. #endif
  738. #if defined(SUNRISE_BLHELI_SLIM_30A_MAIN)
  739. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  740. .INCLUDE "Sunrise_BLHeli_Slim_30A.inc" ; Select Sunrise BLHeli slim 30A pinout
  741. #endif
  742. #if defined(SUNRISE_BLHELI_SLIM_30A_TAIL)
  743. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  744. .INCLUDE "Sunrise_BLHeli_Slim_30A.inc" ; Select Sunrise BLHeli slim 30A pinout
  745. #endif
  746. #if defined(SUNRISE_BLHELI_SLIM_30A_MULTI)
  747. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  748. .INCLUDE "Sunrise_BLHeli_Slim_30A.inc" ; Select Sunrise BLHeli slim 30A pinout
  749. #endif
  750. #if defined(DYS_SN20A_MAIN)
  751. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  752. .INCLUDE "DYS_SN20A.inc" ; Select DYS SN20A pinout
  753. #endif
  754. #if defined(DYS_SN20A_TAIL)
  755. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  756. .INCLUDE "DYS_SN20A.inc" ; Select DYS SN20A pinout
  757. #endif
  758. #if defined(DYS_SN20A_MULTI)
  759. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  760. .INCLUDE "DYS_SN20A.inc" ; Select DYS SN20A pinout
  761. #endif
  762. #if defined(TBS_CUBE_20A_MAIN)
  763. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  764. .INCLUDE "TBS_CUBE_20A.inc" ; Select TBS CUBE 20A pinout
  765. #endif
  766. #if defined(TBS_CUBE_20A_TAIL)
  767. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  768. .INCLUDE "TBS_CUBE_20A.inc" ; Select TBS CUBE 20A pinout
  769. #endif
  770. #if defined(TBS_CUBE_20A_MULTI)
  771. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  772. .INCLUDE "TBS_CUBE_20A.inc" ; Select TBS CUBE 20A pinout
  773. #endif
  774. #if defined(ZTW_SPIDER_LITE_18Av2_MAIN)
  775. .EQU MODE = 0 ; Choose mode. Set to 0 for main motor
  776. .INCLUDE "ZTW_Spider_Lite_18Av2.inc" ; Select ZTW Spider Lite 18Av2 pinout
  777. #endif
  778. #if defined(ZTW_SPIDER_LITE_18Av2_TAIL)
  779. .EQU MODE = 1 ; Choose mode. Set to 1 for tail motor
  780. .INCLUDE "ZTW_Spider_Lite_18Av2.inc" ; Select ZTW Spider Lite 18Av2 pinout
  781. #endif
  782. #if defined(ZTW_SPIDER_LITE_18Av2_MULTI)
  783. .EQU MODE = 2 ; Choose mode. Set to 2 for multirotor
  784. .INCLUDE "ZTW_Spider_Lite_18Av2.inc" ; Select ZTW Spider Lite 18Av2 pinout
  785. #endif
  786. ;**** **** **** **** ****
  787. ; TX programming defaults
  788. ;
  789. ; Parameter dependencies:
  790. ; - Governor P gain, I gain and Range is only used if one of the three governor modes is selected
  791. ; - Governor setup target is only used if Setup governor mode is selected (or closed loop mode is on for multi)
  792. ;
  793. ; Main
  794. .EQU DEFAULT_PGM_MAIN_P_GAIN = 7 ; 1=0.13 2=0.17 3=0.25 4=0.38 5=0.50 6=0.75 7=1.00 8=1.5 9=2.0 10=3.0 11=4.0 12=6.0 13=8.0
  795. .EQU DEFAULT_PGM_MAIN_I_GAIN = 7 ; 1=0.13 2=0.17 3=0.25 4=0.38 5=0.50 6=0.75 7=1.00 8=1.5 9=2.0 10=3.0 11=4.0 12=6.0 13=8.0
  796. .EQU DEFAULT_PGM_MAIN_GOVERNOR_MODE = 1 ; 1=Tx 2=Arm 3=Setup 4=Off
  797. .EQU DEFAULT_PGM_MAIN_GOVERNOR_RANGE = 1 ; 1=High 2=Middle 3=Low
  798. .EQU DEFAULT_PGM_MAIN_LOW_VOLTAGE_LIM = 4 ; 1=Off 2=3.0V/c 3=3.1V/c 4=3.2V/c 5=3.3V/c 6=3.4V/c
  799. .EQU DEFAULT_PGM_MAIN_COMM_TIMING = 3 ; 1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=High
  800. .IF DAMPED_MODE_ENABLE == 1
  801. .EQU DEFAULT_PGM_MAIN_PWM_FREQ = 2 ; 1=High 2=Low 3=DampedLight
  802. .ELSE
  803. .EQU DEFAULT_PGM_MAIN_PWM_FREQ = 2 ; 1=High 2=Low
  804. .ENDIF
  805. .EQU DEFAULT_PGM_MAIN_DEMAG_COMP = 1 ; 1=Disabled 2=Low 3=High
  806. .EQU DEFAULT_PGM_MAIN_DIRECTION = 1 ; 1=Normal 2=Reversed
  807. .EQU DEFAULT_PGM_MAIN_RCP_PWM_POL = 1 ; 1=Positive 2=Negative
  808. .EQU DEFAULT_PGM_MAIN_GOV_SETUP_TARGET = 180; Target for governor in setup mode. Corresponds to 70% throttle
  809. .EQU DEFAULT_PGM_MAIN_REARM_START = 0 ; 1=Enabled 0=Disabled
  810. .EQU DEFAULT_PGM_MAIN_BEEP_STRENGTH = 120; Beep strength
  811. .EQU DEFAULT_PGM_MAIN_BEACON_STRENGTH = 200; Beacon strength
  812. .EQU DEFAULT_PGM_MAIN_BEACON_DELAY = 4 ; 1=1m 2=2m 3=5m 4=10m 5=Infinite
  813. ; Tail
  814. .EQU DEFAULT_PGM_TAIL_GAIN = 3 ; 1=0.75 2=0.88 3=1.00 4=1.12 5=1.25
  815. .EQU DEFAULT_PGM_TAIL_IDLE_SPEED = 4 ; 1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=High
  816. .EQU DEFAULT_PGM_TAIL_COMM_TIMING = 3 ; 1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=High
  817. .IF DAMPED_MODE_ENABLE == 1
  818. .EQU DEFAULT_PGM_TAIL_PWM_FREQ = 3 ; 1=High 2=Low 3=DampedLight
  819. .ELSE
  820. .EQU DEFAULT_PGM_TAIL_PWM_FREQ = 1 ; 1=High 2=Low
  821. .ENDIF
  822. .EQU DEFAULT_PGM_TAIL_DEMAG_COMP = 1 ; 1=Disabled 2=Low 3=High
  823. .EQU DEFAULT_PGM_TAIL_DIRECTION = 1 ; 1=Normal 2=Reversed 3=Bidirectional
  824. .EQU DEFAULT_PGM_TAIL_RCP_PWM_POL = 1 ; 1=Positive 2=Negative
  825. .EQU DEFAULT_PGM_TAIL_BEEP_STRENGTH = 250; Beep strength
  826. .EQU DEFAULT_PGM_TAIL_BEACON_STRENGTH = 250; Beacon strength
  827. .EQU DEFAULT_PGM_TAIL_BEACON_DELAY = 4 ; 1=1m 2=2m 3=5m 4=10m 5=Infinite
  828. .EQU DEFAULT_PGM_TAIL_PWM_DITHER = 3 ; 1=Off 2=7 3=15 4=31 5=63
  829. ; Multi
  830. .EQU DEFAULT_PGM_MULTI_P_GAIN = 9 ; 1=0.13 2=0.17 3=0.25 4=0.38 5=0.50 6=0.75 7=1.00 8=1.5 9=2.0 10=3.0 11=4.0 12=6.0 13=8.0
  831. .EQU DEFAULT_PGM_MULTI_I_GAIN = 9 ; 1=0.13 2=0.17 3=0.25 4=0.38 5=0.50 6=0.75 7=1.00 8=1.5 9=2.0 10=3.0 11=4.0 12=6.0 13=8.0
  832. .EQU DEFAULT_PGM_MULTI_GOVERNOR_MODE = 4 ; 1=HiRange 2=MidRange 3=LoRange 4=Off
  833. .EQU DEFAULT_PGM_MULTI_GAIN = 3 ; 1=0.75 2=0.88 3=1.00 4=1.12 5=1.25
  834. .EQU DEFAULT_PGM_MULTI_COMM_TIMING = 3 ; 1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=High
  835. .IF DAMPED_MODE_ENABLE == 1
  836. .EQU DEFAULT_PGM_MULTI_PWM_FREQ = 3 ; 1=High 2=Low 3=DampedLight
  837. .ELSE
  838. .EQU DEFAULT_PGM_MULTI_PWM_FREQ = 1 ; 1=High 2=Low
  839. .ENDIF
  840. .EQU DEFAULT_PGM_MULTI_DEMAG_COMP = 2 ; 1=Disabled 2=Low 3=High
  841. .EQU DEFAULT_PGM_MULTI_DIRECTION = 1 ; 1=Normal 2=Reversed 3=Bidirectional
  842. .EQU DEFAULT_PGM_MULTI_RCP_PWM_POL = 1 ; 1=Positive 2=Negative
  843. .EQU DEFAULT_PGM_MULTI_BEEP_STRENGTH = 40 ; Beep strength
  844. .EQU DEFAULT_PGM_MULTI_BEACON_STRENGTH = 80 ; Beacon strength
  845. .EQU DEFAULT_PGM_MULTI_BEACON_DELAY = 4 ; 1=1m 2=2m 3=5m 4=10m 5=Infinite
  846. .EQU DEFAULT_PGM_MULTI_PWM_DITHER = 3 ; 1=Off 2=7 3=15 4=31 5=63
  847. ; Common
  848. .EQU DEFAULT_PGM_ENABLE_TX_PROGRAM = 1 ; 1=Enabled 0=Disabled
  849. .EQU DEFAULT_PGM_PPM_MIN_THROTTLE = 37 ; 4*37+1000=1148
  850. .EQU DEFAULT_PGM_PPM_MAX_THROTTLE = 208; 4*208+1000=1832
  851. .EQU DEFAULT_PGM_PPM_CENTER_THROTTLE = 122; 4*122+1000=1488 (used in bidirectional mode)
  852. .EQU DEFAULT_PGM_BEC_VOLTAGE_HIGH = 0 ; 0=Low 1= High
  853. .EQU DEFAULT_PGM_ENABLE_TEMP_PROT = 0 ; 1=Enabled 0=Disabled
  854. .EQU DEFAULT_PGM_ENABLE_POWER_PROT = 1 ; 1=Enabled 0=Disabled
  855. .EQU DEFAULT_PGM_ENABLE_PWM_INPUT = 0 ; 1=Enabled 0=Disabled
  856. ;**** **** **** **** ****
  857. ; Constant definitions for main
  858. .IF MODE == 0
  859. .EQU GOV_SPOOLRATE = 2 ; Number of steps for governor requested pwm per 32ms
  860. .EQU RCP_TIMEOUT_PPM = 10 ; Number of timer2H overflows (about 32ms) before considering rc pulse lost
  861. .EQU RCP_TIMEOUT = 64 ; Number of timer2L overflows (about 128us) before considering rc pulse lost
  862. .EQU RCP_SKIP_RATE = 32 ; Number of timer2L overflows (about 128us) before reenabling rc pulse detection
  863. .EQU RCP_MIN = 0 ; This is minimum RC pulse length
  864. .EQU RCP_MAX = 255 ; This is maximum RC pulse length
  865. .EQU RCP_VALIDATE = 2 ; Require minimum this pulse length to validate RC pulse
  866. .EQU RCP_STOP = 1 ; Stop motor at or below this pulse length
  867. .EQU RCP_STOP_LIMIT = 250 ; Stop motor if this many timer2H overflows (~32ms) are below stop limit
  868. .EQU PWM_START = 50 ; PWM used as max power during start
  869. .EQU COMM_TIME_MIN = 1 ; Minimum time (in us) for commutation wait
  870. .EQU TEMP_CHECK_RATE = 8 ; Number of adc conversions for each check of temperature (the other conversions are used for voltage)
  871. .ENDIF
  872. ; Constant definitions for tail
  873. .IF MODE == 1
  874. .EQU GOV_SPOOLRATE = 1 ; Number of steps for governor requested pwm per 32ms
  875. .EQU RCP_TIMEOUT_PPM = 10 ; Number of timer2H overflows (about 32ms) before considering rc pulse lost
  876. .EQU RCP_TIMEOUT = 24 ; Number of timer2L overflows (about 128us) before considering rc pulse lost
  877. .EQU RCP_SKIP_RATE = 6 ; Number of timer2L overflows (about 128us) before reenabling rc pulse detection
  878. .EQU RCP_MIN = 0 ; This is minimum RC pulse length
  879. .EQU RCP_MAX = 255 ; This is maximum RC pulse length
  880. .EQU RCP_VALIDATE = 2 ; Require minimum this pulse length to validate RC pulse
  881. .EQU RCP_STOP = 1 ; Stop motor at or below this pulse length
  882. .EQU RCP_STOP_LIMIT = 130 ; Stop motor if this many timer2H overflows (~32ms) are below stop limit
  883. .EQU PWM_START = 50 ; PWM used as max power during start
  884. .EQU COMM_TIME_MIN = 1 ; Minimum time (in us) for commutation wait
  885. .EQU TEMP_CHECK_RATE = 8 ; Number of adc conversions for each check of temperature (the other conversions are used for voltage)
  886. .ENDIF
  887. ; Constant definitions for multi
  888. .IF MODE == 2
  889. .EQU GOV_SPOOLRATE = 1 ; Number of steps for governor requested pwm per 32ms
  890. .EQU RCP_TIMEOUT_PPM = 10 ; Number of timer2H overflows (about 32ms) before considering rc pulse lost
  891. .EQU RCP_TIMEOUT = 24 ; Number of timer2L overflows (about 128us) before considering rc pulse lost
  892. .EQU RCP_SKIP_RATE = 6 ; Number of timer2L overflows (about 128us) before reenabling rc pulse detection
  893. .EQU RCP_MIN = 0 ; This is minimum RC pulse length
  894. .EQU RCP_MAX = 255 ; This is maximum RC pulse length
  895. .EQU RCP_VALIDATE = 2 ; Require minimum this pulse length to validate RC pulse
  896. .EQU RCP_STOP = 1 ; Stop motor at or below this pulse length
  897. .EQU RCP_STOP_LIMIT = 250 ; Stop motor if this many timer2H overflows (~32ms) are below stop limit
  898. .EQU PWM_START = 50 ; PWM used as max power during start
  899. .EQU COMM_TIME_MIN = 1 ; Minimum time (in us) for commutation wait
  900. .EQU TEMP_CHECK_RATE = 8 ; Number of adc conversions for each check of temperature (the other conversions are used for voltage)
  901. .ENDIF
  902. ;**** **** **** **** ****
  903. ; Register definitions
  904. .DEF Mul_Res_L = R0 ; Reserved for mul instruction
  905. .DEF Mul_Res_H = R1 ; Reserved for mul instruction
  906. .DEF Zero = R2 ; Register variable initialized to 0, always at 0
  907. .DEF I_Sreg = R3 ; Status register saved in interrupts
  908. .DEF II_Sreg = R4 ; Status register saved in nested interrupts (pwm interrupts from timer2)
  909. .DEF Current_Pwm_Limited = R5 ; Current_Pwm_Limited is allocated to a register for fast access
  910. .DEF Next_Wt_L = R14 ; Next wait value (lo byte)
  911. .DEF Next_Wt_H = R15 ; Next wait value (hi byte)
  912. .DEF Temp1 = R16 ; Main temporary
  913. .DEF Temp2 = R17 ; Main temporary (Temp1/2 must be two consecutive registers)
  914. .DEF Temp3 = R18 ; Main temporary
  915. .DEF Temp4 = R19 ; Main temporary
  916. .DEF Temp5 = R6 ; Main temporary (limited operations)
  917. .DEF Temp6 = R7 ; Main temporary (limited operations)
  918. .DEF Temp7 = R8 ; Main temporary (limited operations)
  919. .DEF Temp8 = R9 ; Main temporary (limited operations)
  920. .DEF I_Temp1 = R20 ; Interrupt temporary
  921. .DEF I_Temp2 = R21 ; Interrupt temporary
  922. .DEF I_Temp3 = R10 ; Interrupt temporary (limited operations)
  923. .DEF I_Temp4 = R11 ; Interrupt temporary (limited operations)
  924. .DEF I_Temp5 = R12 ; Interrupt temporary (limited operations)
  925. .DEF I_Temp6 = R13 ; Interrupt temporary (limited operations)
  926. .DEF Flags0 = R22 ; State flags. Reset upon init_start
  927. .EQU OC1A_PENDING = 0 ; Timer1 output compare pending flag
  928. .EQU RCP_MEAS_PWM_FREQ = 1 ; Measure RC pulse pwm frequency
  929. .EQU PWM_ON = 2 ; Set in on part of pwm cycle
  930. .EQU DEMAG_ENABLED = 3 ; Set when demag compensation is enabled (above a min speed and throttle)
  931. .EQU DEMAG_DETECTED = 4 ; Set when excessive demag time is detected
  932. .EQU DEMAG_CUT_POWER = 5 ; Set when demag compensation cuts power
  933. .EQU GOV_ACTIVE = 6 ; Set when governor is active
  934. .EQU DIR_CHANGE_BRAKE = 7 ; Set when braking before direction change
  935. .DEF Flags1 = R23 ; State flags. Reset upon init_start
  936. .EQU MOTOR_SPINNING = 0 ; Set when in motor is spinning
  937. .EQU STARTUP_PHASE = 1 ; Set when in startup phase
  938. .EQU INITIAL_RUN_PHASE = 2 ; Set when in initial run phase, before synchronized run is achieved
  939. .EQU ADC_READ_TEMP = 3 ; Set when ADC input shall be set to read temperature
  940. .EQU COMP_TIMED_OUT = 4 ; Set when comparator reading timed out
  941. .EQU SKIP_DAMP_ON = 5 ; Set when turning damping fet on is skipped
  942. .EQU HIGH_RPM = 6 ; Set when motor rpm is high (Comm_Period4x_H less than 2)
  943. ; = 7
  944. .DEF Flags2 = R24 ; State flags. NOT reset upon init_start
  945. .EQU RCP_UPDATED = 0 ; New RC pulse length value available
  946. .EQU RCP_EDGE_NO = 1 ; RC pulse edge no. 0=rising, 1=falling
  947. .EQU PGM_PWMOFF_DAMPED = 2 ; Programmed pwm off damped mode
  948. .EQU PGM_PWM_HIGH_FREQ = 3 ; Progremmed pwm high frequency
  949. .EQU RCP_INT_NESTED_ENABLED = 4 ; Set when RC pulse interrupt is enabled around nested interrupts
  950. .EQU RCP_PPM = 5 ; RC pulse ppm type input (set also when oneshot is set)
  951. .EQU RCP_PPM_ONESHOT125 = 6 ; RC pulse ppm type input is OneShot125
  952. .EQU RCP_DIR_REV = 7 ; RC pulse direction in bidirectional mode
  953. .DEF Flags3 = R25 ; State flags. NOT reset upon init_start
  954. .EQU RCP_PWM_FREQ_1KHZ = 0 ; RC pulse pwm frequency is 1kHz
  955. .EQU RCP_PWM_FREQ_2KHZ = 1 ; RC pulse pwm frequency is 2kHz
  956. .EQU RCP_PWM_FREQ_4KHZ = 2 ; RC pulse pwm frequency is 4kHz
  957. .EQU RCP_PWM_FREQ_8KHZ = 3 ; RC pulse pwm frequency is 8kHz
  958. .EQU RCP_PWM_FREQ_12KHZ = 4 ; RC pulse pwm frequency is 12kHz
  959. .EQU PGM_DIR_REV = 5 ; Programmed direction. 0=normal, 1=reversed
  960. .EQU PGM_RCP_PWM_POL = 6 ; Programmed RC pulse pwm polarity. 0=positive, 1=negative
  961. .EQU FULL_THROTTLE_RANGE = 7 ; When set full throttle range is used (1000-2000us) and stored calibration values are ignored
  962. ; Here the general temporary register XYZ are placed (R26-R31)
  963. ; XH: General temporary used by main routines
  964. ; XL: General temporary used by interrupt routines
  965. ; Y: General temporary used by timer2 pwm interrupt routine
  966. ; Z: Address of current PWM FET ON routine (eg: pwm_afet_on)
  967. ;**** **** **** **** ****
  968. ; RAM definitions
  969. .DSEG ; Data segment
  970. .ORG SRAM_START
  971. Timer0_Int_Cnt: .BYTE 1 ; Timer0 interrupt counter
  972. Requested_Pwm: .BYTE 1 ; Requested pwm (from RC pulse value)
  973. Governor_Req_Pwm: .BYTE 1 ; Governor requested pwm (sets governor target)
  974. Current_Pwm: .BYTE 1 ; Current pwm
  975. Current_Pwm_Lim_Dith: .BYTE 1 ; Current pwm that is limited and dithered (applied to the motor output)
  976. Rcp_Prev_Edge_L: .BYTE 1 ; RC pulse previous edge timer3 timestamp (lo byte)
  977. Rcp_Prev_Edge_H: .BYTE 1 ; RC pulse previous edge timer3 timestamp (hi byte)
  978. Rcp_Outside_Range_Cnt: .BYTE 1 ; RC pulse outside range counter (incrementing)
  979. Rcp_Timeout_Cntd: .BYTE 1 ; RC pulse timeout counter (decrementing)
  980. Rcp_Skip_Cntd: .BYTE 1 ; RC pulse skip counter (decrementing)
  981. Initial_Arm: .BYTE 1 ; Variable that is set during the first arm sequence after power on
  982. Power_On_Wait_Cnt_L: .BYTE 1 ; Power on wait counter (lo byte)
  983. Power_On_Wait_Cnt_H: .BYTE 1 ; Power on wait counter (hi byte)
  984. Startup_Cnt: .BYTE 1 ; Startup phase commutations counter (incrementing)
  985. Startup_Zc_Timeout_Cntd: .BYTE 1 ; Startup zero cross timeout counter (decrementing)
  986. Initial_Run_Rot_Cnt: .BYTE 1 ; Initial run rotations counter (incrementing)
  987. Stall_Cnt: .BYTE 1 ; Counts start/run attempts that resulted in stall. Reset upon a proper stop
  988. Demag_Detected_Metric: .BYTE 1 ; Metric used to gauge demag event frequency
  989. Demag_Pwr_Off_Thresh: .BYTE 1 ; Metric threshold above which power is cut
  990. Low_Rpm_Pwr_Slope: .BYTE 1 ; Sets the slope of power increase for low rpms
  991. Timer2_X: .BYTE 1 ; Timer 2 extended byte
  992. Prev_Comm_L: .BYTE 1 ; Previous commutation timer timestamp (lo byte)
  993. Prev_Comm_H: .BYTE 1 ; Previous commutation timer timestamp (hi byte)
  994. Prev_Comm_X: .BYTE 1 ; Previous commutation timer3 timestamp (ext byte)
  995. Prev_Prev_Comm_L: .BYTE 1 ; Pre-previous commutation timer timestamp (lo byte)
  996. Prev_Prev_Comm_H: .BYTE 1 ; Pre-previous commutation timer timestamp (hi byte)
  997. Comm_Period4x_L: .BYTE 1 ; Timer3 counts between the last 4 commutations (lo byte)
  998. Comm_Period4x_H: .BYTE 1 ; Timer3 counts between the last 4 commutations (hi byte)
  999. Comm_Diff: .BYTE 1 ; Timer3 count difference between the last two commutations
  1000. Comm_Phase: .BYTE 1 ; Current commutation phase
  1001. Comparator_Read_Cnt: .BYTE 1 ; Number of comparator reads done
  1002. Gov_Target_L: .BYTE 1 ; Governor target (lo byte)
  1003. Gov_Target_H: .BYTE 1 ; Governor target (hi byte)
  1004. Gov_Integral_L: .BYTE 1 ; Governor integral error (lo byte)
  1005. Gov_Integral_H: .BYTE 1 ; Governor integral error (hi byte)
  1006. Gov_Integral_X: .BYTE 1 ; Governor integral error (ex byte)
  1007. Gov_Proportional_L: .BYTE 1 ; Governor proportional error (lo byte)
  1008. Gov_Proportional_H: .BYTE 1 ; Governor proportional error (hi byte)
  1009. Gov_Prop_Pwm: .BYTE 1 ; Governor calculated new pwm based upon proportional error
  1010. Gov_Arm_Target: .BYTE 1 ; Governor arm target value
  1011. Wt_Advance_L: .BYTE 1 ; Timer3 counts for commutation advance timing (lo byte)
  1012. Wt_Advance_H: .BYTE 1 ; Timer3 counts for commutation advance timing (hi byte)
  1013. Wt_Zc_Scan_L: .BYTE 1 ; Timer3 counts from commutation to zero cross scan (lo byte)
  1014. Wt_Zc_Scan_H: .BYTE 1 ; Timer3 counts from commutation to zero cross scan (hi byte)
  1015. Wt_Zc_Timeout_L: .BYTE 1 ; Timer3 counts for zero cross scan timeout (lo byte)
  1016. Wt_Zc_Timeout_H: .BYTE 1 ; Timer3 counts for zero cross scan timeout (hi byte)
  1017. Wt_Comm_L: .BYTE 1 ; Timer3 counts from zero cross to commutation (lo byte)
  1018. Wt_Comm_H: .BYTE 1 ; Timer3 counts from zero cross to commutation (hi byte)
  1019. Rcp_PrePrev_Edge_L: .BYTE 1 ; RC pulse pre previous edge pca timestamp (lo byte)
  1020. Rcp_PrePrev_Edge_H: .BYTE 1 ; RC pulse pre previous edge pca timestamp (hi byte)
  1021. Rcp_Edge_L: .BYTE 1 ; RC pulse edge pca timestamp (lo byte)
  1022. Rcp_Edge_H: .BYTE 1 ; RC pulse edge pca timestamp (hi byte)
  1023. Rcp_Prev_Period_L: .BYTE 1 ; RC pulse previous period (lo byte)
  1024. Rcp_Prev_Period_H: .BYTE 1 ; RC pulse previous period (hi byte)
  1025. Rcp_Period_Diff_Accepted: .BYTE 1 ; RC pulse period difference acceptable
  1026. New_Rcp: .BYTE 1 ; New RC pulse value in pca counts
  1027. Prev_Rcp_Pwm_Freq: .BYTE 1 ; Previous RC pulse pwm frequency (used during pwm frequency measurement)
  1028. Curr_Rcp_Pwm_Freq: .BYTE 1 ; Current RC pulse pwm frequency (used during pwm frequency measurement)
  1029. Rcp_Stop_Cnt: .BYTE 1 ; Counter for RC pulses below stop value (lo byte)
  1030. Auto_Bailout_Armed: .BYTE 1 ; Set when auto rotation bailout is armed
  1031. Pwm_Limit: .BYTE 1 ; Maximum allowed pwm
  1032. Pwm_Limit_Spoolup: .BYTE 1 ; Maximum allowed pwm during spoolup of main
  1033. Pwm_Limit_By_Rpm: .BYTE 1 ; Maximum allowed pwm for low or high rpms
  1034. Pwm_Spoolup_Beg: .BYTE 1 ; Pwm to begin main spoolup with
  1035. Pwm_Motor_Idle: .BYTE 1 ; Motor idle speed pwm
  1036. Pwm_Prev_Edge: .BYTE 1 ; Timestamp from timer 2 when pwm toggles on or off
  1037. Pwm_Dither_Decoded: .BYTE 1 ; Decoded pwm dither value
  1038. Pwm_Dither_Excess_Power: .BYTE 1 ; Excess power (above max) from pwm dither
  1039. Random: .BYTE 1 ; Random number from LFSR
  1040. Spoolup_Limit_Cnt: .BYTE 1 ; Interrupt count for spoolup limit
  1041. Spoolup_Limit_Skip: .BYTE 1 ; Interrupt skips for spoolup limit increment (1=no skips, 2=skip one etc)
  1042. Main_Spoolup_Time_3x: .BYTE 1 ; Main spoolup time x3
  1043. Main_Spoolup_Time_10x: .BYTE 1 ; Main spoolup time x10
  1044. Main_Spoolup_Time_15x: .BYTE 1 ; Main spoolup time x15
  1045. Lipo_Adc_Limit_L: .BYTE 1 ; Low voltage limit adc value (lo byte)
  1046. Lipo_Adc_Limit_H: .BYTE 1 ; Low voltage limit adc value (hi byte)
  1047. Adc_Conversion_Cnt: .BYTE 1 ; Adc conversion counter
  1048. Current_Average_Temp_Adc: .BYTE 1 ; Current average temp ADC reading (lo byte of ADC, assuming hi byte is 0)
  1049. Ppm_Throttle_Gain: .BYTE 1 ; Gain to be applied to RCP value for PPM input
  1050. Beep_Strength: .BYTE 1 ; Strength of beeps
  1051. Tx_Pgm_Func_No: .BYTE 1 ; Function number when doing programming by tx
  1052. Tx_Pgm_Paraval_No: .BYTE 1 ; Parameter value number when doing programming by tx
  1053. Tx_Pgm_Beep_No: .BYTE 1 ; Beep number when doing programming by tx
  1054. DampingFET: .BYTE 1 ; Port position of fet used for damping
  1055. ; The variables below must be in this sequence
  1056. Pgm_Gov_P_Gain: .BYTE 1 ; Programmed governor P gain
  1057. Pgm_Gov_I_Gain: .BYTE 1 ; Programmed governor I gain
  1058. Pgm_Gov_Mode: .BYTE 1 ; Programmed governor mode
  1059. Pgm_Low_Voltage_Lim: .BYTE 1 ; Programmed low voltage limit
  1060. Pgm_Motor_Gain: .BYTE 1 ; Programmed motor gain
  1061. Pgm_Motor_Idle: .BYTE 1 ; Programmed motor idle speed
  1062. Pgm_Startup_Pwr: .BYTE 1 ; Programmed startup power
  1063. Pgm_Pwm_Freq: .BYTE 1 ; Programmed pwm frequency
  1064. Pgm_Direction: .BYTE 1 ; Programmed rotation direction
  1065. Pgm_Input_Pol: .BYTE 1 ; Programmed input pwm polarity
  1066. Initialized_L_Dummy: .BYTE 1 ; Place holder
  1067. Initialized_H_Dummy: .BYTE 1 ; Place holder
  1068. Pgm_Enable_TX_Program: .BYTE 1 ; Programmed enable/disable value for TX programming
  1069. Pgm_Main_Rearm_Start: .BYTE 1 ; Programmed enable/disable re-arming main every start
  1070. Pgm_Gov_Setup_Target: .BYTE 1 ; Programmed main governor setup target
  1071. _Pgm_Startup_Rpm: .BYTE 1 ; Programmed startup rpm (unused - place holder)
  1072. _Pgm_Startup_Accel: .BYTE 1 ; Programmed startup acceleration (unused - place holder)
  1073. _Pgm_Volt_Comp: .BYTE 1 ; Place holder
  1074. Pgm_Comm_Timing: .BYTE 1 ; Programmed commutation timing
  1075. _Pgm_Damping_Force: .BYTE 1 ; Programmed damping force (unused - place holder)
  1076. Pgm_Gov_Range: .BYTE 1 ; Programmed governor range
  1077. Pgm_Startup_Method: .BYTE 1 ; Programmed startup method
  1078. Pgm_Ppm_Min_Throttle: .BYTE 1 ; Programmed throttle minimum
  1079. Pgm_Ppm_Max_Throttle: .BYTE 1 ; Programmed throttle maximum
  1080. Pgm_Beep_Strength: .BYTE 1 ; Programmed beep strength
  1081. Pgm_Beacon_Strength: .BYTE 1 ; Programmed beacon strength
  1082. Pgm_Beacon_Delay: .BYTE 1 ; Programmed beacon delay
  1083. _Pgm_Throttle_Rate: .BYTE 1 ; Programmed throttle rate (unused - place holder)
  1084. Pgm_Demag_Comp: .BYTE 1 ; Programmed demag compensation
  1085. Pgm_BEC_Voltage_High: .BYTE 1 ; Programmed BEC voltage
  1086. Pgm_Ppm_Center_Throttle: .BYTE 1 ; Programmed throttle center (in bidirectional mode)
  1087. Pgm_Main_Spoolup_Time: .BYTE 1 ; Programmed main spoolup time
  1088. Pgm_Enable_Temp_Prot: .BYTE 1 ; Programmed temperature protection enable
  1089. Pgm_Enable_Power_Prot: .BYTE 1 ; Programmed low rpm power protection enable
  1090. Pgm_Enable_Pwm_Input: .BYTE 1 ; Programmed PWM input signal enable
  1091. Pgm_Pwm_Dither: .BYTE 1 ; Programmed output PWM dither
  1092. ; The sequence of the variables below is no longer of importance
  1093. Pgm_Gov_P_Gain_Decoded: .BYTE 1 ; Programmed governor decoded P gain
  1094. Pgm_Gov_I_Gain_Decoded: .BYTE 1 ; Programmed governor decoded I gain
  1095. Pgm_Startup_Pwr_Decoded: .BYTE 1 ; Programmed startup power decoded
  1096. .EQU SRAM_BYTES = 255 ; Bytes used in SRAM. Used for number of bytes to reset
  1097. ;**** **** **** **** ****
  1098. .ESEG ; Eeprom segment
  1099. .ORG 0
  1100. .EQU EEPROM_FW_MAIN_REVISION = 14 ; Main revision of the firmware
  1101. .EQU EEPROM_FW_SUB_REVISION = 4 ; Sub revision of the firmware
  1102. .EQU EEPROM_LAYOUT_REVISION = 20 ; Revision of the EEPROM layout
  1103. Eep_FW_Main_Revision: .DB EEPROM_FW_MAIN_REVISION ; EEPROM firmware main revision number
  1104. Eep_FW_Sub_Revision: .DB EEPROM_FW_SUB_REVISION ; EEPROM firmware sub revision number
  1105. Eep_Layout_Revision: .DB EEPROM_LAYOUT_REVISION ; EEPROM layout revision number
  1106. .IF MODE == 0
  1107. Eep_Pgm_Gov_P_Gain: .DB DEFAULT_PGM_MAIN_P_GAIN ; EEPROM copy of programmed governor P gain
  1108. Eep_Pgm_Gov_I_Gain: .DB DEFAULT_PGM_MAIN_I_GAIN ; EEPROM copy of programmed governor I gain
  1109. Eep_Pgm_Gov_Mode: .DB DEFAULT_PGM_MAIN_GOVERNOR_MODE ; EEPROM copy of programmed governor mode
  1110. Eep_Pgm_Low_Voltage_Lim: .DB DEFAULT_PGM_MAIN_LOW_VOLTAGE_LIM ; EEPROM copy of programmed low voltage limit
  1111. _Eep_Pgm_Motor_Gain: .DB 0xFF
  1112. _Eep_Pgm_Motor_Idle: .DB 0xFF
  1113. Eep_Pgm_Startup_Pwr: .DB DEFAULT_PGM_MAIN_STARTUP_PWR ; EEPROM copy of programmed startup power
  1114. Eep_Pgm_Pwm_Freq: .DB DEFAULT_PGM_MAIN_PWM_FREQ ; EEPROM copy of programmed pwm frequency
  1115. Eep_Pgm_Direction: .DB DEFAULT_PGM_MAIN_DIRECTION ; EEPROM copy of programmed rotation direction
  1116. Eep_Pgm_Input_Pol: .DB DEFAULT_PGM_MAIN_RCP_PWM_POL ; EEPROM copy of programmed input polarity
  1117. Eep_Initialized_L: .DB 0xA5 ; EEPROM initialized signature low byte
  1118. Eep_Initialized_H: .DB 0x5A ; EEPROM initialized signature high byte
  1119. Eep_Enable_TX_Program: .DB DEFAULT_PGM_ENABLE_TX_PROGRAM ; EEPROM TX programming enable
  1120. Eep_Main_Rearm_Start: .DB DEFAULT_PGM_MAIN_REARM_START ; EEPROM re-arming main enable
  1121. Eep_Pgm_Gov_Setup_Target: .DB DEFAULT_PGM_MAIN_GOV_SETUP_TARGET ; EEPROM main governor setup target
  1122. _Eep_Pgm_Startup_Rpm: .DB 0xFF
  1123. _Eep_Pgm_Startup_Accel: .DB 0xFF
  1124. _Eep_Pgm_Volt_Comp: .DB 0xFF
  1125. Eep_Pgm_Comm_Timing: .DB DEFAULT_PGM_MAIN_COMM_TIMING ; EEPROM copy of programmed commutation timing
  1126. _Eep_Pgm_Damping_Force: .DB 0xFF
  1127. Eep_Pgm_Gov_Range: .DB DEFAULT_PGM_MAIN_GOVERNOR_RANGE ; EEPROM copy of programmed governor range
  1128. _Eep_Pgm_Startup_Method: .DB 0xFF
  1129. Eep_Pgm_Ppm_Min_Throttle: .DB DEFAULT_PGM_PPM_MIN_THROTTLE ; EEPROM copy of programmed minimum throttle (final value is 4x+1000=1148)
  1130. Eep_Pgm_Ppm_Max_Throttle: .DB DEFAULT_PGM_PPM_MAX_THROTTLE ; EEPROM copy of programmed minimum throttle (final value is 4x+1000=1832)
  1131. Eep_Pgm_Beep_Strength: .DB DEFAULT_PGM_MAIN_BEEP_STRENGTH ; EEPROM copy of programmed beep strength
  1132. Eep_Pgm_Beacon_Strength: .DB DEFAULT_PGM_MAIN_BEACON_STRENGTH ; EEPROM copy of programmed beacon strength
  1133. Eep_Pgm_Beacon_Delay: .DB DEFAULT_PGM_MAIN_BEACON_DELAY ; EEPROM copy of programmed beacon delay
  1134. _Eep_Pgm_Throttle_Rate: .DB 0xFF
  1135. Eep_Pgm_Demag_Comp: .DB DEFAULT_PGM_MAIN_DEMAG_COMP ; EEPROM copy of programmed demag compensation
  1136. Eep_Pgm_BEC_Voltage_High: .DB DEFAULT_PGM_BEC_VOLTAGE_HIGH ; EEPROM copy of programmed BEC voltage
  1137. _Eep_Pgm_Ppm_Center_Throttle: .DB 0xFF
  1138. Eep_Pgm_Main_Spoolup_Time: .DB DEFAULT_PGM_MAIN_SPOOLUP_TIME ; EEPROM copy of programmed main spoolup time
  1139. Eep_Pgm_Temp_Prot_Enable: .DB DEFAULT_PGM_ENABLE_TEMP_PROT ; EEPROM copy of programmed temperature protection enable
  1140. Eep_Pgm_Enable_Power_Prot: .DB DEFAULT_PGM_ENABLE_POWER_PROT ; EEPROM copy of programmed low rpm power protection enable
  1141. Eep_Pgm_Enable_Pwm_Input: .DB DEFAULT_PGM_ENABLE_PWM_INPUT ; EEPROM copy of programmed PWM input signal enable
  1142. _Eep_Pgm_Pwm_Dither: .DB 0xFF
  1143. .ENDIF
  1144. .IF MODE == 1
  1145. _Eep_Pgm_Gov_P_Gain: .DB 0xFF
  1146. _Eep_Pgm_Gov_I_Gain: .DB 0xFF
  1147. _Eep_Pgm_Gov_Mode: .DB 0xFF
  1148. _Eep_Pgm_Low_Voltage_Lim: .DB 0xFF
  1149. Eep_Pgm_Motor_Gain: .DB DEFAULT_PGM_TAIL_GAIN ; EEPROM copy of programmed tail gain
  1150. Eep_Pgm_Motor_Idle: .DB DEFAULT_PGM_TAIL_IDLE_SPEED ; EEPROM copy of programmed tail idle speed
  1151. Eep_Pgm_Startup_Pwr: .DB DEFAULT_PGM_TAIL_STARTUP_PWR ; EEPROM copy of programmed startup power
  1152. Eep_Pgm_Pwm_Freq: .DB DEFAULT_PGM_TAIL_PWM_FREQ ; EEPROM copy of programmed pwm frequency
  1153. Eep_Pgm_Direction: .DB DEFAULT_PGM_TAIL_DIRECTION ; EEPROM copy of programmed rotation direction
  1154. Eep_Pgm_Input_Pol: .DB DEFAULT_PGM_TAIL_RCP_PWM_POL ; EEPROM copy of programmed input polarity
  1155. Eep_Initialized_L: .DB 0x5A ; EEPROM initialized signature low byte
  1156. Eep_Initialized_H: .DB 0xA5 ; EEPROM initialized signature high byte
  1157. Eep_Enable_TX_Program: .DB DEFAULT_PGM_ENABLE_TX_PROGRAM ; EEPROM TX programming enable
  1158. _Eep_Main_Rearm_Start: .DB 0xFF
  1159. _Eep_Pgm_Gov_Setup_Target: .DB 0xFF
  1160. _Eep_Pgm_Startup_Rpm: .DB 0xFF
  1161. _Eep_Pgm_Startup_Accel: .DB 0xFF
  1162. _Eep_Pgm_Volt_Comp: .DB 0xFF
  1163. Eep_Pgm_Comm_Timing: .DB DEFAULT_PGM_TAIL_COMM_TIMING ; EEPROM copy of programmed commutation timing
  1164. _Eep_Pgm_Damping_Force: .DB 0xFF
  1165. _Eep_Pgm_Gov_Range: .DB 0xFF
  1166. _Eep_Pgm_Startup_Method: .DB 0xFF
  1167. Eep_Pgm_Ppm_Min_Throttle: .DB DEFAULT_PGM_PPM_MIN_THROTTLE ; EEPROM copy of programmed minimum throttle (final value is 4x+1000=1148)
  1168. Eep_Pgm_Ppm_Max_Throttle: .DB DEFAULT_PGM_PPM_MAX_THROTTLE ; EEPROM copy of programmed minimum throttle (final value is 4x+1000=1832)
  1169. Eep_Pgm_Beep_Strength: .DB DEFAULT_PGM_TAIL_BEEP_STRENGTH ; EEPROM copy of programmed beep strength
  1170. Eep_Pgm_Beacon_Strength: .DB DEFAULT_PGM_TAIL_BEACON_STRENGTH ; EEPROM copy of programmed beacon strength
  1171. Eep_Pgm_Beacon_Delay: .DB DEFAULT_PGM_TAIL_BEACON_DELAY ; EEPROM copy of programmed beacon delay
  1172. _Eep_Pgm_Throttle_Rate: .DB 0xFF
  1173. Eep_Pgm_Demag_Comp: .DB DEFAULT_PGM_TAIL_DEMAG_COMP ; EEPROM copy of programmed demag compensation
  1174. Eep_Pgm_BEC_Voltage_High: .DB DEFAULT_PGM_BEC_VOLTAGE_HIGH ; EEPROM copy of programmed BEC voltage
  1175. Eep_Pgm_Ppm_Center_Throttle: .DB DEFAULT_PGM_PPM_CENTER_THROTTLE ; EEPROM copy of programmed center throttle (final value is 4x+1000=1488)
  1176. _Eep_Pgm_Main_Spoolup_Time: .DB 0xFF
  1177. Eep_Pgm_Temp_Prot_Enable: .DB DEFAULT_PGM_ENABLE_TEMP_PROT ; EEPROM copy of programmed temperature protection enable
  1178. Eep_Pgm_Enable_Power_Prot: .DB DEFAULT_PGM_ENABLE_POWER_PROT ; EEPROM copy of programmed low rpm power protection enable
  1179. Eep_Pgm_Enable_Pwm_Input: .DB DEFAULT_PGM_ENABLE_PWM_INPUT ; EEPROM copy of programmed PWM input signal enable
  1180. Eep_Pgm_Pwm_Dither: .DB DEFAULT_PGM_TAIL_PWM_DITHER ; EEPROM copy of programmed output PWM dither
  1181. .ENDIF
  1182. .IF MODE == 2
  1183. Eep_Pgm_Gov_P_Gain: .DB DEFAULT_PGM_MULTI_P_GAIN ; EEPROM copy of programmed closed loop P gain
  1184. Eep_Pgm_Gov_I_Gain: .DB DEFAULT_PGM_MULTI_I_GAIN ; EEPROM copy of programmed closed loop I gain
  1185. Eep_Pgm_Gov_Mode: .DB DEFAULT_PGM_MULTI_GOVERNOR_MODE ; EEPROM copy of programmed closed loop mode
  1186. _Eep_Pgm_Low_Voltage_Lim: .DB 0xFF
  1187. Eep_Pgm_Motor_Gain: .DB DEFAULT_PGM_MULTI_GAIN ; EEPROM copy of programmed tail gain
  1188. _Eep_Pgm_Motor_Idle: .DB 0xFF ; EEPROM copy of programmed tail idle speed
  1189. Eep_Pgm_Startup_Pwr: .DB DEFAULT_PGM_MULTI_STARTUP_PWR ; EEPROM copy of programmed startup power
  1190. Eep_Pgm_Pwm_Freq: .DB DEFAULT_PGM_MULTI_PWM_FREQ ; EEPROM copy of programmed pwm frequency
  1191. Eep_Pgm_Direction: .DB DEFAULT_PGM_MULTI_DIRECTION ; EEPROM copy of programmed rotation direction
  1192. Eep_Pgm_Input_Pol: .DB DEFAULT_PGM_MULTI_RCP_PWM_POL ; EEPROM copy of programmed input polarity
  1193. Eep_Initialized_L: .DB 0x55 ; EEPROM initialized signature low byte
  1194. Eep_Initialized_H: .DB 0xAA ; EEPROM initialized signature high byte
  1195. Eep_Enable_TX_Program: .DB DEFAULT_PGM_ENABLE_TX_PROGRAM ; EEPROM TX programming enable
  1196. _Eep_Main_Rearm_Start: .DB 0xFF
  1197. _Eep_Pgm_Gov_Setup_Target: .DB 0xFF
  1198. _Eep_Pgm_Startup_Rpm: .DB 0xFF
  1199. _Eep_Pgm_Startup_Accel: .DB 0xFF
  1200. _Eep_Pgm_Volt_Comp: .DB 0xFF
  1201. Eep_Pgm_Comm_Timing: .DB DEFAULT_PGM_MULTI_COMM_TIMING ; EEPROM copy of programmed commutation timing
  1202. _Eep_Pgm_Damping_Force: .DB 0xFF
  1203. _Eep_Pgm_Gov_Range: .DB 0xFF
  1204. _Eep_Pgm_Startup_Method: .DB 0xFF
  1205. Eep_Pgm_Ppm_Min_Throttle: .DB DEFAULT_PGM_PPM_MIN_THROTTLE ; EEPROM copy of programmed minimum throttle (final value is 4x+1000=1148)
  1206. Eep_Pgm_Ppm_Max_Throttle: .DB DEFAULT_PGM_PPM_MAX_THROTTLE ; EEPROM copy of programmed minimum throttle (final value is 4x+1000=1832)
  1207. Eep_Pgm_Beep_Strength: .DB DEFAULT_PGM_MULTI_BEEP_STRENGTH ; EEPROM copy of programmed beep strength
  1208. Eep_Pgm_Beacon_Strength: .DB DEFAULT_PGM_MULTI_BEACON_STRENGTH ; EEPROM copy of programmed beacon strength
  1209. Eep_Pgm_Beacon_Delay: .DB DEFAULT_PGM_MULTI_BEACON_DELAY ; EEPROM copy of programmed beacon delay
  1210. _Eep_Pgm_Throttle_Rate: .DB 0xFF
  1211. Eep_Pgm_Demag_Comp: .DB DEFAULT_PGM_MULTI_DEMAG_COMP ; EEPROM copy of programmed demag compensation
  1212. Eep_Pgm_BEC_Voltage_High: .DB DEFAULT_PGM_BEC_VOLTAGE_HIGH ; EEPROM copy of programmed BEC voltage
  1213. Eep_Pgm_Ppm_Center_Throttle: .DB DEFAULT_PGM_PPM_CENTER_THROTTLE ; EEPROM copy of programmed center throttle (final value is 4x+1000=1488)
  1214. _Eep_Pgm_Main_Spoolup_Time: .DB 0xFF
  1215. Eep_Pgm_Temp_Prot_Enable: .DB DEFAULT_PGM_ENABLE_TEMP_PROT ; EEPROM copy of programmed temperature protection enable
  1216. Eep_Pgm_Enable_Power_Prot: .DB DEFAULT_PGM_ENABLE_POWER_PROT ; EEPROM copy of programmed low rpm power protection enable
  1217. Eep_Pgm_Enable_Pwm_Input: .DB DEFAULT_PGM_ENABLE_PWM_INPUT ; EEPROM copy of programmed PWM input signal enable
  1218. Eep_Pgm_Pwm_Dither: .DB DEFAULT_PGM_MULTI_PWM_DITHER ; EEPROM copy of programmed output PWM dither
  1219. .ENDIF
  1220. Eep_Dummy: .DB 0xFF ; EEPROM address for safety reason
  1221. .ORG 0x60
  1222. Eep_Name: .DB " " ; Name tag (16 Bytes)
  1223. ;**** **** **** **** ****
  1224. .CSEG ; Code segment
  1225. .ORG 0
  1226. Interrupt_Table_Definition ; ATmega interrupts
  1227. ;**** **** **** **** ****
  1228. ; Table definitions
  1229. GOV_GAIN_TABLE: .DB 0x02, 0x03, 0x04, 0x06, 0x08, 0x0C, 0x10, 0x18, 0x20, 0x30, 0x40, 0x60, 0x80, 0 ; Padded zero for an even number
  1230. STARTUP_POWER_TABLE: .DB 0x04, 0x06, 0x08, 0x0C, 0x10, 0x18, 0x20, 0x30, 0x40, 0x60, 0x80, 0xA0, 0xC0, 0 ; Padded zero for an even number
  1231. PWM_DITHER_TABLE: .DB 0x00, 0x07, 0x0F, 0x1F, 0x3F, 0 ; Padded zero for an even number
  1232. .IF MODE == 0
  1233. .IF DAMPED_MODE_ENABLE == 1
  1234. TX_PGM_PARAMS_MAIN: .DB 13, 13, 4, 3, 6, 13, 5, 3, 3, 2, 2, 0 ; Padded zero for an even number
  1235. .ENDIF
  1236. .IF DAMPED_MODE_ENABLE == 0
  1237. TX_PGM_PARAMS_MAIN: .DB 13, 13, 4, 3, 6, 13, 5, 2, 3, 2, 2, 0 ; Padded zero for an even number
  1238. .ENDIF
  1239. .ENDIF
  1240. .IF MODE == 1
  1241. .IF DAMPED_MODE_ENABLE == 1
  1242. TX_PGM_PARAMS_TAIL: .DB 5, 5, 13, 5, 3, 5, 3, 3, 2, 0 ; Padded zero for an even number
  1243. .ENDIF
  1244. .IF DAMPED_MODE_ENABLE == 0
  1245. TX_PGM_PARAMS_TAIL: .DB 5, 5, 13, 5, 2, 5, 3, 3, 2, 0 ; Padded zero for an even number
  1246. .ENDIF
  1247. .ENDIF
  1248. .IF MODE == 2
  1249. .IF DAMPED_MODE_ENABLE == 1
  1250. TX_PGM_PARAMS_MULTI: .DB 13, 13, 4, 5, 13, 5, 3, 5, 3, 3, 2, 0 ; Padded zero for an even number
  1251. .ENDIF
  1252. .IF DAMPED_MODE_ENABLE == 0
  1253. TX_PGM_PARAMS_MULTI: .DB 13, 13, 4, 5, 13, 5, 2, 5, 3, 3, 2, 0 ; Padded zero for an even number
  1254. .ENDIF
  1255. .ENDIF
  1256. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1257. ;
  1258. ; Timer2 interrupt routine
  1259. ;
  1260. ; Assumptions: Z register must be set to desired pwm_nfet_on label
  1261. ; Requirements: I_Temp variables can NOT be used
  1262. ;
  1263. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1264. t2_int: ; Used for pwm control
  1265. in II_Sreg, SREG
  1266. ; Check if pwm is on
  1267. sbrc Flags0, PWM_ON ; Is pwm on?
  1268. rjmp t2_int_pwm_off
  1269. ; Pwm on cycle
  1270. tst Current_Pwm_Limited
  1271. breq t2_int_pwm_on_exit
  1272. ijmp ; Jump to pwm on routines. Z should be set to one of the pwm_nfet_on labels
  1273. t2_int_pwm_on_exit:
  1274. ; Set timer for coming on cycle length
  1275. mov YL, Current_Pwm_Limited ; Load current pwm
  1276. com YL ; com is 255-x
  1277. sec
  1278. sbrc Flags2, PGM_PWM_HIGH_FREQ ; Use half the time when pwm frequency is high
  1279. ror YL
  1280. Set_TCNT2 YL ; Write start point for timer
  1281. ; Set other variables
  1282. sts Pwm_Prev_Edge, YL ; Set timestamp
  1283. sbr Flags0, (1<<PWM_ON) ; Set pwm on flag
  1284. ; Exit interrupt
  1285. out SREG, II_Sreg
  1286. reti
  1287. ; Pwm off cycle
  1288. t2_int_pwm_off:
  1289. lds YL, Current_Pwm_Lim_Dith
  1290. sec
  1291. sbrc Flags2, PGM_PWM_HIGH_FREQ ; Use half the time when pwm frequency is high
  1292. ror YL
  1293. Set_TCNT2 YL ; Load new timer setting
  1294. sts Pwm_Prev_Edge, YL ; Set timestamp
  1295. ; Clear pwm on flag
  1296. cbr Flags0, (1<<PWM_ON)
  1297. ; Set full PWM (on all the time) if current PWM near max. This will give full power, but at the cost of a small "jump" in power
  1298. lds YL, Current_Pwm_Lim_Dith ; Load current pwm dithered
  1299. cpi YL, 0xFF ; Full pwm?
  1300. brne PC+2 ; No - branch
  1301. rjmp t2_int_pwm_off_fullpower_exit ; Yes - exit
  1302. .IF DAMPED_MODE_ENABLE == 1
  1303. ; Do not execute damped pwm when stopped
  1304. sbrs Flags1, MOTOR_SPINNING
  1305. rjmp t2_int_pwm_off_exit_nfets_off
  1306. ; If damped operation, set pFETs on in pwm_off
  1307. sbrc Flags2, PGM_PWMOFF_DAMPED ; Damped operation?
  1308. rjmp t2_int_pwm_off_damped
  1309. .ENDIF
  1310. t2_int_pwm_off_exit_nfets_off:
  1311. ; Separate exit commands here for minimum delay
  1312. out SREG, II_Sreg
  1313. All_nFETs_Off ; Switch off all nfets
  1314. reti
  1315. t2_int_pwm_off_damped:
  1316. .IF PFETON_DELAY < 128
  1317. All_nFETs_Off ; Switch off all nfets
  1318. sbrc Flags1, SKIP_DAMP_ON
  1319. rjmp t2_int_pwm_off_damp_done
  1320. sbrc Flags0, DEMAG_CUT_POWER
  1321. rjmp t2_int_pwm_off_damp_done
  1322. .IF PFETON_DELAY != 0
  1323. ldi YL, PFETON_DELAY
  1324. dec YL
  1325. brne PC-1
  1326. .ENDIF
  1327. Damping_FET_on YL ; Damping fet on
  1328. t2_int_pwm_off_damp_done:
  1329. .ENDIF
  1330. .IF PFETON_DELAY >= 128 ; "Negative", 1's complement
  1331. sbrc Flags1, SKIP_DAMP_ON
  1332. rjmp t2_int_pwm_off_damp_done
  1333. sbrc Flags0, DEMAG_CUT_POWER
  1334. rjmp t2_int_pwm_off_damp_done
  1335. Damping_FET_on YL ; Damping fet on
  1336. ldi YL, PFETON_DELAY
  1337. com YL
  1338. dec YL
  1339. brne PC-1
  1340. t2_int_pwm_off_damp_done:
  1341. All_nFETs_Off ; Switch off all nfets
  1342. .ENDIF
  1343. t2_int_pwm_off_exit:
  1344. sts Pwm_Prev_Edge, Zero ; Set timestamp to zero
  1345. out SREG, II_Sreg
  1346. reti
  1347. t2_int_pwm_off_fullpower_exit:
  1348. Set_TCNT2 Zero ; Write start point for timer
  1349. T2_Clear_Int_Flag YL ; Clear interrupt flag
  1350. sbr Flags0, (1<<PWM_ON)
  1351. rjmp t2_int_pwm_off_exit
  1352. pwm_nofet: ; Dummy pwm on cycle
  1353. rjmp t2_int_pwm_on_exit
  1354. pwm_afet : ; Pwm on cycle afet on
  1355. sbrs Flags1, MOTOR_SPINNING
  1356. rjmp t2_int_pwm_on_exit
  1357. sbrc Flags0, DEMAG_CUT_POWER
  1358. rjmp t2_int_pwm_on_exit
  1359. AnFET_on
  1360. rjmp t2_int_pwm_on_exit
  1361. pwm_bfet: ; Pwm on cycle bfet on
  1362. sbrs Flags1, MOTOR_SPINNING
  1363. rjmp t2_int_pwm_on_exit
  1364. sbrc Flags0, DEMAG_CUT_POWER
  1365. rjmp t2_int_pwm_on_exit
  1366. BnFET_on
  1367. rjmp t2_int_pwm_on_exit
  1368. pwm_cfet: ; Pwm on cycle cfet on
  1369. sbrs Flags1, MOTOR_SPINNING
  1370. rjmp t2_int_pwm_on_exit
  1371. sbrc Flags0, DEMAG_CUT_POWER
  1372. rjmp t2_int_pwm_on_exit
  1373. CnFET_on
  1374. rjmp t2_int_pwm_on_exit
  1375. pwm_afet_damped:
  1376. ApFET_off
  1377. sbrs Flags1, MOTOR_SPINNING
  1378. rjmp t2_int_pwm_on_exit
  1379. sbrc Flags0, DEMAG_CUT_POWER
  1380. rjmp t2_int_pwm_on_exit
  1381. .IF NFETON_DELAY != 0
  1382. ldi YL, NFETON_DELAY ; Set delay
  1383. dec YL
  1384. brne PC-1
  1385. .ENDIF
  1386. pwm_afet_damped_done:
  1387. AnFET_on ; Switch nFET
  1388. rjmp t2_int_pwm_on_exit
  1389. pwm_bfet_damped:
  1390. BpFET_off
  1391. sbrs Flags1, MOTOR_SPINNING
  1392. rjmp t2_int_pwm_on_exit
  1393. sbrc Flags0, DEMAG_CUT_POWER
  1394. rjmp t2_int_pwm_on_exit
  1395. .IF NFETON_DELAY != 0
  1396. ldi YL, NFETON_DELAY ; Set delay
  1397. dec YL
  1398. brne PC-1
  1399. .ENDIF
  1400. pwm_bfet_damped_done:
  1401. BnFET_on ; Switch nFET
  1402. rjmp t2_int_pwm_on_exit
  1403. pwm_cfet_damped:
  1404. CpFET_off
  1405. sbrs Flags1, MOTOR_SPINNING
  1406. rjmp t2_int_pwm_on_exit
  1407. sbrc Flags0, DEMAG_CUT_POWER
  1408. rjmp t2_int_pwm_on_exit
  1409. .IF NFETON_DELAY != 0
  1410. ldi YL, NFETON_DELAY ; Set delay
  1411. dec YL
  1412. brne PC-1
  1413. .ENDIF
  1414. pwm_cfet_damped_done:
  1415. CnFET_on ; Switch nFET
  1416. rjmp t2_int_pwm_on_exit
  1417. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1418. ;
  1419. ; Timer 1 output compare A interrupt
  1420. ;
  1421. ; No assumptions
  1422. ;
  1423. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1424. t1oca_int:
  1425. in II_Sreg, SREG
  1426. T1oca_Int_Disable YL ; Disable timer1 OCA interrupt
  1427. cbr Flags0, (1<<OC1A_PENDING) ; Flag that OC1A value is passed
  1428. ; Set up next wait
  1429. Read_TCNT1L YL
  1430. add YL, Next_Wt_L ; Set wait value
  1431. Read_TCNT1H YH
  1432. adc YH, Next_Wt_H
  1433. Set_OCR1AH YH ; Update high byte first to avoid false output compare
  1434. Set_OCR1AL YL
  1435. out SREG, II_Sreg
  1436. reti
  1437. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1438. ;
  1439. ; Timer0 interrupt routine
  1440. ;
  1441. ; No assumptions
  1442. ;
  1443. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1444. t0_int: ; Happens every 128us
  1445. in I_Sreg, SREG
  1446. ; Disable RCP interrupts
  1447. cbr Flags2, (1<<RCP_INT_NESTED_ENABLED) ; Set flag default to disabled
  1448. Get_Rcp_Int_Enable_State XL ; Get rcp interrupt state
  1449. cpse XL, Zero
  1450. sbr Flags2, (1<<RCP_INT_NESTED_ENABLED) ; Set flag to enabled
  1451. Rcp_Int_Disable XL ; Disable rcp interrupts
  1452. T0_Int_Disable XL ; Disable timer0 interrupts
  1453. sei ; Enable interrupts
  1454. ; Check RC pulse timeout counter
  1455. lds XL, Rcp_Timeout_Cntd ; RC pulse timeout count zero?
  1456. tst XL
  1457. breq t0_int_pulses_absent ; Yes - pulses are absent
  1458. ; Decrement timeout counter (if PWM)
  1459. sbrc Flags2, RCP_PPM
  1460. rjmp t0_int_skip_start ; If flag is set (PPM) - branch
  1461. lds XL, Rcp_Timeout_Cntd ; No - decrement
  1462. dec XL
  1463. sts Rcp_Timeout_Cntd, XL
  1464. rjmp t0_int_skip_start
  1465. t0_int_pulses_absent:
  1466. ; Timeout counter has reached zero, pulses are absent
  1467. ldi I_Temp1, RCP_MIN ; RCP_MIN as default
  1468. ldi I_Temp2, RCP_MIN
  1469. sbrc Flags2, RCP_PPM
  1470. rjmp t0_int_pulses_absent_no_max ; If flag is set (PPM) - branch
  1471. Read_Rcp_Int XL ; Look at value of Rcp_In
  1472. sbrc XL, Rcp_In ; Is it high?
  1473. ldi I_Temp1, RCP_MAX ; Yes - set RCP_MAX
  1474. Rcp_Int_First XL ; Set interrupt trig to first again
  1475. Rcp_Clear_Int_Flag XL ; Clear interrupt flag
  1476. cbr Flags2, (1<<RCP_EDGE_NO) ; Set first edge flag
  1477. Read_Rcp_Int XL ; Look once more at value of Rcp_In
  1478. sbrc XL, Rcp_In ; Is it high?
  1479. ldi I_Temp2, RCP_MAX ; Yes - set RCP_MAX
  1480. cp I_Temp1, I_Temp2
  1481. brne t0_int_pulses_absent ; Go back if they are not equal
  1482. t0_int_pulses_absent_no_max:
  1483. ldi XL, RCP_TIMEOUT ; Load timeout count
  1484. sbrc Flags0, RCP_MEAS_PWM_FREQ ; Is measure RCP pwm frequency flag set?
  1485. sts Rcp_Timeout_Cntd, XL ; Yes - set timeout count to start value
  1486. sbrc Flags2, RCP_PPM
  1487. rjmp t0_int_ppm_timeout_set ; If flag is set (PPM) - branch
  1488. ldi XL, RCP_TIMEOUT ; For PWM, set timeout count to start value
  1489. sts Rcp_Timeout_Cntd, XL
  1490. t0_int_ppm_timeout_set:
  1491. sts New_Rcp, I_Temp1 ; Store new pulse length
  1492. sbr Flags2, (1<<RCP_UPDATED) ; Set updated flag
  1493. t0_int_skip_start:
  1494. sbrc Flags2, RCP_PPM
  1495. rjmp t0_int_rcp_update_start ; If flag is set (PPM) - branch
  1496. ; Check RC pulse skip counter
  1497. lds XL, Rcp_Skip_Cntd
  1498. tst XL
  1499. breq t0_int_skip_end ; If RC pulse skip count is zero - end skipping RC pulse detection
  1500. ; Decrement skip counter (only if edge counter is zero)
  1501. lds XL, Rcp_Skip_Cntd ; Decrement
  1502. dec XL
  1503. sts Rcp_Skip_Cntd, XL
  1504. rjmp t0_int_rcp_update_start
  1505. t0_int_skip_end:
  1506. ; Skip counter has reached zero, start looking for RC pulses again
  1507. sbr Flags2, (1<<RCP_INT_NESTED_ENABLED) ; Set flag to enabled
  1508. Rcp_Clear_Int_Flag XL ; Clear interrupt flag
  1509. t0_int_rcp_update_start:
  1510. ; Process updated RC pulse
  1511. sbrs Flags2, RCP_UPDATED ; Is there an updated RC pulse available?
  1512. rjmp t0_int_current_pwm_update ; No - update pwm limits and exit
  1513. lds XL, New_Rcp ; Load new pulse value
  1514. mov I_Temp1, XL
  1515. sbrs Flags0, RCP_MEAS_PWM_FREQ ; If measure RCP pwm frequency flag set - do not clear flag
  1516. cbr Flags2, (1<<RCP_UPDATED) ; Flag that pulse has been evaluated
  1517. ; Use a gain of 1.0625x for pwm input if not governor mode
  1518. sbrc Flags2, RCP_PPM
  1519. rjmp t0_int_pwm_min_run ; If flag is set (PPM) - branch
  1520. .IF MODE == 0 ; Main - do not adjust gain
  1521. rjmp t0_int_pwm_min_run
  1522. .ELSE
  1523. .IF MODE == 2 ; Multi
  1524. lds XL, Pgm_Gov_Mode ; Closed loop mode?
  1525. cpi XL, 4
  1526. brne t0_int_pwm_min_run ; Yes - branch
  1527. .ENDIF
  1528. ; Limit the maximum value to avoid wrap when scaled to pwm range
  1529. cpi I_Temp1, 240 ; 240 = (255/1.0625) Needs to be updated according to multiplication factor below
  1530. brcs t0_int_rcp_update_mult
  1531. ldi I_Temp1, 240 ; Set requested pwm to max
  1532. t0_int_rcp_update_mult:
  1533. ; Multiply by 1.0625 (optional adjustment gyro gain)
  1534. mov XL, I_Temp1
  1535. swap XL ; After this "0.0625"
  1536. andi XL, 0x0F
  1537. add I_Temp1, XL
  1538. ; Adjust tail gain
  1539. lds I_Temp2, Pgm_Motor_Gain ; Is gain 1?
  1540. cpi I_Temp2, 3
  1541. breq t0_int_pwm_min_run ; Yes - skip adjustment
  1542. lsr XL ; After this "0.5"
  1543. lsr XL ; After this "0.25"
  1544. sbrc I_Temp2, 0 ; (I_Temp2 has Pgm_Motor_Gain)
  1545. rjmp t0_int_rcp_gain_corr ; Branch if bit 0 in gain is set
  1546. lsr XL ; After this "0.125"
  1547. t0_int_rcp_gain_corr:
  1548. sbrc I_Temp2, 2 ; (I_Temp2 has Pgm_Motor_Gain)
  1549. rjmp t0_int_rcp_gain_pos ; Branch if bit 2 in gain is set
  1550. sub XL, I_Temp1 ; Apply negative correction
  1551. mov I_Temp1, XL
  1552. rjmp t0_int_pwm_min_run
  1553. t0_int_rcp_gain_pos:
  1554. add I_Temp1, XL ; Apply positive correction
  1555. brcc t0_int_pwm_min_run ; Above max?
  1556. ldi I_Temp1, 0xFF ; Yes - limit
  1557. .ENDIF
  1558. t0_int_pwm_min_run:
  1559. .IF MODE == 1 ; Tail - limit minimum pwm
  1560. ; Limit minimum pwm
  1561. lds XL, Pwm_Motor_Idle ; Is requested pwm lower than minimum?
  1562. cp I_Temp1, XL
  1563. brcc t0_int_pwm_update ; No - branch
  1564. mov I_Temp1, XL ; Yes - limit pwm to Pwm_Motor_Idle
  1565. .ENDIF
  1566. t0_int_pwm_update:
  1567. ; Update requested_pwm
  1568. sts Requested_Pwm, I_Temp1 ; Set requested pwm
  1569. .IF MODE >= 1 ; Tail or multi
  1570. ; Boost pwm during direct start
  1571. mov XL, Flags1
  1572. andi XL, ((1<<STARTUP_PHASE)+(1<<INITIAL_RUN_PHASE))
  1573. breq t0_int_current_pwm_update
  1574. lds XL, Startup_Cnt ; Add an extra power boost during start
  1575. lsr XL
  1576. lsr XL
  1577. ldi I_Temp2, 6
  1578. add XL, I_Temp2
  1579. lds I_Temp2, Requested_Pwm
  1580. add XL, I_Temp2
  1581. sts Requested_Pwm, XL
  1582. brcc PC+4
  1583. ldi XL, 0xFF
  1584. sts Requested_Pwm, XL
  1585. .ENDIF
  1586. t0_int_current_pwm_update:
  1587. .IF MODE == 0 || MODE == 2 ; Main or multi
  1588. lds I_Temp1, Pgm_Gov_Mode ; Governor mode?
  1589. cpi I_Temp1, 4
  1590. breq PC+2
  1591. rjmp t0_int_pwm_exit ; Yes - branch
  1592. .ENDIF
  1593. lds XL, Requested_Pwm ; Set equal as default
  1594. sts Current_Pwm, XL
  1595. .IF MODE >= 1 ; Tail or multi
  1596. ; Set current_pwm_limited
  1597. lds I_Temp1, Current_Pwm ; Default not limited
  1598. lds XL, Pwm_Limit ; Check against limit
  1599. cp I_Temp1, XL
  1600. brcs PC+2 ; If current pwm below limit - branch
  1601. mov I_Temp1, XL ; Limit pwm
  1602. .IF MODE == 2 ; Multi
  1603. ; Limit pwm for low rpms
  1604. lds XL, Pwm_Limit_By_Rpm ; Check against limit
  1605. cp I_Temp1, XL
  1606. brcs PC+2 ; If current pwm below limit - branch
  1607. mov I_Temp1, XL ; Limit pwm
  1608. .ENDIF
  1609. mov Current_Pwm_Limited, I_Temp1
  1610. ; Dither
  1611. lds XL, Pwm_Dither_Decoded ; Load pwm dither
  1612. tst XL
  1613. brne PC+2 ; If active - branch
  1614. rjmp t0_int_current_pwm_no_dither
  1615. mov I_Temp2, I_Temp1
  1616. sub I_Temp2, XL ; Calculate pwm minus dither value
  1617. brcc t0_int_current_pwm_full_dither; If pwm more than dither value, then do full dither
  1618. mov XL, I_Temp1 ; Set dither level to current pwm
  1619. ldi I_Temp2, 0 ; Set pwm minus dither
  1620. t0_int_current_pwm_full_dither:
  1621. mov I_Temp4, XL ; Store dither value in I_Temp4
  1622. clc
  1623. rol XL ; Shift left once
  1624. mov I_Temp3, XL
  1625. lds XL, Random ; Load random number
  1626. com XL ; Invert to create proper DC bias in random code
  1627. and I_Temp3, XL ; And with double dither value
  1628. add I_Temp3, I_Temp2 ; Add pwm minus dither
  1629. brcs t0_int_current_pwm_dither_max_excess_power ; If dither cause power above max - branch and increase excess
  1630. lds XL, Pwm_Dither_Excess_Power ; Get excess power
  1631. cp XL, Zero ; Decrement excess power
  1632. breq PC+2
  1633. dec XL
  1634. add I_Temp3, XL ; Add excess power from previous cycles
  1635. sts Pwm_Dither_Excess_Power, XL
  1636. brcs t0_int_current_pwm_dither_max_power; If dither cause power above max - branch
  1637. mov I_Temp1, I_Temp3
  1638. rjmp t0_int_current_pwm_no_dither
  1639. t0_int_current_pwm_dither_max_excess_power:
  1640. lds XL, Pwm_Dither_Excess_Power
  1641. cp I_Temp4, XL ; Limit excess power to one above in order to always reach max power
  1642. brcs PC+2
  1643. inc XL
  1644. sts Pwm_Dither_Excess_Power, XL
  1645. t0_int_current_pwm_dither_max_power:
  1646. ldi I_Temp1, 255 ; Set power to max
  1647. t0_int_current_pwm_no_dither:
  1648. sts Current_Pwm_Lim_Dith, I_Temp1
  1649. .IF DAMPED_MODE_ENABLE == 1
  1650. ; Skip damping fet switching for high throttle
  1651. cbr Flags1, (1<<SKIP_DAMP_ON)
  1652. subi I_Temp1, 248
  1653. brcs t0_int_pwm_exit
  1654. sbr Flags1, (1<<SKIP_DAMP_ON)
  1655. .ENDIF
  1656. .ENDIF
  1657. t0_int_pwm_exit:
  1658. ; Set demag enabled if pwm is above limit
  1659. mov XL, Current_Pwm_Limited
  1660. cpi XL, 0x40 ; Set if above 25%
  1661. brcs PC+2
  1662. sbr Flags0, (1<<DEMAG_ENABLED)
  1663. ; Increment counter and check if high "interrupt"
  1664. lds XL, Timer0_Int_Cnt
  1665. inc XL
  1666. sts Timer0_Int_Cnt, XL
  1667. breq t0h_int
  1668. cli ; Disable interrupts
  1669. T0_Int_Enable XL ; Enable timer0 interrupts
  1670. sbrs Flags2, RCP_INT_NESTED_ENABLED; Restore rcp interrupt state
  1671. rjmp t0_int_pwm_ret
  1672. Rcp_Int_Enable XL
  1673. t0_int_pwm_ret:
  1674. out SREG, I_Sreg
  1675. reti
  1676. t0h_int:
  1677. ; Every 256th interrupt (happens every 32ms)
  1678. lds XL, Timer2_X
  1679. inc XL
  1680. sts Timer2_X, XL
  1681. ldi I_Temp1, GOV_SPOOLRATE ; Load governor spool rate
  1682. ; Check RC pulse timeout counter (used here for PPM only)
  1683. lds I_Temp2, Rcp_Timeout_Cntd ; RC pulse timeout count zero?
  1684. tst I_Temp2
  1685. breq t0h_int_rcp_stop_check ; Yes - do not decrement
  1686. ; Decrement timeout counter (if PPM)
  1687. sbrs Flags2, RCP_PPM
  1688. rjmp t0h_int_rcp_stop_check ; If flag is not set (PWM) - branch
  1689. dec I_Temp2 ; No flag set (PPM) - decrement
  1690. sts Rcp_Timeout_Cntd, I_Temp2
  1691. t0h_int_rcp_stop_check:
  1692. ; Check RC pulse against stop value
  1693. lds XL, New_Rcp ; Load new pulse value
  1694. cpi XL, RCP_STOP ; Check if pulse is below stop value
  1695. brcs t0h_int_rcp_stop
  1696. ; RC pulse higher than stop value, reset stop counter
  1697. sts Rcp_Stop_Cnt, Zero ; Reset rcp stop counter
  1698. rjmp t0h_int_rcp_gov_pwm
  1699. t0h_int_rcp_stop:
  1700. ; RC pulse less than stop value
  1701. sts Auto_Bailout_Armed, Zero ; Disarm bailout
  1702. sts Spoolup_Limit_Cnt, Zero
  1703. lds XL, Rcp_Stop_Cnt ; Increment stop counter
  1704. subi XL, 0xFF ; Subtract minus one
  1705. sts Rcp_Stop_Cnt, XL
  1706. brcs t0h_int_rcp_gov_pwm ; Branch if counter has not wrapped
  1707. ldi XL, 0xFF ; Set stop counter to max
  1708. sts Rcp_Stop_Cnt, XL
  1709. t0h_int_rcp_gov_pwm:
  1710. .IF MODE == 0 ; Main
  1711. ; Update governor variables
  1712. lds I_Temp4, Requested_Pwm ; Load requested pwm
  1713. lds I_Temp2, Pgm_Gov_Mode ; Governor target by arm mode?
  1714. cpi I_Temp2, 2
  1715. brne t0h_int_rcp_gov_by_setup ; No - branch
  1716. sbrs Flags0, GOV_ACTIVE ; Is governor active?
  1717. breq t0h_int_rcp_gov_by_tx ; No - branch (this ensures soft spoolup by tx)
  1718. ldi XL, 50
  1719. cp I_Temp4, XL ; Is requested pwm below 20%? (Requested_Pwm in I_Temp4)
  1720. brcs t0h_int_rcp_gov_by_tx ; Yes - branch (this enables a soft spooldown)
  1721. lds XL, Gov_Arm_Target ; Yes - load arm target
  1722. sts Requested_Pwm, XL
  1723. t0h_int_rcp_gov_by_setup:
  1724. cpi I_Temp2, 3 ; Governor target by setup mode? (Pgm_Gov_Mode in I_Temp2)
  1725. brne t0h_int_rcp_gov_by_tx ; No - branch
  1726. sbrs Flags0, GOV_ACTIVE ; Is governor active?
  1727. breq t0h_int_rcp_gov_by_tx ; No - branch (this ensures soft spoolup by tx)
  1728. ldi XL, 50
  1729. cp I_Temp4, XL ; Is requested pwm below 20%? (Requested_Pwm in I_Temp4)
  1730. brcs t0h_int_rcp_gov_by_tx ; Yes - branch (this enables a soft spooldown)
  1731. lds XL, Pgm_Gov_Setup_Target ; Gov by setup - load setup target
  1732. sts Requested_Pwm, XL
  1733. t0h_int_rcp_gov_by_tx:
  1734. lds I_Temp2, Governor_Req_Pwm
  1735. cp I_Temp2, I_Temp4 ; Is governor requested pwm equal to requested pwm? (Requested_Pwm in I_Temp4)
  1736. breq t0h_int_rcp_gov_pwm_done ; Yes - branch
  1737. brcs t0h_int_rcp_gov_pwm_inc ; No - if lower, then increment
  1738. dec I_Temp2 ; No - if higher, then decrement
  1739. rjmp t0h_int_rcp_gov_pwm_done
  1740. t0h_int_rcp_gov_pwm_inc:
  1741. inc I_Temp2 ; Increment
  1742. t0h_int_rcp_gov_pwm_done:
  1743. sts Governor_Req_Pwm, I_Temp2 ; Store governor requested pwm
  1744. dec I_Temp1 ; Decrement spoolrate variable
  1745. brne t0h_int_rcp_gov_pwm ; If not number of steps processed - go back
  1746. lds I_Temp2, Spoolup_Limit_Cnt ; Load spoolup count
  1747. inc I_Temp2 ; Increment
  1748. brne PC+2 ; Wrapped?
  1749. dec I_Temp2 ; Yes - decrement
  1750. sts Spoolup_Limit_Cnt, I_Temp2
  1751. lds XL, Spoolup_Limit_Skip ; Load skip count
  1752. dec XL ; Decrement
  1753. sts Spoolup_Limit_Skip, XL ; Store skip count
  1754. breq PC+2
  1755. rjmp t0h_int_exit ; Jump if skip count is not reached
  1756. ldi XL, 1 ; Reset skip count. Default is fast spoolup
  1757. sts Spoolup_Limit_Skip, XL
  1758. ldi I_Temp1, 5 ; Default fast increase
  1759. lds XL, Main_Spoolup_Time_3x ; No spoolup until 3*N*32ms (Spoolup_Limit_Cnt in I_Temp2)
  1760. cp I_Temp2, XL
  1761. brcs t0h_int_exit
  1762. lds XL, Main_Spoolup_Time_10x ; Slow spoolup until 10*N*32ms (Spoolup_Limit_Cnt in I_Temp2)
  1763. cp I_Temp2, XL
  1764. brcc t0h_int_rcp_limit_middle_ramp
  1765. ldi I_Temp1, 1 ; Slow initial spoolup
  1766. ldi XL, 3
  1767. sts Spoolup_Limit_Skip, XL
  1768. rjmp t0h_int_rcp_set_limit
  1769. t0h_int_rcp_limit_middle_ramp:
  1770. lds XL, Main_Spoolup_Time_15x ; Faster spoolup until 15*N*32ms (Spoolup_Limit_Cnt in I_Temp2)
  1771. cp I_Temp2, XL
  1772. brcc t0h_int_rcp_set_limit
  1773. ldi I_Temp1, 1 ; Faster middle spoolup
  1774. sts Spoolup_Limit_Skip, I_Temp1
  1775. t0h_int_rcp_set_limit:
  1776. ; Do not increment spoolup limit if higher pwm is not requested, unless governor is active
  1777. lds I_Temp6, Pwm_Limit_Spoolup ; Load pwm limit spoolup
  1778. lds I_Temp5, Current_Pwm ; Load current pwm
  1779. cp I_Temp6, I_Temp5
  1780. brcs t0h_int_rcp_inc_limit ; If Current_Pwm is larger than Pwm_Limit_Spoolup - branch
  1781. lds XL, Pgm_Gov_Mode ; Governor mode?
  1782. cpi XL, 4
  1783. breq t0h_int_rcp_bailout_arm ; No - branch
  1784. sbrc Flags0, GOV_ACTIVE ; Is governor active?
  1785. rjmp t0h_int_rcp_inc_limit ; Yes - branch
  1786. sts Pwm_Limit_Spoolup, I_Temp5 ; Set limit to what current pwm is
  1787. inc I_Temp2 ; Check if spoolup limit count is 255
  1788. breq PC+3 ; If it is, then this is a "bailout" ramp
  1789. lds XL, Main_Spoolup_Time_3x ; Stay in an early part of the spoolup sequence (unless "bailout" ramp)
  1790. sts Spoolup_Limit_Cnt, XL
  1791. ldi XL, 1 ; Set skip count
  1792. sts Spoolup_Limit_Skip, XL
  1793. ldi XL, 60 ; Set governor requested speed to ensure that it requests higher speed
  1794. sts Governor_Req_Pwm, XL
  1795. ; 20=Fail on jerk when governor activates
  1796. ; 30=Ok
  1797. ; 100=Fail on small governor settling overshoot on low headspeeds
  1798. ; 200=Fail on governor settling overshoot
  1799. rjmp t0h_int_exit ; Exit
  1800. t0h_int_rcp_inc_limit:
  1801. lds XL, Pwm_Limit_Spoolup ; Increment spoolup pwm
  1802. add XL, I_Temp1
  1803. brcc t0h_int_rcp_no_limit ; If below 255 - branch
  1804. ldi I_Temp2, 0xFF
  1805. sts Pwm_Limit_Spoolup, I_Temp2
  1806. rjmp t0h_int_rcp_bailout_arm
  1807. t0h_int_rcp_no_limit:
  1808. sts Pwm_Limit_Spoolup, XL
  1809. t0h_int_rcp_bailout_arm:
  1810. lds XL, Pwm_Limit_Spoolup
  1811. cpi XL, 0xFF
  1812. brne t0h_int_exit
  1813. ldi XL, 0xFF
  1814. sts Auto_Bailout_Armed, XL ; Arm bailout
  1815. sts Spoolup_Limit_Cnt, XL
  1816. .ENDIF
  1817. t0h_int_exit:
  1818. cli ; Disable interrupts
  1819. T0_Int_Enable XL ; Enable timer0 interrupts
  1820. sbrs Flags2, RCP_INT_NESTED_ENABLED; Restore rcp interrupt state
  1821. rjmp t0h_int_pwm_ret
  1822. Rcp_Int_Enable XL
  1823. t0h_int_pwm_ret:
  1824. out SREG, I_Sreg
  1825. reti
  1826. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1827. ;
  1828. ; RC pulse processing interrupt routine
  1829. ;
  1830. ; No assumptions
  1831. ; Can come from int0 or icp
  1832. ;
  1833. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  1834. rcp_int: ; Used for RC pulse timing
  1835. in I_Sreg, SREG
  1836. ; Get the timer counter values
  1837. Get_Rcp_Capture_Values I_Temp1, I_Temp2
  1838. ; Disable RCP interrupts
  1839. cbr Flags2, (1<<RCP_INT_NESTED_ENABLED) ; Set flag default to disabled
  1840. Get_Rcp_Int_Enable_State XL ; Get rcp interrupt state
  1841. cpse XL, Zero
  1842. sbr Flags2, (1<<RCP_INT_NESTED_ENABLED) ; Set flag to enabled
  1843. Rcp_Int_Disable XL ; Disable rcp interrupts
  1844. T0_Int_Disable XL ; Disable timer0 interrupts
  1845. sei ; Enable interrupts
  1846. ; Check which edge it is
  1847. sbrc Flags2, RCP_EDGE_NO ; Is it a first edge trig?
  1848. rjmp rcp_int_second_meas_pwm_freq ; No - branch to second
  1849. Rcp_Int_Second XL ; Yes - set second edge trig
  1850. sbr Flags2, (1<<RCP_EDGE_NO) ; Set second edge flag
  1851. ; Read RC signal level
  1852. Read_Rcp_Int XL
  1853. ; Test RC signal level
  1854. sbrs XL, Rcp_In ; Is it high?
  1855. rjmp rcp_int_fail_minimum ; No - jump to fail minimum
  1856. ; RC pulse was high, store RC pulse start timestamp
  1857. sts Rcp_Prev_Edge_L, I_Temp1
  1858. sts Rcp_Prev_Edge_H, I_Temp2
  1859. rjmp rcp_int_exit ; Exit
  1860. rcp_int_fail_minimum:
  1861. ; Prepare for next interrupt
  1862. Rcp_Int_First XL ; Set interrupt trig to first again
  1863. Rcp_Clear_Int_Flag XL ; Clear interrupt flag
  1864. cbr Flags2, (1<<RCP_EDGE_NO) ; Set first edge flag
  1865. sbrs Flags2, RCP_PPM
  1866. rjmp PC+2 ; If flag is not set (PWM) - branch
  1867. rjmp rcp_int_set_timeout ; If no flag is set (PPM) - ignore trig as noise
  1868. ldi I_Temp1, RCP_MIN ; Set RC pulse value to minimum
  1869. Read_Rcp_Int XL ; Test RC signal level again
  1870. sbrc XL, Rcp_In ; Is it high?
  1871. rjmp rcp_int_set_timeout ; Yes - set new timeout and exit
  1872. sts New_Rcp, I_Temp1 ; Store new pulse length
  1873. rjmp rcp_int_limited ; Set new RC pulse, new timeout and exit
  1874. rcp_int_second_meas_pwm_freq:
  1875. ; Prepare for next interrupt
  1876. Rcp_Int_First XL ; Set first edge trig
  1877. cbr Flags2, (1<<RCP_EDGE_NO) ; Set first edge flag
  1878. ; Check if pwm frequency shall be measured
  1879. sbrs Flags0, RCP_MEAS_PWM_FREQ ; Is measure RCP pwm frequency flag set?
  1880. rjmp rcp_int_fall ; No - skip measurements
  1881. ; Set second edge trig only during pwm frequency measurement
  1882. Rcp_Int_Second XL ; Set second edge trig
  1883. Rcp_Clear_Int_Flag XL ; Clear interrupt flag
  1884. sbr Flags2, (1<<RCP_EDGE_NO) ; Set second edge flag
  1885. ; Store edge data to RAM
  1886. sts Rcp_Edge_L, I_Temp1
  1887. sts Rcp_Edge_H, I_Temp2
  1888. ; Calculate pwm frequency
  1889. lds I_Temp3, Rcp_PrePrev_Edge_L
  1890. sub I_Temp1, I_Temp3
  1891. lds I_Temp4, Rcp_PrePrev_Edge_H
  1892. sbc I_Temp2, I_Temp4
  1893. clr I_Temp4
  1894. mov I_Temp3, Zero ; (LSB)
  1895. ; Check if pulse is too short
  1896. cpi I_Temp1, low(140) ; If pulse below 70us, not accepted
  1897. cpc I_Temp2, Zero
  1898. brcc rcp_int_check_12kHz
  1899. sts Rcp_Period_Diff_Accepted, Zero ; Set not accepted
  1900. rjmp rcp_int_store_data
  1901. rcp_int_check_12kHz:
  1902. lds XL, Pgm_Enable_PWM_Input ; Check if PWM input is enabled
  1903. tst XL
  1904. breq rcp_int_restore_edge ; If it is not - branch
  1905. ; Check if pwm frequency is 12kHz
  1906. cpi I_Temp1, low(200) ; If below 100us, 12kHz pwm is assumed
  1907. cpc I_Temp2, Zero
  1908. brcc rcp_int_check_8kHz
  1909. ldi XL, (1<<RCP_PWM_FREQ_12KHZ)
  1910. mov I_Temp4, XL
  1911. ldi XL, 10 ; Set period tolerance requirement (LSB)
  1912. mov I_Temp3, XL
  1913. rjmp rcp_int_restore_edge
  1914. rcp_int_check_8kHz:
  1915. ; Check if pwm frequency is 8kHz
  1916. cpi I_Temp1, low(360) ; If below 180us, 8kHz pwm is assumed
  1917. ldi XL, high(360)
  1918. cpc I_Temp2, XL
  1919. brcc rcp_int_check_4kHz
  1920. ldi XL, (1<<RCP_PWM_FREQ_8KHZ)
  1921. mov I_Temp4, XL
  1922. ldi XL, 15 ; Set period tolerance requirement (LSB)
  1923. mov I_Temp3, XL
  1924. rjmp rcp_int_restore_edge
  1925. rcp_int_check_4kHz:
  1926. ; Check if pwm frequency is 4kHz
  1927. cpi I_Temp1, low(720) ; If below 360us, 4kHz pwm is assumed
  1928. ldi XL, high(720)
  1929. cpc I_Temp2, XL
  1930. brcc rcp_int_check_2kHz
  1931. ldi XL, (1<<RCP_PWM_FREQ_4KHZ)
  1932. mov I_Temp4, XL
  1933. ldi XL, 30 ; Set period tolerance requirement (LSB)
  1934. mov I_Temp3, XL
  1935. rjmp rcp_int_restore_edge
  1936. rcp_int_check_2kHz:
  1937. ; Check if pwm frequency is 2kHz
  1938. cpi I_Temp1, low(1440) ; If below 720us, 2kHz pwm is assumed
  1939. ldi XL, high(1440)
  1940. cpc I_Temp2, XL
  1941. brcc rcp_int_check_1kHz
  1942. ldi XL, (1<<RCP_PWM_FREQ_2KHZ)
  1943. mov I_Temp4, XL
  1944. ldi XL, 60 ; Set period tolerance requirement (LSB)
  1945. mov I_Temp3, XL
  1946. rjmp rcp_int_restore_edge
  1947. rcp_int_check_1kHz:
  1948. ; Check if pwm frequency is 1kHz
  1949. cpi I_Temp1, low(2200) ; If below 1100us, 1kHz pwm is assumed
  1950. ldi XL, high(2200)
  1951. cpc I_Temp2, XL
  1952. brcc rcp_int_restore_edge
  1953. ldi XL, (1<<RCP_PWM_FREQ_1KHZ)
  1954. mov I_Temp4, XL
  1955. ldi XL, 120 ; Set period tolerance requirement (LSB)
  1956. mov I_Temp3, XL
  1957. rcp_int_restore_edge:
  1958. ; Calculate difference between this period and previous period
  1959. mov I_Temp5, I_Temp1
  1960. lds XL, Rcp_Prev_Period_L
  1961. sub I_Temp5, XL
  1962. mov I_Temp6, I_Temp2
  1963. lds XL, Rcp_Prev_Period_H
  1964. sbc I_Temp6, XL
  1965. ; Make positive
  1966. tst I_Temp6
  1967. brpl rcp_int_check_diff
  1968. ldi XL, 0xFF ; Change sign - invert and subtract minus one
  1969. com I_Temp5
  1970. com I_Temp6
  1971. sub I_Temp5, XL
  1972. sbc I_Temp6, XL
  1973. rcp_int_check_diff:
  1974. ; Check difference
  1975. sts Rcp_Period_Diff_Accepted, Zero ; Set not accepted as default
  1976. ldi XL, 8 ; Set default period tolerance requirement
  1977. cpse I_Temp3, Zero ; Check if no tolerance requirement is set
  1978. ldi XL, 0 ; Set tolerance MSB to zero
  1979. cp I_Temp5, I_Temp3 ; Check difference
  1980. cpc I_Temp6, XL
  1981. brcc rcp_int_store_data
  1982. ldi XL, 1 ; Set accepted
  1983. sts Rcp_Period_Diff_Accepted, XL
  1984. rcp_int_store_data:
  1985. ; Store previous period
  1986. sts Rcp_Prev_Period_L, I_Temp1
  1987. sts Rcp_Prev_Period_H, I_Temp2
  1988. ; Restore edge data from RAM
  1989. lds I_Temp1, Rcp_Edge_L
  1990. lds I_Temp2, Rcp_Edge_H
  1991. ; Store pre previous edge
  1992. sts Rcp_PrePrev_Edge_L, I_Temp1
  1993. sts Rcp_PrePrev_Edge_H, I_Temp2
  1994. ldi Temp1, RCP_VALIDATE
  1995. rjmp rcp_int_limited
  1996. rcp_int_fall:
  1997. ; RC pulse edge was second, calculate new pulse length
  1998. lds XL, Rcp_Prev_Edge_L
  1999. sub I_Temp1, XL
  2000. lds XL, Rcp_Prev_Edge_H
  2001. sbc I_Temp2, XL
  2002. sbrc Flags3, RCP_PWM_FREQ_12KHZ ; Is RC input pwm frequency 12kHz?
  2003. rjmp rcp_int_pwm_divide_done ; Yes - branch forward
  2004. sbrc Flags3, RCP_PWM_FREQ_8KHZ ; Is RC input pwm frequency 8kHz?
  2005. rjmp rcp_int_pwm_divide_done ; Yes - branch forward
  2006. sbrc Flags3, RCP_PWM_FREQ_4KHZ ; Is RC input pwm frequency 4kHz?
  2007. rjmp rcp_int_pwm_divide ; Yes - branch forward
  2008. sbrs Flags2, RCP_PPM_ONESHOT125
  2009. rjmp rcp_int_fall_not_oneshot
  2010. mov I_Temp6, I_Temp2 ; Oneshot125 - move to I_Temp5/6
  2011. mov I_Temp5, I_Temp1
  2012. rjmp rcp_int_fall_check_range
  2013. rcp_int_fall_not_oneshot:
  2014. lsr I_Temp2 ; No - 2kHz. Divide by 2
  2015. ror I_Temp1
  2016. sbrc Flags3, RCP_PWM_FREQ_2KHZ ; Is RC input pwm frequency 2kHz?
  2017. rjmp rcp_int_pwm_divide ; Yes - branch forward
  2018. lsr I_Temp2 ; No - 1kHz. Divide by 2 again
  2019. ror I_Temp1
  2020. sbrc Flags3, RCP_PWM_FREQ_1KHZ ; Is RC input pwm frequency 1kHz?
  2021. rjmp rcp_int_pwm_divide ; Yes - branch forward
  2022. mov I_Temp6, I_Temp2 ; No - PPM. Divide by 2 (to bring range to 256) and move to I_Temp5/6
  2023. mov I_Temp5, I_Temp1
  2024. lsr I_Temp6
  2025. ror I_Temp5
  2026. rcp_int_fall_check_range:
  2027. ; Skip range limitation if pwm frequency measurement
  2028. sbrc Flags0, RCP_MEAS_PWM_FREQ
  2029. rjmp rcp_int_ppm_check_full_range
  2030. ; Check if 2160us or above (in order to ignore false pulses)
  2031. mov XL, I_Temp5 ; Is pulse 2160us or higher?
  2032. subi XL, 28
  2033. mov XL, I_Temp6
  2034. sbci XL, 2
  2035. brcs PC+2
  2036. rjmp rcp_int_ppm_outside_range ; Yes - ignore pulse
  2037. ; Check if below 800us (in order to ignore false pulses)
  2038. tst I_Temp6
  2039. brne rcp_int_ppm_check_full_range
  2040. mov XL, I_Temp5 ; Is pulse below 800us?
  2041. subi XL, 200
  2042. brcc rcp_int_ppm_check_full_range ; No - branch
  2043. rcp_int_ppm_outside_range:
  2044. lds XL, Rcp_Outside_Range_Cnt
  2045. inc XL
  2046. tst XL
  2047. breq PC+3
  2048. sts Rcp_Outside_Range_Cnt, XL
  2049. cpi XL, 10 ; Allow a given number of outside pulses
  2050. brcc PC+2
  2051. rjmp rcp_int_set_timeout ; If below limit - ignore pulse
  2052. sts New_Rcp, Zero ; Set pulse length to zero
  2053. sbr Flags2, (1<<RCP_UPDATED) ; Set updated flag
  2054. rjmp rcp_int_set_timeout
  2055. rcp_int_ppm_check_full_range:
  2056. ; Decrement outside range counter
  2057. lds XL, Rcp_Outside_Range_Cnt
  2058. tst XL
  2059. breq PC+4
  2060. dec XL
  2061. sts Rcp_Outside_Range_Cnt, XL
  2062. ; Calculate "1000us" plus throttle minimum
  2063. mov I_Temp3, Zero ; Set 1000us as default minimum
  2064. lds I_Temp2, Pgm_Direction ; Check if bidirectional operation (store in I_Temp2)
  2065. sbrc Flags3, FULL_THROTTLE_RANGE ; Check if full range is chosen
  2066. rjmp rcp_int_ppm_calculate ; Yes - branch
  2067. lds I_Temp3, Pgm_Ppm_Min_Throttle ; Min throttle value is in 4us units
  2068. .IF MODE >= 1 ; Tail or multi
  2069. cpi I_Temp2, 3
  2070. brne PC+3 ; No - branch
  2071. lds I_Temp3, Pgm_Ppm_Center_Throttle ; Center throttle value is in 4us units
  2072. .ENDIF
  2073. rcp_int_ppm_calculate:
  2074. ldi XL, 250 ; Add 1000us to minimum
  2075. add I_Temp3, XL
  2076. mov XL, Zero
  2077. adc XL, Zero
  2078. sub I_Temp5, I_Temp3 ; Subtract minimum
  2079. sbc I_Temp6, XL
  2080. in I_Temp1, SREG
  2081. andi I_Temp1, (1<<SREG_C)
  2082. .IF MODE >= 1 ; Tail or multi
  2083. cpi I_Temp2, 3 ; Check if bidirectional operation
  2084. brne rcp_int_ppm_bidir_dir_set ; No - branch
  2085. tst I_Temp1
  2086. breq rcp_int_ppm_bidir_fwd ; If result is positive - branch
  2087. rcp_int_ppm_bidir_rev:
  2088. sbrc Flags2, RCP_DIR_REV
  2089. rjmp rcp_int_ppm_bidir_dir_set ; If same direction - branch
  2090. sbr Flags2, (1<<RCP_DIR_REV)
  2091. rjmp rcp_int_ppm_bidir_dir_set
  2092. rcp_int_ppm_bidir_fwd:
  2093. sbrs Flags2, RCP_DIR_REV
  2094. rjmp rcp_int_ppm_bidir_dir_set ; If same direction - branch
  2095. cbr Flags2, (1<<RCP_DIR_REV)
  2096. rcp_int_ppm_bidir_dir_set:
  2097. .ENDIF
  2098. tst I_Temp1
  2099. breq rcp_int_ppm_neg_checked ; If result is positive - branch
  2100. .IF MODE >= 1 ; Tail or multi
  2101. cpi I_Temp2, 3 ; Check if bidirectional operation
  2102. brne rcp_int_ppm_unidir_neg ; No - branch
  2103. ldi XL, 0xFF ; Change sign - invert and subtract minus one
  2104. com I_Temp5
  2105. com I_Temp6
  2106. sub I_Temp5, XL
  2107. sbc I_Temp6, XL
  2108. rjmp rcp_int_ppm_neg_checked
  2109. rcp_int_ppm_unidir_neg:
  2110. .ENDIF
  2111. ldi I_Temp1, RCP_MIN ; Yes - set to minimum
  2112. ldi I_Temp2, 0
  2113. rjmp rcp_int_pwm_divide_done
  2114. rcp_int_ppm_neg_checked:
  2115. .IF MODE >= 1 ; Tail or multi
  2116. cpi I_Temp2, 3 ; Check if bidirectional operation
  2117. brne rcp_int_ppm_bidir_done ; No - branch
  2118. lsl I_Temp5 ; Multiply value by 2
  2119. rol I_Temp6
  2120. ldi XL, 10 ; Subtract deadband
  2121. sub I_Temp5, XL
  2122. sbc I_Temp6, Zero
  2123. brcc rcp_int_ppm_bidir_done
  2124. ldi XL, RCP_MIN
  2125. mov I_Temp5, XL
  2126. mov I_Temp6, Zero
  2127. rcp_int_ppm_bidir_done:
  2128. .ENDIF
  2129. ldi XL, RCP_MAX ; Check that RC pulse is within legal range (max 255)
  2130. cp I_Temp5, XL
  2131. cpc I_Temp6, Zero
  2132. brcs rcp_int_ppm_max_checked
  2133. ldi I_Temp1, RCP_MAX
  2134. ldi I_Temp2, 0
  2135. rjmp rcp_int_pwm_divide_done
  2136. rcp_int_ppm_max_checked:
  2137. lds XL, Ppm_Throttle_Gain ; Multiply throttle value by gain
  2138. mul I_Temp5, XL
  2139. mov I_Temp1, Mul_Res_H ; Transfer result
  2140. lsl Mul_Res_L ; Multiply result by 2 (unity gain is 128)
  2141. rol I_Temp1
  2142. ldi I_Temp2, 0
  2143. brcs rcp_int_ppm_limit_after_mult
  2144. rjmp rcp_int_limited
  2145. rcp_int_ppm_limit_after_mult:
  2146. ldi I_Temp1, RCP_MAX
  2147. ldi I_Temp2, 0
  2148. rjmp rcp_int_limited
  2149. rcp_int_pwm_divide:
  2150. lsr I_Temp2 ; Divide by 2
  2151. ror I_Temp1
  2152. rcp_int_pwm_divide_done:
  2153. sbrs Flags3, RCP_PWM_FREQ_12KHZ ; Is RC input pwm frequency 12kHz?
  2154. rjmp rcp_int_check_legal_range
  2155. tst I_Temp2 ; Yes - check that value is not more than 255
  2156. breq PC+2
  2157. ldi I_Temp1, RCP_MAX
  2158. mov XL, I_Temp1 ; Multiply by 1.5
  2159. lsr XL
  2160. add I_Temp1, XL
  2161. adc I_Temp2, Zero
  2162. rcp_int_check_legal_range:
  2163. ; Check that RC pulse is within legal range
  2164. cpi I_Temp1, RCP_MAX
  2165. cpc I_Temp2, Zero
  2166. brcs rcp_int_limited
  2167. ldi I_Temp1, RCP_MAX
  2168. rcp_int_limited:
  2169. ; RC pulse value accepted
  2170. sts New_Rcp, I_Temp1 ; Store new pulse length
  2171. sbr Flags2, (1<<RCP_UPDATED) ; Set updated flag
  2172. sbrs Flags0, RCP_MEAS_PWM_FREQ ; Is measure RCP pwm frequency flag set?
  2173. rjmp rcp_int_set_timeout ; No - skip measurements
  2174. ldi XL, ((1<<RCP_PWM_FREQ_1KHZ)+(1<<RCP_PWM_FREQ_2KHZ)+(1<<RCP_PWM_FREQ_4KHZ)+(1<<RCP_PWM_FREQ_8KHZ)+(1<<RCP_PWM_FREQ_12KHZ))
  2175. com XL
  2176. and XL, Flags3 ; Clear all pwm frequency flags
  2177. or XL, I_Temp4 ; Store pwm frequency value in flags
  2178. mov Flags3, XL
  2179. cbr Flags2, (1<<RCP_PPM) ; Default, flag is not set (PWM)
  2180. tst I_Temp4 ; Check if all flags are cleared
  2181. brne rcp_int_set_timeout
  2182. sbr Flags2, (1<<RCP_PPM) ; Flag is set (PPM)
  2183. rcp_int_set_timeout:
  2184. ldi XL, RCP_TIMEOUT ; Set timeout count to start value
  2185. sts Rcp_Timeout_Cntd, XL
  2186. sbrs Flags2, RCP_PPM
  2187. rjmp rcp_int_ppm_timeout_set ; If flag is not set (PWM) - branch
  2188. ldi XL, RCP_TIMEOUT_PPM ; No flag set means PPM. Set timeout count
  2189. sts Rcp_Timeout_Cntd, XL
  2190. rcp_int_ppm_timeout_set:
  2191. sbrc Flags0, RCP_MEAS_PWM_FREQ ; Is measure RCP pwm frequency flag set?
  2192. rjmp rcp_int_exit ; Yes - exit
  2193. sbrc Flags2, RCP_PPM
  2194. rjmp rcp_int_exit ; If flag is set (PPM) - branch
  2195. cbr Flags2, (1<<RCP_INT_NESTED_ENABLED) ; Set flag to disabled
  2196. rcp_int_exit: ; Exit interrupt routine
  2197. sbrc Flags2, RCP_PPM ; If flag is set (PPM) - branch
  2198. rjmp PC+4
  2199. ldi XL, RCP_SKIP_RATE ; Load number of skips
  2200. sts Rcp_Skip_Cntd, XL
  2201. cli ; Disable interrupts
  2202. T0_Int_Enable XL ; Enable timer0 interrupts
  2203. sbrs Flags2, RCP_INT_NESTED_ENABLED; Restore rcp interrupt state
  2204. rjmp rcp_int_ret
  2205. Rcp_Int_Enable XL
  2206. rcp_int_ret:
  2207. out SREG, I_Sreg
  2208. reti
  2209. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2210. ;
  2211. ; Wait xms ~(x*4*250) (Different entry points)
  2212. ;
  2213. ; No assumptions
  2214. ;
  2215. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2216. wait1ms:
  2217. ldi Temp2, 1
  2218. rjmp waitxms_o
  2219. wait3ms:
  2220. ldi Temp2, 3
  2221. rjmp waitxms_o
  2222. wait10ms:
  2223. ldi Temp2, 10
  2224. rjmp waitxms_o
  2225. wait30ms:
  2226. ldi Temp2, 30
  2227. rjmp waitxms_o
  2228. wait100ms:
  2229. ldi Temp2, 100
  2230. rjmp waitxms_o
  2231. wait200ms:
  2232. ldi Temp2, 200
  2233. rjmp waitxms_o
  2234. waitxms_o: ; Outer loop
  2235. ldi Temp1, 21
  2236. waitxms_m: ; Middle loop
  2237. clr XH
  2238. dec XH
  2239. brne PC-1 ; Inner loop
  2240. dec Temp1
  2241. brne waitxms_m
  2242. dec Temp2
  2243. brne waitxms_o
  2244. ret
  2245. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2246. ;
  2247. ; Beeper routines (4 different entry points)
  2248. ;
  2249. ; No assumptions
  2250. ;
  2251. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2252. beep_f1: ; Entry point 1, load beeper frequency 1 settings
  2253. ldi Temp3, 58 ; Off wait loop length
  2254. ldi Temp4, 120 ; Number of beep pulses
  2255. rjmp beep
  2256. beep_f2: ; Entry point 2, load beeper frequency 2 settings
  2257. ldi Temp3, 48
  2258. ldi Temp4, 140
  2259. rjmp beep
  2260. beep_f3: ; Entry point 3, load beeper frequency 3 settings
  2261. ldi Temp3, 42
  2262. ldi Temp4, 180
  2263. rjmp beep
  2264. beep_f4: ; Entry point 4, load beeper frequency 4 settings
  2265. ldi Temp3, 37
  2266. ldi Temp4, 200
  2267. rjmp beep
  2268. beep: ; Beep loop start
  2269. ldi Temp2, 2 ; Must be an even number (or direction will change)
  2270. beep_onoff:
  2271. sbrc Flags3, PGM_DIR_REV ; Toggle between using A fet and C fet
  2272. cbr Flags3, (1<<PGM_DIR_REV)
  2273. sbrs Flags3, PGM_DIR_REV ; Toggle between using A fet and C fet
  2274. sbr Flags3, (1<<PGM_DIR_REV)
  2275. clr XH
  2276. BpFET_off ; BpFET off
  2277. dec XH ; Allow some time after pfet is turned off
  2278. brne PC-1
  2279. BnFET_on ; BnFET on (in order to charge the driver of the BpFET)
  2280. dec XH ; Let the nfet be turned on a while
  2281. brne PC-1
  2282. BnFET_off ; BnFET off again
  2283. dec XH ; Allow some time after nfet is turned off
  2284. brne PC-1
  2285. BpFET_on ; BpFET on
  2286. dec XH ; Allow some time after pfet is turned on
  2287. brne PC-1
  2288. ; Turn on nfet
  2289. AnFET_on ; AnFET on
  2290. lds XH, Beep_Strength
  2291. dec XH
  2292. brne PC-1
  2293. ; Turn off nfet
  2294. AnFET_off ; AnFET off
  2295. ldi XH, 135 ; 25�s off
  2296. dec XH
  2297. brne PC-1
  2298. dec Temp2
  2299. brne beep_onoff
  2300. ; Copy variable
  2301. mov Temp1, Temp3
  2302. beep_off: ; Fets off loop
  2303. mov XH, Temp3
  2304. dec XH
  2305. brne PC-1
  2306. dec Temp1
  2307. brne beep_off
  2308. dec Temp4
  2309. brne beep
  2310. BpFET_off ; BpFET off
  2311. ret
  2312. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2313. ;
  2314. ; Division 8bit unsigned by 8bit unsigned
  2315. ;
  2316. ; Dividend shall be in Temp1, divisor in Temp2
  2317. ; Result will be in Temp1, remainder will be in Temp3
  2318. ;
  2319. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2320. div_u8_by_u8:
  2321. sub Temp3, Temp3 ; Clear remainder and carry
  2322. ldi Temp4, 9 ; Initialize loop counter
  2323. div_u8_by_u8_1:
  2324. rol Temp1 ; Shift left dividend
  2325. dec Temp4 ; Decrement counter
  2326. brne div_u8_by_u8_2 ; If done
  2327. ret ; Return
  2328. div_u8_by_u8_2:
  2329. rol Temp3 ; Shift dividend into remainder
  2330. sub Temp3, Temp2 ; Remainder = remainder - divisor
  2331. brcc div_u8_by_u8_3 ; If result negative
  2332. add Temp3, Temp2 ; Restore remainder
  2333. clc ; Clear carry to be shifted into result
  2334. rjmp div_u8_by_u8_1 ; Else
  2335. div_u8_by_u8_3:
  2336. sec ; Set carry to be shifted into result
  2337. rjmp div_u8_by_u8_1
  2338. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2339. ;
  2340. ; Division 16bit unsigned by 16bit unsigned
  2341. ;
  2342. ; Dividend shall be in Temp2/Temp1, divisor in Temp4/Temp3
  2343. ; Result will be in Temp2/Temp1
  2344. ;
  2345. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2346. div_u16_by_u16:
  2347. mov Temp5, Zero
  2348. mov Temp6, Zero
  2349. ldi XH, 0
  2350. clc
  2351. div_u16_by_u16_div1:
  2352. inc XH ; Increment counter for each left shift
  2353. rol Temp3 ; Shift left the divisor
  2354. rol Temp4
  2355. brcc div_u16_by_u16_div1 ; Repeat until carry flag is set from high-byte
  2356. div_u16_by_u16_div2:
  2357. ror Temp4 ; Shift right the divisor
  2358. ror Temp3
  2359. clc
  2360. mov Temp8, Temp2 ; Make a safe copy of the dividend
  2361. mov Temp7, Temp1
  2362. sbc Temp1, Temp3 ; Dividend - shifted divisor = result bit (no factor, only 0 or 1)
  2363. sbc Temp2, Temp4 ; Subtract high-byte of divisor (all together 16-bit substraction)
  2364. brcc div_u16_by_u16_div3 ; If carry flag is NOT set, result is 1
  2365. mov Temp2, Temp8 ; Otherwise result is 0, save copy of divisor to undo subtraction
  2366. mov Temp1, Temp7
  2367. div_u16_by_u16_div3:
  2368. brcc PC+3 ; Invert carry, so it can be directly copied into result
  2369. clc
  2370. rjmp PC+2
  2371. sec
  2372. rol Temp5 ; Shift carry flag into temporary result
  2373. rol Temp6
  2374. dec XH
  2375. brne div_u16_by_u16_div2 ;Now count backwards and repeat until "B" is zero
  2376. mov Temp2, Temp6 ; Move result to Temp2/Temp1
  2377. mov Temp1, Temp5
  2378. ret
  2379. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2380. ;
  2381. ; Multiplication 16bit signed by 8bit unsigned
  2382. ;
  2383. ; Multiplicand shall be in Temp2/Temp1, multiplicator in Temp3
  2384. ; Result will be in Temp2/Temp1. Result will divided by 16
  2385. ;
  2386. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2387. mult_s16_by_u8_div_16:
  2388. mov Temp4, Zero ; Set default sign in Temp4
  2389. tst Temp2 ; Test sign
  2390. brpl mult_s16_by_u8_positive
  2391. dec Temp4 ; Set sign to 0xFF
  2392. com Temp1 ; Change sign - invert and subtract minus one
  2393. com Temp2
  2394. sub Temp1, Temp4
  2395. sbc Temp2, Temp4
  2396. mult_s16_by_u8_positive:
  2397. cli ; Disable interrupts in order to avoid interference with mul ops in interrupt routines
  2398. mul Temp1, Temp3 ; Multiply LSB with multiplicator
  2399. mov Temp6, Mul_Res_H ; Place MSB in Temp6
  2400. mov Temp1, Mul_Res_L ; Place LSB in Temp1 (result)
  2401. mul Temp2, Temp3 ; Multiply MSB with multiplicator
  2402. mov Temp8, Mul_Res_H ; Place in Temp8/7
  2403. mov Temp7, Mul_Res_L
  2404. sei
  2405. add Temp6, Temp7 ; Add up into Temp3/2
  2406. mov Temp2, Temp6
  2407. adc Temp8, Zero
  2408. mov Temp3, Temp8
  2409. ldi XH, 4
  2410. mov Temp5, XH ; Set number of divisions
  2411. mult_s16_by_u8_div_loop:
  2412. lsr Temp3 ; Rotate right
  2413. ror Temp2
  2414. ror Temp1
  2415. dec Temp5
  2416. brne mult_s16_by_u8_div_loop
  2417. tst Temp4 ; Test sign
  2418. breq mult_s16_by_u8_exit
  2419. com Temp1 ; Change sign - invert and subtract minus one
  2420. com Temp2
  2421. sub Temp1, Temp4
  2422. sbc Temp2, Temp4
  2423. mult_s16_by_u8_exit:
  2424. ret
  2425. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2426. ;
  2427. ; Calculate governor routines
  2428. ;
  2429. ; No assumptions
  2430. ;
  2431. ; Governs headspeed based upon the Comm_Period4x variable and pwm
  2432. ; The governor task is split into several routines in order to distribute processing time
  2433. ;
  2434. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2435. ; First governor routine - calculate governor target
  2436. .IF MODE == 0 ; Main
  2437. calc_governor_target:
  2438. lds Temp1, Pgm_Gov_Mode ; Governor mode?
  2439. cpi Temp1, 4
  2440. brne PC+2
  2441. rjmp calc_governor_target_exit ; No
  2442. governor_speed_check:
  2443. ; Stop governor for stop RC pulse
  2444. lds XH, New_Rcp ; Check RC pulse against stop value
  2445. subi XH, (RCP_MAX/10) ; Is pulse below stop value?
  2446. brcs governor_deactivate ; Yes - deactivate
  2447. mov XH, Flags1
  2448. andi XH, ((1<<STARTUP_PHASE)+(1<<INITIAL_RUN_PHASE))
  2449. brne governor_deactivate ; Deactivate if any startup phase set
  2450. ; Skip speed check if governor is already active
  2451. sbrc Flags0, GOV_ACTIVE ; Is governor active?
  2452. rjmp governor_target_calc
  2453. ; Check speed (do not run governor for low speeds)
  2454. ldi Temp1, 5 ; Default high range activation limit value (~62500 eRPM)
  2455. lds Temp8, Pgm_Gov_Range
  2456. mov XH, Temp8 ; Check if high range (Temp8 has Pgm_Gov_Range)
  2457. dec XH
  2458. breq governor_act_lim_set ; If high range - branch
  2459. ldi Temp1, 10 ; Middle range activation limit value (~31250 eRPM)
  2460. dec XH
  2461. breq governor_act_lim_set ; If middle range - branch
  2462. ldi Temp1, 18 ; Low range activation limit value (~17400 eRPM)
  2463. governor_act_lim_set:
  2464. lds XH, Comm_Period4x_H
  2465. sub XH, Temp1
  2466. brcs governor_activate ; If speed above min limit - run governor
  2467. governor_deactivate:
  2468. sbrs Flags0, GOV_ACTIVE ; Is governor active?
  2469. rjmp governor_first_deactivate_done; This code is executed continuously. Only execute the code below the first time
  2470. lds XH, Pwm_Spoolup_Beg
  2471. sts Pwm_Limit_Spoolup, XH
  2472. ldi XH, 255
  2473. sts Spoolup_Limit_Cnt, XH
  2474. ldi XH, 1
  2475. sts Spoolup_Limit_Skip, XH
  2476. governor_first_deactivate_done:
  2477. lds XH, Requested_Pwm ; Set current pwm to requested
  2478. sts Current_Pwm, XH
  2479. sts Gov_Target_L, Zero ; Set target to zero
  2480. sts Gov_Target_H, Zero
  2481. sts Gov_Integral_L, Zero ; Set integral to zero
  2482. sts Gov_Integral_H, Zero
  2483. sts Gov_Integral_X, Zero
  2484. cbr Flags0, (1<<GOV_ACTIVE)
  2485. rjmp calc_governor_target_exit
  2486. governor_activate:
  2487. sbr Flags0, (1<<GOV_ACTIVE)
  2488. governor_target_calc:
  2489. ; Governor calculations
  2490. lds Temp8, Pgm_Gov_Range
  2491. mov XH, Temp8 ; Check high, middle or low range
  2492. dec XH
  2493. brne calc_governor_target_middle
  2494. lds XH, Governor_Req_Pwm ; Load governor requested pwm
  2495. com XH ; Calculate 255-pwm (invert pwm)
  2496. ; Calculate comm period target (1 + 2*((255-Requested_Pwm)/256) - 0.25)
  2497. rol XH ; Msb to carry
  2498. rol XH ; To bit0
  2499. mov Temp2, XH ; Now 1 lsb is valid for H
  2500. ror XH
  2501. mov Temp1, XH ; Now 7 msbs are valid for L
  2502. mov XH, Temp2
  2503. andi XH, 0x01 ; Calculate H byte
  2504. inc XH ; Add 1
  2505. mov Temp2, XH
  2506. mov XH, Temp1
  2507. andi XH, 0xFE ; Calculate L byte
  2508. rjmp calc_governor_subtract_025
  2509. calc_governor_target_middle:
  2510. mov XH, Temp8 ; Check middle or low range (Temp8 has Pgm_Gov_Range)
  2511. dec XH
  2512. dec XH
  2513. brne calc_governor_target_low
  2514. lds XH, Governor_Req_Pwm ; Load governor requested pwm
  2515. com XH ; Calculate 255-pwm (invert pwm)
  2516. ; Calculate comm period target (1 + 4*((255-Requested_Pwm)/256))
  2517. rol XH ; Msb to carry
  2518. rol XH ; To bit0
  2519. rol XH ; To bit1
  2520. mov Temp2, XH ; Now 2 lsbs are valid for H
  2521. ror XH
  2522. mov Temp1, XH ; Now 6 msbs are valid for L
  2523. mov XH, Temp2
  2524. andi XH, 0x03 ; Calculate H byte
  2525. inc XH ; Add 1
  2526. mov Temp2, XH
  2527. mov XH, Temp1
  2528. andi XH, 0xFC ; Calculate L byte
  2529. rjmp calc_governor_store_target
  2530. calc_governor_target_low:
  2531. lds XH, Governor_Req_Pwm ; Load governor requested pwm
  2532. com XH ; Calculate 255-pwm (invert pwm)
  2533. ; Calculate comm period target (2 + 8*((255-Requested_Pwm)/256) - 0.25)
  2534. rol XH ; Msb to carry
  2535. rol XH ; To bit0
  2536. rol XH ; To bit1
  2537. rol XH ; To bit2
  2538. mov Temp2, XH ; Now 3 lsbs are valid for H
  2539. ror XH
  2540. mov Temp1, XH ; Now 5 msbs are valid for L
  2541. mov XH, Temp2
  2542. andi XH, 0x07 ; Calculate H byte
  2543. inc XH ; Add 1
  2544. inc XH ; Add 1 more
  2545. mov Temp2, XH
  2546. mov XH, Temp1
  2547. andi XH, 0xF8 ; Calculate L byte
  2548. calc_governor_subtract_025:
  2549. subi XH, 0x40 ; Subtract 0.25
  2550. mov Temp1, XH
  2551. sbc Temp2, Zero
  2552. calc_governor_store_target:
  2553. ; Store governor target
  2554. sts Gov_Target_L, Temp1
  2555. sts Gov_Target_H, Temp2
  2556. calc_governor_target_exit:
  2557. ret
  2558. .ENDIF
  2559. .IF MODE == 1 ; Tail
  2560. calc_governor_target:
  2561. ret
  2562. .ENDIF
  2563. .IF MODE == 2 ; Multi
  2564. calc_governor_target:
  2565. lds Temp1, Pgm_Gov_Mode ; Closed loop mode?
  2566. cpi Temp1, 4
  2567. breq calc_governor_target_exit ; No
  2568. governor_target_calc:
  2569. ; Stop governor for stop RC pulse
  2570. lds XH, New_Rcp ; Check RC pulse against stop value
  2571. subi XH, RCP_STOP ; Is pulse below stop value?
  2572. brcs governor_deactivate ; Yes - deactivate
  2573. rjmp governor_activate ; No - activate
  2574. governor_deactivate:
  2575. lds XH, Requested_Pwm ; Set current pwm to requested
  2576. sts Current_Pwm, XH
  2577. sts Gov_Target_L, Zero ; Set target to zero
  2578. sts Gov_Target_H, Zero
  2579. sts Gov_Integral_L, Zero ; Set integral to zero
  2580. sts Gov_Integral_H, Zero
  2581. sts Gov_Integral_X, Zero
  2582. cbr Flags0, (1<<GOV_ACTIVE)
  2583. rjmp calc_governor_target_exit
  2584. governor_activate:
  2585. lds Temp5, Pgm_Gov_Mode ; Store gov mode in Temp5
  2586. sbr Flags0, (1<<GOV_ACTIVE)
  2587. lds XH, Requested_Pwm ; Load requested pwm
  2588. sts Governor_Req_Pwm, XH ; Set governor requested pwm
  2589. ; Calculate comm period target 2*(51000/Requested_Pwm)
  2590. ldi Temp1, 0x38 ; Load 51000
  2591. ldi Temp2, 0xC7
  2592. lds Temp3, Comm_Period4x_L ; Load comm period
  2593. lds Temp4, Comm_Period4x_H
  2594. ; Set speed range
  2595. lsr Temp4
  2596. ror Temp3 ; 200k eRPM range here
  2597. ; Check range
  2598. mov XH, Temp5
  2599. dec XH
  2600. breq governor_activate_range_set ; 200k eRPM? - branch
  2601. governor_activate_100k:
  2602. lsr Temp4
  2603. ror Temp3 ; 100k eRPM range here
  2604. mov XH, Temp5 ; Check range again
  2605. dec XH
  2606. dec XH
  2607. breq governor_activate_range_set ; 100k eRPM? - branch
  2608. governor_activate_50k:
  2609. lsr Temp4
  2610. ror Temp3 ; 50k eRPM range here
  2611. governor_activate_range_set:
  2612. xcall div_u16_by_u16
  2613. ; Store governor target
  2614. sts Gov_Target_L, Temp1
  2615. sts Gov_Target_H, Temp2
  2616. calc_governor_target_exit:
  2617. ret
  2618. .ENDIF
  2619. ; Second governor routine - calculate governor proportional error
  2620. calc_governor_prop_error:
  2621. .IF MODE <= 1 ; Main or tail
  2622. ; Load comm period and divide by 2
  2623. lds Temp2, Comm_Period4x_H
  2624. lsr Temp2
  2625. lds Temp1, Comm_Period4x_L
  2626. ror Temp1
  2627. ; Calculate error
  2628. lds XH, Gov_Target_L
  2629. sub XH, Temp1
  2630. mov Temp1, XH
  2631. lds XH, Gov_Target_H
  2632. sbc XH, Temp2
  2633. mov Temp2, XH
  2634. .ENDIF
  2635. .IF MODE == 2 ; Multi
  2636. ; Calculate error
  2637. lds Temp1, Gov_Target_L
  2638. lds XH, Governor_Req_Pwm
  2639. sub Temp1, XH
  2640. lds Temp2, Gov_Target_H
  2641. sbc Temp2, Zero
  2642. .ENDIF
  2643. ; Check error and limit
  2644. brcc governor_check_prop_limit_pos ; Check carry
  2645. cpi Temp1, 0x80 ; Is error too negative?
  2646. ldi XH, 0xFF
  2647. cpc Temp2, XH
  2648. brcs governor_limit_prop_error_neg ; Yes - limit
  2649. rjmp governor_store_prop_error
  2650. governor_check_prop_limit_pos:
  2651. cpi Temp1, 0x7F ; Is error too positive?
  2652. cpc Temp2, Zero
  2653. brcc governor_limit_prop_error_pos ; Yes - limit
  2654. rjmp governor_store_prop_error
  2655. governor_limit_prop_error_pos:
  2656. ldi Temp1, 0x7F ; Limit to max positive (2's complement)
  2657. ldi Temp2, 0x00
  2658. rjmp governor_store_prop_error
  2659. governor_limit_prop_error_neg:
  2660. ldi Temp1, 0x80 ; Limit to max negative (2's complement)
  2661. ldi Temp2, 0xFF
  2662. governor_store_prop_error:
  2663. ; Store proportional
  2664. sts Gov_Proportional_L, Temp1
  2665. sts Gov_Proportional_H, Temp2
  2666. calc_governor_prop_error_exit:
  2667. ret
  2668. ; Third governor routine - calculate governor integral error
  2669. calc_governor_int_error:
  2670. ; Add proportional to integral
  2671. lds Temp1, Gov_Proportional_L
  2672. lds XH, Gov_Integral_L
  2673. add Temp1, XH
  2674. lds Temp2, Gov_Proportional_H
  2675. lds XH, Gov_Integral_H
  2676. adc Temp2, XH
  2677. ldi Temp3, 0 ; Sign extend high byte
  2678. lds Temp4, Gov_Proportional_H
  2679. tst Temp4
  2680. brpl PC+2
  2681. dec Temp3
  2682. lds XH, Gov_Integral_X
  2683. adc Temp3, XH
  2684. ; Check integral and limit
  2685. brpl governor_check_int_limit_pos ; Check sign bit
  2686. cpi Temp3, 0xF0 ; Is error too negative?
  2687. brcs governor_limit_int_error_neg ; Yes - limit
  2688. rjmp governor_check_pwm
  2689. governor_check_int_limit_pos:
  2690. cpi Temp3, 0x0F ; Is error too positive?
  2691. brcc governor_limit_int_error_pos ; Yes - limit
  2692. rjmp governor_check_pwm
  2693. governor_limit_int_error_pos:
  2694. ldi Temp1, 0xFF ; Limit to max positive (2's complement)
  2695. ldi Temp2, 0xFF
  2696. ldi Temp3, 0x0F
  2697. rjmp governor_check_pwm
  2698. governor_limit_int_error_neg:
  2699. ldi Temp1, 0x00 ; Limit to max negative (2's complement)
  2700. ldi Temp2, 0x00
  2701. ldi Temp3, 0xF0
  2702. governor_check_pwm:
  2703. ; Check current pwm
  2704. lds Temp4, Current_Pwm ; Is current pwm at or above pwm limit?
  2705. lds XH, Pwm_Limit
  2706. cp Temp4, XH
  2707. brcc governor_int_max_pwm ; Yes - branch
  2708. cpi Temp4, 2 ; Is current pwm at minimum?
  2709. brcs governor_int_min_pwm ; Yes - branch
  2710. rjmp governor_store_int_error ; No - store integral error
  2711. governor_int_max_pwm:
  2712. lds XH, Gov_Proportional_H
  2713. tst XH
  2714. brmi calc_governor_int_error_exit ; Is proportional error negative - branch (high byte is always zero)
  2715. rjmp governor_store_int_error ; Positive - store integral error
  2716. governor_int_min_pwm:
  2717. lds XH, Gov_Proportional_H
  2718. tst XH
  2719. brpl calc_governor_int_error_exit ; Is proportional error positive - branch (high byte is always zero)
  2720. governor_store_int_error:
  2721. ; Store integral
  2722. sts Gov_Integral_L, Temp1
  2723. sts Gov_Integral_H, Temp2
  2724. sts Gov_Integral_X, Temp3
  2725. calc_governor_int_error_exit:
  2726. ret
  2727. ; Fourth governor routine - calculate governor proportional correction
  2728. calc_governor_prop_correction:
  2729. ; Load proportional gain
  2730. lds Temp3, Pgm_Gov_P_Gain_Decoded; Load proportional gain and store in Temp3
  2731. ; Load proportional
  2732. lds Temp1, Gov_Proportional_L ; Nominal multiply by 2
  2733. lsl Temp1
  2734. lds Temp2, Gov_Proportional_H
  2735. rol Temp2
  2736. ; Apply gain
  2737. xcall mult_s16_by_u8_div_16
  2738. ; Check error and limit (to low byte)
  2739. tst Temp2
  2740. brpl governor_check_prop_corr_limit_pos ; Check sign bit
  2741. cpi Temp1, 0x80 ; Is error too negative?
  2742. ldi XH, 0xFF
  2743. cpc Temp2, XH
  2744. brcs governor_limit_prop_corr_neg ; Yes - limit
  2745. rjmp governor_apply_prop_corr
  2746. governor_check_prop_corr_limit_pos:
  2747. cpi Temp1, 0x7F ; Is error too positive?
  2748. cpc Temp2, Zero
  2749. brcc governor_limit_prop_corr_pos ; Yes - limit
  2750. rjmp governor_apply_prop_corr
  2751. governor_limit_prop_corr_pos:
  2752. ldi Temp1, 0x7F ; Limit to max positive (2's complement)
  2753. ldi Temp2, 0x00
  2754. rjmp governor_apply_prop_corr
  2755. governor_limit_prop_corr_neg:
  2756. ldi Temp1, 0x80 ; Limit to max negative (2's complement)
  2757. ldi Temp2, 0xFF
  2758. governor_apply_prop_corr:
  2759. ; Test proportional sign
  2760. tst Temp1
  2761. brmi governor_corr_neg_prop ; If proportional negative - go to correct negative
  2762. ; Subtract positive proportional
  2763. lds XH, Governor_Req_Pwm
  2764. sub XH, Temp1
  2765. mov Temp1, XH
  2766. ; Check result
  2767. brcs governor_corr_prop_min_pwm ; Is result negative?
  2768. cpi Temp1, 1 ; Is result below pwm min?
  2769. brcs governor_corr_prop_min_pwm ; Yes
  2770. rjmp governor_store_prop_corr ; No - store proportional correction
  2771. governor_corr_prop_min_pwm:
  2772. ldi Temp1, 1 ; Load minimum pwm
  2773. rjmp governor_store_prop_corr
  2774. governor_corr_neg_prop:
  2775. ; Add negative proportional
  2776. com Temp1
  2777. subi Temp1, 0xFF ; "Add one"
  2778. lds XH, Governor_Req_Pwm
  2779. add Temp1, XH
  2780. ; Check result
  2781. brcs governor_corr_prop_max_pwm ; Is result above max?
  2782. rjmp governor_store_prop_corr ; No - store proportional correction
  2783. governor_corr_prop_max_pwm:
  2784. ldi Temp1, 255 ; Load maximum pwm
  2785. governor_store_prop_corr:
  2786. ; Store proportional pwm
  2787. sts Gov_Prop_Pwm, Temp1
  2788. calc_governor_prop_corr_exit:
  2789. ret
  2790. ; Fifth governor routine - calculate governor integral correction
  2791. calc_governor_int_correction:
  2792. ; Load integral gain
  2793. lds Temp3, Pgm_Gov_I_Gain_Decoded ; Load integral gain and store in Temp3
  2794. ; Load integral
  2795. lds Temp1, Gov_Integral_H
  2796. lds Temp2, Gov_Integral_X
  2797. ; Apply gain
  2798. xcall mult_s16_by_u8_div_16
  2799. ; Check integral and limit
  2800. tst Temp2
  2801. brpl governor_check_int_corr_limit_pos ; Check sign bit
  2802. cpi Temp1, 0x01 ; Is integral too negative?
  2803. ldi XH, 0xFF
  2804. cpc Temp2, XH
  2805. brcs governor_limit_int_corr_neg ; Yes - limit
  2806. rjmp governor_apply_int_corr
  2807. governor_check_int_corr_limit_pos:
  2808. cpi Temp1, 0xFF ; Is integral too positive?
  2809. cpc Temp2, Zero
  2810. brcc governor_limit_int_corr_pos ; Yes - limit
  2811. rjmp governor_apply_int_corr
  2812. governor_limit_int_corr_pos:
  2813. ldi Temp1, 0xFF ; Limit to max positive (2's complement)
  2814. ldi Temp2, 0x00
  2815. rjmp governor_apply_int_corr
  2816. governor_limit_int_corr_neg:
  2817. ldi Temp1, 0x01 ; Limit to max negative (2's complement)
  2818. ldi Temp2, 0xFF
  2819. governor_apply_int_corr:
  2820. ; Test integral sign
  2821. tst Temp2
  2822. brmi governor_corr_neg_int ; If integral negative - go to correct negative
  2823. ; Subtract positive integral
  2824. lds XH, Gov_Prop_Pwm
  2825. sub XH, Temp1
  2826. mov Temp1, XH
  2827. ; Check result
  2828. brcs governor_corr_int_min_pwm ; Is result negative?
  2829. cpi Temp1, 1 ; Is result below pwm min?
  2830. brcs governor_corr_int_min_pwm ; Yes
  2831. rjmp governor_store_int_corr ; No - store correction
  2832. governor_corr_int_min_pwm:
  2833. ldi Temp1, 1 ; Load minimum pwm
  2834. rjmp governor_store_int_corr
  2835. governor_corr_neg_int:
  2836. ; Add negative integral
  2837. com Temp1
  2838. subi Temp1, 0xFF ; "Add one"
  2839. lds XH, Gov_Prop_Pwm
  2840. add Temp1, XH
  2841. ; Check result
  2842. brcs governor_corr_int_max_pwm ; Is result above max?
  2843. rjmp governor_store_int_corr ; No - store correction
  2844. governor_corr_int_max_pwm:
  2845. ldi Temp1, 255 ; Load maximum pwm
  2846. governor_store_int_corr:
  2847. ; Store current pwm
  2848. sts Current_Pwm, Temp1
  2849. calc_governor_int_corr_exit:
  2850. ret
  2851. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2852. ;
  2853. ; Set pwm limit low rpm
  2854. ;
  2855. ; No assumptions
  2856. ;
  2857. ; Sets power limit for low rpms and disables demag for low rpms
  2858. ;
  2859. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2860. set_pwm_limit_low_rpm:
  2861. ; Set pwm limit and demag disable for low rpms
  2862. ldi Temp1, 0xFF ; Default full power
  2863. cbr Flags0, (1<<DEMAG_ENABLED) ; Default disabled
  2864. sbrc Flags1, STARTUP_PHASE ; Exit if startup phase set
  2865. rjmp set_pwm_limit_low_rpm_exit
  2866. sbrc Flags1, INITIAL_RUN_PHASE ; Skip demag portion if initial run phase set
  2867. rjmp set_pwm_demag_done
  2868. sbr Flags0, (1<<DEMAG_ENABLED) ; Enable demag
  2869. lds XH, Comm_Period4x_H
  2870. cpi XH, 0x0A ; ~31250 eRPM
  2871. brcs set_pwm_demag_done ; If speed above - branch
  2872. mov XH, Current_Pwm_Limited
  2873. cpi XH, 0x40 ; Do not disable if pwm above 25%
  2874. brcc set_pwm_demag_done
  2875. cbr Flags0, (1<<DEMAG_ENABLED) ; Disable demag
  2876. set_pwm_demag_done:
  2877. lds XH, Pgm_Enable_Power_Prot ; Check if low RPM power protection is enabled
  2878. tst XH
  2879. breq set_pwm_limit_low_rpm_exit ; Exit if disabled
  2880. lds XH, Comm_Period4x_H
  2881. tst XH
  2882. breq set_pwm_limit_low_rpm_exit ; Avoid divide by zero
  2883. ldi Temp1, 255 ; Divide 255 by Comm_Period4x_H
  2884. lds Temp2, Comm_Period4x_H
  2885. xcall div_u8_by_u8
  2886. cli ; Disable interrupts in order to avoid interference with mul ops in interrupt routines
  2887. lds XH, Low_Rpm_Pwr_Slope ; Multiply by slope
  2888. sbrc Flags1, INITIAL_RUN_PHASE ; More protection for initial run phase
  2889. ldi XH, 5
  2890. mul Temp1, XH
  2891. mov Temp2, Mul_Res_H ; Transfer result
  2892. mov Temp1, Mul_Res_L
  2893. sei
  2894. tst Temp2
  2895. breq PC+2 ; Limit to max
  2896. ldi Temp1, 0xFF
  2897. lds XH, Pwm_Spoolup_Beg
  2898. cp Temp1, XH ; Limit to min
  2899. brcc set_pwm_limit_low_rpm_exit
  2900. lds Temp1, Pwm_Spoolup_Beg
  2901. set_pwm_limit_low_rpm_exit:
  2902. sts Pwm_Limit_By_Rpm, Temp1
  2903. ret
  2904. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2905. ;
  2906. ; Set pwm limit high rpm
  2907. ;
  2908. ; No assumptions
  2909. ;
  2910. ; Sets power limit for high rpms
  2911. ;
  2912. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2913. set_pwm_limit_high_rpm:
  2914. lds Temp1, Comm_Period4x_L
  2915. lds Temp2, Comm_Period4x_H
  2916. subi Temp1, 0x40 ; Limit Comm_Period to 320, which is 250k erpm
  2917. sbci Temp2, 0x01
  2918. lds XH, Pwm_Limit_By_Rpm
  2919. brcc set_pwm_limit_high_rpm_inc_limit
  2920. dec XH
  2921. rjmp set_pwm_limit_high_rpm_store
  2922. set_pwm_limit_high_rpm_inc_limit:
  2923. inc XH
  2924. set_pwm_limit_high_rpm_store:
  2925. breq PC+3
  2926. sts Pwm_Limit_By_Rpm, XH
  2927. ret
  2928. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2929. ;
  2930. ; Measure lipo cells
  2931. ;
  2932. ; No assumptions
  2933. ;
  2934. ; Measure voltage and calculate lipo cells
  2935. ;
  2936. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  2937. measure_lipo_cells:
  2938. .IF MODE >= 1 ; Tail or multi
  2939. ; If not supported, then exit
  2940. rjmp measure_lipo_exit
  2941. .ENDIF
  2942. .IF MODE == 0 ; Main
  2943. ; Set commutation to BpFET on
  2944. xcall comm5comm6
  2945. ; Start adc
  2946. Start_Adc XH
  2947. ; Wait for ADC reference to settle, and then start again
  2948. xcall wait1ms
  2949. Start_Adc XH
  2950. ; Wait for ADC conversion to complete
  2951. measure_lipo_wait_adc:
  2952. Get_Adc_Status XH
  2953. sbrc XH, ADSC
  2954. rjmp measure_lipo_wait_adc
  2955. ; Read ADC result
  2956. Read_Adc_Result Temp1, Temp2
  2957. ; Stop ADC
  2958. Stop_Adc XH
  2959. ; Switch power off
  2960. xcall switch_power_off
  2961. ; Set limit step
  2962. ldi Temp3, ADC_LIMIT_L
  2963. sts Lipo_Adc_Limit_L, Temp3
  2964. tst Temp3
  2965. brne PC+2
  2966. rjmp measure_lipo_exit ; Exit if disabled
  2967. ldi Temp4, ADC_LIMIT_H
  2968. sts Lipo_Adc_Limit_H, Temp4
  2969. mov Temp6, Temp4 ; Divide 3.0V value by 2
  2970. lsr Temp6
  2971. mov Temp5, Temp3
  2972. lsr Temp5
  2973. add Temp5, Temp3 ; Calculate 1.5*3.0V=4.5V value
  2974. adc Temp6, Temp4
  2975. mov Temp3, Temp5 ; Copy step
  2976. mov Temp4, Temp6
  2977. measure_lipo_cell_loop:
  2978. ; Check voltage against xS lower limit
  2979. cp Temp1, Temp3 ; Voltage above limit?
  2980. cpc Temp2, Temp4
  2981. brcs measure_lipo_adjust ; No - branch
  2982. ; Set xS voltage limit
  2983. lds Temp7, Lipo_Adc_Limit_L
  2984. ldi XH, ADC_LIMIT_L
  2985. add XH, Temp7
  2986. sts Lipo_Adc_Limit_L, XH
  2987. lds Temp7, Lipo_Adc_Limit_H
  2988. ldi XH, ADC_LIMIT_H
  2989. adc XH, Temp7
  2990. sts Lipo_Adc_Limit_H, XH
  2991. ; Set (x+1)S lower limit
  2992. add Temp3, Temp5 ; Add step
  2993. adc Temp4, Temp6
  2994. rjmp measure_lipo_cell_loop ; Check for one more battery cell
  2995. measure_lipo_adjust:
  2996. lds Temp7, Lipo_Adc_Limit_L
  2997. lds Temp8, Lipo_Adc_Limit_H
  2998. ; Calculate 3.125%
  2999. lds Temp2, Lipo_Adc_Limit_H
  3000. lsr Temp2
  3001. lds Temp1, Lipo_Adc_Limit_L
  3002. ror Temp1 ; After this 50%
  3003. lsr Temp2
  3004. ror Temp1 ; After this 25%
  3005. ; Divide three times to get to 3.125%
  3006. ldi Temp3, 3
  3007. measure_lipo_divide_loop:
  3008. lsr Temp2
  3009. ror Temp1
  3010. dec Temp3
  3011. brne measure_lipo_divide_loop
  3012. ; Add the programmed number of 0.1V (or 3.125% increments)
  3013. lds Temp3, Pgm_Low_Voltage_Lim ; Load programmed limit
  3014. dec Temp3
  3015. brne measure_lipo_limit_on ; Is low voltage limiting on?
  3016. sts Lipo_Adc_Limit_L, Zero ; No - set limit to zero
  3017. sts Lipo_Adc_Limit_H, Zero
  3018. rjmp measure_lipo_exit
  3019. measure_lipo_limit_on:
  3020. dec Temp3
  3021. breq measure_lipo_update
  3022. measure_lipo_add_loop:
  3023. add Temp7, Temp1 ; Add 3.125%
  3024. adc Temp8, Temp2
  3025. dec Temp3
  3026. brne measure_lipo_add_loop
  3027. measure_lipo_update:
  3028. ; Set ADC limit
  3029. sts Lipo_Adc_Limit_L, Temp7
  3030. sts Lipo_Adc_Limit_H, Temp8
  3031. .ENDIF
  3032. measure_lipo_exit:
  3033. ret
  3034. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3035. ;
  3036. ; Check temperature, power supply voltage and limit power
  3037. ;
  3038. ; No assumptions
  3039. ;
  3040. ; Used to limit main motor power in order to maintain the required voltage
  3041. ;
  3042. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3043. check_temp_voltage_and_limit_power:
  3044. ; Load programmed low voltage limit
  3045. lds Temp3, Pgm_Low_Voltage_Lim ; Store in Temp3
  3046. ; Wait for ADC conversion to complete
  3047. Get_Adc_Status XH
  3048. sbrc XH, ADSC
  3049. rjmp check_temp_voltage_and_limit_power
  3050. ; Read ADC result
  3051. Read_Adc_Result Temp1, Temp2
  3052. ; Stop ADC
  3053. Stop_Adc XH
  3054. lds XH, Adc_Conversion_Cnt ; Increment conversion counter
  3055. inc XH
  3056. sts Adc_Conversion_Cnt, XH
  3057. cpi XH, TEMP_CHECK_RATE ; Is conversion count equal to temp rate?
  3058. brcs check_voltage_start ; No - check voltage
  3059. sts Adc_Conversion_Cnt, Zero ; Yes - temperature check. Reset counter
  3060. lds XH, Pgm_Enable_Temp_Prot ; Is temp protection enabled?
  3061. tst XH
  3062. breq temp_check_exit ; No - branch
  3063. tst Temp2 ; Is temperature ADC reading below 256?
  3064. breq temp_average_inc_dec ; Yes - proceed
  3065. lds XH, Current_Average_Temp_Adc ; No - increment average
  3066. cpi XH, 0xFF
  3067. breq temp_average_updated ; Already max - no change
  3068. rjmp temp_average_inc ; Increment
  3069. temp_average_inc_dec:
  3070. lds XH, Current_Average_Temp_Adc ; Check if current temp ADC is above or below average
  3071. cp Temp1, XH
  3072. breq temp_average_updated ; Equal - no change
  3073. brcc temp_average_inc ; Above - increment average
  3074. tst XH ; Below - decrement average if average is not already zero
  3075. breq temp_average_updated
  3076. temp_average_dec:
  3077. dec XH ; Decrement average
  3078. rjmp temp_average_updated
  3079. temp_average_inc:
  3080. inc XH ; Increment average
  3081. breq temp_average_dec
  3082. temp_average_updated:
  3083. lds Temp1, Pwm_Limit
  3084. sts Current_Average_Temp_Adc, XH
  3085. cpi XH, TEMP_LIMIT ; Is temp ADC above first limit?
  3086. brcc temp_check_set_limit ; Yes - exit
  3087. ldi Temp1, 192 ; No - limit pwm
  3088. cpi XH, (TEMP_LIMIT-TEMP_LIMIT_STEP) ; Is temp ADC above second limit
  3089. brcc temp_check_set_limit ; Yes - exit
  3090. ldi Temp1, 128 ; No - limit pwm
  3091. cpi XH, (TEMP_LIMIT-2*TEMP_LIMIT_STEP) ; Is temp ADC above third limit
  3092. brcc temp_check_set_limit ; Yes - exit
  3093. ldi Temp1, 64 ; No - limit pwm
  3094. cpi XH, (TEMP_LIMIT-3*TEMP_LIMIT_STEP) ; Is temp ADC above final limit
  3095. brcc temp_check_set_limit ; Yes - exit
  3096. ldi Temp1, 0 ; No - limit pwm
  3097. temp_check_set_limit:
  3098. sts Pwm_Limit, Temp1 ; Set pwm limit
  3099. temp_check_exit:
  3100. Set_Adc_Ip_Volt ; Select adc input for next conversion
  3101. ret
  3102. check_voltage_start:
  3103. .IF MODE == 0 ; Main
  3104. ; Check if low voltage limiting is enabled
  3105. cpi Temp3, 1 ; Is low voltage limit disabled?
  3106. breq check_voltage_good ; Yes - voltage declared good
  3107. ; Check if ADC is saturated
  3108. cpi Temp1, 0xFF
  3109. ldi XH, 3
  3110. cpc Temp2, XH
  3111. brcc check_voltage_good ; ADC saturated, can not make judgement
  3112. ldi XH, ADC_LIMIT_L ; Is low voltage limit zero (ESC does not support it)?
  3113. tst XH
  3114. breq check_voltage_good ; Yes - voltage declared good
  3115. ; Check voltage against limit
  3116. lds XH, Lipo_Adc_Limit_L
  3117. cp Temp1, XH
  3118. lds XH, Lipo_Adc_Limit_H
  3119. cpc Temp2, XH
  3120. brcc check_voltage_good ; If voltage above limit - branch
  3121. ; Decrease pwm limit
  3122. lds XH, Pwm_Limit
  3123. tst XH
  3124. breq check_voltage_lim ; If limit zero - branch
  3125. dec XH ; Decrement limit
  3126. sts Pwm_Limit, XH
  3127. rjmp check_voltage_lim
  3128. check_voltage_good:
  3129. ; Increase pwm limit
  3130. lds XH, Pwm_Limit
  3131. cpi XH, 0xFF
  3132. breq check_voltage_lim ; If limit max - branch
  3133. inc XH ; Increment limit
  3134. sts Pwm_Limit, XH
  3135. check_voltage_lim:
  3136. lds Temp1, Pwm_Limit ; Set limit
  3137. lds XH, Current_Pwm
  3138. sub XH, Temp1
  3139. brcc check_voltage_spoolup_lim ; If current pwm above limit - branch and limit
  3140. lds Temp1, Current_Pwm ; Set current pwm (no limiting)
  3141. check_voltage_spoolup_lim:
  3142. ; Slow spoolup
  3143. lds XH, Pwm_Limit_Spoolup
  3144. cp Temp1, XH
  3145. brcs check_voltage_exit ; If current pwm below limit - branch
  3146. lds Temp1, Pwm_Limit_Spoolup
  3147. lds XH, Pwm_Limit_Spoolup ; Check if spoolup limit is max
  3148. cpi XH, 0xFF
  3149. breq check_voltage_exit ; If max - branch
  3150. lds XH, Pwm_Limit_Spoolup ; Set pwm limit to spoolup limit during ramp (to avoid governor integral buildup)
  3151. sts Pwm_Limit, XH
  3152. check_voltage_exit:
  3153. mov Current_Pwm_Limited, Temp1
  3154. sts Current_Pwm_Lim_Dith, Temp1
  3155. .ENDIF
  3156. .IF MODE == 1 ; Tail
  3157. ; Increase pwm limit
  3158. lds XH, Pwm_Limit
  3159. inc XH
  3160. breq check_voltage_lim ; If limit max - branch
  3161. sts Pwm_Limit, XH ; Increment limit
  3162. check_voltage_lim:
  3163. .ENDIF
  3164. .IF MODE == 2 ; Multi
  3165. ; Increase pwm limit
  3166. lds Temp2, Pwm_Limit
  3167. ldi XH, 16
  3168. add Temp2, XH
  3169. brcc PC+2 ; If not max - branch
  3170. ldi Temp2, 255
  3171. sts Pwm_Limit, Temp2 ; Increment limit
  3172. ; Set current pwm limited if closed loop mode
  3173. lds XH, Pgm_Gov_Mode ; Governor mode?
  3174. cpi XH, 4
  3175. breq check_voltage_pwm_done ; No - branch
  3176. lds Temp1, Pwm_Limit ; Set limit
  3177. lds XH, Current_Pwm
  3178. sub XH, Temp1
  3179. brcc check_voltage_low_rpm ; If current pwm above limit - branch and limit
  3180. lds Temp1, Current_Pwm ; Set current pwm (no limiting)
  3181. check_voltage_low_rpm:
  3182. ; Limit pwm for low rpms
  3183. lds XH, Pwm_Limit_By_Rpm ; Check against limit
  3184. cp Temp1, XH
  3185. brcs PC+2 ; If current pwm below limit - branch
  3186. mov Temp1, XH ; Limit pwm
  3187. mov Current_Pwm_Limited, Temp1
  3188. sts Current_Pwm_Lim_Dith, Temp1
  3189. check_voltage_pwm_done:
  3190. .ENDIF
  3191. ; Set adc mux for next conversion
  3192. lds XH, Adc_Conversion_Cnt ; Is next conversion for temperature?
  3193. cpi XH, (TEMP_CHECK_RATE-1)
  3194. brne check_voltage_ret
  3195. Set_Adc_Ip_Temp ; Select temp sensor for next conversion
  3196. check_voltage_ret:
  3197. ret
  3198. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3199. ;
  3200. ; Set startup PWM routine
  3201. ;
  3202. ; No assumptions
  3203. ;
  3204. ; Used for pwm control during startup
  3205. ;
  3206. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3207. set_startup_pwm:
  3208. ; Adjust startup power
  3209. ldi Temp1, PWM_START ; Set power
  3210. cli ; Disable interrupts in order to avoid interference with mul ops in interrupt routines
  3211. lds XH, Pgm_Startup_Pwr_Decoded ; Multiply startup power by programmed value
  3212. mul Temp1, XH
  3213. mov Temp1, Mul_Res_H ; Transfer result
  3214. lsl Mul_Res_L ; Multiply result by 2 (unity gain is 128)
  3215. rol Temp1
  3216. sei
  3217. lds XH, Pwm_Limit ; Check against limit
  3218. cp Temp1, XH
  3219. brcs startup_pwm_set_pwm ; If pwm below limit - branch
  3220. lds Temp1, Pwm_Limit ; Limit pwm
  3221. startup_pwm_set_pwm:
  3222. ; Set pwm variables
  3223. sts Requested_Pwm, Temp1 ; Update requested pwm
  3224. sts Current_Pwm, Temp1 ; Update current pwm
  3225. mov Current_Pwm_Limited, Temp1 ; Update limited version of current pwm
  3226. sts Current_Pwm_Lim_Dith, Temp1
  3227. sts Pwm_Spoolup_Beg, Temp1 ; Update spoolup beginning pwm
  3228. ret
  3229. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3230. ;
  3231. ; Initialize timing routine
  3232. ;
  3233. ; No assumptions
  3234. ;
  3235. ; Part of initialization before motor start
  3236. ;
  3237. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3238. initialize_timing:
  3239. sts Comm_Period4x_L, Zero ; Set commutation period registers
  3240. ldi XH, 0xF0
  3241. sts Comm_Period4x_H, XH
  3242. ret
  3243. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3244. ;
  3245. ; Calculate next commutation timing routine
  3246. ;
  3247. ; No assumptions
  3248. ;
  3249. ; Called immediately after each commutation
  3250. ; Also sets up timer 1 to wait advance timing
  3251. ;
  3252. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3253. calc_next_comm_timing: ; Entry point for run phase
  3254. ; Read commutation time
  3255. cli ; Disable interrupts while reading timer 1
  3256. Read_TCNT1L Temp1
  3257. Read_TCNT1H Temp2
  3258. lds Temp3, Timer2_X
  3259. sei
  3260. ; Calculate this commutation time
  3261. lds Temp4, Prev_Comm_L
  3262. lds Temp5, Prev_Comm_H
  3263. sts Prev_Comm_L, Temp1 ; Store timestamp as previous commutation
  3264. sts Prev_Comm_H, Temp2
  3265. sub Temp1, Temp4 ; Calculate the new commutation time
  3266. sbc Temp2, Temp5
  3267. sbrc Flags1, STARTUP_PHASE
  3268. rjmp calc_next_comm_startup
  3269. sbrc Flags1, HIGH_RPM ; Branch if high rpm
  3270. rjmp calc_next_comm_timing_fast
  3271. rjmp calc_next_comm_normal
  3272. calc_next_comm_startup:
  3273. lds Temp6, Prev_Comm_X
  3274. sts Prev_Comm_X, Temp3 ; Store extended timestamp as previous commutation
  3275. sbc Temp3, Temp6 ; Calculate the new extended commutation time
  3276. tst Temp3
  3277. breq PC+3
  3278. ldi Temp1, 0xFF
  3279. ldi Temp2, 0xFF
  3280. lds Temp7, Prev_Prev_Comm_L
  3281. lds Temp8, Prev_Prev_Comm_H
  3282. sts Prev_Prev_Comm_L, Temp4
  3283. sts Prev_Prev_Comm_H, Temp5
  3284. sub Temp5, Temp8 ; Calculate previous commutation time (hi byte only)
  3285. sub Temp2, Temp5 ; Calculate the difference between the two previous commutation times (hi bytes only)
  3286. sts Comm_Diff, Temp2
  3287. lds Temp1, Prev_Comm_L ; Reload this commutation time
  3288. lds Temp2, Prev_Comm_H
  3289. sub Temp1, Temp7 ; Calculate the new commutation time based upon the two last commutations (to reduce sensitivity to offset)
  3290. sbc Temp2, Temp8
  3291. lds Temp3, Comm_Period4x_L ; Average with previous and save
  3292. lds Temp4, Comm_Period4x_H
  3293. lsr Temp4
  3294. ror Temp3
  3295. add Temp1, Temp3
  3296. adc Temp2, Temp4
  3297. brcc PC+3
  3298. ldi Temp1, 0xFF
  3299. ldi Temp2, 0xFF
  3300. sts Comm_Period4x_L, Temp1
  3301. sts Comm_Period4x_H, Temp2
  3302. rjmp calc_new_wait_times_setup
  3303. calc_next_comm_normal:
  3304. ; Calculate new commutation time
  3305. lds Temp3, Comm_Period4x_L ; Comm_Period4x(-l-h) holds the time of 4 commutations
  3306. lds Temp4, Comm_Period4x_H
  3307. movw Temp5, Temp3 ; Copy variables
  3308. ldi XH, 4 ; Divide Comm_Period4x 4 times as default
  3309. mov Temp7, XH
  3310. ldi XH, 2 ; Divide new commutation time 2 times as default
  3311. mov Temp8, XH
  3312. cpi Temp4, 0x04
  3313. brcs PC+3
  3314. dec Temp7 ; Reduce averaging time constant for low speeds
  3315. dec Temp8
  3316. cpi Temp4, 0x08
  3317. brcs PC+3
  3318. dec Temp7 ; Reduce averaging time constant more for even lower speeds
  3319. dec Temp8
  3320. calc_next_comm_avg_period_div:
  3321. lsr Temp6 ; Divide by 2
  3322. ror Temp5
  3323. dec Temp7
  3324. brne calc_next_comm_avg_period_div
  3325. sub Temp3, Temp5 ; Subtract a fraction
  3326. sbc Temp4, Temp6
  3327. tst Temp8 ; Divide new time
  3328. breq calc_next_comm_new_period_div_done
  3329. calc_next_comm_new_period_div:
  3330. lsr Temp2 ; Divide by 2
  3331. ror Temp1
  3332. dec Temp8
  3333. brne calc_next_comm_new_period_div
  3334. calc_next_comm_new_period_div_done:
  3335. add Temp3, Temp1 ; Add the divided new time
  3336. adc Temp4, Temp2
  3337. sts Comm_Period4x_L, Temp3 ; Store Comm_Period4x_X
  3338. sts Comm_Period4x_H, Temp4
  3339. brcc calc_new_wait_times_setup; If period larger than 0xffff - go to slow case
  3340. ldi Temp4, 0xFF
  3341. sts Comm_Period4x_L, Temp4 ; Set commutation period registers to very slow timing (0xffff)
  3342. sts Comm_Period4x_H, Temp4
  3343. calc_new_wait_times_setup:
  3344. ; Set high rpm bit (if above 156k erpm)
  3345. cpi Temp4, 2
  3346. brcc PC+2
  3347. sbr Flags1, (1<<HIGH_RPM) ; Set high rpm bit
  3348. ; Load programmed commutation timing
  3349. sbrs Flags1, STARTUP_PHASE ; Set dedicated timing during startup
  3350. rjmp calc_new_wait_per_startup_done
  3351. ldi XH, 3
  3352. rjmp calc_new_wait_per_demag_done
  3353. calc_new_wait_per_startup_done:
  3354. lds XH, Pgm_Comm_Timing ; Store in XH
  3355. lds Temp1, Demag_Detected_Metric; Check demag metric
  3356. cpi Temp1, 130
  3357. brcs calc_new_wait_per_demag_done
  3358. inc XH ; Increase timing
  3359. cpi Temp1, 160
  3360. brcs PC+2
  3361. inc XH ; Increase timing again
  3362. cpi XH, 6 ; Limit timing to max
  3363. brcs PC+2
  3364. ldi XH, 5 ; Set timing to max
  3365. calc_new_wait_per_demag_done:
  3366. mov Temp8, XH ; Store timing in Temp8
  3367. ; Set timing reduction
  3368. ldi XH, 4
  3369. mov Temp7, XH
  3370. ; Load current commutation timing
  3371. lds Temp2, Comm_Period4x_H ; Load Comm_Period4x
  3372. lds Temp1, Comm_Period4x_L
  3373. ldi Temp3, 4 ; Divide 4 times
  3374. divide_wait_times:
  3375. lsr Temp2 ; Divide by 2
  3376. ror Temp1
  3377. dec Temp3
  3378. brne divide_wait_times
  3379. sub Temp1, Temp7
  3380. sbc Temp2, Zero
  3381. brcs load_min_time ; Check that result is still positive
  3382. mov XH, Temp1
  3383. subi XH, (COMM_TIME_MIN<<1)
  3384. mov XH, Temp2
  3385. sbc XH, Zero
  3386. brcc calc_new_wait_times_exit ; Check that result is still above minumum
  3387. load_min_time:
  3388. ldi Temp1, (COMM_TIME_MIN<<1)
  3389. ldi Temp2, 0
  3390. calc_new_wait_times_exit:
  3391. movw Temp3, Temp1
  3392. rjmp wait_advance_timing
  3393. ; Fast calculation (Comm_Period4x_H less than 2)
  3394. calc_next_comm_timing_fast:
  3395. ; Calculate new commutation time
  3396. lds Temp3, Comm_Period4x_L ; Comm_Period4x(-l-h) holds the time of 4 commutations
  3397. lds Temp4, Comm_Period4x_H
  3398. movw Temp5, Temp3 ; Copy variables
  3399. lsr Temp6 ; Divide by 2 4 times
  3400. ror Temp5
  3401. lsr Temp5
  3402. lsr Temp5
  3403. lsr Temp5
  3404. sub Temp3, Temp5 ; Subtract a fraction
  3405. sbc Temp4, Zero
  3406. lsr Temp1 ; Divide by 2 2 times
  3407. lsr Temp1
  3408. add Temp3, Temp1 ; Add the divided new time
  3409. adc Temp4, Zero
  3410. sts Comm_Period4x_L, Temp3 ; Store Comm_Period4x_X
  3411. sts Comm_Period4x_H, Temp4
  3412. cpi Temp4, 2 ; If erpm below 156k - go to normal case
  3413. brcs PC+2
  3414. cbr Flags1, (1<<HIGH_RPM) ; Clear high rpm bit
  3415. ldi Temp1, 4 ; Set timing reduction
  3416. lsr Temp4 ; Divide Comm_Period4x by 2 4 times
  3417. ror Temp3
  3418. lsr Temp4
  3419. ror Temp3
  3420. lsr Temp3
  3421. lsr Temp3
  3422. sub Temp3, Temp1
  3423. brcs load_min_time_fast ; Check that result is still positive
  3424. cpi Temp3, (COMM_TIME_MIN<<1)
  3425. brcc calc_new_wait_times_fast_done ; Check that result is still above minumum
  3426. load_min_time_fast:
  3427. ldi Temp3, (COMM_TIME_MIN<<1)
  3428. calc_new_wait_times_fast_done:
  3429. movw Temp1, Temp3
  3430. lds Temp8, Pgm_Comm_Timing ; Store timing in Temp8
  3431. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3432. ;
  3433. ; Wait advance timing routine
  3434. ;
  3435. ; No assumptions
  3436. ; NOTE: Be VERY careful if using temp registers. They are passed over this routine
  3437. ;
  3438. ; Waits for the advance timing to elapse and sets up the next zero cross wait
  3439. ;
  3440. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3441. wait_advance_timing:
  3442. sbrc Flags0, OC1A_PENDING
  3443. rjmp wait_advance_timing
  3444. ; Setup next wait time
  3445. lds Next_Wt_L, Wt_ZC_Timeout_L
  3446. lds Next_Wt_H, Wt_ZC_Timeout_H
  3447. sbr Flags0, (1<<OC1A_PENDING)
  3448. T1oca_Int_Enable XH ; Enable timer1 OCA interrupt
  3449. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3450. ;
  3451. ; Calculate new wait times routine
  3452. ;
  3453. ; No assumptions
  3454. ;
  3455. ; Calculates new wait times
  3456. ;
  3457. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3458. calc_new_wait_times:
  3459. movw Temp5, Temp1 ; Copy values
  3460. sbrc Flags1, HIGH_RPM ; Branch if high rpm
  3461. rjmp calc_new_wait_times_fast
  3462. lsr Temp6 ; Divide by 2
  3463. ror Temp5
  3464. sts Wt_Zc_Timeout_L, Temp1 ; Set 15deg time for zero cross scan timeout
  3465. sts Wt_Zc_Timeout_H, Temp2
  3466. mov XH, Temp8 ; (Temp8 has Pgm_Comm_Timing)
  3467. cpi XH, 3 ; Is timing normal?
  3468. breq store_times_decrease ; Yes - branch
  3469. sbrc XH, 0 ; If an odd number - branch
  3470. rjmp adjust_timing_two_steps
  3471. add Temp1, Temp5 ; Add 7.5deg and store in Temp1/2
  3472. adc Temp2, Temp6
  3473. movw Temp3, Temp5 ; Store 7.5deg in Temp3/4
  3474. rjmp store_times_up_or_down
  3475. adjust_timing_two_steps:
  3476. lsl Temp1 ; Add 15deg and store in Temp1/2
  3477. rol Temp2
  3478. subi Temp1, (COMM_TIME_MIN<<1)
  3479. sbc Temp2, Zero
  3480. ldi Temp3, (COMM_TIME_MIN<<1); Store minimum time in Temp3/4
  3481. ldi Temp4, 0
  3482. store_times_up_or_down:
  3483. mov XH, Temp8 ; Is timing higher than normal?
  3484. cpi XH, 3
  3485. brcs store_times_decrease ; No - branch
  3486. store_times_increase:
  3487. sts Wt_Comm_L, Temp3 ; Now commutation time (~60deg) divided by 4 (~15deg nominal)
  3488. sts Wt_Comm_H, Temp4
  3489. sts Wt_Advance_L, Temp1 ; New commutation advance time (~15deg nominal)
  3490. sts Wt_Advance_H, Temp2
  3491. sts Wt_Zc_Scan_L, Temp5 ; Use this value for zero cross scan delay (7.5deg)
  3492. sts Wt_Zc_Scan_H, Temp6
  3493. rjmp set_comparator_phase
  3494. store_times_decrease:
  3495. sts Wt_Comm_L, Temp1 ; Now commutation time (~60deg) divided by 4 (~15deg nominal)
  3496. sts Wt_Comm_H, Temp2
  3497. sts Wt_Advance_L, Temp3 ; New commutation advance time (~15deg nominal)
  3498. sts Wt_Advance_H, Temp4
  3499. sts Wt_Zc_Scan_L, Temp5 ; Use this value for zero cross scan delay (7.5deg)
  3500. sts Wt_Zc_Scan_H, Temp6
  3501. sbrs Flags1, STARTUP_PHASE
  3502. rjmp store_times_exit
  3503. lds XH, Startup_Cnt
  3504. cpi XH, 3
  3505. brcs store_times_exit
  3506. lds Temp1, Wt_Comm_H ; Compensate commutation wait for comparator offset
  3507. lds Temp2, Comm_Diff
  3508. asr Temp2
  3509. add Temp1, Temp2
  3510. brcs store_times_exit
  3511. brmi store_times_exit
  3512. sts Wt_Comm_L, Zero
  3513. sts Wt_Comm_H, Temp1
  3514. store_times_exit:
  3515. rjmp set_comparator_phase
  3516. calc_new_wait_times_fast:
  3517. lsr Temp5 ; Divide by 2
  3518. sts Wt_Zc_Timeout_L, Temp1 ; Set 15deg time for zero cross scan timeout
  3519. mov XH, Temp8 ; (Temp8 has Pgm_Comm_Timing)
  3520. cpi XH, 3 ; Is timing normal?
  3521. breq store_times_decrease_fast ; Yes - branch
  3522. sbrc XH, 0 ; If an odd number - branch
  3523. rjmp adjust_timing_two_steps_fast
  3524. add Temp1, Temp5 ; Add 7.5deg and store in Temp1
  3525. mov Temp3, Temp5 ; Store 7.5deg in Temp3
  3526. rjmp store_times_up_or_down_fast
  3527. adjust_timing_two_steps_fast:
  3528. lsl Temp1 ; Add 15deg and store in Temp1
  3529. subi Temp1, (COMM_TIME_MIN<<1)
  3530. ldi Temp3, (COMM_TIME_MIN<<1); Store minimum time in Temp3
  3531. store_times_up_or_down_fast:
  3532. mov XH, Temp8 ; Is timing higher than normal?
  3533. cpi XH, 3
  3534. brcs store_times_decrease_fast ; No - branch
  3535. store_times_increase_fast:
  3536. sts Wt_Comm_L, Temp3 ; Now commutation time (~60deg) divided by 4 (~15deg nominal)
  3537. sts Wt_Advance_L, Temp1 ; New commutation advance time (~15deg nominal)
  3538. sts Wt_Zc_Scan_L, Temp5 ; Use this value for zero cross scan delay (7.5deg)
  3539. rjmp set_comparator_phase
  3540. store_times_decrease_fast:
  3541. sts Wt_Comm_L, Temp1 ; Now commutation time (~60deg) divided by 4 (~15deg nominal)
  3542. sts Wt_Advance_L, Temp3 ; New commutation advance time (~15deg nominal)
  3543. sts Wt_Zc_Scan_L, Temp5 ; Use this value for zero cross scan delay (7.5deg)
  3544. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3545. ;
  3546. ; Set comparator phase
  3547. ;
  3548. ; No assumptions
  3549. ;
  3550. ; Sets up comparator muxes
  3551. ;
  3552. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3553. set_comparator_phase:
  3554. lds Temp1, Comm_Phase
  3555. sbrc Temp1, 2
  3556. subi Temp1, 3
  3557. cpi Temp1, 1
  3558. brne set_comp_phase_not1
  3559. sbrc Flags3, PGM_DIR_REV
  3560. rjmp set_comp_phase_to_C
  3561. set_comp_phase_to_A:
  3562. Set_Comp_Phase_A XH
  3563. rjmp set_comp_phase_exit
  3564. set_comp_phase_not1:
  3565. cpi Temp1, 2
  3566. brne set_comp_phase_3
  3567. Set_Comp_Phase_B XH
  3568. rjmp set_comp_phase_exit
  3569. set_comp_phase_3:
  3570. sbrc Flags3, PGM_DIR_REV
  3571. rjmp set_comp_phase_to_A
  3572. set_comp_phase_to_C:
  3573. Set_Comp_Phase_C XH
  3574. set_comp_phase_exit:
  3575. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3576. ;
  3577. ; Wait before zero cross scan routine
  3578. ;
  3579. ; No assumptions
  3580. ;
  3581. ; Waits for the zero cross scan wait time to elapse
  3582. ; Also sets up timer 3 for the zero cross scan timeout time
  3583. ;
  3584. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3585. wait_before_zc_scan:
  3586. ; Calculate random number
  3587. lds Temp1, Random
  3588. lsl Temp1
  3589. brcc wait_before_zc_scan_rand
  3590. ldi XH, 0x6B ; Sequence length of 35, when initialized to 1
  3591. eor Temp1, XH
  3592. wait_before_zc_scan_rand:
  3593. sts Random, Temp1
  3594. wait_before_zc_scan_wait:
  3595. sbrc Flags0, OC1A_PENDING
  3596. rjmp wait_before_zc_scan_wait
  3597. ldi XH, 2
  3598. sts Startup_Zc_Timeout_Cntd, XH
  3599. setup_zc_scan_timeout:
  3600. sbr Flags0, (1<<OC1A_PENDING)
  3601. T1oca_Int_Enable XH ; Enable timer1 OCA interrupt
  3602. mov XH, Flags1
  3603. andi XH, ((1<<STARTUP_PHASE)+(1<<INITIAL_RUN_PHASE))
  3604. breq wait_before_zc_scan_exit
  3605. lds Temp3, Comm_Period4x_L ; Set long timeout when starting
  3606. lds Temp4, Comm_Period4x_H
  3607. lsr Temp4
  3608. ror Temp3
  3609. cli ; Disable interrupts while reading timer 1
  3610. Read_TCNT1L Temp1
  3611. Read_TCNT1H Temp2
  3612. add Temp1, Temp3 ; Set new output compare value
  3613. adc Temp2, Temp4
  3614. Set_OCR1AH Temp2 ; Update high byte first to avoid false output compare
  3615. Set_OCR1AL Temp1
  3616. sbr Flags0, (1<<OC1A_PENDING)
  3617. T1oca_Clear_Int_Flag XH ; Clear t1oca interrupt flag if set
  3618. T1oca_Int_Enable XH ; Enable interrupt
  3619. sei
  3620. wait_before_zc_scan_exit:
  3621. ret
  3622. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3623. ;
  3624. ; Wait for comparator to go low/high routines
  3625. ;
  3626. ; No assumptions
  3627. ;
  3628. ; Waits for the zero cross scan wait time to elapse
  3629. ; Then scans for comparator going low/high
  3630. ;
  3631. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3632. wait_for_comp_out_low:
  3633. sbr Flags0, (1<<DEMAG_DETECTED) ; Set demag detected flag as default
  3634. sts Comparator_Read_Cnt, Zero ; Reset number of comparator reads
  3635. ldi Temp3, 0 ; Desired comparator output
  3636. sbrc Flags0, DIR_CHANGE_BRAKE
  3637. ldi Temp3, (1<<ACO)
  3638. rjmp wait_for_comp_out_start
  3639. wait_for_comp_out_high:
  3640. sbr Flags0, (1<<DEMAG_DETECTED) ; Set demag detected flag as default
  3641. sts Comparator_Read_Cnt, Zero ; Reset number of comparator reads
  3642. ldi Temp3, (1<<ACO) ; Desired comparator output
  3643. sbrc Flags0, DIR_CHANGE_BRAKE
  3644. ldi Temp3, 0
  3645. wait_for_comp_out_start:
  3646. sei ; Enable interrupts
  3647. ; Set number of comparator readings
  3648. ldi Temp1, 1 ; Number of OK readings required
  3649. sbrc Flags1, HIGH_RPM ; Branch if high rpm
  3650. rjmp comp_wait_on_comp_able
  3651. mov XH, Flags1 ; Clear demag detected flag if start phases
  3652. andi XH, ((1<<STARTUP_PHASE)+(1<<INITIAL_RUN_PHASE))
  3653. breq PC+2
  3654. cbr Flags0, (1<<DEMAG_DETECTED)
  3655. lds XH, Comm_Period4x_H ; Set number of readings higher for lower speeds
  3656. subi XH, 0x05
  3657. brcs comp_wait_on_comp_able
  3658. ldi Temp1, 2
  3659. subi XH, 0x05
  3660. brcs comp_wait_no_of_readings
  3661. ldi Temp1, 3
  3662. subi XH, 0x05
  3663. brcs comp_wait_no_of_readings
  3664. ldi Temp1, 6
  3665. comp_wait_no_of_readings:
  3666. sbrc Flags1, STARTUP_PHASE ; Set many samples during startup
  3667. ldi Temp1, 10
  3668. comp_wait_on_comp_able:
  3669. sbrc Flags0, OC1A_PENDING ; Has zero cross scan timeout elapsed?
  3670. rjmp comp_wait_on_comp_able_not_timed_out
  3671. lds XH, Comparator_Read_Cnt ; Check that comparator has been read
  3672. tst XH
  3673. breq comp_wait_on_comp_able_not_timed_out ; If not read - branch
  3674. sbrs Flags1, STARTUP_PHASE ; Extend timeout during startup
  3675. rjmp comp_wait_on_comp_able_timeout_extended
  3676. lds XH, Startup_Cnt ; Do not extend timeout for the first commutations
  3677. cpi XH, 3
  3678. brcs comp_wait_on_comp_able_timeout_extended
  3679. lds XH, Startup_Zc_Timeout_Cntd
  3680. dec XH
  3681. brne comp_wait_on_comp_able_extend_timeout
  3682. comp_wait_on_comp_able_timeout_extended:
  3683. sei ; Enable interrupts
  3684. sbr Flags1, (1<<COMP_TIMED_OUT)
  3685. rjmp setup_comm_wait
  3686. comp_wait_on_comp_able_extend_timeout:
  3687. xcall setup_zc_scan_timeout
  3688. comp_wait_on_comp_able_not_timed_out:
  3689. sei ; Enable interrupts
  3690. nop ; Allocate only just enough time to capture interrupt
  3691. nop
  3692. cli ; Disable interrupts
  3693. sbrc Flags1, HIGH_RPM ; Branch if high rpm
  3694. rjmp comp_wait_read_comp
  3695. lds Temp2, Comm_Period4x_H ; Reduce required distance to pwm transition for higher speeds
  3696. cpi Temp2, 0x07
  3697. brcs PC+2
  3698. ldi Temp2, 0x07
  3699. ldi XH, 5
  3700. add Temp2, XH
  3701. sbrc Flags1, INITIAL_RUN_PHASE
  3702. ldi Temp2, 16
  3703. sbrs Flags2, PGM_PWM_HIGH_FREQ ; Use twice the time when pwm frequency is low
  3704. lsl Temp2
  3705. sbrs Flags0, PWM_ON ; More delay for pwm off
  3706. lsl Temp2
  3707. sbrc Flags1, STARTUP_PHASE ; Set a long delay from pwm on/off events during direct startup
  3708. ldi Temp2, 130
  3709. Read_TCNT2 XH
  3710. lds Temp5, Pwm_Prev_Edge
  3711. sub XH, Temp5
  3712. brcs comp_wait_on_comp_able ; Re-evaluate pwm cycle if timer has wrapped
  3713. sbc XH, Temp2
  3714. brcs comp_wait_on_comp_able ; Re-evaluate pwm cycle
  3715. comp_wait_read_comp:
  3716. Comp_Init XH, Temp2 ; Toggling ACME improves comparator performance on many ESCs
  3717. lds XH, Comparator_Read_Cnt ; Increment comparator read count
  3718. inc XH
  3719. sts Comparator_Read_Cnt, XH
  3720. Read_Comp_Out XH ; Read comparator output
  3721. sbrc Flags1, HIGH_RPM ; Branch if high rpm
  3722. rjmp comp_read_done
  3723. ldi XH, 1
  3724. sbrc Flags1, STARTUP_PHASE
  3725. ldi XH, 2 ; More delay between readings for startup
  3726. dec XH
  3727. brne PC-1
  3728. Read_Comp_Out XH ; Another reading reduces comparator noise on some ESCs (for some reason...)
  3729. comp_read_done:
  3730. andi XH, (1<<ACO)
  3731. cp XH, Temp3
  3732. breq comp_read_wrong
  3733. rjmp comp_read_ok
  3734. comp_read_wrong:
  3735. sbrs Flags1, STARTUP_PHASE
  3736. rjmp comp_read_wrong_not_startup
  3737. inc Temp1 ; Increment number of OK readings required
  3738. cpi Temp1, 10 ; If above initial requirement - go back and restart
  3739. brcs PC+2
  3740. inc Temp1
  3741. rjmp comp_wait_on_comp_able ; If below initial requirement - continue to look for good ones
  3742. comp_read_wrong_not_startup:
  3743. sbrs Flags0, DEMAG_DETECTED
  3744. rjmp wait_for_comp_out_start ; If comparator output is not correct, and timeout already extended - go back and restart
  3745. cbr Flags0, (1<<DEMAG_DETECTED) ; Clear demag detected flag
  3746. Read_TCNT1L Temp4 ; Assuming interrupts are disabled
  3747. Read_TCNT1H Temp5
  3748. lds Temp6, Comm_Period4x_L ; Set timeout to zero comm period 4x value
  3749. lds Temp7, Comm_Period4x_H
  3750. add Temp4, Temp6 ; Set new output compare value
  3751. adc Temp5, Temp7
  3752. Set_OCR1AH Temp5 ; Update high byte first to avoid false output compare
  3753. Set_OCR1AL Temp4
  3754. sbr Flags0, (1<<OC1A_PENDING)
  3755. T1oca_Clear_Int_Flag XH ; Clear interrupt flag in case there are pending interrupts
  3756. T1oca_Int_Enable XH ; Enable timer1 OCA interrupt
  3757. rjmp wait_for_comp_out_start ; If comparator output is not correct - go back and restart
  3758. comp_read_ok:
  3759. lds XH, Startup_Cnt ; Force a timeout for the first commutations
  3760. cpi XH, 2
  3761. brcc PC+2
  3762. rjmp wait_for_comp_out_start
  3763. sbrc Flags0, DEMAG_DETECTED ; Do not accept correct comparator output if it is demag
  3764. rjmp wait_for_comp_out_start
  3765. dec Temp1 ; Decrement readings counter - repeat comparator reading if not zero
  3766. breq PC+2
  3767. rjmp comp_wait_on_comp_able
  3768. cbr Flags1, (1<<COMP_TIMED_OUT)
  3769. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3770. ;
  3771. ; Setup commutation timing routine
  3772. ;
  3773. ; No assumptions
  3774. ;
  3775. ; Sets up and starts wait from commutation to zero cross
  3776. ;
  3777. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3778. setup_comm_wait:
  3779. lds Temp3, Wt_Comm_L ; Set wait commutation value
  3780. lds Temp4, Wt_Comm_H
  3781. cli ; Disable interrupts while reading timer 1
  3782. T1oca_Clear_Int_Flag XH ; Clear t1oca interrupt flag if set
  3783. Read_TCNT1L Temp1
  3784. Read_TCNT1H Temp2
  3785. add Temp1, Temp3 ; Set new output compare value
  3786. adc Temp2, Temp4
  3787. Set_OCR1AH Temp2 ; Update high byte first to avoid false output compare
  3788. Set_OCR1AL Temp1
  3789. ; Setup next wait time
  3790. lds Next_Wt_L, Wt_Advance_L
  3791. lds Next_Wt_H, Wt_Advance_H
  3792. sbr Flags0, (1<<OC1A_PENDING)
  3793. T1oca_Int_Enable XH ; Enable timer1 OCA interrupt
  3794. sei
  3795. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3796. ;
  3797. ; Evaluate comparator integrity
  3798. ;
  3799. ; No assumptions
  3800. ;
  3801. ; Checks comparator signal behaviour versus expected behaviour
  3802. ;
  3803. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3804. evaluate_comparator_integrity:
  3805. mov XH, Flags1 ; Check if startup or intial run
  3806. andi XH, ((1<<STARTUP_PHASE)+(1<<INITIAL_RUN_PHASE))
  3807. breq eval_comp_check_timeout
  3808. lds XH, Startup_Cnt ; Increment counter
  3809. sbrs Flags1, INITIAL_RUN_PHASE ; Do not increment beyond startup phase
  3810. inc XH
  3811. sts Startup_Cnt, XH
  3812. rjmp eval_comp_exit ; Do not exit run mode, even if comparator has timed out
  3813. eval_comp_check_timeout:
  3814. sbrs Flags1, COMP_TIMED_OUT ; Has timeout elapsed?
  3815. rjmp eval_comp_exit
  3816. sbrc Flags0, DIR_CHANGE_BRAKE ; Do not exit run mode if it is braking
  3817. rjmp eval_comp_exit
  3818. sbrc Flags0, DEMAG_DETECTED ; Do not exit run mode if it is a demag situation
  3819. rjmp eval_comp_exit
  3820. pop XH ; Routine exit without "ret" command (dummy pops to increment stack pointer)
  3821. pop XH
  3822. rjmp run_to_wait_for_power_on_fail ; Yes - exit run mode
  3823. eval_comp_exit:
  3824. ret
  3825. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3826. ;
  3827. ; Wait for commutation routine
  3828. ;
  3829. ; No assumptions
  3830. ;
  3831. ; Waits from zero cross to commutation
  3832. ;
  3833. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3834. wait_for_comm:
  3835. ; Update demag metric
  3836. ldi Temp1, 0
  3837. sbrs Flags0, DEMAG_ENABLED
  3838. rjmp PC+4
  3839. sbrs Flags0, DEMAG_DETECTED
  3840. rjmp PC+2
  3841. ldi Temp1, 1
  3842. cli ; Disable interrupts in order to avoid interference with mul ops in interrupt routines
  3843. lds Temp2, Demag_Detected_Metric ; Sliding average of 8, 256 when demag and 0 when not. Limited to minimum 120
  3844. ldi XH, 7 ; Multiply by 7
  3845. mul Temp2, XH ; Multiply
  3846. mov Temp3, Mul_Res_H ; Place MSB in Temp3
  3847. mov Temp2, Mul_Res_L ; Place LSB in Temp2
  3848. sei
  3849. add Temp3, Temp1 ; Add new value for current demag status
  3850. lsr Temp3 ; Divide by 8
  3851. ror Temp2
  3852. lsr Temp3
  3853. ror Temp2
  3854. lsr Temp3
  3855. ror Temp2
  3856. sts Demag_Detected_Metric, Temp2
  3857. cpi Temp2, 120 ; Limit to minimum 120
  3858. brcc PC+4
  3859. ldi XH, 120
  3860. sts Demag_Detected_Metric, XH
  3861. lds Temp1, Demag_Detected_Metric ; Check demag metric
  3862. lds XH, Demag_Pwr_Off_Thresh
  3863. cp Temp1, XH
  3864. brcs wait_for_comm_wait ; Cut power if many consecutive demags. This will help retain sync during hard accelerations
  3865. sbr Flags0, (1<<DEMAG_CUT_POWER) ; Turn off motor power
  3866. All_nFETs_off
  3867. wait_for_comm_wait:
  3868. sbrc Flags0, OC1A_PENDING
  3869. rjmp wait_for_comm_wait
  3870. ; Setup next wait time
  3871. lds Next_Wt_L, Wt_Zc_Scan_L
  3872. lds Next_Wt_H, Wt_Zc_Scan_H
  3873. sbr Flags0, (1<<OC1A_PENDING)
  3874. T1oca_Int_Enable XH ; Enable timer1 OCA interrupt
  3875. ret
  3876. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3877. ;
  3878. ; Commutation routines
  3879. ;
  3880. ; No assumptions
  3881. ;
  3882. ; Performs commutation switching
  3883. ; Damped routines uses all pfets on when in pwm off to dampen the motor
  3884. ;
  3885. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  3886. ; Comm phase 1 to comm phase 2
  3887. comm1comm2:
  3888. Set_RPM_Out
  3889. ldi XH, 2
  3890. sbrc Flags3, PGM_DIR_REV
  3891. rjmp comm12_rev
  3892. cli ; Disable all interrupts
  3893. sts Comm_Phase, XH
  3894. BpFET_off ; Turn off pfet
  3895. ApFET_on ; Turn on pfet
  3896. sei
  3897. rjmp comm_exit
  3898. comm12_rev:
  3899. cli ; Disable all interrupts
  3900. sts Comm_Phase, XH
  3901. BpFET_off ; Turn off pfet
  3902. CpFET_on ; Turn on pfet (reverse)
  3903. sei
  3904. rjmp comm_exit
  3905. ; Comm phase 2 to comm phase 3
  3906. comm2comm3:
  3907. Clear_RPM_Out
  3908. ldi XH, 3
  3909. sbrs Flags2, PGM_PWMOFF_DAMPED
  3910. rjmp comm23_nondamp
  3911. ; Comm2Comm3 Damped
  3912. sbrc Flags3, PGM_DIR_REV
  3913. rjmp comm23_damp_rev
  3914. ldi Temp1, 2 ; Damping on BFET
  3915. cli ; Disable all interrupts
  3916. sts Comm_Phase, XH
  3917. ldi ZL, low(pwm_bfet_damped)
  3918. ldi ZH, high(pwm_bfet_damped)
  3919. sts DampingFET, Temp1
  3920. CnFET_off ; Turn off fets
  3921. CpFET_off
  3922. sbrs Flags0, PWM_ON ; Is pwm on?
  3923. rjmp comm23_nfet_off ; Yes - branch
  3924. BnFET_on ; Pwm on - turn on nfet
  3925. rjmp comm23_fets_done
  3926. comm23_nfet_off:
  3927. BpFET_on ; Pwm off - switch damping fets
  3928. comm23_fets_done:
  3929. sei
  3930. rjmp comm_exit
  3931. ; Comm2Comm3 Damped reverse
  3932. comm23_damp_rev:
  3933. ldi Temp1, 2 ; Damping on BFET
  3934. cli ; Disable all interrupts
  3935. sts Comm_Phase, XH
  3936. ldi ZL, low(pwm_bfet_damped) ; (reverse)
  3937. ldi ZH, high(pwm_bfet_damped)
  3938. sts DampingFET, Temp1
  3939. AnFET_off ; Turn off fets (reverse)
  3940. ApFET_off
  3941. sbrs Flags0, PWM_ON ; Is pwm on?
  3942. rjmp comm23_nfet_off_rev ; Yes - branch
  3943. BnFET_on ; Pwm on - turn on nfet
  3944. rjmp comm23_fets_done_rev
  3945. comm23_nfet_off_rev:
  3946. BpFET_on ; Pwm off - switch damping fets
  3947. comm23_fets_done_rev:
  3948. sei
  3949. rjmp comm_exit
  3950. ; Comm2Comm3 Non-damped
  3951. comm23_nondamp:
  3952. sbrc Flags3, PGM_DIR_REV
  3953. rjmp comm23_nondamp_rev
  3954. cli ; Disable all interrupts
  3955. sts Comm_Phase, XH
  3956. ldi ZL, low(pwm_bfet)
  3957. ldi ZH, high(pwm_bfet)
  3958. CnFET_off ; Turn off nfet
  3959. sbrs Flags0, PWM_ON ; Is pwm on?
  3960. rjmp comm23_nfet_done
  3961. comm23_nfet:
  3962. BnFET_on ; Yes - Turn on nfet
  3963. comm23_nfet_done:
  3964. sei
  3965. rjmp comm_exit
  3966. ; Comm2Comm3 Non-damped reverse
  3967. comm23_nondamp_rev:
  3968. cli ; Disable all interrupts
  3969. sts Comm_Phase, XH
  3970. ldi ZL, low(pwm_bfet)
  3971. ldi ZH, high(pwm_bfet)
  3972. AnFET_off ; Turn off nfet (reverse)
  3973. sbrs Flags0, PWM_ON ; Is pwm on?
  3974. rjmp comm23_nfet_done_rev
  3975. BnFET_on ; Yes - Turn on nfet
  3976. comm23_nfet_done_rev:
  3977. sei
  3978. rjmp comm_exit
  3979. ; Comm phase 3 to comm phase 4
  3980. comm3comm4:
  3981. Set_RPM_Out
  3982. ldi XH, 4
  3983. sbrc Flags3, PGM_DIR_REV
  3984. rjmp comm34_rev
  3985. cli ; Disable all interrupts
  3986. sts Comm_Phase, XH
  3987. ApFET_off ; Turn off pfet
  3988. CpFET_on ; Turn on pfet
  3989. sei
  3990. rjmp comm_exit
  3991. comm34_rev:
  3992. cli ; Disable all interrupts
  3993. sts Comm_Phase, XH
  3994. CpFET_off ; Turn off pfet (reverse)
  3995. ApFET_on ; Turn on pfet (reverse)
  3996. sei
  3997. rjmp comm_exit
  3998. ; Comm phase 4 to comm phase 5
  3999. comm4comm5:
  4000. Clear_RPM_Out
  4001. ldi XH, 5
  4002. sbrs Flags2, PGM_PWMOFF_DAMPED
  4003. rjmp comm45_nondamp
  4004. ; Comm4Comm5 Damped
  4005. sbrc Flags3, PGM_DIR_REV
  4006. rjmp comm45_damp_rev
  4007. ldi Temp1, 1 ; Damping on AFET
  4008. cli ; Disable all interrupts
  4009. sts Comm_Phase, XH
  4010. ldi ZL, low(pwm_afet_damped)
  4011. ldi ZH, high(pwm_afet_damped)
  4012. sts DampingFET, Temp1
  4013. BnFET_off ; Turn off fets
  4014. BpFET_off
  4015. sbrs Flags0, PWM_ON ; Is pwm on?
  4016. rjmp comm45_nfet_off ; Yes - branch
  4017. AnFET_on ; Pwm on - turn on nfet
  4018. rjmp comm45_fets_done
  4019. comm45_nfet_off:
  4020. ApFET_on ; Pwm off - switch damping fets
  4021. comm45_fets_done:
  4022. sei
  4023. rjmp comm_exit
  4024. ; Comm4Comm5 Damped reverse
  4025. comm45_damp_rev:
  4026. ldi Temp1, 4 ; Damping on CFET
  4027. cli ; Disable all interrupts
  4028. sts Comm_Phase, XH
  4029. ldi ZL, low(pwm_cfet_damped) ; (reverse)
  4030. ldi ZH, high(pwm_cfet_damped)
  4031. sts DampingFET, Temp1
  4032. BnFET_off ; Turn off fets
  4033. BpFET_off
  4034. sbrs Flags0, PWM_ON ; Is pwm on?
  4035. rjmp comm45_nfet_off_rev ; Yes - branch
  4036. CnFET_on ; Pwm on - turn on nfet (reverse)
  4037. rjmp comm45_fets_done_rev
  4038. comm45_nfet_off_rev:
  4039. CpFET_on ; Pwm off - switch damping fets (reverse)
  4040. comm45_fets_done_rev:
  4041. sei
  4042. rjmp comm_exit
  4043. ; Comm4Comm5 Non-damped
  4044. comm45_nondamp:
  4045. sbrc Flags3, PGM_DIR_REV
  4046. rjmp comm45_nondamp_rev
  4047. cli ; Disable all interrupts
  4048. sts Comm_Phase, XH
  4049. ldi ZL, low(pwm_afet)
  4050. ldi ZH, high(pwm_afet)
  4051. BnFET_off ; Turn off nfet
  4052. sbrs Flags0, PWM_ON ; Is pwm on?
  4053. rjmp comm45_nfet_done
  4054. AnFET_on ; Yes - Turn on nfet
  4055. comm45_nfet_done:
  4056. sei
  4057. rjmp comm_exit
  4058. ; Comm4Comm5 Non-damped reverse
  4059. comm45_nondamp_rev:
  4060. cli ; Disable all interrupts
  4061. sts Comm_Phase, XH
  4062. ldi ZL, low(pwm_cfet) ; (reverse)
  4063. ldi ZH, high(pwm_cfet)
  4064. BnFET_off ; Turn off nfet
  4065. sbrs Flags0, PWM_ON ; Is pwm on?
  4066. rjmp comm45_nfet_done_rev
  4067. CnFET_on ; Yes - Turn on nfet (reverse)
  4068. comm45_nfet_done_rev:
  4069. sei
  4070. rjmp comm_exit
  4071. ; Comm phase 5 to comm phase 6
  4072. comm5comm6:
  4073. Set_RPM_Out
  4074. ldi XH, 6
  4075. sbrc Flags3, PGM_DIR_REV
  4076. rjmp comm56_rev
  4077. cli ; Disable all interrupts
  4078. sts Comm_Phase, XH
  4079. CpFET_off ; Turn off pfet
  4080. BpFET_on ; Turn on pfet
  4081. sei
  4082. rjmp comm_exit
  4083. comm56_rev:
  4084. cli ; Disable all interrupts
  4085. sts Comm_Phase, XH
  4086. ApFET_off ; Turn off pfet (reverse)
  4087. BpFET_on ; Turn on pfet
  4088. sei
  4089. rjmp comm_exit
  4090. ; Comm phase 6 to comm phase 1
  4091. comm6comm1:
  4092. Clear_RPM_Out
  4093. ldi XH, 1
  4094. sbrs Flags2, PGM_PWMOFF_DAMPED
  4095. rjmp comm61_nondamp
  4096. ; Comm6Comm1 Damped
  4097. sbrc Flags3, PGM_DIR_REV
  4098. rjmp comm61_damp_rev
  4099. ldi Temp1, 4 ; Damping on CFET
  4100. cli ; Disable all interrupts
  4101. sts Comm_Phase, XH
  4102. ldi ZL, low(pwm_cfet_damped)
  4103. ldi ZH, high(pwm_cfet_damped)
  4104. sts DampingFET, Temp1
  4105. AnFET_off ; Turn off fets
  4106. ApFET_off
  4107. sbrs Flags0, PWM_ON ; Is pwm on?
  4108. rjmp comm61_nfet_off ; Yes - branch
  4109. CnFET_on ; Pwm on - turn on nfet
  4110. rjmp comm61_fets_done
  4111. comm61_nfet_off:
  4112. CpFET_on ; Pwm off - switch damping fets
  4113. comm61_fets_done:
  4114. sei
  4115. rjmp comm_exit
  4116. ; Comm6Comm1 Damped reverse
  4117. comm61_damp_rev:
  4118. ldi Temp1, 1 ; Damping on AFET
  4119. cli ; Disable all interrupts
  4120. sts Comm_Phase, XH
  4121. ldi ZL, low(pwm_afet_damped) ; (reverse)
  4122. ldi ZH, high(pwm_afet_damped)
  4123. sts DampingFET, Temp1
  4124. CnFET_off ; Turn off fets (reverse)
  4125. CpFET_off
  4126. sbrs Flags0, PWM_ON ; Is pwm on?
  4127. rjmp comm61_nfet_off_rev ; Yes - branch
  4128. AnFET_on ; Pwm on - turn on nfet (reverse)
  4129. rjmp comm61_fets_done_rev
  4130. comm61_nfet_off_rev:
  4131. ApFET_on ; Pwm off - switch damping fets (reverse)
  4132. comm61_fets_done_rev:
  4133. sei
  4134. rjmp comm_exit
  4135. ; Comm6Comm1 Non-damped
  4136. comm61_nondamp:
  4137. sbrc Flags3, PGM_DIR_REV
  4138. rjmp comm61_nondamp_rev
  4139. cli ; Disable all interrupts
  4140. sts Comm_Phase, XH
  4141. ldi ZL, low(pwm_cfet)
  4142. ldi ZH, high(pwm_cfet)
  4143. AnFET_off ; Turn off nfet
  4144. sbrs Flags0, PWM_ON ; Is pwm on?
  4145. rjmp comm61_nfet_done
  4146. CnFET_on ; Yes - Turn on nfet
  4147. comm61_nfet_done:
  4148. sei
  4149. rjmp comm_exit
  4150. ; Comm6Comm1 Non-damped reverse
  4151. comm61_nondamp_rev:
  4152. cli ; Disable all interrupts
  4153. sts Comm_Phase, XH
  4154. ldi ZL, low(pwm_afet) ; (reverse)
  4155. ldi ZH, high(pwm_afet)
  4156. CnFET_off ; Turn off nfet (reverse)
  4157. sbrs Flags0, PWM_ON ; Is pwm on?
  4158. rjmp comm61_nfet_done_rev
  4159. AnFET_on ; Yes - Turn on nfet (reverse)
  4160. comm61_nfet_done_rev:
  4161. sei
  4162. comm_exit:
  4163. cbr Flags0, (1<<DEMAG_CUT_POWER) ; Clear demag power cut flag
  4164. ret
  4165. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4166. ;
  4167. ; Switch power off routine
  4168. ;
  4169. ; No assumptions
  4170. ;
  4171. ; Switches all fets off
  4172. ;
  4173. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4174. switch_power_off:
  4175. ldi Temp3, low(pwm_nofet) ; Set Z register to pwm_nofet
  4176. ldi Temp4, high(pwm_nofet)
  4177. movw ZL, Temp3 ; Set Z register in one instruction
  4178. sts DampingFET, Zero
  4179. All_nFETs_Off ; Turn off all nfets
  4180. All_pFETs_Off ; Turn off all pfets
  4181. cbr Flags0, (1<<PWM_ON) ; Set pwm cycle to pwm off
  4182. ret
  4183. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4184. ;
  4185. ; Set default parameters
  4186. ;
  4187. ; Assumes interrupt is disabled
  4188. ;
  4189. ; Sets default programming parameters
  4190. ;
  4191. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4192. set_default_parameters:
  4193. .IF MODE == 0 ; Main
  4194. ldi XL, low(Pgm_Gov_P_Gain)
  4195. ldi XH, high(Pgm_Gov_P_Gain)
  4196. ldi Temp1, DEFAULT_PGM_MAIN_P_GAIN
  4197. st X+, Temp1
  4198. ldi Temp1, DEFAULT_PGM_MAIN_I_GAIN
  4199. st X+, Temp1
  4200. ldi Temp1, DEFAULT_PGM_MAIN_GOVERNOR_MODE
  4201. st X+, Temp1
  4202. ldi Temp1, DEFAULT_PGM_MAIN_LOW_VOLTAGE_LIM
  4203. st X+, Temp1
  4204. ldi Temp1, 0xFF
  4205. st X+, Temp1
  4206. st X+, Temp1
  4207. ldi Temp1, DEFAULT_PGM_MAIN_STARTUP_PWR
  4208. st X+, Temp1
  4209. ldi Temp1, DEFAULT_PGM_MAIN_PWM_FREQ
  4210. st X+, Temp1
  4211. ldi Temp1, DEFAULT_PGM_MAIN_DIRECTION
  4212. st X+, Temp1
  4213. ldi Temp1, DEFAULT_PGM_MAIN_RCP_PWM_POL
  4214. st X+, Temp1
  4215. ldi XL, low(Pgm_Enable_TX_Program)
  4216. ldi XH, high(Pgm_Enable_TX_Program)
  4217. ldi Temp1, DEFAULT_PGM_ENABLE_TX_PROGRAM
  4218. st X+, Temp1
  4219. ldi Temp1, DEFAULT_PGM_MAIN_REARM_START
  4220. st X+, Temp1
  4221. ldi Temp1, DEFAULT_PGM_MAIN_GOV_SETUP_TARGET
  4222. st X+, Temp1
  4223. ldi Temp1, 0xFF
  4224. st X+, Temp1
  4225. st X+, Temp1
  4226. st X+, Temp1
  4227. ldi Temp1, DEFAULT_PGM_MAIN_COMM_TIMING
  4228. st X+, Temp1
  4229. ldi Temp1, 0xFF
  4230. st X+, Temp1
  4231. ldi Temp1, DEFAULT_PGM_MAIN_GOVERNOR_RANGE
  4232. st X+, Temp1
  4233. ldi Temp1, 0xFF
  4234. st X+, Temp1
  4235. ldi Temp1, DEFAULT_PGM_PPM_MIN_THROTTLE
  4236. st X+, Temp1
  4237. ldi Temp1, DEFAULT_PGM_PPM_MAX_THROTTLE
  4238. st X+, Temp1
  4239. ldi Temp1, DEFAULT_PGM_MAIN_BEEP_STRENGTH
  4240. st X+, Temp1
  4241. ldi Temp1, DEFAULT_PGM_MAIN_BEACON_STRENGTH
  4242. st X+, Temp1
  4243. ldi Temp1, DEFAULT_PGM_MAIN_BEACON_DELAY
  4244. st X+, Temp1
  4245. ldi Temp1, 0xFF
  4246. st X+, Temp1
  4247. ldi Temp1, DEFAULT_PGM_MAIN_DEMAG_COMP
  4248. st X+, Temp1
  4249. ldi Temp1, DEFAULT_PGM_BEC_VOLTAGE_HIGH
  4250. st X+, Temp1
  4251. ldi Temp1, DEFAULT_PGM_PPM_CENTER_THROTTLE
  4252. st X+, Temp1
  4253. ldi Temp1, DEFAULT_PGM_MAIN_SPOOLUP_TIME
  4254. st X+, Temp1
  4255. ldi Temp1, DEFAULT_PGM_ENABLE_TEMP_PROT
  4256. st X+, Temp1
  4257. ldi Temp1, DEFAULT_PGM_ENABLE_POWER_PROT
  4258. st X+, Temp1
  4259. ldi Temp1, DEFAULT_PGM_ENABLE_PWM_INPUT
  4260. st X+, Temp1
  4261. ldi Temp1, 0xFF
  4262. st X+, Temp1
  4263. .ENDIF
  4264. .IF MODE == 1 ; Tail
  4265. ldi XL, low(Pgm_Gov_P_Gain)
  4266. ldi XH, high(Pgm_Gov_P_Gain)
  4267. ldi Temp1, 0xFF
  4268. st X+, Temp1
  4269. st X+, Temp1
  4270. st X+, Temp1
  4271. st X+, Temp1
  4272. ldi Temp1, DEFAULT_PGM_TAIL_GAIN
  4273. st X+, Temp1
  4274. ldi Temp1, DEFAULT_PGM_TAIL_IDLE_SPEED
  4275. st X+, Temp1
  4276. ldi Temp1, DEFAULT_PGM_TAIL_STARTUP_PWR
  4277. st X+, Temp1
  4278. ldi Temp1, DEFAULT_PGM_TAIL_PWM_FREQ
  4279. st X+, Temp1
  4280. ldi Temp1, DEFAULT_PGM_TAIL_DIRECTION
  4281. st X+, Temp1
  4282. ldi Temp1, DEFAULT_PGM_TAIL_RCP_PWM_POL
  4283. st X+, Temp1
  4284. ldi XL, low(Pgm_Enable_TX_Program)
  4285. ldi XH, high(Pgm_Enable_TX_Program)
  4286. ldi Temp1, DEFAULT_PGM_ENABLE_TX_PROGRAM
  4287. st X+, Temp1
  4288. ldi Temp1, 0xFF
  4289. st X+, Temp1
  4290. st X+, Temp1
  4291. st X+, Temp1
  4292. st X+, Temp1
  4293. st X+, Temp1
  4294. ldi Temp1, DEFAULT_PGM_TAIL_COMM_TIMING
  4295. st X+, Temp1
  4296. ldi Temp1, 0xFF
  4297. st X+, Temp1
  4298. ldi Temp1, 0xFF
  4299. st X+, Temp1
  4300. st X+, Temp1
  4301. ldi Temp1, DEFAULT_PGM_PPM_MIN_THROTTLE
  4302. st X+, Temp1
  4303. ldi Temp1, DEFAULT_PGM_PPM_MAX_THROTTLE
  4304. st X+, Temp1
  4305. ldi Temp1, DEFAULT_PGM_TAIL_BEEP_STRENGTH
  4306. st X+, Temp1
  4307. ldi Temp1, DEFAULT_PGM_TAIL_BEACON_STRENGTH
  4308. st X+, Temp1
  4309. ldi Temp1, DEFAULT_PGM_TAIL_BEACON_DELAY
  4310. st X+, Temp1
  4311. ldi Temp1, 0xFF
  4312. st X+, Temp1
  4313. ldi Temp1, DEFAULT_PGM_TAIL_DEMAG_COMP
  4314. st X+, Temp1
  4315. ldi Temp1, DEFAULT_PGM_BEC_VOLTAGE_HIGH
  4316. st X+, Temp1
  4317. ldi Temp1, DEFAULT_PGM_PPM_CENTER_THROTTLE
  4318. st X+, Temp1
  4319. ldi Temp1, 0xFF
  4320. st X+, Temp1
  4321. ldi Temp1, DEFAULT_PGM_ENABLE_TEMP_PROT
  4322. st X+, Temp1
  4323. ldi Temp1, DEFAULT_PGM_ENABLE_POWER_PROT
  4324. st X+, Temp1
  4325. ldi Temp1, DEFAULT_PGM_ENABLE_PWM_INPUT
  4326. st X+, Temp1
  4327. ldi Temp1, DEFAULT_PGM_TAIL_PWM_DITHER
  4328. st X+, Temp1
  4329. .ENDIF
  4330. .IF MODE == 2 ; Multi
  4331. ldi XL, low(Pgm_Gov_P_Gain)
  4332. ldi XH, high(Pgm_Gov_P_Gain)
  4333. ldi Temp1, DEFAULT_PGM_MULTI_P_GAIN
  4334. st X+, Temp1
  4335. ldi Temp1, DEFAULT_PGM_MULTI_I_GAIN
  4336. st X+, Temp1
  4337. ldi Temp1, DEFAULT_PGM_MULTI_GOVERNOR_MODE
  4338. st X+, Temp1
  4339. ldi Temp1, 0xFF
  4340. st X+, Temp1
  4341. ldi Temp1, DEFAULT_PGM_MULTI_GAIN
  4342. st X+, Temp1
  4343. ldi Temp1, 0xFF
  4344. st X+, Temp1
  4345. ldi Temp1, DEFAULT_PGM_MULTI_STARTUP_PWR
  4346. st X+, Temp1
  4347. ldi Temp1, DEFAULT_PGM_MULTI_PWM_FREQ
  4348. st X+, Temp1
  4349. ldi Temp1, DEFAULT_PGM_MULTI_DIRECTION
  4350. st X+, Temp1
  4351. ldi Temp1, DEFAULT_PGM_MULTI_RCP_PWM_POL
  4352. st X+, Temp1
  4353. ldi XL, low(Pgm_Enable_TX_Program)
  4354. ldi XH, high(Pgm_Enable_TX_Program)
  4355. ldi Temp1, DEFAULT_PGM_ENABLE_TX_PROGRAM
  4356. st X+, Temp1
  4357. ldi Temp1, 0xFF
  4358. st X+, Temp1
  4359. st X+, Temp1
  4360. st X+, Temp1
  4361. st X+, Temp1
  4362. st X+, Temp1
  4363. ldi Temp1, DEFAULT_PGM_MULTI_COMM_TIMING
  4364. st X+, Temp1
  4365. ldi Temp1, 0xFF
  4366. st X+, Temp1
  4367. ldi Temp1, 0xFF
  4368. st X+, Temp1
  4369. st X+, Temp1
  4370. ldi Temp1, DEFAULT_PGM_PPM_MIN_THROTTLE
  4371. st X+, Temp1
  4372. ldi Temp1, DEFAULT_PGM_PPM_MAX_THROTTLE
  4373. st X+, Temp1
  4374. ldi Temp1, DEFAULT_PGM_MULTI_BEEP_STRENGTH
  4375. st X+, Temp1
  4376. ldi Temp1, DEFAULT_PGM_MULTI_BEACON_STRENGTH
  4377. st X+, Temp1
  4378. ldi Temp1, DEFAULT_PGM_MULTI_BEACON_DELAY
  4379. st X+, Temp1
  4380. ldi Temp1, 0xFF
  4381. st X+, Temp1
  4382. ldi Temp1, DEFAULT_PGM_MULTI_DEMAG_COMP
  4383. st X+, Temp1
  4384. ldi Temp1, DEFAULT_PGM_BEC_VOLTAGE_HIGH
  4385. st X+, Temp1
  4386. ldi Temp1, DEFAULT_PGM_PPM_CENTER_THROTTLE
  4387. st X+, Temp1
  4388. ldi Temp1, 0xFF
  4389. st X+, Temp1
  4390. ldi Temp1, DEFAULT_PGM_ENABLE_TEMP_PROT
  4391. st X+, Temp1
  4392. ldi Temp1, DEFAULT_PGM_ENABLE_POWER_PROT
  4393. st X+, Temp1
  4394. ldi Temp1, DEFAULT_PGM_ENABLE_PWM_INPUT
  4395. st X+, Temp1
  4396. ldi Temp1, DEFAULT_PGM_MULTI_PWM_DITHER
  4397. st X+, Temp1
  4398. .ENDIF
  4399. ret
  4400. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4401. ;
  4402. ; Decode parameters
  4403. ;
  4404. ; No assumptions
  4405. ;
  4406. ; Decodes programming parameters
  4407. ;
  4408. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4409. decode_parameters:
  4410. ; Load programmed pwm frequency
  4411. lds Temp3, Pgm_Pwm_Freq ; Load pwm freq and store in Temp3
  4412. cbr Flags2, (1<<PGM_PWMOFF_DAMPED)
  4413. .IF DAMPED_MODE_ENABLE == 1
  4414. cpi Temp3, 3
  4415. brne PC+2
  4416. sbr Flags2, (1<<PGM_PWMOFF_DAMPED)
  4417. .ENDIF
  4418. ; Load programmed direction
  4419. lds XH, Pgm_Direction
  4420. .IF MODE >= 1 ; Tail or multi
  4421. cpi XH, 3
  4422. breq decode_params_dir_set
  4423. .ENDIF
  4424. cbr Flags3, (1<<PGM_DIR_REV)
  4425. sbrc XH, 1
  4426. sbr Flags3, (1<<PGM_DIR_REV)
  4427. decode_params_dir_set:
  4428. cbr Flags3, (1<<PGM_RCP_PWM_POL)
  4429. lds XH, Pgm_Input_Pol
  4430. sbrc XH, 1
  4431. sbr Flags3, (1<<PGM_RCP_PWM_POL)
  4432. cpi Temp3, 2
  4433. breq decode_pwm_freq_low
  4434. sbr Flags2, (1<<PGM_PWM_HIGH_FREQ)
  4435. rjmp decode_pwm_freq_end
  4436. decode_pwm_freq_low:
  4437. cbr Flags2, (1<<PGM_PWM_HIGH_FREQ)
  4438. decode_pwm_freq_end:
  4439. ret
  4440. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4441. ;
  4442. ; Load flash table entry
  4443. ;
  4444. ; Assumptions: Z must be loaded with table address, Temp1 must be
  4445. ; set to table entry number, result is delivered in XH
  4446. ;
  4447. ; Loads the content of a flash table entry
  4448. ;
  4449. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4450. load_flash_table_entry:
  4451. dec Temp1
  4452. add ZL, Temp1
  4453. adc ZH, Zero
  4454. lpm XH, Z
  4455. ret
  4456. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4457. ;
  4458. ; Decode settings
  4459. ;
  4460. ; No assumptions
  4461. ;
  4462. ; Decodes various settings
  4463. ;
  4464. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4465. decode_settings:
  4466. ; Decode governor gains
  4467. ldi ZL, low(GOV_GAIN_TABLE<<1)
  4468. ldi ZH, high(GOV_GAIN_TABLE<<1)
  4469. lds Temp1, Pgm_Gov_P_Gain ; Decode governor P gain
  4470. xcall load_flash_table_entry
  4471. sts Pgm_Gov_P_Gain_Decoded, XH
  4472. ldi ZL, low(GOV_GAIN_TABLE<<1)
  4473. ldi ZH, high(GOV_GAIN_TABLE<<1)
  4474. lds Temp1, Pgm_Gov_I_Gain ; Decode governor I gain
  4475. xcall load_flash_table_entry
  4476. sts Pgm_Gov_I_Gain_Decoded, XH
  4477. ; Decode startup power
  4478. ldi ZL, low(STARTUP_POWER_TABLE<<1)
  4479. ldi ZH, high(STARTUP_POWER_TABLE<<1)
  4480. lds Temp1, Pgm_Startup_Pwr
  4481. xcall load_flash_table_entry
  4482. sts Pgm_Startup_Pwr_Decoded, XH
  4483. .IF MODE == 0 ; Main
  4484. ; Decode spoolup time
  4485. lds Temp1, Pgm_Main_Spoolup_Time
  4486. tst Temp1
  4487. brne PC+2 ; If not zero - branch
  4488. inc Temp1
  4489. cpi Temp1, 17 ; Limit to 17 max
  4490. brcs PC+2
  4491. ldi Temp1, 17
  4492. mov XH, Temp1
  4493. add XH, Temp1
  4494. add XH, Temp1 ; Now 3x
  4495. sts Main_Spoolup_Time_3x, XH
  4496. mov Temp2, XH
  4497. add XH, Temp2
  4498. add XH, Temp2
  4499. add XH, Temp1 ; Now 10x
  4500. sts Main_Spoolup_Time_10x, XH
  4501. add XH, Temp2
  4502. add XH, Temp1
  4503. add XH, Temp1 ; Now 15x
  4504. sts Main_Spoolup_Time_15x, XH
  4505. .ENDIF
  4506. ; Decode demag compensation
  4507. lds XH, Pgm_Demag_Comp
  4508. ldi Temp1, 255 ; Set defaults
  4509. ldi Temp2, 12
  4510. cpi XH, 2
  4511. brne decode_demag_high
  4512. ldi Temp1, 160 ; Settings for demag comp low
  4513. ldi Temp2, 10
  4514. decode_demag_high:
  4515. cpi XH, 3
  4516. brne decode_demag_done
  4517. ldi Temp1, 130 ; Settings for demag comp high
  4518. ldi Temp2, 5
  4519. decode_demag_done:
  4520. sts Demag_Pwr_Off_Thresh, Temp1 ; Set variables
  4521. sts Low_Rpm_Pwr_Slope, Temp2
  4522. ; Decode pwm dither
  4523. ldi ZL, low(PWM_DITHER_TABLE<<1)
  4524. ldi ZH, high(PWM_DITHER_TABLE<<1)
  4525. lds Temp1, Pgm_Pwm_Dither
  4526. xcall load_flash_table_entry
  4527. sts Pwm_Dither_Decoded, XH
  4528. ret
  4529. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4530. ;
  4531. ; Set BEC voltage
  4532. ;
  4533. ; No assumptions
  4534. ;
  4535. ; Sets the BEC output voltage low or high
  4536. ;
  4537. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4538. set_bec_voltage:
  4539. ; Set bec voltage
  4540. .IF HIGH_BEC_VOLTAGE == 1
  4541. Set_BEC_Lo XH ; Set default to low
  4542. lds Temp1, Pgm_BEC_Voltage_High
  4543. tst Temp1
  4544. breq set_bec_voltage_exit
  4545. Set_BEC_Hi XH ; Set to high
  4546. set_bec_voltage_exit:
  4547. .ENDIF
  4548. .IF HIGH_BEC_VOLTAGE == 2
  4549. Set_BEC_0 ; Set default to low
  4550. lds Temp1, Pgm_BEC_Voltage_High
  4551. cpi Temp1, 1
  4552. brne set_bec_voltage_2
  4553. Set_BEC_1 ; Set to level 1
  4554. set_bec_voltage_2:
  4555. cpi Temp1, 2
  4556. brne set_bec_voltage_exit
  4557. Set_BEC_2 ; Set to level 2
  4558. set_bec_voltage_exit:
  4559. .ENDIF
  4560. ret
  4561. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4562. ;
  4563. ; Find throttle gain
  4564. ;
  4565. ; Assumes that interrupts are disabled
  4566. ; Assumes that the difference between max and min throttle must be more than 520us (a Pgm_Ppm_xxx_Throttle difference of 130)
  4567. ;
  4568. ; Finds throttle gain from throttle calibration values
  4569. ;
  4570. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4571. find_throttle_gain:
  4572. ; Load programmed minimum and maximum throttle
  4573. lds Temp3, Pgm_Ppm_Min_Throttle
  4574. lds Temp4, Pgm_Ppm_Max_Throttle
  4575. lds XH, Pgm_Direction ; Check if bidirectional operation
  4576. cpi XH, 3
  4577. brne find_throttle_gain_check_full
  4578. subi Temp4, 14 ; Compensate for higher deadband in bidirectional
  4579. find_throttle_gain_check_full:
  4580. ; Check if full range is chosen
  4581. sbrs Flags3, FULL_THROTTLE_RANGE
  4582. rjmp find_throttle_gain_calculate
  4583. ldi Temp3, 0
  4584. ldi Temp4, 255
  4585. find_throttle_gain_calculate:
  4586. ; Calculate difference
  4587. mov XH, Temp4
  4588. sub XH, Temp3
  4589. mov Temp5, XH
  4590. ; Check that difference is minimum 130
  4591. subi XH, 130
  4592. brcc PC+3
  4593. ldi XH, 130
  4594. mov Temp5, XH
  4595. ; Find gain
  4596. ldi Temp1, 0
  4597. test_throttle_gain:
  4598. inc Temp1
  4599. mul Temp5, Temp1 ; Temp5 has difference, Temp1 has gain
  4600. mov XH, Mul_Res_H
  4601. subi XH, 125
  4602. brcs test_throttle_gain
  4603. sts Ppm_Throttle_Gain, Temp1 ; Store gain
  4604. ret
  4605. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4606. ;
  4607. ; Average throttle
  4608. ;
  4609. ; Outputs result in Temp7
  4610. ;
  4611. ; Averages throttle calibration readings
  4612. ;
  4613. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4614. average_throttle:
  4615. sbr Flags3, (1<<FULL_THROTTLE_RANGE) ; Set range to 1000-2020us
  4616. xcall find_throttle_gain ; Set throttle gain
  4617. xcall wait30ms
  4618. ldi Temp3, 0
  4619. ldi Temp4, 0
  4620. ldi XH, 16 ; Average 16 measurments
  4621. mov Temp5, XH
  4622. average_throttle_meas:
  4623. xcall wait3ms ; Wait for new RC pulse value
  4624. lds XH, New_Rcp ; Get new RC pulse value
  4625. add Temp3, XH
  4626. adc Temp4, Zero
  4627. dec Temp5
  4628. brne average_throttle_meas
  4629. ldi XH, 4 ; Shift 4 times
  4630. average_throttle_div:
  4631. lsr Temp4 ; Shift right
  4632. ror Temp3
  4633. dec XH
  4634. brne average_throttle_div
  4635. mov Temp7, Temp3 ; Copy to Temp7
  4636. cbr Flags3, (1<<FULL_THROTTLE_RANGE)
  4637. xcall find_throttle_gain ; Set throttle gain
  4638. ret
  4639. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4640. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4641. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4642. ;
  4643. ; Main program start
  4644. ;
  4645. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4646. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4647. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4648. pgm_start:
  4649. ; Disable interrupts explicitly
  4650. cli
  4651. ; Check fuse high bits
  4652. ldi ZL, 0x03
  4653. ldi ZH, 0x00
  4654. Prepare_Lock_Or_Fuse_Read XH
  4655. lpm XH, Z
  4656. andi XH, 0x80
  4657. breq pgm_start ; If RSTDISBL is programmed, then loop here
  4658. ; Disable watchdog
  4659. Disable_Watchdog XH
  4660. ; Initialize MCU
  4661. Initialize_MCU XH
  4662. ; Initialize stack
  4663. ldi XH, high(RAMEND) ; Stack = RAMEND
  4664. out SPH, XH
  4665. ldi XH, low(RAMEND)
  4666. out SPL, XH
  4667. ; Switch power off
  4668. xcall switch_power_off
  4669. ; PortB initialization
  4670. ldi XH, INIT_PB
  4671. out PORTB, XH
  4672. ldi XH, DIR_PB
  4673. out DDRB, XH
  4674. ; PortC initialization
  4675. ldi XH, INIT_PC
  4676. out PORTC, XH
  4677. ldi XH, DIR_PC
  4678. out DDRC, XH
  4679. ; PortD initialization
  4680. ldi XH, INIT_PD
  4681. out PORTD, XH
  4682. ldi XH, DIR_PD
  4683. out DDRD, XH
  4684. ; Clear registers r0 through r25
  4685. clr Zero
  4686. ldi XL, low(0) ; Register number
  4687. ldi XH, low(0)
  4688. clear_regs:
  4689. st X+, Zero ; Clear register and post increment register number
  4690. cpi XL, 26 ; Check register number - last register?
  4691. brne clear_regs ; If not last register, go back
  4692. ; Clear RAM
  4693. ldi XL, low(SRAM_START)
  4694. ldi XH, high(SRAM_START)
  4695. ldi Temp1, SRAM_BYTES
  4696. clear_ram:
  4697. st X+, Zero
  4698. dec Temp1
  4699. brne clear_ram
  4700. ; Initialize LFSR
  4701. ldi XH, 1
  4702. sts Random, XH
  4703. ; Set default programmed parameters
  4704. xcall set_default_parameters
  4705. ; Read all programmed parameters
  4706. xcall read_all_eeprom_parameters
  4707. ; Initialize ADC
  4708. Initialize_Adc XH ; Initialize ADC operation
  4709. ; Set beep strength
  4710. lds Temp1, Pgm_Beep_Strength
  4711. sts Beep_Strength, Temp1
  4712. ; Set initial arm variable
  4713. ldi XH, 1
  4714. sts Initial_Arm, XH
  4715. ; Initializing beep
  4716. cli ; Disable interrupts explicitly
  4717. xcall wait200ms
  4718. xcall beep_f1
  4719. xcall wait30ms
  4720. xcall beep_f2
  4721. xcall wait30ms
  4722. xcall beep_f3
  4723. xcall wait30ms
  4724. .IF MODE <= 1 ; Main or tail
  4725. ; Wait for receiver to initialize
  4726. xcall wait1s
  4727. xcall wait200ms
  4728. xcall wait200ms
  4729. xcall wait100ms
  4730. .ENDIF
  4731. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4732. ;
  4733. ; No signal entry point
  4734. ;
  4735. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  4736. init_no_signal:
  4737. ; Disable interrupts explicitly
  4738. cli
  4739. ; Check if input signal is high for more than 30ms
  4740. ldi Temp1, 250
  4741. input_high_check_1:
  4742. ldi Temp2, 250
  4743. input_high_check_2:
  4744. Read_Rcp_Int XL ; Read RCP input
  4745. sbrs XL, Rcp_In ; Is it high?
  4746. rjmp bootloader_done ; No - run normally
  4747. dec Temp2
  4748. brne input_high_check_2
  4749. dec Temp1
  4750. brne input_high_check_1
  4751. ; Jump to bootloader if present
  4752. ldi ZL, 0x00
  4753. .IF THIRDBOOTSTART == 0xe00
  4754. ldi ZH, 0x1C
  4755. .ENDIF
  4756. .IF THIRDBOOTSTART == 0x1e00
  4757. ldi ZH, 0x3C
  4758. .ENDIF
  4759. lpm XH, Z+ ; Check for first bytes of SimonK bootloader
  4760. cpi XH, 0xE4
  4761. brne SK_bootloader_done
  4762. lpm XH, Z+
  4763. cpi XH, 0xE0
  4764. brne SK_bootloader_done
  4765. lpm XH, Z+
  4766. cpi XH, 0xFF
  4767. brne SK_bootloader_done
  4768. rjmp jmp_to_bootloader
  4769. SK_bootloader_done:
  4770. ldi ZL, 0x00
  4771. inc ZH ; BLHeli bootloader is smaller
  4772. inc ZH
  4773. lpm XH, Z+ ; Check for first bytes of BLHeli bootloader
  4774. cpi XH, 0xF8
  4775. brne bootloader_done
  4776. lpm XH, Z+
  4777. cpi XH, 0x94
  4778. brne bootloader_done
  4779. lpm XH, Z+
  4780. cpi XH, 0xAA
  4781. brne bootloader_done
  4782. jmp_to_bootloader:
  4783. clr ZL
  4784. lsr ZH
  4785. ijmp ; Jump to bootloader
  4786. bootloader_done:
  4787. ; Decode parameters
  4788. xcall decode_parameters
  4789. ; Decode settings
  4790. xcall decode_settings
  4791. ; Set BEC voltage
  4792. xcall set_bec_voltage
  4793. ; Find throttle gain from stored min and max settings
  4794. xcall find_throttle_gain
  4795. ; Set beep strength
  4796. lds Temp1, Pgm_Beep_Strength
  4797. sts Beep_Strength, Temp1
  4798. ; Switch power off
  4799. xcall switch_power_off
  4800. ; Timer0: clk/8 for regular interrupts
  4801. ldi XH, (1<<CS01)
  4802. Set_Timer0_CS0 XH
  4803. ; Timer1: clk/8 for commutation control and RC pulse measurement
  4804. ldi XH, (1<<CS11)
  4805. Set_Timer1_CS1 XH
  4806. ; Timer2: clk/8 for pwm
  4807. ldi XH, (1<<CS21)
  4808. Set_Timer2_CS2 XH
  4809. ; Initialize interrupts and registers
  4810. Initialize_Interrupts XH ; Set all
  4811. ; Initialize comparator
  4812. Comp_Init XH, Temp1 ; Initialize comparator
  4813. xcall wait1ms
  4814. sei ; Enable all interrupts
  4815. ; Measure number of lipo cells
  4816. xcall Measure_Lipo_Cells ; Measure number of lipo cells
  4817. ; Initialize RC pulse
  4818. Rcp_Int_First XH ; Enable interrupt and set to first edge
  4819. Rcp_Int_Enable XH ; Enable interrupt
  4820. Rcp_Clear_Int_Flag XH ; Clear interrupt flag
  4821. cbr Flags2, (1<<RCP_EDGE_NO) ; Set first edge flag
  4822. xcall wait200ms
  4823. ; Measure PWM frequency
  4824. measure_pwm_freq_init:
  4825. sbr Flags0, (1<<RCP_MEAS_PWM_FREQ) ; Set measure pwm frequency flag
  4826. ldi Temp4, 3 ; Number of attempts before going back to detect input signal
  4827. measure_pwm_freq_start:
  4828. ldi Temp3, 12 ; Number of pulses to measure
  4829. measure_pwm_freq_loop:
  4830. ; Check if period diff was accepted
  4831. lds XH, Rcp_Period_Diff_Accepted
  4832. tst XH
  4833. brne measure_pwm_freq_wait
  4834. ldi Temp3, 12 ; Reset number of pulses to measure
  4835. dec Temp4
  4836. brne PC+2
  4837. rjmp init_no_signal
  4838. measure_pwm_freq_wait:
  4839. xcall wait30ms ; Wait 30ms for new pulse
  4840. sbrs Flags2, RCP_UPDATED ; Is there an updated RC pulse available - proceed
  4841. rjmp init_no_signal ; Go back to detect input signal
  4842. cbr Flags2, (1<<RCP_UPDATED) ; Flag that pulse has been evaluated
  4843. lds XH, New_Rcp ; Load value
  4844. cpi XH, RCP_VALIDATE ; Higher than validate level?
  4845. brcs measure_pwm_freq_start ; No - start over
  4846. mov XH, Flags3 ; Check pwm frequency flags
  4847. andi XH, ((1<<RCP_PWM_FREQ_1KHZ)+(1<<RCP_PWM_FREQ_2KHZ)+(1<<RCP_PWM_FREQ_4KHZ)+(1<<RCP_PWM_FREQ_8KHZ)+(1<<RCP_PWM_FREQ_12KHZ))
  4848. lds Temp1, Curr_Rcp_Pwm_Freq ; Store as previous flags for next pulse
  4849. sts Prev_Rcp_Pwm_Freq, Temp1
  4850. sts Curr_Rcp_Pwm_Freq, XH ; Store current flags for next pulse
  4851. cp XH, Temp1
  4852. brne measure_pwm_freq_start ; Go back if new flags not same as previous
  4853. dec Temp3
  4854. brne measure_pwm_freq_loop ; Go back if not required number of pulses seen
  4855. ; Clear measure pwm frequency flag
  4856. cbr Flags0, (1<<RCP_MEAS_PWM_FREQ)
  4857. ; Set up RC pulse interrupts after pwm frequency measurement
  4858. Rcp_Int_First XH ; Enable interrupt and set to first edge
  4859. Rcp_Clear_Int_Flag XH ; Clear interrupt flag
  4860. cbr Flags2, (1<<RCP_EDGE_NO) ; Set first edge flag
  4861. lds XH, Pgm_Enable_PWM_Input ; Check if PWM input is enabled
  4862. tst XH
  4863. brne test_for_oneshot ; If it is - proceed
  4864. sbr Flags2, (1<<RCP_PPM) ; Set PPM flag
  4865. mov XH, Flags3 ; Clear pwm frequency flags
  4866. andi XH, !((1<<RCP_PWM_FREQ_1KHZ)+(1<<RCP_PWM_FREQ_2KHZ)+(1<<RCP_PWM_FREQ_4KHZ)+(1<<RCP_PWM_FREQ_8KHZ)+(1<<RCP_PWM_FREQ_12KHZ))
  4867. mov Flags3, XH
  4868. test_for_oneshot:
  4869. ; Test whether signal is OnShot125
  4870. cbr Flags2, (1<<RCP_PPM_ONESHOT125) ; Clear OneShot125 flag
  4871. sts Rcp_Outside_Range_Cnt, Zero ; Reset out of range counter
  4872. xcall wait100ms ; Wait for new RC pulses
  4873. sbrs Flags2, RCP_PPM
  4874. rjmp validate_rcp_start ; If flag is not set (PWM) - branch
  4875. lds XH, Rcp_Outside_Range_Cnt ; Check how many pulses were outside normal PPM range (800-2160us)
  4876. cpi XH, 10
  4877. brcs validate_rcp_start
  4878. sbr Flags2, (1<<RCP_PPM_ONESHOT125) ; Set OneShot125 flag
  4879. ; Validate RC pulse
  4880. validate_rcp_start:
  4881. xcall wait3ms ; Wait for next pulse (NB: Uses Temp1/2!)
  4882. ldi Temp1, RCP_VALIDATE ; Set validate level as default
  4883. sbrs Flags2, RCP_PPM
  4884. rjmp PC+2 ; If flag is not set (PWM) - branch
  4885. ldi Temp1, 0 ; Set level to zero for PPM (any level will be accepted)
  4886. lds XH, New_Rcp ; Load value
  4887. cp XH, Temp1 ; Higher than validate level?
  4888. brcs validate_rcp_start ; No - start over
  4889. ; Beep arm sequence start signal
  4890. cli ; Disable all interrupts
  4891. xcall beep_f1 ; Signal that RC pulse is ready
  4892. xcall beep_f1
  4893. xcall beep_f1
  4894. sei ; Enable all interrupts
  4895. xcall wait200ms
  4896. ; Arming sequence start
  4897. sts Gov_Arm_Target, Zero ; Clear governor arm target
  4898. arming_start:
  4899. .IF MODE >= 1 ; Tail or multi
  4900. lds XH, Pgm_Direction ; Check if bidirectional operation
  4901. cpi XH, 3
  4902. brne PC+2
  4903. rjmp program_by_tx_checked ; Disable tx programming if bidirectional operation
  4904. .ENDIF
  4905. xcall wait3ms
  4906. lds XH, Pgm_Enable_TX_Program; Start programming mode entry if enabled
  4907. cpi XH, 1 ; Is TX programming enabled?
  4908. brcc arming_initial_arm_check ; Yes - proceed
  4909. rjmp program_by_tx_checked ; No - branch
  4910. arming_initial_arm_check:
  4911. lds XH, Initial_Arm ; Yes - check if it is initial arm sequence
  4912. cpi XH, 1 ; Is it the initial arm sequence?
  4913. brcc arming_ppm_check ; Yes - proceed
  4914. rjmp program_by_tx_checked ; No - branch
  4915. arming_ppm_check:
  4916. sbrc Flags2, RCP_PPM
  4917. rjmp throttle_high_cal_start ; If flag is set (PPM) - branch
  4918. ; PWM tx program entry
  4919. lds XH, New_Rcp ; Load new RC pulse value
  4920. cpi XH, RCP_MAX ; Is RC pulse max?
  4921. brcc program_by_tx_entry_pwm ; Yes - proceed
  4922. rjmp program_by_tx_checked ; No - branch
  4923. program_by_tx_entry_pwm:
  4924. cli ; Disable all interrupts
  4925. xcall beep_f4
  4926. sei ; Enable all interrupts
  4927. xcall wait100ms
  4928. lds XH, New_Rcp ; Load new RC pulse value
  4929. cpi XH, RCP_STOP ; Below stop?
  4930. brcc program_by_tx_entry_pwm ; No - start over
  4931. program_by_tx_entry_wait_pwm:
  4932. cli ; Disable all interrupts
  4933. xcall beep_f1
  4934. xcall wait10ms
  4935. xcall beep_f1
  4936. sei ; Enable all interrupts
  4937. xcall wait100ms
  4938. lds XH, New_Rcp ; Load new RC pulse value
  4939. cpi XH, RCP_MAX ; At or above max?
  4940. brcs program_by_tx_entry_wait_pwm ; No - start over
  4941. rjmp program_by_tx ; Yes - enter programming mode
  4942. ; PPM throttle calibration and tx program entry
  4943. throttle_high_cal_start:
  4944. .IF MODE <= 1 ; Main or tail
  4945. ldi XH, 8 ; Set 3 seconds wait time
  4946. .ELSE
  4947. ldi XH, 3 ; Set 1 second wait time
  4948. .ENDIF
  4949. mov Temp8, XH
  4950. throttle_high_cal:
  4951. sbr Flags3, (1<<FULL_THROTTLE_RANGE) ; Set range to 1000-2020us
  4952. cli
  4953. xcall find_throttle_gain ; Set throttle gain
  4954. sei
  4955. xcall wait100ms ; Wait for new throttle value
  4956. cli ; Disable interrupts (freeze New_Rcp value)
  4957. cbr Flags3, (1<<FULL_THROTTLE_RANGE) ; Set programmed range
  4958. xcall find_throttle_gain ; Set throttle gain
  4959. lds Temp7, New_Rcp ; Store new RC pulse value
  4960. lds XH, New_Rcp ; Load new RC pulse value
  4961. cpi XH, (RCP_MAX/2) ; Is RC pulse above midstick?
  4962. sei ; Enable interrupts
  4963. brcc PC+2
  4964. rjmp arm_target_updated ; No - branch
  4965. xcall wait1ms
  4966. cli ; Disable all interrupts
  4967. xcall beep_f4
  4968. sei ; Enable all interrupts
  4969. dec Temp8
  4970. brne throttle_high_cal ; Continue to wait
  4971. xcall average_throttle
  4972. mov XH, Temp7 ; Limit to max 250
  4973. subi XH, 5 ; Subtract about 2% and ensure that it is 250 or lower
  4974. sts Pgm_Ppm_Max_Throttle, XH ; Store
  4975. xcall wait200ms
  4976. cli
  4977. xcall store_all_in_eeprom
  4978. xcall success_beep
  4979. sei
  4980. throttle_low_cal_start:
  4981. ldi XH, 10 ; Set 3 seconds wait time
  4982. mov Temp8, XH
  4983. throttle_low_cal:
  4984. sbr Flags3, (1<<FULL_THROTTLE_RANGE) ; Set range to 1000-2020us
  4985. cli
  4986. xcall find_throttle_gain ; Set throttle gain
  4987. sei
  4988. xcall wait100ms
  4989. cli ; Disable interrupts (freeze New_Rcp value)
  4990. cbr Flags3, (1<<FULL_THROTTLE_RANGE) ; Set programmed range
  4991. xcall find_throttle_gain ; Set throttle gain
  4992. lds Temp7, New_Rcp ; Store new RC pulse value
  4993. lds XH, New_Rcp ; Load new RC pulse value
  4994. cpi XH, (RCP_MAX/2) ; Below midstick?
  4995. sei ; Enable interrupts
  4996. brcc throttle_low_cal_start ; No - start over
  4997. xcall wait1ms
  4998. cli ; Disable all interrupts
  4999. xcall beep_f1
  5000. xcall wait10ms
  5001. xcall beep_f1
  5002. sei ; Enable all interrupts
  5003. dec Temp8
  5004. brne throttle_low_cal ; Continue to wait
  5005. xcall average_throttle
  5006. mov XH, Temp7
  5007. subi XH, 0xFB ; Add about 2% (subtract negative number)
  5008. sts Pgm_Ppm_Min_Throttle, XH ; Store
  5009. xcall wait200ms
  5010. cli
  5011. xcall store_all_in_eeprom
  5012. xcall success_beep_inverted
  5013. sei
  5014. program_by_tx_entry_wait_ppm:
  5015. xcall wait100ms
  5016. cli
  5017. xcall find_throttle_gain ; Set throttle gain
  5018. sei
  5019. lds XH, New_Rcp ; Load new RC pulse value
  5020. cpi XH, RCP_MAX ; At or above max?
  5021. brcc PC+2
  5022. rjmp arming_ppm_check ; No - go back
  5023. rjmp program_by_tx ; Yes - enter programming mode
  5024. program_by_tx_checked:
  5025. lds Temp1, New_Rcp ; Load new RC pulse value
  5026. lds XH, Gov_Arm_Target ; Is RC pulse larger than arm target?
  5027. cp Temp1, XH
  5028. brcs arm_target_updated ; No - do not update
  5029. sts Gov_Arm_Target, Temp1 ; Yes - update arm target
  5030. arm_target_updated:
  5031. xcall wait100ms ; Wait for new throttle value
  5032. ldi Temp1, RCP_STOP ; Default stop value
  5033. lds XH, Pgm_Direction ; Check if bidirectional operation
  5034. subi XH, 3
  5035. brne PC+2 ; No - branch
  5036. ldi Temp1, (RCP_STOP+4) ; Higher stop value for bidirectional
  5037. lds XH, New_Rcp ; Load new RC pulse value
  5038. cp XH, Temp1 ; Below stop?
  5039. brcs arm_end_beep ; Yes - proceed
  5040. rjmp arming_start ; No - start over
  5041. arm_end_beep:
  5042. ; Beep arm sequence end signal
  5043. cli ; Disable all interrupts
  5044. xcall beep_f4 ; Signal that rcpulse is ready
  5045. xcall beep_f4
  5046. xcall beep_f4
  5047. sei ; Enable all interrupts
  5048. xcall wait200ms
  5049. ; Clear initial arm variable
  5050. sts Initial_Arm, Zero
  5051. ; Armed and waiting for power on
  5052. wait_for_power_on:
  5053. sts Power_On_Wait_Cnt_L, Zero; Clear wait counter
  5054. sts Power_On_Wait_Cnt_H, Zero
  5055. wait_for_power_on_loop:
  5056. lds XH, Power_On_Wait_Cnt_L ; Increment low wait counter
  5057. inc XH
  5058. sts Power_On_Wait_Cnt_L, XH
  5059. cpi XH, 0xFF
  5060. brne wait_for_power_on_no_beep; Counter wrapping (about 1 sec)?
  5061. lds XH, Power_On_Wait_Cnt_H ; Increment high wait counter
  5062. inc XH
  5063. sts Power_On_Wait_Cnt_H, XH
  5064. lds XH, Pgm_Beacon_Delay
  5065. ldi Temp1, 25 ; Approximately 1 min
  5066. dec XH
  5067. breq beep_delay_set
  5068. ldi Temp1, 50 ; Approximately 2 min
  5069. dec XH
  5070. breq beep_delay_set
  5071. ldi Temp1, 125 ; Approximately 5 min
  5072. dec XH
  5073. breq beep_delay_set
  5074. ldi Temp1, 250 ; Approximately 10 min
  5075. dec XH
  5076. breq beep_delay_set
  5077. sts Power_On_Wait_Cnt_H, Zero; Reset counter for infinite delay
  5078. beep_delay_set:
  5079. lds XH, Power_On_Wait_Cnt_H
  5080. cp XH, Temp1 ; Check against chosen delay
  5081. brcs wait_for_power_on_no_beep; Has delay elapsed?
  5082. lds XH, Power_On_Wait_Cnt_H ; Decrement high wait counter
  5083. dec XH
  5084. sts Power_On_Wait_Cnt_H, XH
  5085. ldi XH, 180 ; Set low wait counter
  5086. sts Power_On_Wait_Cnt_L, XH
  5087. lds XH, Pgm_Beacon_Strength
  5088. sts Beep_Strength, XH
  5089. cli ; Disable all interrupts
  5090. xcall beep_f4 ; Signal that there is no signal
  5091. sei ; Enable all interrupts
  5092. lds XH, Pgm_Beep_Strength
  5093. sts Beep_Strength, XH
  5094. xcall wait100ms ; Wait for new RC pulse to be measured
  5095. wait_for_power_on_no_beep:
  5096. xcall wait10ms
  5097. lds XH, Rcp_Timeout_Cntd ; Load RC pulse timeout counter value
  5098. tst XH
  5099. brne wait_for_power_on_ppm_not_missing ; If it is not zero - proceed
  5100. sbrs Flags2, RCP_PPM
  5101. rjmp wait_for_power_on_ppm_not_missing ; If flag is not set (PWM) - branch
  5102. rjmp init_no_signal ; If ppm and pulses missing - go back to detect input signal
  5103. wait_for_power_on_ppm_not_missing:
  5104. ldi Temp1, RCP_STOP
  5105. sbrc Flags2, RCP_PPM
  5106. rjmp PC+2 ; If flag is set (PPM) - branch
  5107. ldi Temp1, (RCP_STOP+5) ; Higher than stop (for pwm)
  5108. lds XH, New_Rcp ; Load new RC pulse value
  5109. cp XH, Temp1
  5110. brcc PC+2
  5111. rjmp wait_for_power_on_loop ; No - start over
  5112. .IF MODE >= 1 ; Tail or multi
  5113. lds XH, Pgm_Direction ; Check if bidirectional operation
  5114. subi XH, 3
  5115. breq wait_for_power_on_check_timeout ; Do not wait if bidirectional operation
  5116. .ENDIF
  5117. xcall wait100ms ; Wait to see if start pulse was only a glitch
  5118. wait_for_power_on_check_timeout:
  5119. lds XH, Rcp_Timeout_Cntd ; Load RC pulse timeout counter value
  5120. tst XH
  5121. brne PC+2 ; If it is not zero - proceed
  5122. rjmp init_no_signal ; If it is zero (pulses missing) - go back to detect input signal
  5123. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  5124. ;
  5125. ; Start entry point
  5126. ;
  5127. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  5128. init_start:
  5129. cli
  5130. xcall switch_power_off
  5131. sts Requested_Pwm, Zero ; Set requested pwm to zero
  5132. sts Governor_Req_Pwm, Zero ; Set governor requested pwm to zero
  5133. sts Current_Pwm, Zero ; Set current pwm to zero
  5134. mov Current_Pwm_Limited, Zero; Set limited current pwm to zero
  5135. sts Current_Pwm_Lim_Dith, Zero
  5136. sts Pwm_Dither_Excess_Power, Zero
  5137. sei
  5138. lds XH, Pgm_Motor_Idle ; Set idle pwm to programmed value
  5139. lsl XH
  5140. sts Pwm_Motor_Idle, XH
  5141. sts Gov_Target_L, Zero ; Set target to zero
  5142. sts Gov_Target_H, Zero
  5143. sts Gov_Integral_L, Zero ; Set integral to zero
  5144. sts Gov_Integral_H, Zero
  5145. sts Gov_Integral_X, Zero
  5146. sts Adc_Conversion_Cnt, Zero
  5147. ldi Flags0, 0 ; Clear flags0
  5148. ldi Flags1, 0 ; Clear flags1
  5149. sts Demag_Detected_Metric, Zero ; Clear demag metric
  5150. ;**** **** **** **** ****
  5151. ; Motor start beginning
  5152. ;**** **** **** **** ****
  5153. ldi XH, TEMP_CHECK_RATE ; Make sure a temp reading is done
  5154. sts Adc_Conversion_Cnt, XH
  5155. Set_Adc_Ip_Temp
  5156. xcall wait1ms
  5157. Start_Adc XH
  5158. read_initial_temp:
  5159. Get_Adc_Status XH
  5160. sbrc XH, ADSC
  5161. rjmp read_initial_temp
  5162. Read_Adc_Result Temp1, Temp2 ; Read initial temperature
  5163. Stop_Adc XH
  5164. tst Temp2
  5165. breq PC+2 ; Is reading below 256?
  5166. ldi Temp1, 0xFF ; No - set average temperature value to 255
  5167. sts Current_Average_Temp_Adc, Temp1 ; Set initial average temp ADC reading
  5168. xcall check_temp_voltage_and_limit_power
  5169. ldi XH, TEMP_CHECK_RATE ; Make sure a temp reading is done next time
  5170. sts Adc_Conversion_Cnt, XH
  5171. Set_Adc_Ip_Temp
  5172. ; Set up start operating conditions
  5173. lds Temp7, Pgm_Pwm_Freq ; Store setting in Temp7
  5174. ldi XH, 2 ; Set nondamped low frequency pwm mode
  5175. sts Pgm_Pwm_Freq, XH
  5176. xcall decode_parameters ; (Decode_parameters uses Temp1 and Temp8)
  5177. sts Pgm_Pwm_Freq, Temp7 ; Restore settings
  5178. ; Set max allowed power
  5179. cli ; Disable interrupts to avoid that Requested_Pwm is overwritten
  5180. ldi XH, 0xFF ; Set pwm limit to max
  5181. sts Pwm_Limit, XH
  5182. xcall set_startup_pwm
  5183. lds XH, Requested_Pwm
  5184. sts Pwm_Limit, XH
  5185. sts Pwm_Limit_Spoolup, XH
  5186. sts Pwm_Limit_By_Rpm, XH
  5187. sei
  5188. ldi XH, 1 ; Set low pwm again after calling set_startup_pwm
  5189. sts Requested_Pwm, XH
  5190. sts Current_Pwm, XH
  5191. mov Current_Pwm_Limited, XH
  5192. sts Current_Pwm_Lim_Dith, XH
  5193. sts Spoolup_Limit_Skip, XH
  5194. lds XH, Auto_Bailout_Armed
  5195. sts Spoolup_Limit_Cnt, XH
  5196. ; Begin startup sequence
  5197. lds XH, Pgm_Direction ; Check if bidirectional operation
  5198. cpi XH, 3
  5199. brne init_start_bidir_done
  5200. cbr Flags3, (1<<PGM_DIR_REV) ; Set spinning direction. Default fwd
  5201. sbrc Flags2, RCP_DIR_REV ; Check force direction
  5202. sbr Flags3, (1<<PGM_DIR_REV) ; Set spinning direction
  5203. init_start_bidir_done:
  5204. sbr Flags1, (1<<MOTOR_SPINNING) ; Set motor spinning flag
  5205. sbr Flags1, (1<<STARTUP_PHASE) ; Set startup phase flag
  5206. sts Startup_Cnt, Zero ; Reset counter
  5207. xcall comm5comm6 ; Initialize commutation
  5208. xcall comm6comm1
  5209. xcall initialize_timing ; Initialize timing
  5210. xcall calc_next_comm_timing ; Set virtual commutation point
  5211. xcall initialize_timing ; Initialize timing
  5212. xcall calc_next_comm_timing
  5213. xcall initialize_timing ; Initialize timing
  5214. rjmp run1
  5215. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  5216. ;
  5217. ; Run entry point
  5218. ;
  5219. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  5220. damped_transition:
  5221. ; Transition from nondamped to damped if applicable
  5222. cli
  5223. xcall decode_parameters ; Set programmed parameters
  5224. sei
  5225. sts Adc_Conversion_Cnt, Zero ; Make sure a voltage reading is done next time
  5226. Set_Adc_Ip_Volt ; Set adc measurement to voltage
  5227. ; Run 1 = B(p-on) + C(n-pwm) - comparator A evaluated
  5228. ; Out_cA changes from low to high
  5229. run1:
  5230. xcall wait_for_comp_out_high ; Wait zero cross wait and wait for high
  5231. ; setup_comm_wait ; Setup wait time from zero cross to commutation
  5232. ; evaluate_comparator_integrity ; Check whether comparator reading has been normal
  5233. xcall calc_governor_target ; Calculate governor target
  5234. xcall wait_for_comm ; Wait from zero cross to commutation
  5235. xcall comm1comm2 ; Commutate
  5236. xcall calc_next_comm_timing ; Calculate next timing and start advance timing wait
  5237. ; wait_advance_timing ; Wait advance timing and start zero cross wait
  5238. ; calc_new_wait_times
  5239. ; set_comparator_phase ; Set comparator phase
  5240. ; wait_before_zc_scan ; Wait zero cross wait and start zero cross timeout
  5241. ; Run 2 = A(p-on) + C(n-pwm) - comparator B evaluated
  5242. ; Out_cB changes from high to low
  5243. run2:
  5244. xcall wait_for_comp_out_low
  5245. ; setup_comm_wait
  5246. ; evaluate_comparator_integrity
  5247. sbrc Flags0, GOV_ACTIVE
  5248. xcall calc_governor_prop_error
  5249. sbrs Flags1, HIGH_RPM ; Skip if high rpm
  5250. xcall set_pwm_limit_low_rpm
  5251. sbrc Flags1, HIGH_RPM ; Do if high rpm
  5252. xcall set_pwm_limit_high_rpm
  5253. xcall wait_for_comm
  5254. xcall comm2comm3
  5255. xcall calc_next_comm_timing
  5256. ; wait_advance_timing
  5257. ; calc_new_wait_times
  5258. ; set_comparator_phase
  5259. ; wait_before_zc_scan
  5260. ; Run 3 = A(p-on) + B(n-pwm) - comparator C evaluated
  5261. ; Out_cC changes from low to high
  5262. run3:
  5263. xcall wait_for_comp_out_high
  5264. ; setup_comm_wait
  5265. ; evaluate_comparator_integrity
  5266. sbrc Flags0, GOV_ACTIVE
  5267. xcall calc_governor_int_error
  5268. xcall wait_for_comm
  5269. xcall comm3comm4
  5270. xcall calc_next_comm_timing
  5271. ; wait_advance_timing
  5272. ; calc_new_wait_times
  5273. ; set_comparator_phase
  5274. ; wait_before_zc_scan
  5275. ; Run 4 = C(p-on) + B(n-pwm) - comparator A evaluated
  5276. ; Out_cA changes from high to low
  5277. run4:
  5278. xcall wait_for_comp_out_low
  5279. ; setup_comm_wait
  5280. ; evaluate_comparator_integrity
  5281. sbrc Flags0, GOV_ACTIVE
  5282. xcall calc_governor_prop_correction
  5283. xcall wait_for_comm
  5284. xcall comm4comm5
  5285. xcall calc_next_comm_timing
  5286. ; wait_advance_timing
  5287. ; calc_new_wait_times
  5288. ; set_comparator_phase
  5289. ; wait_before_zc_scan
  5290. ; Run 5 = C(p-on) + A(n-pwm) - comparator B evaluated
  5291. ; Out_cB changes from low to high
  5292. run5:
  5293. xcall wait_for_comp_out_high
  5294. ; setup_comm_wait
  5295. ; evaluate_comparator_integrity
  5296. sbrc Flags0, GOV_ACTIVE
  5297. xcall calc_governor_int_correction
  5298. xcall wait_for_comm
  5299. xcall comm5comm6
  5300. xcall calc_next_comm_timing
  5301. ; wait_advance_timing
  5302. ; calc_new_wait_times
  5303. ; set_comparator_phase
  5304. ; wait_before_zc_scan
  5305. ; Run 6 = B(p-on) + A(n-pwm) - comparator C evaluated
  5306. ; Out_cC changes from high to low
  5307. run6:
  5308. xcall wait_for_comp_out_low
  5309. ; setup_comm_wait
  5310. ; evaluate_comparator_integrity
  5311. Start_Adc XH
  5312. xcall wait_for_comm
  5313. xcall comm6comm1
  5314. xcall calc_next_comm_timing
  5315. ; wait_advance_timing
  5316. ; calc_new_wait_times
  5317. ; set_comparator_phase
  5318. ; wait_before_zc_scan
  5319. xcall check_temp_voltage_and_limit_power
  5320. ; Check if it is startup
  5321. sbrs Flags1, STARTUP_PHASE
  5322. rjmp normal_run_checks
  5323. sbrc Flags0, DIR_CHANGE_BRAKE ; If a direction change - branch
  5324. rjmp normal_run_checks
  5325. ; Set spoolup power variables
  5326. lds XH, Pwm_Spoolup_Beg
  5327. sts Pwm_Limit, XH ; Set initial max power
  5328. sts Pwm_Limit_Spoolup, XH ; Set initial slow spoolup power
  5329. lds XH, Auto_Bailout_Armed
  5330. sts Spoolup_Limit_Cnt, XH
  5331. ldi XH, 1
  5332. sts Spoolup_Limit_Skip, XH
  5333. ; Check startup counter
  5334. ldi Temp2, 24 ; Set nominal startup parameters
  5335. ldi Temp3, 12
  5336. lds XH, Startup_Cnt ; Load counter
  5337. cp XH, Temp2 ; Is counter above requirement?
  5338. brcs start_check_rcp ; No - proceed
  5339. cbr Flags1, (1<<STARTUP_PHASE) ; Clear startup phase flag
  5340. sbr Flags1, (1<<INITIAL_RUN_PHASE); Set initial run phase flag
  5341. sts Initial_Run_Rot_Cnt, Temp3 ; Set initial run rotation count
  5342. .IF MODE == 1 ; Tail
  5343. ldi XH, 0xFF
  5344. sts Pwm_Limit, XH ; Allow full power
  5345. .ENDIF
  5346. .IF MODE == 2 ; Multi
  5347. lds XH, Pwm_Spoolup_Beg
  5348. sts Pwm_Limit, XH
  5349. sts Pwm_Limit_By_Rpm, XH
  5350. .ENDIF
  5351. rjmp normal_run_checks
  5352. start_check_rcp:
  5353. lds XH, New_Rcp ; Load new pulse value
  5354. cpi XH, RCP_STOP ; Check if pulse is below stop value
  5355. brcs PC+2
  5356. rjmp run1 ; Continue to run
  5357. rjmp run_to_wait_for_power_on
  5358. normal_run_checks:
  5359. ; Check if it is initial run phase
  5360. sbrs Flags1, INITIAL_RUN_PHASE ; If not initial run phase - branch
  5361. rjmp initial_run_phase_done
  5362. sbrc Flags0, DIR_CHANGE_BRAKE ; If a direction change - branch
  5363. rjmp initial_run_phase_done
  5364. ; Decrement startup rotation count
  5365. lds XH, Initial_Run_Rot_Cnt
  5366. dec XH
  5367. ; Check number of nondamped rotations
  5368. brne normal_run_check_startup_rot ; Branch if counter is not zero
  5369. cbr Flags1, (1<<INITIAL_RUN_PHASE); Clear initial run phase flag
  5370. rjmp damped_transition ; Do damped transition if counter is zero
  5371. normal_run_check_startup_rot:
  5372. sts Initial_Run_Rot_Cnt, XH ; Not zero - store counter
  5373. lds XH, New_Rcp ; Load new pulse value
  5374. cpi XH, RCP_STOP ; Check if pulse is below stop value
  5375. brcs PC+2
  5376. rjmp run1 ; Continue to run
  5377. rjmp run_to_wait_for_power_on
  5378. initial_run_phase_done:
  5379. ; Reset stall count
  5380. sts Stall_Cnt, Zero
  5381. .IF MODE == 0 ; Main
  5382. ; Check if throttle is zeroed
  5383. lds XH, Rcp_Stop_Cnt ; Load stop RC pulse counter value
  5384. cpi XH, 1 ; Is number of stop RC pulses above limit?
  5385. brcs run6_check_rcp_stop_count ; If no - branch
  5386. lds XH, Pwm_Spoolup_Beg ; If yes - set initial max powers
  5387. sts Pwm_Limit_Spoolup, XH
  5388. lds XH, Auto_Bailout_Armed ; And set spoolup parameters
  5389. sts Spoolup_Limit_Cnt, XH
  5390. ldi XH, 1
  5391. sts Spoolup_Limit_Skip, XH
  5392. run6_check_rcp_stop_count:
  5393. .ENDIF
  5394. ; Exit run loop after a given time
  5395. lds XH, Rcp_Stop_Cnt ; Load stop RC pulse counter value
  5396. ldi Temp1, RCP_STOP_LIMIT
  5397. cp XH, Temp1 ; Is number of stop RC pulses above limit?
  5398. brcs PC+2
  5399. rjmp run_to_wait_for_power_on ; Yes, go back to wait for poweron
  5400. sbrs Flags2, RCP_PPM
  5401. rjmp run6_check_dir ; If flag is not set (PWM) - branch
  5402. lds XH, Rcp_Timeout_Cntd ; Load RC pulse timeout counter value
  5403. tst XH
  5404. breq run_to_wait_for_power_on ; If it is zero - go back to wait for poweron
  5405. run6_check_dir:
  5406. .IF MODE >= 1 ; Tail or multi
  5407. lds XH, Pgm_Direction ; Check if bidirectional operation
  5408. cpi XH, 3
  5409. brne run6_check_speed
  5410. sbrc Flags3, PGM_DIR_REV ; Check if actual rotation direction
  5411. rjmp run6_check_dir_rev
  5412. sbrc Flags2, RCP_DIR_REV ; Matches force direction
  5413. rjmp run6_check_dir_change
  5414. rjmp run6_check_speed
  5415. run6_check_dir_rev:
  5416. sbrs Flags2, RCP_DIR_REV
  5417. rjmp run6_check_dir_change
  5418. rjmp run6_check_speed
  5419. run6_check_dir_change:
  5420. sbrc Flags0, DIR_CHANGE_BRAKE
  5421. rjmp run6_check_speed
  5422. sbr Flags0, (1<<DIR_CHANGE_BRAKE) ; Set brake flag
  5423. lds XH, Pwm_Spoolup_Beg ; Set max power while braking
  5424. sts Pwm_Limit, XH
  5425. rjmp run4 ; Go back to run 4, thereby changing force direction
  5426. run6_check_speed:
  5427. .ENDIF
  5428. ldi Temp1, 0xF0 ; Default minimum speed
  5429. sbrs Flags0, DIR_CHANGE_BRAKE ; Is it a direction change?
  5430. rjmp run6_brake_done
  5431. lds XH, Pwm_Spoolup_Beg ; Set max power while braking
  5432. sts Pwm_Limit, XH
  5433. ldi Temp1, 0x20 ; Bidirectional braking termination speed
  5434. run6_brake_done:
  5435. lds XH, Comm_Period4x_H ; Is Comm_Period4x more than 32ms (~1220 eRPM)?
  5436. cp XH, Temp1
  5437. brcc PC+2 ; Yes - stop or turn direction
  5438. rjmp run1 ; No - go back to run 1
  5439. .IF MODE >= 1 ; Tail or multi
  5440. sbrs Flags0, DIR_CHANGE_BRAKE ; If it is not a direction change - stop
  5441. rjmp run_to_wait_for_power_on
  5442. cbr Flags0, (1<<DIR_CHANGE_BRAKE) ; Clear brake flag
  5443. cbr Flags3, (1<<PGM_DIR_REV) ; Set spinning direction. Default fwd
  5444. sbrc Flags2, RCP_DIR_REV ; Check force direction
  5445. sbr Flags3, (1<<PGM_DIR_REV) ; Set spinning direction
  5446. sbr Flags1, (1<<INITIAL_RUN_PHASE)
  5447. ldi XH, 18
  5448. sts Initial_Run_Rot_Cnt, XH
  5449. lds XH, Pwm_Spoolup_Beg ; Set initial max power
  5450. sts Pwm_Limit, XH
  5451. rjmp run1 ; Go back to run 1
  5452. .ENDIF
  5453. run_to_wait_for_power_on_fail:
  5454. lds XH, Stall_Cnt ; Increment stall count
  5455. inc XH
  5456. sts Stall_Cnt, XH
  5457. lds XH, New_Rcp ; Check if RCP is zero, then it is a normal stop
  5458. tst XH
  5459. breq run_to_wait_for_power_on
  5460. rjmp run_to_wait_for_power_on_stall_done
  5461. run_to_wait_for_power_on:
  5462. sts Stall_Cnt, Zero
  5463. run_to_wait_for_power_on_stall_done:
  5464. cli
  5465. xcall switch_power_off
  5466. lds Temp7, Pgm_Pwm_Freq ; Store setting in Temp7
  5467. ldi XH, 2 ; Set low pwm mode (in order to turn off damping)
  5468. sts Pgm_Pwm_Freq, XH
  5469. xcall decode_parameters ; (Decode_parameters uses Temp1 and Temp8)
  5470. sts Pgm_Pwm_Freq, Temp7 ; Restore settings
  5471. sts Requested_Pwm, Zero ; Set requested pwm to zero
  5472. sts Governor_Req_Pwm, Zero ; Set governor requested pwm to zero
  5473. sts Current_Pwm, Zero ; Set current pwm to zero
  5474. mov Current_Pwm_Limited, Zero ; Set limited current pwm to zero
  5475. sts Current_Pwm_Lim_Dith, Zero
  5476. sts Pwm_Motor_Idle, Zero ; Set motor idle to zero
  5477. cbr Flags1, (1<<MOTOR_SPINNING) ; Clear motor spinning flag
  5478. sei
  5479. xcall wait1ms ; Wait for pwm to be stopped
  5480. xcall switch_power_off
  5481. Initialize_Adc XH ; Initialize ADC, to keep reference on for selected ESCs
  5482. .IF MODE == 0 ; Main
  5483. sbrs Flags2, RCP_PPM
  5484. rjmp run_to_next_state_main ; If flag is not set (PWM) - branch
  5485. lds XH, Rcp_Timeout_Cntd ; Load RC pulse timeout counter value
  5486. tst XH
  5487. brne run_to_next_state_main ; If it is not zero - branch
  5488. rjmp init_no_signal ; If it is zero (pulses missing) - go back to detect input signal
  5489. run_to_next_state_main:
  5490. lds XH, Pgm_Main_Rearm_Start
  5491. cpi XH, 1 ; Is re-armed start enabled?
  5492. brcs jmp_wait_for_power_on ; No - do like tail and start immediately
  5493. rjmp validate_rcp_start ; Yes - go back to validate RC pulse
  5494. jmp_wait_for_power_on:
  5495. rjmp wait_for_power_on ; Go back to wait for power on
  5496. .ENDIF
  5497. .IF MODE >= 1 ; Tail or multi
  5498. sbrs Flags2, RCP_PPM
  5499. rjmp jmp_wait_for_power_on ; If flag is not set (PWM) - branch
  5500. lds XH, Stall_Cnt
  5501. cpi XH, 5
  5502. brcs jmp_wait_for_power_on
  5503. rjmp init_no_signal
  5504. jmp_wait_for_power_on:
  5505. rjmp wait_for_power_on ; Go back to wait for power on
  5506. .ENDIF
  5507. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  5508. .INCLUDE "BLHeliTxPgm.inc" ; Include source code for programming the ESC with the TX
  5509. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  5510. reset:
  5511. rjmp pgm_start
  5512. .EXIT