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.

683 lines
16 KiB

  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 SiLabs
  26. ;
  27. ; EEPROM is not available in SiLabs MCUs
  28. ; Therefore a segment of the flash is used as "EEPROM"
  29. ;
  30. ;**** **** **** **** ****
  31. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  32. ;
  33. ; Read all eeprom parameters routine
  34. ;
  35. ; No assumptions
  36. ;
  37. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  38. read_all_eeprom_parameters:
  39. ; Check initialized signature
  40. mov DPTR, #Eep_Initialized_L
  41. mov Temp1, #Bit_Access
  42. call read_eeprom_byte
  43. mov A, Bit_Access
  44. IF MODE == 0
  45. cjne A, #0A5h, read_eeprom_store_defaults
  46. ENDIF
  47. IF MODE == 1
  48. cjne A, #05Ah, read_eeprom_store_defaults
  49. ENDIF
  50. IF MODE == 2
  51. cjne A, #055h, read_eeprom_store_defaults
  52. ENDIF
  53. inc DPTR ; Now Eep_Initialized_H
  54. call read_eeprom_byte
  55. mov A, Bit_Access
  56. IF MODE == 0
  57. cjne A, #05Ah, read_eeprom_store_defaults
  58. ENDIF
  59. IF MODE == 1
  60. cjne A, #0A5h, read_eeprom_store_defaults
  61. ENDIF
  62. IF MODE == 2
  63. cjne A, #0AAh, read_eeprom_store_defaults
  64. ENDIF
  65. jmp read_eeprom_read
  66. read_eeprom_store_defaults:
  67. call set_default_parameters
  68. call erase_and_store_all_in_eeprom
  69. jmp read_eeprom_exit
  70. read_eeprom_read:
  71. ; Read eeprom
  72. IF MODE == 0 ;Main
  73. mov DPTR, #Eep_Pgm_Gov_P_Gain
  74. ENDIF
  75. IF MODE == 1 ; Tail
  76. mov DPTR, #_Eep_Pgm_Gov_P_Gain
  77. ENDIF
  78. mov Temp1, #Pgm_Gov_P_Gain
  79. mov Temp4, #10
  80. IF MODE == 2 ; multi
  81. mov DPTR, #Eep_Pgm_Fir_Key
  82. mov Temp1, #Pgm_Fir_Key
  83. mov Temp4, #12
  84. ENDIF
  85. read_eeprom_block1:
  86. call read_eeprom_byte
  87. inc DPTR
  88. inc Temp1
  89. djnz Temp4, read_eeprom_block1
  90. mov DPTR, #Eep_Enable_TX_Program
  91. mov Temp1, #Pgm_Enable_TX_Program
  92. mov Temp4, #21 ; 21 parameters
  93. read_eeprom_block2:
  94. call read_eeprom_byte
  95. inc DPTR
  96. inc Temp1
  97. djnz Temp4, read_eeprom_block2
  98. mov DPTR, #Eep_Dummy ; Set pointer to uncritical area
  99. read_eeprom_exit:
  100. ret
  101. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  102. ;
  103. ; Erase flash and store all parameter value in EEPROM routine
  104. ;
  105. ; No assumptions
  106. ;
  107. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  108. erase_and_store_all_in_eeprom:
  109. clr EA ; Disable interrupts
  110. ; call read_tags ;2015-02-11
  111. call erase_flash ; Erase flash
  112. mov DPTR, #Eep_FW_Main_Revision ; Store firmware main revision
  113. mov A, #EEPROM_FW_MAIN_REVISION
  114. call write_eeprom_byte_from_acc
  115. inc DPTR ; Now firmware sub revision
  116. mov A, #EEPROM_FW_SUB_REVISION
  117. call write_eeprom_byte_from_acc
  118. inc DPTR ; Now layout revision
  119. mov A, #EEPROM_LAYOUT_REVISION
  120. call write_eeprom_byte_from_acc
  121. ; Write eeprom
  122. IF MODE == 0 ;Main
  123. mov DPTR, #Eep_Pgm_Gov_P_Gain
  124. ENDIF
  125. IF MODE == 1 ; Tail
  126. mov DPTR, #_Eep_Pgm_Gov_P_Gain
  127. ENDIF
  128. mov Temp1, #Pgm_Gov_P_Gain
  129. mov Temp4, #10
  130. IF MODE == 2 ;multi
  131. mov DPTR, #Eep_Pgm_Fir_Key
  132. mov Temp1, #Pgm_Fir_Key
  133. mov Temp4, #12
  134. ENDIF
  135. write_eeprom_block1:
  136. call write_eeprom_byte
  137. inc DPTR
  138. inc Temp1
  139. djnz Temp4, write_eeprom_block1
  140. mov DPTR, #Eep_Enable_TX_Program
  141. mov Temp1, #Pgm_Enable_TX_Program
  142. mov Temp4, #21 ; 21 parameters
  143. write_eeprom_block2:
  144. call write_eeprom_byte
  145. inc DPTR
  146. inc Temp1
  147. djnz Temp4, write_eeprom_block2
  148. ; call write_tags ;2015-02-11
  149. call write_eeprom_signature
  150. mov DPTR, #Eep_Dummy ; Set pointer to uncritical area
  151. ret
  152. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  153. CSEG AT 1C00h ; Last code segment. Take care that there is enough space!
  154. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  155. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  156. ;
  157. ; Read eeprom byte routine
  158. ;
  159. ; Gives data in A and in address given by Temp1. Assumes address in DPTR
  160. ; Also assumes address high byte to be zero
  161. ;
  162. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  163. read_eeprom_byte:
  164. clr A
  165. movc A, @A+DPTR ; Read from flash
  166. mov @Temp1, A
  167. ret
  168. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  169. ;
  170. ; Write eeprom byte routine
  171. ;
  172. ; Assumes data in address given by Temp1, or in accumulator. Assumes address in DPTR
  173. ; Also assumes address high byte to be zero
  174. ;
  175. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  176. write_eeprom_byte:
  177. mov A, @Temp1
  178. write_eeprom_byte_from_acc:
  179. orl PSCTL, #01h ; Set the PSWE bit
  180. anl PSCTL, #0FDh ; Clear the PSEE bit
  181. mov RSTSRC, #02h ; Set VDD monitor as a reset source (PORSF)
  182. mov FLKEY, #0A5h ; First key code
  183. mov FLKEY, #0F1h ; Second key code
  184. movx @DPTR, A ; Write to flash
  185. anl PSCTL, #0FEh ; Clear the PSWE bit
  186. ret
  187. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  188. ;
  189. ; Erase flash routine (erases the flash segment used for "eeprom" variables)
  190. ;
  191. ; No assumptions
  192. ;
  193. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  194. erase_flash:
  195. orl PSCTL, #02h ; Set the PSEE bit
  196. orl PSCTL, #01h ; Set the PSWE bit
  197. mov RSTSRC, #02h ; Set VDD monitor as a reset source (PORSF)
  198. mov FLKEY, #0A5h ; First key code
  199. mov FLKEY, #0F1h ; Second key code
  200. mov DPTR, #Eep_Initialized_L
  201. movx @DPTR, A
  202. anl PSCTL, #0FCh ; Clear the PSEE and PSWE bits
  203. ret
  204. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  205. ;
  206. ; Write eeprom signature routine
  207. ;
  208. ; No assumptions
  209. ;
  210. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  211. write_eeprom_signature:
  212. IF MODE == 0
  213. mov DPTR, #Eep_Initialized_L
  214. mov A, #0A5h
  215. call write_eeprom_byte_from_acc
  216. mov DPTR, #Eep_Initialized_H
  217. mov A, #05Ah
  218. call write_eeprom_byte_from_acc
  219. ENDIF
  220. IF MODE == 1
  221. mov DPTR, #Eep_Initialized_L
  222. mov A, #05Ah
  223. call write_eeprom_byte_from_acc
  224. mov DPTR, #Eep_Initialized_H
  225. mov A, #0A5h
  226. call write_eeprom_byte_from_acc
  227. ENDIF
  228. IF MODE == 2
  229. mov DPTR, #Eep_Initialized_L
  230. mov A, #055h
  231. call write_eeprom_byte_from_acc
  232. mov DPTR, #Eep_Initialized_H
  233. mov A, #0AAh
  234. call write_eeprom_byte_from_acc
  235. ENDIF
  236. ret
  237. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  238. ;
  239. ; Read all tags from flash and store in temporary storage
  240. ;
  241. ; No assumptions
  242. ;
  243. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  244. read_tags:
  245. mov Temp3, #48 ; Number of tags
  246. mov Temp2, #Tag_Temporary_Storage ; Set RAM address
  247. mov Temp1, #Bit_Access
  248. mov DPTR, #Eep_ESC_Layout ; Set flash address
  249. read_tag:
  250. call read_eeprom_byte
  251. mov A, Bit_Access
  252. mov @Temp2, A ; Write to RAM
  253. inc Temp2
  254. inc DPTR
  255. djnz Temp3, read_tag
  256. ret
  257. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  258. ;
  259. ; Write all tags from temporary storage and store in flash
  260. ;
  261. ; No assumptions
  262. ;
  263. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  264. write_tags:
  265. mov Temp3, #48 ; Number of tags
  266. mov Temp2, #Tag_Temporary_Storage ; Set RAM address
  267. mov DPTR, #Eep_ESC_Layout ; Set flash address
  268. write_tag:
  269. mov A, @Temp2 ; Read from RAM
  270. call write_eeprom_byte_from_acc
  271. inc Temp2
  272. inc DPTR
  273. djnz Temp3, write_tag
  274. ret
  275. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  276. ;
  277. ; Store new parameter value in ram routine
  278. ;
  279. ; No assumptions
  280. ;
  281. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  282. store_new_value_in_ram:
  283. mov Temp4, Tx_Pgm_Func_No ; Function no
  284. mov Temp1, Tx_Pgm_Paraval_No ; Parameter value no
  285. IF MODE == 0
  286. store_main_func_1:
  287. cjne Temp4, #1, store_main_func_2
  288. mov Temp2, #Pgm_Gov_P_Gain
  289. store_main_func_2:
  290. cjne Temp4, #2, store_main_func_3
  291. mov Temp2, #Pgm_Gov_I_Gain
  292. store_main_func_3:
  293. cjne Temp4, #3, store_main_func_4
  294. mov Temp2, #Pgm_Gov_Mode
  295. store_main_func_4:
  296. cjne Temp4, #4, store_main_func_5
  297. mov Temp2, #Pgm_Gov_Range
  298. store_main_func_5:
  299. cjne Temp4, #5, store_main_func_6
  300. mov Temp2, #Pgm_Low_Voltage_Lim
  301. store_main_func_6:
  302. cjne Temp4, #6, store_main_func_7
  303. mov Temp2, #Pgm_Startup_Pwr
  304. store_main_func_7:
  305. cjne Temp4, #7, store_main_func_8
  306. mov Temp2, #Pgm_Comm_Timing
  307. store_main_func_8:
  308. cjne Temp4, #8, store_main_func_9
  309. mov Temp2, #Pgm_Pwm_Freq
  310. store_main_func_9:
  311. cjne Temp4, #9, store_main_func_10
  312. mov Temp2, #Pgm_Demag_Comp
  313. store_main_func_10:
  314. cjne Temp4, #10, store_main_func_11
  315. mov Temp2, #Pgm_Direction
  316. store_main_func_11:
  317. cjne Temp4, #11, store_in_ram_exit
  318. mov Temp2, #Pgm_Input_Pol
  319. ENDIF
  320. IF MODE == 1
  321. store_tail_func_1:
  322. cjne Temp4, #1, store_tail_func_2
  323. mov Temp2, #Pgm_Motor_Gain
  324. store_tail_func_2:
  325. cjne Temp4, #2, store_tail_func_3
  326. mov Temp2, #Pgm_Motor_Idle
  327. store_tail_func_3:
  328. cjne Temp4, #3, store_tail_func_4
  329. mov Temp2, #Pgm_Startup_Pwr
  330. store_tail_func_4:
  331. cjne Temp4, #4, store_tail_func_5
  332. mov Temp2, #Pgm_Comm_Timing
  333. store_tail_func_5:
  334. cjne Temp4, #5, store_tail_func_6
  335. mov Temp2, #Pgm_Pwm_Freq
  336. store_tail_func_6:
  337. cjne Temp4, #6, store_tail_func_7
  338. mov Temp2, #Pgm_Demag_Comp
  339. store_tail_func_7:
  340. cjne Temp4, #7, store_tail_func_8
  341. mov Temp2, #Pgm_Direction
  342. store_tail_func_8:
  343. cjne Temp4, #8, store_in_ram_exit
  344. mov Temp2, #Pgm_Input_Pol
  345. ENDIF
  346. IF MODE == 2
  347. ;store_multi_func_1:
  348. ; cjne Temp4, #1, store_multi_func_2
  349. ; mov Temp2, #Pgm_Gov_P_Gain
  350. ;store_multi_func_2:
  351. ; cjne Temp4, #2, store_multi_func_3
  352. ; mov Temp2, #Pgm_Gov_I_Gain
  353. ;store_multi_func_3:
  354. ; cjne Temp4, #3, store_multi_func_4
  355. ; mov Temp2, #Pgm_Gov_Mode
  356. ;store_multi_func_4:
  357. ; cjne Temp4, #4, store_multi_func_5
  358. ; mov Temp2, #Pgm_Motor_Gain
  359. ;store_multi_func_5:
  360. ; cjne Temp4, #5, store_multi_func_6
  361. ; mov Temp2, #Pgm_Low_Voltage_Lim
  362. ;store_multi_func_6:
  363. ; cjne Temp4, #6, store_multi_func_7
  364. ; mov Temp2, #Pgm_Startup_Pwr
  365. ;store_multi_func_7:
  366. ; cjne Temp4, #7, store_multi_func_8
  367. ; mov Temp2, #Pgm_Comm_Timing
  368. ;store_multi_func_8:
  369. ; cjne Temp4, #8, store_multi_func_9
  370. ; mov Temp2, #Pgm_Pwm_Freq
  371. ;store_multi_func_9:
  372. ; cjne Temp4, #9, store_multi_func_10
  373. ; mov Temp2, #Pgm_Demag_Comp
  374. ;store_multi_func_10:
  375. ; cjne Temp4, #10, store_multi_func_11
  376. ; mov Temp2, #Pgm_Direction
  377. ;store_multi_func_11:
  378. ; cjne Temp4, #11, store_in_ram_exit
  379. ; mov Temp2, #Pgm_Input_Pol
  380. store_multi_func_1:
  381. cjne Temp4, #1, store_multi_func_2
  382. mov Temp2, #Pgm_Damping_Force ;Damping force()
  383. store_multi_func_2:
  384. cjne Temp4, #2, store_multi_func_3
  385. mov Temp2, #Pgm_Comm_Timing ;Commutation timing()
  386. ;store_multi_func_3:
  387. ; cjne Temp4, #3, store_multi_func_4
  388. ; mov Temp2, #Pgm_Direction_Rev ;Rotation direction()
  389. store_multi_func_3:
  390. cjne Temp4, #3, store_multi_func_4
  391. mov Temp2, #Pgm_Startup_Pwr ;Startup power()
  392. store_multi_func_4:
  393. cjne Temp4, #4, store_multi_func_5
  394. mov Temp2, #Pgm_Gov_Mode ;Closed loop mode(线)
  395. store_multi_func_5:
  396. cjne Temp4, #5, store_multi_func_6
  397. mov Temp2, #Pgm_Pwm_Freq ;Startup method()
  398. store_multi_func_6:
  399. cjne Temp4, #6, store_multi_func_7
  400. mov Temp2, #Pgm_Low_Voltage_Lim ;Low voltage limit()
  401. store_multi_func_7:
  402. cjne Temp4, #7, store_multi_func_8
  403. mov Temp2, #Pgm_Low_Voltage_Ctl ;Low voltage limit()
  404. store_multi_func_8:
  405. cjne Temp4, #8, store_in_ram_exit
  406. mov Temp2, #Pgm_Direction
  407. ENDIF
  408. store_in_ram_exit:
  409. mov A, Temp1
  410. mov @Temp2, A
  411. ret
  412. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  413. ;
  414. ; Program by TX routine
  415. ;
  416. ; No assumptions
  417. ;
  418. ;**** **** **** **** **** **** **** **** **** **** **** **** ****
  419. program_by_tx:
  420. ; Programming mode entry beeps
  421. call success_beep
  422. setb Flags1.PROGRAM_FUNC_FLAG ;
  423. wait_for_program:
  424. mov Temp5, #10
  425. default_set_check:
  426. call wait200ms
  427. clr C
  428. mov A, New_Rcp ; Load new RC pulse value
  429. subb A, #RCP_STOP ; Below stop?
  430. jc wait_for_next_status
  431. clr C
  432. mov A, New_Rcp ; Load new RC pulse value
  433. subb A, #RCP_MAX ; Below max?
  434. jc wait_for_program
  435. djnz Temp5, default_set_check ;
  436. ajmp function_no_entry
  437. wait_for_next_status:
  438. mov Temp5, #10
  439. wait_for_exit:
  440. call wait200ms
  441. clr C
  442. mov A, New_Rcp ; Load new RC pulse value
  443. subb A, #RCP_STOP ; Below stop?
  444. jnc ($+7)
  445. djnz Temp5, wait_for_exit
  446. ajmp program_exit
  447. clr C
  448. mov A, New_Rcp ; Load new RC pulse value
  449. subb A, #RCP_MAX ; Below max?
  450. jc wait_for_next_status
  451. call set_default_parameters ; Load all defaults
  452. call erase_and_store_all_in_eeprom ; Erase flash and program
  453. call success_beep_inverted
  454. ajmp wait_for_program
  455. ; Start at function 1, parameter value 1
  456. function_no_entry:
  457. mov Tx_Pgm_Func_No, #1
  458. function_no_beep:
  459. call function_paraval_beep
  460. mov Temp5, #10 ; Wait is 5x 200ms
  461. function_no_wait:
  462. mov Temp6, New_Rcp
  463. call wait200ms
  464. clr C
  465. mov A, Temp6
  466. subb A, New_Rcp
  467. jnz function_no_wait ; No - branch
  468. clr C
  469. mov A, New_Rcp ; Load new RC pulse value
  470. subb A, #RCP_STOP ; Below stop?
  471. jc paraval_no_wait
  472. clr C
  473. mov A, New_Rcp ; Load new RC pulse value
  474. subb A, #RCP_MAX ; Below max?
  475. jnc function_no_next
  476. ajmp function_no_wait ;
  477. function_no_next: ; Yes - Next function value
  478. jnb Flags1.PROGRAM_FUNC_FLAG, func_paraval_store ;=0
  479. djnz Temp5,function_no_wait
  480. inc Tx_Pgm_Func_No ; Function value no +1
  481. clr C
  482. mov A,Tx_Pgm_Func_No
  483. IF MODE == 0
  484. subb A,#12
  485. ENDIF
  486. IF MODE >= 1
  487. subb A,#9
  488. ENDIF
  489. jc function_no_beep
  490. mov Temp5, #10
  491. func_over_wait:
  492. call wait200ms
  493. clr C
  494. mov A, New_Rcp
  495. subb A, #RCP_STOP
  496. jc program_by_tx_exit ;below stop---exit
  497. clr C
  498. mov A, New_Rcp
  499. subb A, #RCP_MAX
  500. jc func_over_wait ;
  501. djnz Temp5, func_over_wait ; 2s结束
  502. ajmp function_no_entry ;
  503. func_paraval_store:
  504. setb Flags1.PROGRAM_FUNC_FLAG ;
  505. call store_new_value_in_ram ; Yes - store new value in RAM
  506. call erase_and_store_all_in_eeprom ; Store all values in EEPROM
  507. call success_beep_inverted ; Beep success
  508. mov Temp5, #10 ;
  509. store_wait: ;
  510. call wait200ms
  511. clr C
  512. mov A, New_Rcp
  513. subb A, #RCP_STOP
  514. jc program_by_tx_exit ;
  515. clr C
  516. mov A, New_Rcp ; Load new RC pulse value
  517. subb A, #RCP_MAX ; Below max?
  518. jc store_wait
  519. djnz Temp5, store_wait ;2s结束
  520. mov Temp5, #1
  521. ajmp function_no_next
  522. paraval_no_wait:
  523. mov Temp5, #10 ;10*200=1s时间
  524. paraval_wait:
  525. call wait200ms
  526. clr C
  527. mov A, New_Rcp ; Load new RC pulse value
  528. subb A, #RCP_MAX ; Below max?
  529. jnc function_no_wait
  530. clr C
  531. mov A, New_Rcp
  532. subb A, #RCP_STOP ; Below stop?
  533. jnc paraval_wait ;
  534. djnz Temp5,paraval_wait ; 1s结束
  535. clr Flags1.PROGRAM_FUNC_FLAG ;
  536. paraval_no_entry:
  537. mov Tx_Pgm_Paraval_No, #1
  538. paraval_no_beep:
  539. call function_paraval_beep
  540. mov Temp5, #10
  541. paraval_no_next:
  542. call wait200ms
  543. clr C
  544. mov A, New_Rcp ; Load new RC pulse value
  545. subb A, #RCP_MAX ; Below max?
  546. jnc function_no_next ; Yes - branch
  547. clr C
  548. mov A, New_Rcp
  549. subb A, #RCP_STOP ; Below stop?
  550. jnc paraval_no_next ;
  551. djnz Temp5, paraval_no_next ; 1s结束
  552. inc Tx_Pgm_Paraval_No ; Function value no +1
  553. mov A, Tx_Pgm_Func_No ; Decode number of parameters
  554. dec A
  555. IF MODE == 0
  556. mov DPTR, #TX_PGM_PARAMS_MAIN
  557. ENDIF
  558. IF MODE == 1
  559. mov DPTR, #TX_PGM_PARAMS_TAIL
  560. ENDIF
  561. IF MODE == 2
  562. mov DPTR, #TX_PGM_PARAMS_MULTI
  563. ENDIF
  564. movc A, @A+DPTR
  565. mov Temp1, A
  566. inc Temp1
  567. clr C
  568. mov A, Tx_Pgm_Paraval_No
  569. subb A, Temp1
  570. jnc paraval_no_entry ; Last parameter value?
  571. ajmp paraval_no_beep ; No - go back
  572. program_by_tx_exit:
  573. mov Temp5, #10 ;10*200=2s时间
  574. exit_wait:
  575. call wait200ms
  576. clr C
  577. mov A, New_Rcp
  578. subb A, #RCP_STOP ; Below stop?
  579. jnc exit_wait ;退
  580. djnz Temp5,exit_wait
  581. program_exit:
  582. ret