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.

640 lines
13 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
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. ; BLHeliTxPgm Atmel
  26. ;
  27. ;**** **** **** **** ****
  28. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  29. ;
  30. ; Read all eeprom parameters routine
  31. ;
  32. ; No assumptions
  33. ;
  34. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  35. read_all_eeprom_parameters:
  36. .IF MODE == 0 ; Main
  37. ; Check initialized signature
  38. ldi ZL, low(Eep_Initialized_L)
  39. ldi ZH, high(Eep_Initialized_L)
  40. xcall read_eeprom_byte
  41. cpi Temp1, 0xA5
  42. breq PC+2
  43. rjmp read_eeprom_store_defaults
  44. ldi ZL, low(Eep_Initialized_H)
  45. ldi ZH, high(Eep_Initialized_H)
  46. xcall read_eeprom_byte
  47. cpi Temp1, 0x5A
  48. breq PC+2
  49. rjmp read_eeprom_store_defaults
  50. .ENDIF
  51. .IF MODE == 1 ; Tail
  52. ; Check initialized signature
  53. ldi ZL, low(Eep_Initialized_L)
  54. ldi ZH, high(Eep_Initialized_L)
  55. xcall read_eeprom_byte
  56. cpi Temp1, 0x5A
  57. breq PC+2
  58. rjmp read_eeprom_store_defaults
  59. ldi ZL, low(Eep_Initialized_H)
  60. ldi ZH, high(Eep_Initialized_H)
  61. xcall read_eeprom_byte
  62. cpi Temp1, 0xA5
  63. breq PC+2
  64. rjmp read_eeprom_store_defaults
  65. .ENDIF
  66. .IF MODE == 2 ; Multi
  67. ; Check initialized signature
  68. ldi ZL, low(Eep_Initialized_L)
  69. ldi ZH, high(Eep_Initialized_L)
  70. xcall read_eeprom_byte
  71. cpi Temp1, 0x55
  72. breq PC+2
  73. rjmp read_eeprom_store_defaults
  74. ldi ZL, low(Eep_Initialized_H)
  75. ldi ZH, high(Eep_Initialized_H)
  76. xcall read_eeprom_byte
  77. cpi Temp1, 0xAA
  78. breq PC+2
  79. rjmp read_eeprom_store_defaults
  80. .ENDIF
  81. ; Read eeprom
  82. ldi Temp4, 10 ; 10 parameters
  83. ldi XL, low(Pgm_Gov_P_Gain)
  84. ldi XH, high(Pgm_Gov_P_Gain)
  85. .IF MODE == 0 || MODE ==2
  86. ldi ZL, low(Eep_Pgm_Gov_P_Gain)
  87. ldi ZH, high(Eep_Pgm_Gov_P_Gain)
  88. .ENDIF
  89. .IF MODE == 1
  90. ldi ZL, low(_Eep_Pgm_Gov_P_Gain)
  91. ldi ZH, high(_Eep_Pgm_Gov_P_Gain)
  92. .ENDIF
  93. read_eeprom_block1:
  94. xcall read_eeprom_byte
  95. st X+, Temp1
  96. adiw Z, 1
  97. dec Temp4
  98. brne read_eeprom_block1
  99. ldi Temp4, 24 ; 24 parameters
  100. ldi XL, low(Pgm_Enable_TX_Program)
  101. ldi XH, high(Pgm_Enable_TX_Program)
  102. ldi ZL, low(Eep_Enable_TX_Program)
  103. ldi ZH, high(Eep_Enable_TX_Program)
  104. read_eeprom_block2:
  105. xcall read_eeprom_byte
  106. st X+, Temp1
  107. adiw Z, 1
  108. dec Temp4
  109. brne read_eeprom_block2
  110. rjmp read_eeprom_exit
  111. read_eeprom_store_defaults:
  112. xcall set_default_parameters
  113. xcall store_all_in_eeprom
  114. read_eeprom_exit:
  115. ldi ZL, low(Eep_Dummy) ; Set pointer to uncritical area
  116. ldi ZH, high(Eep_Dummy)
  117. ret
  118. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  119. ;
  120. ; Read eeprom byte routine
  121. ;
  122. ; Assumes data in Temp1 and address in Z
  123. ;
  124. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  125. read_eeprom_byte:
  126. ; Wait for completion of previous write
  127. Check_Eeprom_Ready
  128. rjmp read_eeprom_byte
  129. ; Set up address in address register
  130. Set_Eeprom_Address ZL, ZH
  131. ; Start eeprom read
  132. sbi EECR, EERE
  133. in Temp1, EEDR
  134. ret
  135. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  136. ;
  137. ; Store all parameter values in EEPROM routine
  138. ;
  139. ; Assumes interrupts to be off
  140. ;
  141. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  142. store_all_in_eeprom:
  143. ; Clear initialized signature
  144. xcall clear_eeprom_signature
  145. ; Write eeprom
  146. ldi Temp4, 10 ; 10 parameters
  147. ldi XL, low(Pgm_Gov_P_Gain)
  148. ldi XH, high(Pgm_Gov_P_Gain)
  149. .IF MODE == 0 || MODE ==2
  150. ldi ZL, low(Eep_Pgm_Gov_P_Gain)
  151. ldi ZH, high(Eep_Pgm_Gov_P_Gain)
  152. .ENDIF
  153. .IF MODE == 1
  154. ldi ZL, low(_Eep_Pgm_Gov_P_Gain)
  155. ldi ZH, high(_Eep_Pgm_Gov_P_Gain)
  156. .ENDIF
  157. write_eeprom_block1:
  158. ld Temp1, X+
  159. xcall write_eeprom_byte
  160. adiw Z, 1
  161. dec Temp4
  162. brne write_eeprom_block1
  163. ldi Temp4, 24 ; 24 parameters
  164. ldi XL, low(Pgm_Enable_TX_Program)
  165. ldi XH, high(Pgm_Enable_TX_Program)
  166. ldi ZL, low(Eep_Enable_TX_Program)
  167. ldi ZH, high(Eep_Enable_TX_Program)
  168. write_eeprom_block2:
  169. ld Temp1, X+
  170. xcall write_eeprom_byte
  171. adiw Z, 1
  172. dec Temp4
  173. brne write_eeprom_block2
  174. ; Write initialized signature
  175. xcall write_eeprom_signature
  176. ret
  177. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  178. ;
  179. ; Write eeprom byte routine
  180. ;
  181. ; Assumes data in Temp1 and address in Z
  182. ;
  183. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  184. write_eeprom_byte:
  185. ; Wait for completion of previous write
  186. Check_Eeprom_Ready
  187. rjmp write_eeprom_byte
  188. ; Set up address in address register
  189. out EEDR, Temp1
  190. Set_Eeprom_Address ZL, ZH
  191. ; Set write enables and start writing
  192. Start_Eeprom_Write
  193. ret
  194. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  195. ;
  196. ; Clear eeprom signature routine
  197. ;
  198. ; No assumptions
  199. ;
  200. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  201. clear_eeprom_signature:
  202. ldi ZL, low(Eep_Initialized_L)
  203. ldi ZH, high(Eep_Initialized_L)
  204. ldi Temp1, 0xFF
  205. xcall write_eeprom_byte
  206. ldi ZL, low(Eep_Initialized_H)
  207. ldi ZH, high(Eep_Initialized_H)
  208. ldi Temp1, 0xFF
  209. xcall write_eeprom_byte
  210. ret
  211. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  212. ;
  213. ; Write eeprom signature routine
  214. ;
  215. ; No assumptions
  216. ;
  217. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  218. write_eeprom_signature:
  219. .IF MODE == 0 ; Main
  220. ldi ZL, low(Eep_Initialized_L)
  221. ldi ZH, high(Eep_Initialized_L)
  222. ldi Temp1, 0xA5
  223. xcall write_eeprom_byte
  224. ldi ZL, low(Eep_Initialized_H)
  225. ldi ZH, high(Eep_Initialized_H)
  226. ldi Temp1, 0x5A
  227. xcall write_eeprom_byte
  228. .ENDIF
  229. .IF MODE == 1 ; Tail
  230. ldi ZL, low(Eep_Initialized_L)
  231. ldi ZH, high(Eep_Initialized_L)
  232. ldi Temp1, 0x5A
  233. xcall write_eeprom_byte
  234. ldi ZL, low(Eep_Initialized_H)
  235. ldi ZH, high(Eep_Initialized_H)
  236. ldi Temp1, 0xA5
  237. xcall write_eeprom_byte
  238. .ENDIF
  239. .IF MODE == 2 ; Multi
  240. ldi ZL, low(Eep_Initialized_L)
  241. ldi ZH, high(Eep_Initialized_L)
  242. ldi Temp1, 0x55
  243. xcall write_eeprom_byte
  244. ldi ZL, low(Eep_Initialized_H)
  245. ldi ZH, high(Eep_Initialized_H)
  246. ldi Temp1, 0xAA
  247. xcall write_eeprom_byte
  248. .ENDIF
  249. ret
  250. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  251. ;
  252. ; Store new parameter value in ram routine
  253. ;
  254. ; No assumptions
  255. ;
  256. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  257. store_new_value_in_ram:
  258. lds Temp4, Tx_Pgm_Func_No ; Function no
  259. lds Temp1, Tx_Pgm_Paraval_No ; Parameter value no
  260. .IF MODE == 0
  261. cpi Temp4, 1
  262. brne PC+3
  263. sts Pgm_Gov_P_Gain, Temp1
  264. cpi Temp4, 2
  265. brne PC+3
  266. sts Pgm_Gov_I_Gain, Temp1
  267. cpi Temp4, 3
  268. brne PC+3
  269. sts Pgm_Gov_Mode, Temp1
  270. cpi Temp4, 4
  271. brne PC+3
  272. sts Pgm_Gov_Range, Temp1
  273. cpi Temp4, 5
  274. brne PC+3
  275. sts Pgm_Low_Voltage_Lim, Temp1
  276. cpi Temp4, 6
  277. brne PC+3
  278. sts Pgm_Startup_Pwr, Temp1
  279. cpi Temp4, 7
  280. brne PC+3
  281. sts Pgm_Comm_Timing, Temp1
  282. cpi Temp4, 8
  283. brne PC+3
  284. sts Pgm_Pwm_Freq, Temp1
  285. cpi Temp4, 9
  286. brne PC+3
  287. sts Pgm_Demag_Comp, Temp1
  288. cpi Temp4, 10
  289. brne PC+3
  290. sts Pgm_Direction, Temp1
  291. cpi Temp4, 11
  292. brne PC+3
  293. sts Pgm_Input_Pol, Temp1
  294. .ENDIF
  295. .IF MODE == 1
  296. cpi Temp4, 1
  297. brne PC+3
  298. sts Pgm_Motor_Gain, Temp1
  299. cpi Temp4, 2
  300. brne PC+3
  301. sts Pgm_Motor_Idle, Temp1
  302. cpi Temp4, 3
  303. brne PC+3
  304. sts Pgm_Startup_Pwr, Temp1
  305. cpi Temp4, 4
  306. brne PC+3
  307. sts Pgm_Comm_Timing, Temp1
  308. cpi Temp4, 5
  309. brne PC+3
  310. sts Pgm_Pwm_Freq, Temp1
  311. cpi Temp4, 6
  312. brne PC+3
  313. sts Pgm_Pwm_Dither, Temp1
  314. cpi Temp4, 7
  315. brne PC+3
  316. sts Pgm_Demag_Comp, Temp1
  317. cpi Temp4, 8
  318. brne PC+3
  319. sts Pgm_Direction, Temp1
  320. cpi Temp4, 9
  321. brne PC+3
  322. sts Pgm_Input_Pol, Temp1
  323. .ENDIF
  324. .IF MODE == 2
  325. cpi Temp4, 1
  326. brne PC+3
  327. sts Pgm_Gov_P_Gain, Temp1
  328. cpi Temp4, 2
  329. brne PC+3
  330. sts Pgm_Gov_I_Gain, Temp1
  331. cpi Temp4, 3
  332. brne PC+3
  333. sts Pgm_Gov_Mode, Temp1
  334. cpi Temp4, 4
  335. brne PC+3
  336. sts Pgm_Motor_Gain, Temp1
  337. cpi Temp4, 5
  338. brne PC+3
  339. sts Pgm_Startup_Pwr, Temp1
  340. cpi Temp4, 6
  341. brne PC+3
  342. sts Pgm_Comm_Timing, Temp1
  343. cpi Temp4, 7
  344. brne PC+3
  345. sts Pgm_Pwm_Freq, Temp1
  346. cpi Temp4, 8
  347. brne PC+3
  348. sts Pgm_Pwm_Dither, Temp1
  349. cpi Temp4, 9
  350. brne PC+3
  351. sts Pgm_Demag_Comp, Temp1
  352. cpi Temp4, 10
  353. brne PC+3
  354. sts Pgm_Direction, Temp1
  355. cpi Temp4, 11
  356. brne PC+3
  357. sts Pgm_Input_Pol, Temp1
  358. .ENDIF
  359. ret
  360. ;**;**** **** **** **** **** **** **** **** **** **** **** **** ****
  361. ;
  362. ; Wait 1 second routine
  363. ;
  364. ; No assumptions
  365. ;
  366. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  367. wait1s:
  368. ldi Temp3, 5
  369. wait1s_loop:
  370. xcall wait200ms
  371. dec Temp3
  372. brne wait1s_loop
  373. ret
  374. ;**;**** **** **** **** **** **** **** **** **** **** **** **** ****
  375. ;
  376. ; Success beep routine
  377. ;
  378. ; Assumes interrupts to be off
  379. ;
  380. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  381. success_beep:
  382. xcall beep_f1
  383. xcall beep_f2
  384. xcall beep_f3
  385. xcall beep_f4
  386. xcall wait10ms
  387. xcall beep_f1
  388. xcall beep_f2
  389. xcall beep_f3
  390. xcall beep_f4
  391. xcall wait10ms
  392. xcall beep_f1
  393. xcall beep_f2
  394. xcall beep_f3
  395. xcall beep_f4
  396. ret
  397. ;**;**** **** **** **** **** **** **** **** **** **** **** **** ****
  398. ;
  399. ; Success beep inverted routine
  400. ;
  401. ; Assumes interrupts to be off
  402. ;
  403. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  404. success_beep_inverted:
  405. xcall beep_f4
  406. xcall beep_f3
  407. xcall beep_f2
  408. xcall beep_f1
  409. xcall wait10ms
  410. xcall beep_f4
  411. xcall beep_f3
  412. xcall beep_f2
  413. xcall beep_f1
  414. xcall wait10ms
  415. xcall beep_f4
  416. xcall beep_f3
  417. xcall beep_f2
  418. xcall beep_f1
  419. ret
  420. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  421. ;
  422. ; Function and parameter value beep routine
  423. ;
  424. ; No assumptions
  425. ;
  426. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  427. function_paraval_beep:
  428. lds Temp7, Tx_Pgm_Func_No ; Function no
  429. lds Temp8, Tx_Pgm_Paraval_No ; Parameter value no
  430. function_beep:
  431. xcall beep_f1
  432. xcall beep_f1
  433. xcall beep_f1
  434. xcall wait10ms
  435. dec Temp7
  436. brne function_beep
  437. paraval_beep:
  438. xcall beep_f4
  439. xcall wait10ms
  440. dec Temp8
  441. brne paraval_beep
  442. ret
  443. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  444. ;
  445. ; Program by TX routine
  446. ;
  447. ; No assumptions
  448. ;
  449. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  450. program_by_tx:
  451. ; Programming mode entry beeps
  452. cli
  453. xcall success_beep
  454. sei
  455. xcall wait1s
  456. xcall wait1s
  457. ; Start at function 1, parameter value 1
  458. ldi XH, 1
  459. sts Tx_Pgm_Func_No, XH
  460. paraval_no_entry:
  461. ldi XH, 1
  462. sts Tx_Pgm_Paraval_No, XH
  463. beep_no_entry:
  464. sts Tx_Pgm_Beep_No, Zero
  465. func_paraval:
  466. cli
  467. xcall function_paraval_beep
  468. sei
  469. ldi XH, 5 ; Wait is 5x 200ms
  470. mov Temp5, XH
  471. func_paraval_wait:
  472. lds Temp6, New_Rcp ; Load RC pulse
  473. xcall wait200ms
  474. lds XH, New_Rcp
  475. cp Temp6, XH ; Is RC pulse stable? (Avoid issues from 3in1 interference)
  476. brne func_paraval_wait ; No - branch
  477. cpi XH, RCP_STOP ; Below stop?
  478. brcs func_paraval_store ; Yes - branch
  479. cpi XH, RCP_MAX ; Below max?
  480. brcs function_next ; Yes - branch
  481. rjmp func_paraval_cont_wait ; No - branch
  482. func_paraval_store:
  483. xcall store_new_value_in_ram ; Yes - store new value in RAM
  484. cli ; Disable all interrupts
  485. xcall store_all_in_eeprom ; Store all values in EEPROM
  486. xcall success_beep ; Beep success
  487. Enable_Watchdog XH ; Generate hardware reset from watchdog
  488. xcall wait1s
  489. func_paraval_cont_wait:
  490. dec Temp5
  491. brne func_paraval_wait
  492. lds XH, Tx_Pgm_Beep_No ; Check number of beeps
  493. inc XH
  494. sts Tx_Pgm_Beep_No, XH
  495. cpi XH, 3 ; Three beeps done?
  496. brcc paraval_next ; Yes - Next parameter value
  497. rjmp func_paraval ; No - go back
  498. paraval_next:
  499. xcall wait1s
  500. lds XH, Tx_Pgm_Paraval_No ; Parameter value no
  501. inc XH
  502. sts Tx_Pgm_Paraval_No, XH
  503. cli
  504. .IF MODE == 0
  505. lds XH, Tx_Pgm_Func_No ; Decode number of parameters
  506. dec XH
  507. ldi ZL, low(TX_PGM_PARAMS_MAIN<<1)
  508. ldi ZH, high(TX_PGM_PARAMS_MAIN<<1)
  509. add ZL, XH
  510. adc ZH, Zero
  511. lpm Temp1, Z
  512. .ENDIF
  513. .IF MODE == 1
  514. lds XH, Tx_Pgm_Func_No ; Decode number of parameters
  515. dec XH
  516. ldi ZL, low(TX_PGM_PARAMS_TAIL<<1)
  517. ldi ZH, high(TX_PGM_PARAMS_TAIL<<1)
  518. add ZL, XH
  519. adc ZH, Zero
  520. lpm Temp1, Z
  521. .ENDIF
  522. .IF MODE == 2
  523. lds XH, Tx_Pgm_Func_No ; Decode number of parameters
  524. dec XH
  525. ldi ZL, low(TX_PGM_PARAMS_MULTI<<1)
  526. ldi ZH, high(TX_PGM_PARAMS_MULTI<<1)
  527. add ZL, XH
  528. adc ZH, Zero
  529. lpm Temp1, Z
  530. .ENDIF
  531. sei
  532. inc Temp1
  533. lds XH, Tx_Pgm_Paraval_No
  534. cp XH, Temp1
  535. brcc function_next ; Last parameter value?
  536. rjmp beep_no_entry ; No - go back
  537. function_next: ; Yes - Next function value
  538. xcall wait1s
  539. xcall wait1s
  540. lds XH, Tx_Pgm_Func_No ; Function value no
  541. inc XH
  542. sts Tx_Pgm_Func_No, XH
  543. .IF MODE == 0
  544. lds XH, Tx_Pgm_Func_No
  545. cpi XH, 12 ; Main has 11 functions
  546. .ENDIF
  547. .IF MODE == 1
  548. lds XH, Tx_Pgm_Func_No
  549. cpi XH, 10 ; Tail has 9 functions
  550. .ENDIF
  551. .IF MODE == 2
  552. lds XH, Tx_Pgm_Func_No
  553. cpi XH, 12 ; Multi has 11 functions
  554. .ENDIF
  555. brcc program_by_tx_exit ; Last function value?
  556. rjmp paraval_no_entry ; No - go back
  557. program_by_tx_exit:
  558. xcall set_default_parameters ; Load all defaults
  559. cli ; Disable all interrupts
  560. xcall store_all_in_eeprom ; Erase flash and program
  561. Enable_Watchdog XH ; Generate hardware reset from watchdog
  562. xcall wait1s