From 75be5d1682ba6b7ee314230f63d7018b8ebdf743 Mon Sep 17 00:00:00 2001 From: Nick-HZ Date: Wed, 4 Feb 2015 21:31:06 +0800 Subject: [PATCH] Create EMAX_BLHeli_V1.1.a51 Added support for OneShot125 --- .../EMAX/EMAX_BLHeli_V1.1.a51 | 13753 ++++++++++++++++ 1 file changed, 13753 insertions(+) create mode 100644 Vendor specific versions/EMAX/EMAX_BLHeli_V1.1.a51 diff --git a/Vendor specific versions/EMAX/EMAX_BLHeli_V1.1.a51 b/Vendor specific versions/EMAX/EMAX_BLHeli_V1.1.a51 new file mode 100644 index 00000000..631d45f6 --- /dev/null +++ b/Vendor specific versions/EMAX/EMAX_BLHeli_V1.1.a51 @@ -0,0 +1,13753 @@ +$NOMOD51 +;**** **** **** **** **** +; +; BLHeli program for controlling brushless motors in helicopters +; +; Copyright 2011, 2012 Steffen Skaug +; This program is distributed under the terms of the GNU General Public License +; +; This file is part of BLHeli. +; +; BLHeli is free software: you can redistribute it and/or modify +; it under the terms of the GNU General Public License as published by +; the Free Software Foundation, either version 3 of the License, or +; (at your option) any later version. +; +; BLHeli is distributed in the hope that it will be useful, +; but WITHOUT ANY WARRANTY; without even the implied warranty of +; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +; GNU General Public License for more details. +; +; You should have received a copy of the GNU General Public License +; along with BLHeli. If not, see . +; +;**** **** **** **** **** +; +; The software was initially designed for use with Eflite mCP X, but is now adapted to a copters/planes in general +; +; The software was inspired by and started from from Bernard Konze's BLMC: http://home.versanet.de/~bkonze/blc_6a/blc_6a.htm +; And also Simon Kirby's TGY: https://github.com/sim-/tgy +; +; This file is best viewed with tab width set to 5 +; +; The input signal can be positive 1kHz, 2kHz, 4kHz or 8kHz PWM (taken from the "resistor tap" on mCPx) +; And the input signal can be PPM (1-2ms) at rates up to several hundred Hz. +; The code adapts itself to the various input modes/frequencies +; The code ESC can also be programmed to accept inverted input signal. +; +; The first lines of the software must be modified according to the chosen environment: +; Uncomment the selected ESC and main/tail/multi mode +; BESC EQU "ESC"_"mode" +; +;**** **** **** **** **** +; Revision history: +; - Rev1.0: Initial revision based upon BLHeli for AVR controllers +; - Rev2.0: Changed "Eeprom" initialization, layout and defaults +; Various changes and improvements to comparator reading. Now using timer1 for time from pwm on/off +; Beeps are made louder +; Added programmable low voltage limit +; Added programmable damped tail mode (only for 1S ESCs) +; Added programmable motor rotation direction +; - Rev2.1: (minor changes by 4712) +; Added Disable TX Programming by PC Setup Application +; therfore changed EEPROM_LAYOUT_REVISION = 8 +; Added Vdd Monitor as reset source when writing to "EEProm" +; Changed for use of batch file to assemble, link and make hex files +; - Rev2.2: (minor changes by 4712) +; Added Disable Throttle Re-Arming every motor start by PC Setup Application +; - Rev2.3: (minor changes by 4712) +; Added bugfixed (2x CLR C before j(n)c operations)thx Steffen! +; - Rev2.4: Revisions 2.1 to 2.3 integrated +; - Rev3.0: Added PPM (1050us-1866us) as accepted input signal +; Added startup rpm as a programming parameter +; Added startup acceleration as a programming parameter +; Added option for using voltage measurements to compensate motor power +; Added governor target by setup as a governor mode option +; Governor is kept active regardless of rpm +; Smooth governor spoolup/down in arm and setup modes +; Increased governor P and I gain programming ranges +; Increased and changed low voltage limit programming range +; Disabled tx programming entry for all but the first arming sequence after power on +; Made it possible to skip parameters in tx programming by setting throttle midstick +; Made it default not to rearm for every restart +; - Rev3.1: Fixed bug that prevented chosen parameter to be set in tx programming +; - Rev3.2: ...also updated the EEPROM revision parameter +; - Rev3.3: Fixed negative number bug in voltage compensation +; Fixed bug in startup power calculation for non-default power +; Prevented possibility for voltage compensation fighting low voltage limiting +; Applied overall spoolup control to ensure soft spoolup in any mode +; Added a delay of 3 seconds from initiation of main motor stop until new startup is allowed +; Reduced beep power to reduce power consumption for very strong motors/ESCs +; - Rev3.4: Fixed bug that prevented full power in governor arm and setup modes +; Increased NFETON_DELAY for XP_7A and XP_12A to allow for more powerful fets +; Increased initial spoolup power, and linked to startup power +; - Rev4.0: Fixed bug that made tail tx program beeps very weak +; Added thermal protection feature +; Governor P and I gain ranges are extended up to 8.0x gain +; Startup sequence is aborted upon zero throttle +; Avoided voltage compensation function induced latency for tail when voltage compensation is not enabled +; Improved input signal frequency detection robustness +; - Rev4.1: Increased thermal protection temperature limits +; - Rev5.0: Added multi(copter) operating mode. TAIL define changed to MODE with three modes: MAIN, TAIL and MULTI +; Added programmable commutation timing +; Added a damped light mode that has less damping, but that can be used with all escs +; Added programmable damping force +; Added thermal protection for startup too +; Added wait beeps when waiting more than 30 sec for throttle above zero (after having been armed) +; Modified tail idling to provide option for very low speeds +; Changed PPM range to 1150-1830us +; Arming sequence is dropped for PPM input, unless it is governor arm mode +; Loss of input signal will immediately stop the motor for PPM input +; Bug corrected in Turnigy Plush 6A voltage measurement setup +; FET switching delays are set for original fets. Stronger/doubled/tripled etc fets may require faster pfet off switching +; Miscellaneous other changes +; - Rev6.0: Reverted comparator reading routine to rev5.0 equivalent, in order to avoid tail motor stops +; Added governor range programmability +; Implemented startup retry sequence with varying startup power for multi mode +; In damped light mode, damping is now applied to the active nfet phase for fully damped capable ESCs +; - Rev6.1: Added input signal qualification criteria for PPM, to avoid triggering on noise spikes (fix for plush hardware) +; Changed main and multi mode stop criteria. Will now be in run mode, even if RC pulse input is zero +; Fixed bug in commutation that caused rough running in damped light mode +; Miscellaneous other changes +; - Rev7.0 Added direct startup mode programmability +; Added throttle calibration. Min>=1000us and Max<=2000us. Difference must be >520us, otherwise max is shifted so that difference=520us +; Added programmable throttle change rate +; Added programmable beep strength, beacon strength and beacon delay +; Reduced power step to full power significantly +; Miscellaneous other changes +; - Rev8.0 Added a 2 second delay after power up, to wait for receiver initialization +; Added a programming option for disabling low voltage limit, and made it default for MULTI +; Added programable demag compensation, using the concept of SimonK +; Improved robustness against noisy input signal +; Refined direct startup +; Removed voltage compensation +; Miscellaneous other changes +; - Rev9.0 Increased programming range for startup power, and made it's default ESC dependent +; Made default startup method ESC dependent +; Even more smooth and gentle spoolup for MAIN, to suit larger helis +; Improved transition from stepped startup to run +; Refined direct startup +; - Rev9.1 Fixed bug that changed FW revision after throttle calibration or TX programming +; - Rev9.2 Altered timing of throttle calibration in order to work with MultiWii calibration firmware +; Reduced main spoolup time to around 5 seconds +; Changed default beacon delay to 3 minutes +; - Rev9.3 Fixed bug in Plush 60/80A temperature reading, that caused failure in operation above 4S +; Corrected temperature limit for HiModel cool 22/33/41A, RCTimer 6A, Skywalker 20/40A, Turnigy AE45A, Plush 40/60/80A. Limit was previously set too high +; - Rev9.4 Improved timing for increased maximum rpm limit +; - Rev10.0 Added closed loop mode for multi +; Added high/low BEC voltage option (for the ESCs where HW supports it) +; Added method of resetting all programmed parameter values to defaults by TX programming +; Added Turnigy K-force 40A and Turnigy K-force 120A HV ESCs +; Enabled fully damped mode for several ESCs +; Extended startup power range downwards to enable very smooth start for large heli main motors +; Extended damping force with a highest setting +; Corrected temperature limits for F310 chips (Plush 40A and AE 45A) +; Implemented temperature reading average in order to avoid problems with ADC noise on Skywalkers +; Increased switching delays for XP 7A fast, in order to avoid cross conduction of N and P fets +; Miscellaneous other changes +; - Rev10.1 Relaxed RC signal jitter requirement during frequency measurement +; Corrected bug that prevented using governor low +; Enabled vdd monitor always, in order to reduce likelihood of accidental overwriting of adjustments +; Fixed bug that caused stop for PPM input above 2048us, and moved upper accepted limit to 2160us +; - Rev10.2 Corrected temperature limit for AE20-30/XP7-25, where limit was too high +; Corrected temperature limit for 120HV, where limit was too low +; Fixed bug that caused AE20/25/30A not to run in reverse +; +; +;**** **** **** **** **** +; Up to 8K Bytes of In-System Self-Programmable Flash +; 768 Bytes Internal SRAM +; +;**** **** **** **** **** +; Master clock is internal 24MHz oscillator +; Timer 0 (167/500ns counts) always counts up and is used for +; - PWM generation +; Timer 1 (167/500ns counts) always counts up and is not used +; - Time from pwm on/off event +; Timer 2 (500ns counts) always counts up and is used for +; - RC pulse timeout/skip counts and commutation times +; Timer 3 (500ns counts) always counts up and is used for +; - Commutation timeouts +; PCA0 (500ns counts) always counts up and is used for +; - RC pulse measurement +; +;**** **** **** **** **** +; Interrupt handling +; The F330/2 does not disable interrupts when entering an interrupt routine. +; Also some interrupt flags need to be cleared by software +; The code disables interrupts in interrupt routines, in order to avoid nested interrupts +; - Interrupts are disabled during beeps, to avoid audible interference from interrupts +; - RC pulse interrupts are periodically disabled in order to reduce interference with pwm interrupts. +; +;**** **** **** **** **** +; Motor control: +; - Brushless motor control with 6 states for each electrical 360 degrees +; - An advance timing of 0deg has zero cross 30deg after one commutation and 30deg before the next +; - Timing advance in this implementation is set to 15deg nominally +; - "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. +; Motor sequence starting from zero crossing: +; - Timer wait: Wt_Comm 15deg ; Time to wait from zero cross to actual commutation +; - Timer wait: Wt_Advance 15deg ; Time to wait for timing advance. Nominal commutation point is after this +; - Timer wait: Wt_Zc_Scan 7.5deg ; Time to wait before looking for zero cross +; - Scan for zero cross 22.5deg , Nominal, with some motor variations +; +; Motor startup in stepper mode: +; Initial motor rotations are done with the motor controlled as a stepper motor. +; In this stepper motor mode comparator information is not used. +; Settle phase is the first, where there are a few commutations with increasing step length, in order to settle the motor in a predefined position. +; Stepper phase comes next, where there is a step length decrease sequence. +; Direct startup is the last phase, for synchronization before normal bemf commutation run begins. +; Motor startup in direct mode: +; Direct startup is the only phase, before normal bemf commutation run begins. +; +;**** **** **** **** **** +; List of enumerated supported ESCs and modes (main, tail or multi) +XP_3A_Main EQU 1 +XP_3A_Tail EQU 2 +XP_3A_Multi EQU 3 +XP_7A_Main EQU 4 +XP_7A_Tail EQU 5 +XP_7A_Multi EQU 6 +XP_7A_Fast_Main EQU 7 +XP_7A_Fast_Tail EQU 8 +XP_7A_Fast_Multi EQU 9 +XP_12A_Main EQU 10 +XP_12A_Tail EQU 11 +XP_12A_Multi EQU 12 +XP_18A_Main EQU 13 +XP_18A_Tail EQU 14 +XP_18A_Multi EQU 15 +XP_25A_Main EQU 16 +XP_25A_Tail EQU 17 +XP_25A_Multi EQU 18 +DP_3A_Main EQU 19 +DP_3A_Tail EQU 20 +DP_3A_Multi EQU 21 +Supermicro_3p5A_Main EQU 22 +Supermicro_3p5A_Tail EQU 23 +Supermicro_3p5A_Multi EQU 24 +Turnigy_Plush_6A_Main EQU 25 +Turnigy_Plush_6A_Tail EQU 26 +Turnigy_Plush_6A_Multi EQU 27 +Turnigy_Plush_10A_Main EQU 28 +Turnigy_Plush_10A_Tail EQU 29 +Turnigy_Plush_10A_Multi EQU 30 +Turnigy_Plush_12A_Main EQU 31 +Turnigy_Plush_12A_Tail EQU 32 +Turnigy_Plush_12A_Multi EQU 33 +Turnigy_Plush_18A_Main EQU 34 +Turnigy_Plush_18A_Tail EQU 35 +Turnigy_Plush_18A_Multi EQU 36 +Turnigy_Plush_25A_Main EQU 37 +Turnigy_Plush_25A_Tail EQU 38 +Turnigy_Plush_25A_Multi EQU 39 +Turnigy_Plush_30A_Main EQU 40 +Turnigy_Plush_30A_Tail EQU 41 +Turnigy_Plush_30A_Multi EQU 42 +Turnigy_Plush_40A_Main EQU 43 +Turnigy_Plush_40A_Tail EQU 44 +Turnigy_Plush_40A_Multi EQU 45 +Turnigy_Plush_60A_Main EQU 46 +Turnigy_Plush_60A_Tail EQU 47 +Turnigy_Plush_60A_Multi EQU 48 +Turnigy_Plush_80A_Main EQU 49 +Turnigy_Plush_80A_Tail EQU 50 +Turnigy_Plush_80A_Multi EQU 51 +Turnigy_AE_20A_Main EQU 52 +Turnigy_AE_20A_Tail EQU 53 +Turnigy_AE_20A_Multi EQU 54 +Turnigy_AE_25A_Main EQU 55 +Turnigy_AE_25A_Tail EQU 56 +Turnigy_AE_25A_Multi EQU 57 +Turnigy_AE_30A_Main EQU 58 +Turnigy_AE_30A_Tail EQU 59 +Turnigy_AE_30A_Multi EQU 60 +Turnigy_AE_45A_Main EQU 61 +Turnigy_AE_45A_Tail EQU 62 +Turnigy_AE_45A_Multi EQU 63 +Turnigy_KForce_40A_Main EQU 64 +Turnigy_KForce_40A_Tail EQU 65 +Turnigy_KForce_40A_Multi EQU 66 +Turnigy_KForce_120A_HV_Main EQU 67 +Turnigy_KForce_120A_HV_Tail EQU 68 +Turnigy_KForce_120A_HV_Multi EQU 69 +Skywalker_20A_Main EQU 70 +Skywalker_20A_Tail EQU 71 +Skywalker_20A_Multi EQU 72 +Skywalker_40A_Main EQU 73 +Skywalker_40A_Tail EQU 74 +Skywalker_40A_Multi EQU 75 +HiModel_Cool_22A_Main EQU 76 +HiModel_Cool_22A_Tail EQU 77 +HiModel_Cool_22A_Multi EQU 78 +HiModel_Cool_33A_Main EQU 79 +HiModel_Cool_33A_Tail EQU 80 +HiModel_Cool_33A_Multi EQU 81 +HiModel_Cool_41A_Main EQU 82 +HiModel_Cool_41A_Tail EQU 83 +HiModel_Cool_41A_Multi EQU 84 +RCTimer_6A_Main EQU 85 +RCTimer_6A_Tail EQU 86 +RCTimer_6A_Multi EQU 87 +Align_RCE_BL15X_Main EQU 88 +Align_RCE_BL15X_Tail EQU 89 +Align_RCE_BL15X_Multi EQU 90 +Align_RCE_BL35X_Main EQU 91 +Align_RCE_BL35X_Tail EQU 92 +Align_RCE_BL35X_Multi EQU 93 +Align_RCE_BL35P_Main EQU 94 +Align_RCE_BL35P_Tail EQU 95 +Align_RCE_BL35P_Multi EQU 96 +H_King_10A_Main EQU 97 +H_King_10A_Tail EQU 98 +H_King_10A_Multi EQU 99 + +;**** **** **** **** **** +; Select the ESC and mode to use (or unselect all for use with external batch compile file) +;BESC EQU XP_3A_Main +;BESC EQU XP_3A_Tail +;BESC EQU XP_3A_Multi +;BESC EQU XP_7A_Main +;BESC EQU XP_7A_Tail +;BESC EQU XP_7A_Multi +;BESC EQU XP_7A_Fast_Main +;BESC EQU XP_7A_Fast_Tail +;BESC EQU XP_7A_Fast_Multi +;BESC EQU XP_12A_Main +;BESC EQU XP_12A_Tail +;BESC EQU XP_12A_Multi +;BESC EQU XP_18A_Main +;BESC EQU XP_18A_Tail +BESC EQU XP_18A_Multi +;BESC EQU XP_25A_Main +;BESC EQU XP_25A_Tail +;BESC EQU XP_25A_Multi +;BESC EQU DP_3A_Main +;BESC EQU DP_3A_Tail +;BESC EQU DP_3A_Multi +;BESC EQU Supermicro_3p5A_Main +;BESC EQU Supermicro_3p5A_Tail +;BESC EQU Supermicro_3p5A_Multi +;BESC EQU Turnigy_Plush_6A_Main +;BESC EQU Turnigy_Plush_6A_Tail +;BESC EQU Turnigy_Plush_6A_Multi +;BESC EQU Turnigy_Plush_10A_Main +;BESC EQU Turnigy_Plush_10A_Tail +;BESC EQU Turnigy_Plush_10A_Multi +;BESC EQU Turnigy_Plush_12A_Main +;BESC EQU Turnigy_Plush_12A_Tail +;BESC EQU Turnigy_Plush_12A_Multi +;BESC EQU Turnigy_Plush_18A_Main +;BESC EQU Turnigy_Plush_18A_Tail +;BESC EQU Turnigy_Plush_18A_Multi +;BESC EQU Turnigy_Plush_25A_Main +;BESC EQU Turnigy_Plush_25A_Tail +;BESC EQU Turnigy_Plush_25A_Multi +;BESC EQU Turnigy_Plush_30A_Main +;BESC EQU Turnigy_Plush_30A_Tail +;BESC EQU Turnigy_Plush_30A_Multi +;BESC EQU Turnigy_Plush_40A_Main +;BESC EQU Turnigy_Plush_40A_Tail +;BESC EQU Turnigy_Plush_40A_Multi +;BESC EQU Turnigy_Plush_60A_Main +;BESC EQU Turnigy_Plush_60A_Tail +;BESC EQU Turnigy_Plush_60A_Multi +;BESC EQU Turnigy_Plush_80A_Main +;BESC EQU Turnigy_Plush_80A_Tail +;BESC EQU Turnigy_Plush_80A_Multi +;BESC EQU Turnigy_AE_20A_Main +;BESC EQU Turnigy_AE_20A_Tail +;BESC EQU Turnigy_AE_20A_Multi +;BESC EQU Turnigy_AE_25A_Main +;BESC EQU Turnigy_AE_25A_Tail +;BESC EQU Turnigy_AE_25A_Multi +;BESC EQU Turnigy_AE_30A_Main +;BESC EQU Turnigy_AE_30A_Tail +;BESC EQU Turnigy_AE_30A_Multi +;BESC EQU Turnigy_AE_45A_Main +;BESC EQU Turnigy_AE_45A_Tail +;BESC EQU Turnigy_AE_45A_Multi +;BESC EQU Turnigy_KForce_40A_Main +;BESC EQU Turnigy_KForce_40A_Tail +;BESC EQU Turnigy_KForce_40A_Multi +;BESC EQU Turnigy_KForce_120A_HV_Main +;BESC EQU Turnigy_KForce_120A_HV_Tail +;BESC EQU Turnigy_KForce_120A_HV_Multi +;BESC EQU Skywalker_20A_Main +;BESC EQU Skywalker_20A_Tail +;BESC EQU Skywalker_20A_Multi +;BESC EQU Skywalker_40A_Main +;BESC EQU Skywalker_40A_Tail +;BESC EQU Skywalker_40A_Multi +;BESC EQU HiModel_Cool_22A_Main +;BESC EQU HiModel_Cool_22A_Tail +;BESC EQU HiModel_Cool_22A_Multi +;BESC EQU HiModel_Cool_33A_Main +;BESC EQU HiModel_Cool_33A_Tail +;BESC EQU HiModel_Cool_33A_Multi +;BESC EQU HiModel_Cool_41A_Main +;BESC EQU HiModel_Cool_41A_Tail +;BESC EQU HiModel_Cool_41A_Multi +;BESC EQU RCTimer_6A_Main +;BESC EQU RCTimer_6A_Tail +;BESC EQU RCTimer_6A_Multi +;BESC EQU Align_RCE_BL15X_Main +;BESC EQU Align_RCE_BL15X_Tail +;BESC EQU Align_RCE_BL15X_Multi +;BESC EQU Align_RCE_BL35X_Main +;BESC EQU Align_RCE_BL35X_Tail +;BESC EQU Align_RCE_BL35X_Multi +;BESC EQU Align_RCE_BL35P_Main +;BESC EQU Align_RCE_BL35P_Tail +;BESC EQU Align_RCE_BL35P_Multi +;BESC EQU H_King_10A_Main +;BESC EQU H_King_10A_Tail +;BESC EQU H_King_10A_Multi + + +;**** **** **** **** **** +; ESC selection statements +IF BESC == XP_3A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (XP_3A.inc) ; Select XP 3A pinout +ENDIF + +IF BESC == XP_3A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (XP_3A.inc) ; Select XP 3A pinout +ENDIF + +IF BESC == XP_3A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (XP_3A.inc) ; Select XP 3A pinout +ENDIF + +IF BESC == XP_7A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (XP_7A.inc) ; Select XP 7A pinout +ENDIF + +IF BESC == XP_7A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (XP_7A.inc) ; Select XP 7A pinout +ENDIF + +IF BESC == XP_7A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (XP_7A.inc) ; Select XP 7A pinout +ENDIF + +IF BESC == XP_7A_Fast_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (XP_7A_Fast.inc) ; Select XP 7A Fast pinout +ENDIF + +IF BESC == XP_7A_Fast_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (XP_7A_Fast.inc) ; Select XP 7A Fast pinout +ENDIF + +IF BESC == XP_7A_Fast_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (XP_7A_Fast.inc) ; Select XP 7A Fast pinout +ENDIF + +IF BESC == XP_12A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (XP_12A.inc) ; Select XP 12A pinout +ENDIF + +IF BESC == XP_12A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (XP_12A.inc) ; Select XP 12A pinout +ENDIF + +IF BESC == XP_12A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (XP_12A.inc) ; Select XP 12A pinout +ENDIF + +IF BESC == XP_18A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (XP_18A.inc) ; Select XP 18A pinout +ENDIF + +IF BESC == XP_18A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (XP_18A.inc) ; Select XP 18A pinout +ENDIF + +IF BESC == XP_18A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (XP_18A.inc) ; Select XP 18A pinout +ENDIF + +IF BESC == XP_25A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (XP_25A.inc) ; Select XP 25A pinout +ENDIF + +IF BESC == XP_25A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (XP_25A.inc) ; Select XP 25A pinout +ENDIF + +IF BESC == XP_25A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (XP_25A.inc) ; Select XP 25A pinout +ENDIF + +IF BESC == DP_3A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (DP_3A.inc) ; Select DP 3A pinout +ENDIF + +IF BESC == DP_3A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (DP_3A.inc) ; Select DP 3A pinout +ENDIF + +IF BESC == DP_3A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (DP_3A.inc) ; Select DP 3A pinout +ENDIF + +IF BESC == Supermicro_3p5A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Supermicro_3p5A.inc) ; Select Supermicro 3.5A pinout +ENDIF + +IF BESC == Supermicro_3p5A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Supermicro_3p5A.inc) ; Select Supermicro 3.5A pinout +ENDIF + +IF BESC == Supermicro_3p5A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Supermicro_3p5A.inc) ; Select Supermicro 3.5A pinout +ENDIF + +IF BESC == Turnigy_Plush_6A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_Plush_6A.inc) ; Select Turnigy Plush 6A pinout +ENDIF + +IF BESC == Turnigy_Plush_6A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_Plush_6A.inc) ; Select Turnigy Plush 6A pinout +ENDIF + +IF BESC == Turnigy_Plush_6A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_Plush_6A.inc) ; Select Turnigy Plush 6A pinout +ENDIF + +IF BESC == Turnigy_Plush_10A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_Plush_10A.inc) ; Select Turnigy Plush 10A pinout +ENDIF + +IF BESC == Turnigy_Plush_10A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_Plush_10A.inc) ; Select Turnigy Plush 10A pinout +ENDIF + +IF BESC == Turnigy_Plush_10A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_Plush_10A.inc) ; Select Turnigy Plush 10A pinout +ENDIF + +IF BESC == Turnigy_Plush_12A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_Plush_12A.inc) ; Select Turnigy Plush 12A pinout +ENDIF + +IF BESC == Turnigy_Plush_12A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_Plush_12A.inc) ; Select Turnigy Plush 12A pinout +ENDIF + +IF BESC == Turnigy_Plush_12A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_Plush_12A.inc) ; Select Turnigy Plush 12A pinout +ENDIF + +IF BESC == Turnigy_Plush_18A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_Plush_18A.inc) ; Select Turnigy Plush 18A pinout +ENDIF + +IF BESC == Turnigy_Plush_18A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_Plush_18A.inc) ; Select Turnigy Plush 18A pinout +ENDIF + +IF BESC == Turnigy_Plush_18A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_Plush_18A.inc) ; Select Turnigy Plush 18A pinout +ENDIF + +IF BESC == Turnigy_Plush_25A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_Plush_25A.inc) ; Select Turnigy Plush 25A pinout +ENDIF + +IF BESC == Turnigy_Plush_25A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_Plush_25A.inc) ; Select Turnigy Plush 25A pinout +ENDIF + +IF BESC == Turnigy_Plush_25A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_Plush_25A.inc) ; Select Turnigy Plush 25A pinout +ENDIF + +IF BESC == Turnigy_Plush_30A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_Plush_30A.inc) ; Select Turnigy Plush 30A pinout +ENDIF + +IF BESC == Turnigy_Plush_30A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_Plush_30A.inc) ; Select Turnigy Plush 30A pinout +ENDIF + +IF BESC == Turnigy_Plush_30A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_Plush_30A.inc) ; Select Turnigy Plush 30A pinout +ENDIF + +IF BESC == Turnigy_Plush_40A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_Plush_40A.inc) ; Select Turnigy Plush 40A pinout +ENDIF + +IF BESC == Turnigy_Plush_40A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_Plush_40A.inc) ; Select Turnigy Plush 40A pinout +ENDIF + +IF BESC == Turnigy_Plush_40A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_Plush_40A.inc) ; Select Turnigy Plush 40A pinout +ENDIF + +IF BESC == Turnigy_Plush_60A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_Plush_60A.inc) ; Select Turnigy Plush 60A pinout +ENDIF + +IF BESC == Turnigy_Plush_60A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_Plush_60A.inc) ; Select Turnigy Plush 60A pinout +ENDIF + +IF BESC == Turnigy_Plush_60A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_Plush_60A.inc) ; Select Turnigy Plush 60A pinout +ENDIF + +IF BESC == Turnigy_Plush_80A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_Plush_80A.inc) ; Select Turnigy Plush 80A pinout +ENDIF + +IF BESC == Turnigy_Plush_80A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_Plush_80A.inc) ; Select Turnigy Plush 80A pinout +ENDIF + +IF BESC == Turnigy_Plush_80A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_Plush_80A.inc) ; Select Turnigy Plush 80A pinout +ENDIF + +IF BESC == Turnigy_AE_20A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_AE_20A.inc) ; Select Turnigy AE-20A pinout +ENDIF + +IF BESC == Turnigy_AE_20A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_AE_20A.inc) ; Select Turnigy AE-20A pinout +ENDIF + +IF BESC == Turnigy_AE_20A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_AE_20A.inc) ; Select Turnigy AE-20A pinout +ENDIF + +IF BESC == Turnigy_AE_25A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_AE_25A.inc) ; Select Turnigy AE-25A pinout +ENDIF + +IF BESC == Turnigy_AE_25A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_AE_25A.inc) ; Select Turnigy AE-25A pinout +ENDIF + +IF BESC == Turnigy_AE_25A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_AE_25A.inc) ; Select Turnigy AE-25A pinout +ENDIF + +IF BESC == Turnigy_AE_30A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_AE_30A.inc) ; Select Turnigy AE-30A pinout +ENDIF + +IF BESC == Turnigy_AE_30A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_AE_30A.inc) ; Select Turnigy AE-30A pinout +ENDIF + +IF BESC == Turnigy_AE_30A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_AE_30A.inc) ; Select Turnigy AE-30A pinout +ENDIF + +IF BESC == Turnigy_AE_45A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_AE_45A.inc) ; Select Turnigy AE-45A pinout +ENDIF + +IF BESC == Turnigy_AE_45A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_AE_45A.inc) ; Select Turnigy AE-45A pinout +ENDIF + +IF BESC == Turnigy_AE_45A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_AE_45A.inc) ; Select Turnigy AE-45A pinout +ENDIF + +IF BESC == Turnigy_KForce_40A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_KForce_40A.inc) ; Select Turnigy KForce 40A pinout +ENDIF + +IF BESC == Turnigy_KForce_40A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_KForce_40A.inc) ; Select Turnigy KForce 40A pinout +ENDIF + +IF BESC == Turnigy_KForce_40A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_KForce_40A.inc) ; Select Turnigy KForce 40A pinout +ENDIF + +IF BESC == Turnigy_KForce_120A_HV_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_KForce_120A_HV.inc) ; Select Turnigy KForce 120A HV pinout +ENDIF + +IF BESC == Turnigy_KForce_120A_HV_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_KForce_120A_HV.inc) ; Select Turnigy KForce 120A HV pinout +ENDIF + +IF BESC == Turnigy_KForce_120A_HV_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_KForce_120A_HV.inc) ; Select Turnigy KForce 120A HV pinout +ENDIF + +IF BESC == Skywalker_20A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Skywalker_20A.inc) ; Select Skywalker 20A pinout +ENDIF + +IF BESC == Skywalker_20A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Skywalker_20A.inc) ; Select Skywalker 20A pinout +ENDIF + +IF BESC == Skywalker_20A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Skywalker_20A.inc) ; Select Skywalker 20A pinout +ENDIF + +IF BESC == Skywalker_40A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Skywalker_40A.inc) ; Select Skywalker 40A pinout +ENDIF + +IF BESC == Skywalker_40A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Skywalker_40A.inc) ; Select Skywalker 40A pinout +ENDIF + +IF BESC == Skywalker_40A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Skywalker_40A.inc) ; Select Skywalker 40A pinout +ENDIF + +IF BESC == HiModel_Cool_22A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (HiModel_Cool_22A.inc) ; Select HiModel Cool 22A pinout +ENDIF + +IF BESC == HiModel_Cool_22A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (HiModel_Cool_22A.inc) ; Select HiModel Cool 22A pinout +ENDIF + +IF BESC == HiModel_Cool_22A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (HiModel_Cool_22A.inc) ; Select HiModel Cool 22A pinout +ENDIF + +IF BESC == HiModel_Cool_33A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (HiModel_Cool_33A.inc) ; Select HiModel Cool 33A pinout +ENDIF + +IF BESC == HiModel_Cool_33A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (HiModel_Cool_33A.inc) ; Select HiModel Cool 33A pinout +ENDIF + +IF BESC == HiModel_Cool_33A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (HiModel_Cool_33A.inc) ; Select HiModel Cool 33A pinout +ENDIF + +IF BESC == HiModel_Cool_41A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (HiModel_Cool_41A.inc) ; Select HiModel Cool 41A pinout +ENDIF + +IF BESC == HiModel_Cool_41A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (HiModel_Cool_41A.inc) ; Select HiModel Cool 41A pinout +ENDIF + +IF BESC == HiModel_Cool_41A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (HiModel_Cool_41A.inc) ; Select HiModel Cool 41A pinout +ENDIF + +IF BESC == RCTimer_6A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (RCTimer_6A.inc) ; Select RC Timer 6A pinout +ENDIF + +IF BESC == RCTimer_6A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (RCTimer_6A.inc) ; Select RC Timer 6A pinout +ENDIF + +IF BESC == RCTimer_6A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (RCTimer_6A.inc) ; Select RC Timer 6A pinout +ENDIF + +IF BESC == Align_RCE_BL15X_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Align_RCE_BL15X.inc) ; Select Align RCE-BL15X pinout +ENDIF + +IF BESC == Align_RCE_BL15X_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Align_RCE_BL15X.inc) ; Select Align RCE-BL15X pinout +ENDIF + +IF BESC == Align_RCE_BL15X_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Align_RCE_BL15X.inc) ; Select Align RCE-BL15X pinout +ENDIF + +IF BESC == Align_RCE_BL35X_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Align_RCE_BL35X.inc) ; Select Align RCE-BL35X pinout +ENDIF + +IF BESC == Align_RCE_BL35X_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Align_RCE_BL35X.inc) ; Select Align RCE-BL35X pinout +ENDIF + +IF BESC == Align_RCE_BL35X_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Align_RCE_BL35X.inc) ; Select Align RCE-BL35X pinout +ENDIF + +IF BESC == Align_RCE_BL35P_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Align_RCE_BL35P.inc) ; Select Align RCE-BL35P pinout +ENDIF + +IF BESC == Align_RCE_BL35P_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Align_RCE_BL35P.inc) ; Select Align RCE-BL35P pinout +ENDIF + +IF BESC == Align_RCE_BL35P_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Align_RCE_BL35P.inc) ; Select Align RCE-BL35P pinout +ENDIF + +IF BESC == H_King_10A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (H_King_10A.inc) ; Select H-King 10A pinout +ENDIF + +IF BESC == H_King_10A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (H_King_10A.inc) ; Select H-King 10A pinout +ENDIF + +IF BESC == H_King_10A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (H_King_10A.inc) ; Select H-King 10A pinout +ENDIF + +;**** **** **** **** **** +; TX programming defaults +; +; Parameter dependencies: +; - Governor P gain, I gain and Range is only used if one of the three governor modes is selected +; - Governor setup target is only used if Setup governor mode is selected (or closed loop mode is on for multi) +; - Startup rpm and startup accel is only used if stepped startup method is selected +; - Damping force is only used if DampedLight or Damped is selected +; +; Main +DEFAULT_PGM_MAIN_P_GAIN EQU 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 +DEFAULT_PGM_MAIN_I_GAIN EQU 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 +DEFAULT_PGM_MAIN_GOVERNOR_MODE EQU 1 ; 1=Tx 2=Arm 3=Setup 4=Off +DEFAULT_PGM_MAIN_GOVERNOR_RANGE EQU 1 ; 1=High 2=Low +DEFAULT_PGM_MAIN_LOW_VOLTAGE_LIM EQU 4 ; 1=Off 2=3.0V/c 3=3.1V/c 4=3.2V/c 5=3.3V/c 6=3.4V/c +DEFAULT_PGM_MAIN_STARTUP_RPM EQU 3 ; 1=0.67 2=0.8 3=1.00 4=1.25 5=1.5 +DEFAULT_PGM_MAIN_STARTUP_ACCEL EQU 1 ; 1=0.4 2=0.7 3=1.0 4=1.5 5=2.3 +DEFAULT_PGM_MAIN_COMM_TIMING EQU 3 ; 1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=High +DEFAULT_PGM_MAIN_THROTTLE_RATE EQU 13 ; 1=2 2=3 3=4 4=6 5=8 6=12 7=16 8=24 9=32 10=48 11=64 12=128 13=255 +DEFAULT_PGM_MAIN_DAMPING_FORCE EQU 1 ; 1=VeryLow 2=Low 3=MediumLow 4=MediumHigh 5=High 6=Highest +DEFAULT_PGM_MAIN_PWM_FREQ EQU 2 ; 1=High 2=Low 3=DampedLight +DEFAULT_PGM_MAIN_DEMAG_COMP EQU 1 ; 1=Disabled 2=15/0 3=15/7.5 4=7.5/7.5 5=7.5/15 (degrees blind advance / power off) +DEFAULT_PGM_MAIN_DIRECTION_REV EQU 1 ; 1=Normal 2=Reversed +DEFAULT_PGM_MAIN_RCP_PWM_POL EQU 1 ; 1=Positive 2=Negative +DEFAULT_PGM_MAIN_GOV_SETUP_TARGET EQU 180 ; Target for governor in setup mode. Corresponds to 70% throttle +DEFAULT_PGM_MAIN_REARM_START EQU 0 ; 1=Enabled 0=Disabled +DEFAULT_PGM_MAIN_BEEP_STRENGTH EQU 80 ; Beep strength +DEFAULT_PGM_MAIN_BEACON_STRENGTH EQU 100 ; Beacon strength +DEFAULT_PGM_MAIN_BEACON_DELAY EQU 2 ; 1=30s 2=1m 3=2m 4=3m 5=Infinite +; Tail +DEFAULT_PGM_TAIL_GAIN EQU 3 ; 1=0.75 2=0.88 3=1.00 4=1.12 5=1.25 +DEFAULT_PGM_TAIL_IDLE_SPEED EQU 4 ; 1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=High +DEFAULT_PGM_TAIL_STARTUP_RPM EQU 3 ; 1=0.67 2=0.8 3=1.00 4=1.25 5=1.5 +DEFAULT_PGM_TAIL_STARTUP_ACCEL EQU 5 ; 1=0.4 2=0.7 3=1.0 4=1.5 5=2.3 +DEFAULT_PGM_TAIL_COMM_TIMING EQU 3 ; 1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=High +DEFAULT_PGM_TAIL_THROTTLE_RATE EQU 13 ; 1=2 2=3 3=4 4=6 5=8 6=12 7=16 8=24 9=32 10=48 11=64 12=128 13=255 +DEFAULT_PGM_TAIL_DAMPING_FORCE EQU 5 ; 1=VeryLow 2=Low 3=MediumLow 4=MediumHigh 5=High 6=Highest +IF DAMPED_MODE_ENABLE == 1 +DEFAULT_PGM_TAIL_PWM_FREQ EQU 1 ; 1=High 2=Low 3=DampedLight 4=Damped +ELSE +DEFAULT_PGM_TAIL_PWM_FREQ EQU 1 ; 1=High 2=Low 3=DampedLight +ENDIF +DEFAULT_PGM_TAIL_DEMAG_COMP EQU 1 ; 1=Disabled 2=15/0 3=15/7.5 4=7.5/7.5 5=7.5/15 (degrees blind advance / power off) +DEFAULT_PGM_TAIL_DIRECTION_REV EQU 1 ; 1=Normal 2=Reversed +DEFAULT_PGM_TAIL_RCP_PWM_POL EQU 1 ; 1=Positive 2=Negative +DEFAULT_PGM_TAIL_BEEP_STRENGTH EQU 250 ; Beep strength +DEFAULT_PGM_TAIL_BEACON_STRENGTH EQU 250 ; Beacon strength +DEFAULT_PGM_TAIL_BEACON_DELAY EQU 4 ; 1=30s 2=1m 3=2m 4=3m 5=Infinite +; Multi +DEFAULT_PGM_MULTI_FIRST_KEYWORD EQU 66h ;增加首个关键字 2013.8.27 +DEFAULT_PGM_MULTI_P_GAIN EQU 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 +DEFAULT_PGM_MULTI_I_GAIN EQU 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 +DEFAULT_PGM_MULTI_GOVERNOR_MODE EQU 1 ; 1=Off 2=LoRange 3=MidRange 4=HiRange +DEFAULT_PGM_MULTI_GAIN EQU 3 ; 1=0.75 2=0.88 3=1.00 4=1.12 5=1.25 +DEFAULT_PGM_MULTI_LOW_VOLTAGE_LIM EQU 2 +;DEFAULT_PGM_MULTI_LOW_VOLTAGE_LIM EQU 1 ; 1=Off 2=3.0V/c 3=3.1V/c 4=3.2V/c 5=3.3V/c 6=3.4V/c +DEFAULT_PGM_MULTI_LOW_VOLTAGE_CTL EQU 1 ;增加低压保护控制方式默认值 1=功率逐渐降到31% 2=关闭输出 +DEFAULT_PGM_MULTI_STARTUP_RPM EQU 1 ; 1=0.67 2=0.8 3=1.00 4=1.25 5=1.5 +DEFAULT_PGM_MULTI_STARTUP_ACCEL EQU 5 ; 1=0.4 2=0.7 3=1.0 4=1.5 5=2.3 +DEFAULT_PGM_MULTI_COMM_TIMING EQU 3 ; 1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=High +DEFAULT_PGM_MULTI_THROTTLE_RATE EQU 13 ; 1=2 2=3 3=4 4=6 5=8 6=12 7=16 8=24 9=32 10=48 11=64 12=128 13=255 +DEFAULT_PGM_MULTI_DAMPING_FORCE EQU 1 ; 1=VeryLow 2=Low 3=MediumLow 4=MediumHigh 5=High 6=Highest +IF DAMPED_MODE_ENABLE == 1 +DEFAULT_PGM_MULTI_PWM_FREQ EQU 1 ; 1=Low 2=High +ELSE +DEFAULT_PGM_MULTI_PWM_FREQ EQU 1 ; 1=Low 2=High +ENDIF +DEFAULT_PGM_MULTI_DEMAG_COMP EQU 2 ; 1=Disabled 2=15/0 3=15/7.5 4=7.5/7.5 5=7.5/15 (degrees blind advance / power off) +DEFAULT_PGM_MULTI_DIRECTION_REV EQU 1 ; 1=Normal 2=Reversed +DEFAULT_PGM_MULTI_RCP_PWM_POL EQU 1 ; 1=Positive 2=Negative +;DEFAULT_PGM_MULTI_BEEP_STRENGTH EQU 40 ; Beep strength +;DEFAULT_PGM_MULTI_BEACON_STRENGTH EQU 80 ; Beacon strength 等待摇杆操作声强 +;DEFAULT_PGM_MULTI_BEACON_DELAY EQU 4 ; 1=30s 2=1m 3=2m 4=3m 5=Infinite +DEFAULT_PGM_MULTI_BEEP_STRENGTH EQU 80 ; Beep strength 上电到一次信号拉到最低声强 +DEFAULT_PGM_MULTI_BEACON_STRENGTH EQU 100 ; Beacon strength 等待摇杆操作声强 +DEFAULT_PGM_MULTI_BEACON_DELAY EQU 2 ; 1=30s 2=1m 3=2m 4=3m 5=Infinite 等待摇杆操作时间 +; Common +DEFAULT_PGM_ENABLE_TX_PROGRAM EQU 1 ; 1=Enabled 0=Disabled +DEFAULT_PGM_PPM_MIN_THROTTLE EQU 37 ; 4*37+1000=1148 +DEFAULT_PGM_PPM_MAX_THROTTLE EQU 208 ; 4*208+1000=1832 +DEFAULT_PGM_BEC_VOLTAGE_HIGH EQU 0 ; 0=Low 1= High + +;**** **** **** **** **** +; Constant definitions for main +IF MODE == 0 + +GOV_SPOOLRATE EQU 1 ; Number of steps for governor requested pwm per 32ms + +RCP_TIMEOUT_PPM EQU 10 ; Number of timer2H overflows (about 32ms) before considering rc pulse lost +RCP_TIMEOUT EQU 64 ; Number of timer2L overflows (about 128us) before considering rc pulse lost +RCP_SKIP_RATE EQU 32 ; Number of timer2L overflows (about 128us) before reenabling rc pulse detection +RCP_MIN EQU 0 ; This is minimum RC pulse length +RCP_MAX EQU 254 ; This is maximum RC pulse length +RCP_VALIDATE EQU 2 ; Require minimum this pulse length to validate RC pulse +RCP_STOP EQU 1 ; Stop motor at or below this pulse length +RCP_STOP_LIMIT EQU 3 ; Stop motor if this many timer2H overflows (~32ms) are below stop limit + +PWM_SETTLE EQU 50 ; PWM used when in start settling phase (also max power during direct start) +PWM_STEPPER EQU 80 ; PWM used when in start stepper phase + +COMM_TIME_RED EQU 8 ; Fixed reduction (in us) for commutation wait (to account for fixed delays) +COMM_TIME_MIN EQU 1 ; Minimum time (in us) for commutation wait + +TEMP_CHECK_RATE EQU 8 ; Number of adc conversions for each check of temperature (the other conversions are used for voltage) + +ENDIF +; Constant definitions for tail +IF MODE == 1 + +GOV_SPOOLRATE EQU 1 ; Number of steps for governor requested pwm per 32ms +RCP_TIMEOUT_PPM EQU 10 ; Number of timer2H overflows (about 32ms) before considering rc pulse lost +RCP_TIMEOUT EQU 24 ; Number of timer2L overflows (about 128us) before considering rc pulse lost +RCP_SKIP_RATE EQU 6 ; Number of timer2L overflows (about 128us) before reenabling rc pulse detection +RCP_MIN EQU 0 ; This is minimum RC pulse length +RCP_MAX EQU 255 ; This is maximum RC pulse length +RCP_VALIDATE EQU 2 ; Require minimum this pulse length to validate RC pulse +RCP_STOP EQU 1 ; Stop motor at or below this pulse length +RCP_STOP_LIMIT EQU 130 ; Stop motor if this many timer2H overflows (~32ms) are below stop limit + +PWM_SETTLE EQU 50 ; PWM used when in start settling phase (also max power during direct start) +PWM_STEPPER EQU 120 ; PWM used when in start stepper phase + +COMM_TIME_RED EQU 8 ; Fixed reduction (in us) for commutation wait (to account for fixed delays) +COMM_TIME_MIN EQU 1 ; Minimum time (in us) for commutation wait + +TEMP_CHECK_RATE EQU 8 ; Number of adc conversions for each check of temperature (the other conversions are used for voltage) + +ENDIF +; Constant definitions for multi +IF MODE == 2 + +GOV_SPOOLRATE EQU 1 ; Number of steps for governor requested pwm per 32ms + +RCP_TIMEOUT_PPM EQU 64 ; Number of timer2H overflows (about 32ms) before considering rc pulse lost +RCP_TIMEOUT EQU 40 ; Number of timer2L overflows (about 128us) before considering rc pulse lost +RCP_SKIP_RATE EQU 6 ; Number of timer2L overflows (about 128us) before reenabling rc pulse detection +RCP_MIN EQU 0 ; This is minimum RC pulse length +RCP_MAX EQU 255 ; This is maximum RC pulse length +RCP_VALIDATE EQU 2 ; Require minimum this pulse length to validate RC pulse +RCP_STOP EQU 1 ; Stop motor at or below this pulse length +;//////RCP_STOP_LIMIT EQU 20 ; Stop motor if this many timer2H overflows (~32ms) are below stop limit + +PWM_SETTLE EQU 50 ; PWM used when in start settling phase (also max power during direct start) +PWM_STEPPER EQU 120 ; PWM used when in start stepper phase + +COMM_TIME_RED EQU 8 ; Fixed reduction (in us) for commutation wait (to account for fixed delays) +COMM_TIME_MIN EQU 1 ; Minimum time (in us) for commutation wait + +TEMP_CHECK_RATE EQU 8 ; Number of adc conversions for each check of temperature (the other conversions are used for voltage) + +ENDIF + +;**** **** **** **** **** +; Temporary register definitions +Temp1 EQU R0 +Temp2 EQU R1 +Temp3 EQU R2 +Temp4 EQU R3 +Temp5 EQU R4 +Temp6 EQU R5 +Temp7 EQU R6 +Temp8 EQU R7 + +;**** **** **** **** **** +; Register definitions +DSEG AT 20h ; Variables segment + +Bit_Access: DS 1 ; Variable at bit accessible address (for non interrupt routines) +Bit_Access_Int: DS 1 ; Variable at bit accessible address (for interrupts) + +Requested_Pwm: DS 1 ; Requested pwm (from RC pulse value) +Governor_Req_Pwm: DS 1 ; Governor requested pwm (sets governor target) +Current_Pwm: DS 1 ; Current pwm +Current_Pwm_Limited: DS 1 ; Current pwm that is limited (applied to the motor output) +Rcp_Prev_Edge_L: DS 1 ; RC pulse previous edge timer3 timestamp (lo byte) +Rcp_Prev_Edge_H: DS 1 ; RC pulse previous edge timer3 timestamp (hi byte) +Rcp_Timeout_Cnt: DS 1 ; RC pulse timeout counter (decrementing) +Rcp_Skip_Cnt: DS 1 ; RC pulse skip counter (decrementing) +Rcp_Edge_Cnt: DS 1 ; RC pulse edge counter +Rcp_Stop_Limit: DS 1 ;增加刹车速度 + +Flags0: DS 1 ; State flags. Reset upon init_start +T3_PENDING EQU 0 ; Timer3 pending flag +RCP_MEAS_PWM_FREQ EQU 1 ; Measure RC pulse pwm frequency +PWM_ON EQU 2 ; Set in on part of pwm cycle +DEMAG_DETECTED EQU 3 ; Set when excessive demag time is detected +DEMAG_CUT_POWER EQU 4 ; Set when demag compensation cuts power +; EQU 5 +PROGRAM_FUNC_FLAG EQU 6 +SUCCESS_BEEP_FLAG EQU 7 + +Flags1: DS 1 ; State flags. Reset upon init_start +MOTOR_SPINNING EQU 0 ; Set when in motor is spinning +SETTLE_PHASE EQU 1 ; Set when in motor start settling phase +STEPPER_PHASE EQU 2 ; Set when in motor start stepper motor phase +DIRECT_STARTUP_PHASE EQU 3 ; Set when in direct startup phase +INITIAL_RUN_PHASE EQU 4 ; Set when in initial run phase, before synchronized run is achieved +; EQU 5 +; EQU 6 +LOW_LIMIT_STOP EQU 7 + +Flags2: DS 1 ; State flags. NOT reset upon init_start +RCP_UPDATED EQU 0 ; New RC pulse length value available +RCP_EDGE_NO EQU 1 ; RC pulse edge no. 0=rising, 1=falling +PGM_PWMOFF_DAMPED EQU 2 ; Programmed pwm off damped mode. Set when fully damped or damped light mode is selected +PGM_PWMOFF_DAMPED_FULL EQU 3 ; Programmed pwm off fully damped mode. Set when all pfets shall be on in pwm_off period +PGM_PWMOFF_DAMPED_LIGHT EQU 4 ; Programmed pwm off damped light mode. Set when only 2 pfets shall be on in pwm_off period +PGM_PWM_HIGH_FREQ EQU 5 ; Progremmed pwm high frequency +CURR_PWMOFF_DAMPED EQU 6 ; Currently running pwm off cycle is damped +CURR_PWMOFF_COMP_ABLE EQU 7 ; Currently running pwm off cycle is usable for comparator + +Flags3: DS 1 ; State flags. NOT reset upon init_start +RCP_PWM_FREQ_1KHZ EQU 0 ; RC pulse pwm frequency is 1kHz +RCP_PWM_FREQ_2KHZ EQU 1 ; RC pulse pwm frequency is 2kHz +RCP_PWM_FREQ_4KHZ EQU 2 ; RC pulse pwm frequency is 4kHz +RCP_PWM_FREQ_8KHZ EQU 3 ; RC pulse pwm frequency is 8kHz +PGM_DIR_REV EQU 4 ; Programmed direction. 0=normal, 1=reversed +PGM_RCP_PWM_POL EQU 5 ; Programmed RC pulse pwm polarity. 0=positive, 1=negative +FULL_THROTTLE_RANGE EQU 6 ; When set full throttle range is used (1000-2000us) and stored calibration values are ignored +ERRO_DATA EQU 7 ;增加编程卡数据错误标志 2013.8.27 +;**** **** **** **** **** +; RAM definitions +DSEG AT 30h ; Ram data segment, direct addressing +OS_FLAG: DS 1 +Initial_Arm: DS 1 ; Variable that is set during the first arm sequence after power on +;Pca_First_Int: DS 1 +Power_On_Wait_Cnt_L: DS 1 ; Power on wait counter (lo byte) +Power_On_Wait_Cnt_H: DS 1 ; Power on wait counter (hi byte) +Pgm_Pulse_Cnt: DS 1 +;Stepper_Step_Beg_L: DS 1 ; Stepper phase step time at the beginning (lo byte) +;Stepper_Step_Beg_H: DS 1 ; Stepper phase step time at the beginning (hi byte) +;Stepper_Step_End_L: DS 1 ; Stepper phase step time at the end (lo byte) +;Stepper_Step_End_H: DS 1 ; Stepper phase step time at the end (hi byte) +Startup_Rot_Cnt: DS 1 ; Startup phase rotations counter +Direct_Startup_Ok_Cnt: DS 1 ; Direct startup phase ok comparator waits counter (incrementing) + +Prev_Comm_L: DS 1 ; Previous commutation timer3 timestamp (lo byte) +Prev_Comm_H: DS 1 ; Previous commutation timer3 timestamp (hi byte) +Comm_Period4x_L: DS 1 ; Timer3 counts between the last 4 commutations (lo byte) +Comm_Period4x_H: DS 1 ; Timer3 counts between the last 4 commutations (hi byte) +Comm_Phase: DS 1 ; Current commutation phase +Comp_Wait_Reads: DS 1 ; Comparator wait comparator reads + +Gov_Target_L: DS 1 ; Governor target (lo byte) +Gov_Target_H: DS 1 ; Governor target (hi byte) +Gov_Integral_L: DS 1 ; Governor integral error (lo byte) +Gov_Integral_H: DS 1 ; Governor integral error (hi byte) +Gov_Integral_X: DS 1 ; Governor integral error (ex byte) +Gov_Proportional_L: DS 1 ; Governor proportional error (lo byte) +Gov_Proportional_H: DS 1 ; Governor proportional error (hi byte) +Gov_Prop_Pwm: DS 1 ; Governor calculated new pwm based upon proportional error +Gov_Arm_Target: DS 1 ; Governor arm target value +Gov_Active: DS 1 ; Governor active (enabled when speed is above minimum) + +Wt_Advance_L: DS 1 ; Timer3 counts for commutation advance timing (lo byte) +Wt_Advance_H: DS 1 ; Timer3 counts for commutation advance timing (hi byte) +Wt_Zc_Scan_L: DS 1 ; Timer3 counts from commutation to zero cross scan (lo byte) +Wt_Zc_Scan_H: DS 1 ; Timer3 counts from commutation to zero cross scan (hi byte) +Wt_Comm_L: DS 1 ; Timer3 counts from zero cross to commutation (lo byte) +Wt_Comm_H: DS 1 ; Timer3 counts from zero cross to commutation (hi byte) +;Wt_Stepper_Step_L: DS 1 ; Timer3 counts for stepper step (lo byte) +;Wt_Stepper_Step_H: DS 1 ; Timer3 counts for stepper step (hi byte) + +Rcp_PrePrev_Edge_L: DS 1 ; RC pulse pre previous edge pca timestamp (lo byte) +Rcp_PrePrev_Edge_H: DS 1 ; RC pulse pre previous edge pca timestamp (hi byte) +Rcp_Edge_L: DS 1 ; RC pulse edge pca timestamp (lo byte) +Rcp_Edge_H: DS 1 ; RC pulse edge pca timestamp (hi byte) +Rcp_Prev_Period_L: DS 1 ; RC pulse previous period (lo byte) +Rcp_Prev_Period_H: DS 1 ; RC pulse previous period (hi byte) +Rcp_Period_Diff_Accepted: DS 1 ; RC pulse period difference acceptable +New_Rcp: DS 1 ; New RC pulse value in pca counts +Prev_Rcp_Pwm_Freq: DS 1 ; Previous RC pulse pwm frequency (used during pwm frequency measurement) +Curr_Rcp_Pwm_Freq: DS 1 ; Current RC pulse pwm frequency (used during pwm frequency measurement) +Rcp_Stop_Cnt: DS 1 ; Counter for RC pulses below stop value + +Pwm_Limit: DS 1 ; Maximum allowed pwm +Pwm_Limit_Spoolup: DS 1 ; Maximum allowed pwm during spoolup of main +Pwm_Spoolup_Beg: DS 1 ; Pwm to begin main spoolup with +Pwm_Motor_Idle: DS 1 ; Motor idle speed pwm +Pwm_On_Cnt: DS 1 ; Pwm on event counter (used to increase pwm off time for low pwm) +Pwm_Off_Cnt: DS 1 ; Pwm off event counter (used to run some pwm cycles without damping) + +Spoolup_Limit_Cnt: DS 1 ; Interrupt count for spoolup limit +Spoolup_Limit_Skip: DS 1 ; Interrupt skips for spoolup limit increment (0=no skips, 1=skip one etc) + +Damping_Period: DS 1 ; Damping on/off period +Damping_On: DS 1 ; Damping on part of damping period + +;Lipo_Adc_Reference_L: DS 1 ; Voltage reference adc value (lo byte) +;Lipo_Adc_Reference_H: DS 1 ; Voltage reference adc value (hi byte) +Lipo_Adc_Limit_L: DS 1 ; Low voltage limit adc value (lo byte) +Lipo_Adc_Limit_H: DS 1 ; Low voltage limit adc value (hi byte) +Adc_Conversion_Cnt: DS 1 ; Adc conversion counter +Lipo_Cell_Count: DS 1 ;增加电池节数累计变量 2013.5.29 +Limit_Count: DS 1 ;增加逐渐递减计数变量 2013.7.5 + +Current_Average_Temp: DS 1 ; Current average temperature (lo byte ADC reading, assuming hi byte is 1) + +Ppm_Throttle_Gain: DS 1 ; Gain to be applied to RCP value for PPM input +Beep_Strength: DS 1 ; Strength of beeps + +Tx_Pgm_Func_No: DS 1 ; Function number when doing programming by tx +Tx_Pgm_Paraval_No: DS 1 ; Parameter value number when doing programming by tx +Tx_Pgm_Beep_No: DS 1 ; Beep number when doing programming by tx +Commu_Data_Buffer: DS 1 ;编程卡接收，发送数据暂存 2013.8.27 +Commu_Sum: DS 1 ;增加编程卡校验和 2013.8.27 +Min_Throttle: DS 1 + +; Indirect addressing data segment +ISEG AT 080h +Pgm_Fir_Key: DS 1 ;增加首个关键字 2013.8.27 +Pgm_Gov_P_Gain: DS 1 ; Programmed governor P gain +Pgm_Gov_I_Gain: DS 1 ; Programmed governor I gain +Pgm_Gov_Mode: DS 1 ; Programmed governor mode + +Pgm_Low_Voltage_Lim: DS 1 ; Programmed low voltage limit +Pgm_Low_Voltage_Ctl: DS 1 ;增加低压保护控制方式编程项 2013.7.5 +Pgm_Motor_Gain: DS 1 ; Programmed motor gain +Pgm_Motor_Idle: DS 1 ; Programmed motor idle speed +Pgm_Startup_Pwr: DS 1 ; Programmed startup power +Pgm_Pwm_Freq: DS 1 ; Programmed pwm frequency +Pgm_Direction_Rev: DS 1 ; Programmed rotation direction +Pgm_Input_Pol: DS 1 ; Programmed input pwm polarity +Initialized_L_Dummy: DS 1 ; Place holder +Initialized_H_Dummy: DS 1 ; Place holder +Pgm_Enable_TX_Program: DS 1 ; Programmed enable/disable value for TX programming +Pgm_Main_Rearm_Start: DS 1 ; Programmed enable/disable re-arming main every start +Pgm_Gov_Setup_Target: DS 1 ; Programmed main governor setup target +Pgm_Startup_Rpm: DS 1 ; Programmed startup rpm +Pgm_Startup_Accel: DS 1 ; Programmed startup acceleration +Pgm_Volt_Comp_Dummy: DS 1 ; Place holder +Pgm_Comm_Timing: DS 1 ; Programmed commutation timing +Pgm_Damping_Force: DS 1 ; Programmed damping force +Pgm_Gov_Range: DS 1 ; Programmed governor range +;Pgm_Startup_Method: DS 1 ; Programmed startup method +Pgm_Ppm_Min_Throttle: DS 1 ; Programmed throttle minimum +Pgm_Ppm_Max_Throttle: DS 1 ; Programmed throttle maximum +Pgm_Beep_Strength: DS 1 ; Programmed beep strength +Pgm_Beacon_Strength: DS 1 ; Programmed beacon strength +Pgm_Beacon_Delay: DS 1 ; Programmed beacon delay +Pgm_Throttle_Rate: DS 1 ; Programmed throttle rate +Pgm_Demag_Comp: DS 1 ; Programmed demag compensation +Pgm_BEC_Voltage_High: DS 1 ; Programmed BEC voltage + +Pgm_Gov_P_Gain_Decoded: DS 1 ; Programmed governor decoded P gain +Pgm_Gov_I_Gain_Decoded: DS 1 ; Programmed governor decoded I gain +Pgm_Throttle_Rate_Decoded: DS 1 ; Programmed throttle rate decoded +Pgm_Startup_Pwr_Decoded: DS 1 ; Programmed startup power decoded +Pgm_Demag_Comp_Wait_Decoded: DS 1 ; Programmed demag compensation wait decoded +Pgm_Demag_Comp_Power_Decoded: DS 1 ; Programmed demag compensation power cut decoded + + +; Indirect addressing data segment +ISEG AT 0D0h +Tag_Temporary_Storage: DS 48 ; Temporary storage for tags when updating "Eeprom" + + +;**** **** **** **** **** +CSEG AT 1A00h ; "Eeprom" segment +EEPROM_FW_MAIN_REVISION EQU 10 ; Main revision of the firmware +EEPROM_FW_SUB_REVISION EQU 2 ; Sub revision of the firmware +EEPROM_LAYOUT_REVISION EQU 16 ; Revision of the EEPROM layout + +Eep_FW_Main_Revision: DB EEPROM_FW_MAIN_REVISION ; EEPROM firmware main revision number +Eep_FW_Sub_Revision: DB EEPROM_FW_SUB_REVISION ; EEPROM firmware sub revision number +Eep_Layout_Revision: DB EEPROM_LAYOUT_REVISION ; EEPROM layout revision number + +IF MODE == 0 +Eep_Pgm_Gov_P_Gain: DB DEFAULT_PGM_MAIN_P_GAIN ; EEPROM copy of programmed governor P gain +Eep_Pgm_Gov_I_Gain: DB DEFAULT_PGM_MAIN_I_GAIN ; EEPROM copy of programmed governor I gain +Eep_Pgm_Gov_Mode: DB DEFAULT_PGM_MAIN_GOVERNOR_MODE ; EEPROM copy of programmed governor mode + +Eep_Pgm_Low_Voltage_Lim: DB DEFAULT_PGM_MAIN_LOW_VOLTAGE_LIM ; EEPROM copy of programmed low voltage limit +_Eep_Pgm_Motor_Gain: DB 0FFh +_Eep_Pgm_Motor_Idle: DB 0FFh +Eep_Pgm_Startup_Pwr: DB DEFAULT_PGM_MAIN_STARTUP_PWR ; EEPROM copy of programmed startup power +Eep_Pgm_Pwm_Freq: DB DEFAULT_PGM_MAIN_PWM_FREQ ; EEPROM copy of programmed pwm frequency +Eep_Pgm_Direction_Rev: DB DEFAULT_PGM_MAIN_DIRECTION_REV ; EEPROM copy of programmed rotation direction +Eep_Pgm_Input_Pol: DB DEFAULT_PGM_MAIN_RCP_PWM_POL ; EEPROM copy of programmed input polarity +Eep_Initialized_L: DB 0A5h ; EEPROM initialized signature low byte +Eep_Initialized_H: DB 05Ah ; EEPROM initialized signature high byte +Eep_Enable_TX_Program: DB DEFAULT_PGM_ENABLE_TX_PROGRAM ; EEPROM TX programming enable +Eep_Main_Rearm_Start: DB DEFAULT_PGM_MAIN_REARM_START ; EEPROM re-arming main enable +Eep_Pgm_Gov_Setup_Target: DB DEFAULT_PGM_MAIN_GOV_SETUP_TARGET ; EEPROM main governor setup target +Eep_Pgm_Startup_Rpm: DB DEFAULT_PGM_MAIN_STARTUP_RPM ; EEPROM copy of programmed startup rpm +Eep_Pgm_Startup_Accel: DB DEFAULT_PGM_MAIN_STARTUP_ACCEL ; EEPROM copy of programmed startup acceleration +_Eep_Pgm_Volt_Comp: DB 0FFh +Eep_Pgm_Comm_Timing: DB DEFAULT_PGM_MAIN_COMM_TIMING ; EEPROM copy of programmed commutation timing +Eep_Pgm_Damping_Force: DB DEFAULT_PGM_MAIN_DAMPING_FORCE ; EEPROM copy of programmed damping force +Eep_Pgm_Gov_Range: DB DEFAULT_PGM_MAIN_GOVERNOR_RANGE ; EEPROM copy of programmed governor range +Eep_Pgm_Startup_Method: DB DEFAULT_PGM_MAIN_STARTUP_METHOD ; EEPROM copy of programmed startup method +Eep_Pgm_Ppm_Min_Throttle: DB DEFAULT_PGM_PPM_MIN_THROTTLE ; EEPROM copy of programmed minimum throttle (final value is 4x+1000=1148) +Eep_Pgm_Ppm_Max_Throttle: DB DEFAULT_PGM_PPM_MAX_THROTTLE ; EEPROM copy of programmed minimum throttle (final value is 4x+1000=1832) +Eep_Pgm_Beep_Strength: DB DEFAULT_PGM_MAIN_BEEP_STRENGTH ; EEPROM copy of programmed beep strength +Eep_Pgm_Beacon_Strength: DB DEFAULT_PGM_MAIN_BEACON_STRENGTH ; EEPROM copy of programmed beacon strength +Eep_Pgm_Beacon_Delay: DB DEFAULT_PGM_MAIN_BEACON_DELAY ; EEPROM copy of programmed beacon delay +Eep_Pgm_Throttle_Rate: DB DEFAULT_PGM_MAIN_THROTTLE_RATE ; EEPROM copy of programmed throttle rate +Eep_Pgm_Demag_Comp: DB DEFAULT_PGM_MAIN_DEMAG_COMP ; EEPROM copy of programmed demag compensation +Eep_Pgm_BEC_Voltage_High: DB DEFAULT_PGM_BEC_VOLTAGE_HIGH ; EEPROM copy of programmed BEC voltage +ENDIF + +IF MODE == 1 +_Eep_Pgm_Gov_P_Gain: DB 0FFh +_Eep_Pgm_Gov_I_Gain: DB 0FFh +_Eep_Pgm_Gov_Mode: DB 0FFh + +_Eep_Pgm_Low_Voltage_Lim: DB 0FFh +Eep_Pgm_Motor_Gain: DB DEFAULT_PGM_TAIL_GAIN ; EEPROM copy of programmed tail gain +Eep_Pgm_Motor_Idle: DB DEFAULT_PGM_TAIL_IDLE_SPEED ; EEPROM copy of programmed tail idle speed +Eep_Pgm_Startup_Pwr: DB DEFAULT_PGM_TAIL_STARTUP_PWR ; EEPROM copy of programmed startup power +Eep_Pgm_Pwm_Freq: DB DEFAULT_PGM_TAIL_PWM_FREQ ; EEPROM copy of programmed pwm frequency +Eep_Pgm_Direction_Rev: DB DEFAULT_PGM_TAIL_DIRECTION_REV ; EEPROM copy of programmed rotation direction +Eep_Pgm_Input_Pol: DB DEFAULT_PGM_TAIL_RCP_PWM_POL ; EEPROM copy of programmed input polarity +Eep_Initialized_L: DB 05Ah ; EEPROM initialized signature low byte +Eep_Initialized_H: DB 0A5h ; EEPROM initialized signature high byte +Eep_Enable_TX_Program: DB DEFAULT_PGM_ENABLE_TX_PROGRAM ; EEPROM TX programming enable +_Eep_Main_Rearm_Start: DB 0FFh +_Eep_Pgm_Gov_Setup_Target: DB 0FFh +Eep_Pgm_Startup_Rpm: DB DEFAULT_PGM_TAIL_STARTUP_RPM ; EEPROM copy of programmed startup rpm +Eep_Pgm_Startup_Accel: DB DEFAULT_PGM_TAIL_STARTUP_ACCEL ; EEPROM copy of programmed startup acceleration +_Eep_Pgm_Volt_Comp: DB 0FFh +Eep_Pgm_Comm_Timing: DB DEFAULT_PGM_TAIL_COMM_TIMING ; EEPROM copy of programmed commutation timing +Eep_Pgm_Damping_Force: DB DEFAULT_PGM_TAIL_DAMPING_FORCE ; EEPROM copy of programmed damping force +_Eep_Pgm_Gov_Range: DB 0FFh +Eep_Pgm_Startup_Method: DB DEFAULT_PGM_TAIL_STARTUP_METHOD ; EEPROM copy of programmed startup method +Eep_Pgm_Ppm_Min_Throttle: DB DEFAULT_PGM_PPM_MIN_THROTTLE ; EEPROM copy of programmed minimum throttle (final value is 4x+1000=1148) +Eep_Pgm_Ppm_Max_Throttle: DB DEFAULT_PGM_PPM_MAX_THROTTLE ; EEPROM copy of programmed minimum throttle (final value is 4x+1000=1832) +Eep_Pgm_Beep_Strength: DB DEFAULT_PGM_TAIL_BEEP_STRENGTH ; EEPROM copy of programmed beep strength +Eep_Pgm_Beacon_Strength: DB DEFAULT_PGM_TAIL_BEACON_STRENGTH ; EEPROM copy of programmed beacon strength +Eep_Pgm_Beacon_Delay: DB DEFAULT_PGM_TAIL_BEACON_DELAY ; EEPROM copy of programmed beacon delay +Eep_Pgm_Throttle_Rate: DB DEFAULT_PGM_TAIL_THROTTLE_RATE ; EEPROM copy of programmed throttle rate +Eep_Pgm_Demag_Comp: DB DEFAULT_PGM_TAIL_DEMAG_COMP ; EEPROM copy of programmed demag compensation +Eep_Pgm_BEC_Voltage_High: DB DEFAULT_PGM_BEC_VOLTAGE_HIGH ; EEPROM copy of programmed BEC voltage +ENDIF + +IF MODE == 2 +Eep_Pgm_Fir_Key: DB DEFAULT_PGM_MULTI_FIRST_KEYWORD ;增加首个关键字 2013.8.27 +Eep_Pgm_Gov_P_Gain: DB DEFAULT_PGM_MULTI_P_GAIN ; EEPROM copy of programmed closed loop P gain +Eep_Pgm_Gov_I_Gain: DB DEFAULT_PGM_MULTI_I_GAIN ; EEPROM copy of programmed closed loop I gain +Eep_Pgm_Gov_Mode: DB DEFAULT_PGM_MULTI_GOVERNOR_MODE ; EEPROM copy of programmed closed loop mode + +Eep_Pgm_Low_Voltage_Lim: DB DEFAULT_PGM_MULTI_LOW_VOLTAGE_LIM ; EEPROM copy of programmed low voltage limit +Eep_Pgm_Low_Voltage_Ctl: DB DEFAULT_PGM_MULTI_LOW_VOLTAGE_CTL ; 增加低压保护控制编程 +Eep_Pgm_Motor_Gain: DB DEFAULT_PGM_MULTI_GAIN ; EEPROM copy of programmed tail gain +_Eep_Pgm_Motor_Idle: DB 0FFh ; EEPROM copy of programmed tail idle speed +Eep_Pgm_Startup_Pwr: DB DEFAULT_PGM_MULTI_STARTUP_PWR ; EEPROM copy of programmed startup power +Eep_Pgm_Pwm_Freq: DB DEFAULT_PGM_MULTI_PWM_FREQ ; EEPROM copy of programmed pwm frequency +Eep_Pgm_Direction_Rev: DB DEFAULT_PGM_MULTI_DIRECTION_REV ; EEPROM copy of programmed rotation direction +Eep_Pgm_Input_Pol: DB DEFAULT_PGM_MULTI_RCP_PWM_POL ; EEPROM copy of programmed input polarity +Eep_Initialized_L: DB 055h ; EEPROM initialized signature low byte +Eep_Initialized_H: DB 0AAh ; EEPROM initialized signature high byte +Eep_Enable_TX_Program: DB DEFAULT_PGM_ENABLE_TX_PROGRAM ; EEPROM TX programming enable +_Eep_Main_Rearm_Start: DB 0FFh +_Eep_Pgm_Gov_Setup_Target: DB 0FFh +Eep_Pgm_Startup_Rpm: DB DEFAULT_PGM_MULTI_STARTUP_RPM ; EEPROM copy of programmed startup rpm +Eep_Pgm_Startup_Accel: DB DEFAULT_PGM_MULTI_STARTUP_ACCEL ; EEPROM copy of programmed startup acceleration +_Eep_Pgm_Volt_Comp: DB 0FFh +Eep_Pgm_Comm_Timing: DB DEFAULT_PGM_MULTI_COMM_TIMING ; EEPROM copy of programmed commutation timing +Eep_Pgm_Damping_Force: DB DEFAULT_PGM_MULTI_DAMPING_FORCE ; EEPROM copy of programmed damping force +_Eep_Pgm_Gov_Range: DB 0FFh +;Eep_Pgm_Startup_Method: DB DEFAULT_PGM_MULTI_STARTUP_METHOD ; EEPROM copy of programmed startup method +Eep_Pgm_Ppm_Min_Throttle: DB DEFAULT_PGM_PPM_MIN_THROTTLE ; EEPROM copy of programmed minimum throttle (final value is 4x+1000=1148) +Eep_Pgm_Ppm_Max_Throttle: DB DEFAULT_PGM_PPM_MAX_THROTTLE ; EEPROM copy of programmed minimum throttle (final value is 4x+1000=1832) +Eep_Pgm_Beep_Strength: DB DEFAULT_PGM_MULTI_BEEP_STRENGTH ; EEPROM copy of programmed beep strength +Eep_Pgm_Beacon_Strength: DB DEFAULT_PGM_MULTI_BEACON_STRENGTH ; EEPROM copy of programmed beacon strength +Eep_Pgm_Beacon_Delay: DB DEFAULT_PGM_MULTI_BEACON_DELAY ; EEPROM copy of programmed beacon delay +Eep_Pgm_Throttle_Rate: DB DEFAULT_PGM_MULTI_THROTTLE_RATE ; EEPROM copy of programmed throttle rate +Eep_Pgm_Demag_Comp: DB DEFAULT_PGM_MULTI_DEMAG_COMP ; EEPROM copy of programmed demag compensation +Eep_Pgm_BEC_Voltage_High: DB DEFAULT_PGM_BEC_VOLTAGE_HIGH ; EEPROM copy of programmed BEC voltage +ENDIF + + +Eep_Dummy: DB 0FFh ; EEPROM address for safety reason + +CSEG AT 1A60h +Eep_Name: DB " " ; Name tag (16 Bytes) + +;**** **** **** **** **** + Interrupt_Table_Definition ; SiLabs interrupts +CSEG AT 80h ; Code segment after interrupt vectors + +;**** **** **** **** **** + +; Table definitions +GOV_GAIN_TABLE: DB 02h, 03h, 04h, 06h, 08h, 0Ch, 10h, 18h, 20h, 30h, 40h, 60h, 80h +THROTTLE_RATE_TABLE: DB 02h, 03h, 04h, 06h, 08h, 0Ch, 10h, 18h, 20h, 30h, 40h, 80h, 0FFh +STARTUP_POWER_TABLE: DB 04h, 06h, 08h, 0Ch, 10h, 18h, 20h, 30h, 40h, 60h, 80h, 0A0h, 0C0h +DEMAG_WAIT_TABLE: DB 0, 0, 0, 1, 1 +DEMAG_POWER_TABLE: DB 0, 3, 2, 2, 1 + +;FINISH_KEYWORD_TABLE: DB 066h, 08h, 08h, 03h, 01h, 00h, 02h, 00h +; DB 09h, 01h, 00h, 00h, 00h, 00h, 00h, 00h +; DB 0B4h, 00h, 04h, 00h, 02h, 00h, 00h, 025h +; DB 0D0h, 050h, 064h, 01h, 0Dh, 01h, 00h, 00h ;增加编程默认值表 2013.7.30 ;////////////////////////////////////////// +; DB 00h, 00h, 00h, 00h, 00h, 00h, 00h, 00h +; DB 00h, 00h, 00h, 00h, 00h, 00h, 00h, 00h + +IF MODE == 0 +TX_PGM_PARAMS_MAIN: DB 13, 13, 4, 2, 6, 2, 13, 5, 5, 5, 13, 6, 3, 5, 2, 2 +ENDIF +IF MODE == 1 + IF DAMPED_MODE_ENABLE == 1 +TX_PGM_PARAMS_TAIL: DB 5, 5, 2, 13, 5, 5, 5, 13, 6, 4, 5, 2, 2 + ENDIF + IF DAMPED_MODE_ENABLE == 0 +TX_PGM_PARAMS_TAIL: DB 5, 5, 2, 13, 5, 5, 5, 13, 6, 3, 5, 2, 2 + ENDIF +ENDIF +IF MODE == 2 + IF DAMPED_MODE_ENABLE == 1 +;///////TX_PGM_PARAMS_MULTI: DB 13, 13, 4, 5, 6, 2, 13, 5, 5, 5, 13, 6, 4, 5, 2, 2 + TX_PGM_PARAMS_MULTI: DB 6, 5, 13, 4, 2, 4, 2 + ENDIF + IF DAMPED_MODE_ENABLE == 0 +;///////TX_PGM_PARAMS_MULTI: DB 13, 13, 4, 5, 6, 2, 13, 5, 5, 5, 13, 6, 3, 5, 2, 2 + TX_PGM_PARAMS_MULTI: DB 6, 5, 13, 4, 2, 4, 2 + ENDIF +ENDIF + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Timer0 interrupt routine +; +; Assumptions: DPTR register must be set to desired pwm_nfet_on label +; Requirements: Temp variables can NOT be used since PWSW.3 is not set +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +t0_int: ; Used for pwm control + clr EA ; Disable all interrupts + push PSW ; Preserve registers through interrupt + push ACC + ; Check if pwm is on + jb Flags0.PWM_ON, t0_int_pwm_off ; Is pwm on? + + ; Pwm on cycle. + jnb Current_Pwm_Limited.7, t0_int_pwm_on_low_pwm ; Jump for low pwm (<50%) + +t0_int_pwm_on_execute: + clr A + jmp @A+DPTR ; No - jump to pwm on routines. DPTR should be set to one of the pwm_nfet_on labels + +t0_int_pwm_on_low_pwm: + ; Do not execute pwm when stopped + jnb Flags1.MOTOR_SPINNING, t0_int_pwm_on_stopped + +IF MODE == 0 OR MODE == 2 ; Main or multi + jmp t0_int_pwm_on_execute +ENDIF +IF MODE == 1 ; Tail + ; Skip pwm on cycles for very low pwm + inc Pwm_On_Cnt ; Increment event counter + clr C + mov A, #5 ; Only skip for very low pwm + subb A, Current_Pwm_Limited ; Check skipping shall be done (for low pwm only) + jc t0_int_pwm_on_execute + + subb A, Pwm_On_Cnt ; Check if on cycle is to be skipped + jc t0_int_pwm_on_execute + + mov TL0, #120 ; Write start point for timer + mov A, Current_Pwm_Limited + jnz ($+5) + mov TL0, #0 ; Write start point for timer (long time for zero pwm) + jmp t0_int_pwm_on_exit_no_timer_update +ENDIF + +t0_int_pwm_on_stopped: + jmp t0_int_pwm_on_exit + + +t0_int_pwm_off: + jnb Flags1.DIRECT_STARTUP_PHASE, t0_int_pwm_off_start_checked + All_nFETs_Off ; Switch off all nfets early during direct start, for a smooth start +t0_int_pwm_off_start_checked: + ; Pwm off cycle + mov TL0, Current_Pwm_Limited ; Load new timer setting + ; Clear pwm on flag + clr Flags0.PWM_ON + ; 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 + mov A, Current_Pwm_Limited ; Load current pwm + cpl A ; Full pwm? + jnz ($+4) ; No - branch + ajmp t0_int_pwm_off_fullpower_exit ; Yes - exit + + inc Pwm_Off_Cnt ; Increment event counter + ; Do not execute pwm when stopped + jnb Flags1.MOTOR_SPINNING, t0_int_pwm_off_stopped + + ; If damped operation, set pFETs on in pwm_off + jb Flags2.PGM_PWMOFF_DAMPED, t0_int_pwm_off_damped ; Damped operation? + + ; Separate exit commands here for minimum delay + mov TL1, #0 ; Reset timer1 + pop ACC ; Restore preserved registers + pop PSW + All_nFETs_Off ; Switch off all nfets + setb EA ; Enable all interrupts + reti + +t0_int_pwm_off_stopped: + All_nFETs_Off ; Switch off all nfets + jmp t0_int_pwm_off_exit + +t0_int_pwm_off_damped: + setb Flags2.CURR_PWMOFF_DAMPED ; Set damped status + clr Flags2.CURR_PWMOFF_COMP_ABLE ; Set comparator unusable status + mov A, Damping_On + jz t0_int_pwm_off_do_damped ; Highest damping - apply damping always + + clr C + mov A, Pwm_Off_Cnt ; Is damped on number reached? + dec A + subb A, Damping_On + jc t0_int_pwm_off_do_damped ; No - apply damping + + clr Flags2.CURR_PWMOFF_DAMPED ; Set non damped status + setb Flags2.CURR_PWMOFF_COMP_ABLE ; Set comparator usable status + clr C + mov A, Pwm_Off_Cnt + subb A, Damping_Period ; Is damped period number reached? + jnc t0_int_pwm_off_clr_cnt ; Yes - Proceed + + jmp t0_int_pwm_off_exit ; No - Branch + +t0_int_pwm_off_clr_cnt: + mov Pwm_Off_Cnt, #0 ; Yes - clear counter + jmp t0_int_pwm_off_exit ; Not damped cycle - exit + +t0_int_pwm_off_do_damped: + ; Delay to allow nFETs to go off before pFETs are turned on (only in full damped mode) + jb Flags2.PGM_PWMOFF_DAMPED_LIGHT, t0_int_pwm_off_damped_light ; If damped light operation - branch + +; All_pFETs_Off ; Switch off all nfets +; mov A, #PFETON_DELAY +; djnz ACC, $ +; All_nFETs_On ; Switch on all pfets + jmp t0_int_pwm_off_exit + +t0_int_pwm_off_damped_light: +IF DAMPED_MODE_ENABLE == 1 + setb Flags2.CURR_PWMOFF_COMP_ABLE ; Set comparator usable status always for damped light mode on fully damped capable escs +ENDIF + All_nFETs_Off ; Switch off all nfets + mov A, Comm_Phase ; Turn on pfets according to commutation phase + jb ACC.2, t0_int_pwm_off_comm_4_5_6 + jb ACC.1, t0_int_pwm_off_comm_2_3 + +IF DAMPED_MODE_ENABLE == 0 + AnFET_On ; Comm phase 1 - turn on A +ELSE + mov A, #PFETON_DELAY + djnz ACC, $ + CnFET_On ; Comm phase 1 - turn on C +ENDIF + jmp t0_int_pwm_off_exit + +t0_int_pwm_off_comm_2_3: + jb ACC.0, t0_int_pwm_off_comm_3 +IF DAMPED_MODE_ENABLE == 0 + BnFET_On ; Comm phase 2 - turn on B +ELSE + mov A, #PFETON_DELAY + djnz ACC, $ + CnFET_On ; Comm phase 2 - turn on C +ENDIF + jmp t0_int_pwm_off_exit + +t0_int_pwm_off_comm_3: +IF DAMPED_MODE_ENABLE == 0 + CnFET_On ; Comm phase 3 - turn on C +ELSE + mov A, #PFETON_DELAY + djnz ACC, $ + BnFET_On ; Comm phase 3 - turn on B +ENDIF + jmp t0_int_pwm_off_exit + +t0_int_pwm_off_comm_4_5_6: + jb ACC.1, t0_int_pwm_off_comm_6 + jb ACC.0, t0_int_pwm_off_comm_5 + +IF DAMPED_MODE_ENABLE == 0 + AnFET_On ; Comm phase 4 - turn on A +ELSE + mov A, #PFETON_DELAY + djnz ACC, $ + BnFET_On ; Comm phase 4 - turn on B +ENDIF + jmp t0_int_pwm_off_exit + +t0_int_pwm_off_comm_5: +IF DAMPED_MODE_ENABLE == 0 + BnFET_On ; Comm phase 5 - turn on B +ELSE + mov A, #PFETON_DELAY + djnz ACC, $ + AnFET_On ; Comm phase 5 - turn on A +ENDIF + jmp t0_int_pwm_off_exit + +t0_int_pwm_off_comm_6: +IF DAMPED_MODE_ENABLE == 0 + CnFET_On ; Comm phase 6 - turn on C +ELSE + mov A, #PFETON_DELAY + djnz ACC, $ + AnFET_On ; Comm phase 6 - turn on A +ENDIF + +t0_int_pwm_off_exit: ; Exit from pwm off cycle + mov TL1, #0 ; Reset timer1 + pop ACC ; Restore preserved registers + pop PSW + All_nFETs_Off ; Switch off all nfets + setb EA ; Enable all interrupts + reti + +t0_int_pwm_off_fullpower_exit: ; Exit from pwm off cycle, leaving power on + pop ACC ; Restore preserved registers + pop PSW + setb EA ; Enable all interrupts + reti + + + +pwm_nofet_on: ; Dummy pwm on cycle + ajmp t0_int_pwm_on_exit + +pwm_afet_on: ; Pwm on cycle afet on (bfet off) + AnFET_on + BnFET_off + ajmp t0_int_pwm_on_exit + +pwm_bfet_on: ; Pwm on cycle bfet on (cfet off) + BnFET_on + CnFET_off + ajmp t0_int_pwm_on_exit + +pwm_cfet_on: ; Pwm on cycle cfet on (afet off) + CnFET_on + AnFET_off + ajmp t0_int_pwm_on_exit + +pwm_anfet_bpfet_on_fast: ; Pwm on cycle anfet on (bnfet off) and bpfet on (used in damped state 6) + ApFET_off + AnFET_on ; Switch nFETs + CpFET_off + BnFET_off + ajmp t0_int_pwm_on_exit +pwm_anfet_bpfet_on_safe: ; Pwm on cycle anfet on (bnfet off) and bpfet on (used in damped state 6) + ; Delay from pFETs are turned off (only in damped mode) until nFET is turned on (pFETs are slow) + ApFET_off + CpFET_off + mov A, #NFETON_DELAY ; Set full delay + djnz ACC, $ + AnFET_on ; Switch nFETs + BnFET_off + ajmp t0_int_pwm_on_exit + +pwm_anfet_cpfet_on_fast: ; Pwm on cycle anfet on (bnfet off) and cpfet on (used in damped state 5) + ApFET_off + AnFET_on ; Switch nFETs + BpFET_off + BnFET_off + ajmp t0_int_pwm_on_exit +pwm_anfet_cpfet_on_safe: ; Pwm on cycle anfet on (bnfet off) and cpfet on (used in damped state 5) + ; Delay from pFETs are turned off (only in damped mode) until nFET is turned on (pFETs are slow) + ApFET_off + BpFET_off + mov A, #NFETON_DELAY ; Set full delay + djnz ACC, $ + AnFET_on ; Switch nFETs + BnFET_off + ajmp t0_int_pwm_on_exit + +pwm_bnfet_cpfet_on_fast: ; Pwm on cycle bnfet on (cnfet off) and cpfet on (used in damped state 4) + BpFET_off + BnFET_on ; Switch nFETs + ApFET_off + CnFET_off + ajmp t0_int_pwm_on_exit +pwm_bnfet_cpfet_on_safe: ; Pwm on cycle bnfet on (cnfet off) and cpfet on (used in damped state 4) + ; Delay from pFETs are turned off (only in damped mode) until nFET is turned on (pFETs are slow) + BpFET_off + ApFET_off + mov A, #NFETON_DELAY ; Set full delay + djnz ACC, $ + BnFET_on ; Switch nFETs + CnFET_off + ajmp t0_int_pwm_on_exit + +pwm_bnfet_apfet_on_fast: ; Pwm on cycle bnfet on (cnfet off) and apfet on (used in damped state 3) + BpFET_off + BnFET_on ; Switch nFETs + CpFET_off + CnFET_off + ajmp t0_int_pwm_on_exit +pwm_bnfet_apfet_on_safe: ; Pwm on cycle bnfet on (cnfet off) and apfet on (used in damped state 3) + ; Delay from pFETs are turned off (only in damped mode) until nFET is turned on (pFETs are slow) + BpFET_off + CpFET_off + mov A, #NFETON_DELAY ; Set full delay + djnz ACC, $ + BnFET_on ; Switch nFETs + CnFET_off + ajmp t0_int_pwm_on_exit + +pwm_cnfet_apfet_on_fast: ; Pwm on cycle cnfet on (anfet off) and apfet on (used in damped state 2) + CpFET_off + CnFET_on ; Switch nFETs + BpFET_off + AnFET_off + ajmp t0_int_pwm_on_exit +pwm_cnfet_apfet_on_safe: ; Pwm on cycle cnfet on (anfet off) and apfet on (used in damped state 2) + ; Delay from pFETs are turned off (only in damped mode) until nFET is turned on (pFETs are slow) + CpFET_off + BpFET_off + mov A, #NFETON_DELAY ; Set full delay + djnz ACC, $ + CnFET_on ; Switch nFETs + AnFET_off + ajmp t0_int_pwm_on_exit + +pwm_cnfet_bpfet_on_fast: ; Pwm on cycle cnfet on (anfet off) and bpfet on (used in damped state 1) + CpFET_off + CnFET_on ; Switch nFETs + ApFET_off + AnFET_off + ajmp t0_int_pwm_on_exit +pwm_cnfet_bpfet_on_safe: ; Pwm on cycle cnfet on (anfet off) and bpfet on (used in damped state 1) + ; Delay from pFETs are turned off (only in damped mode) until nFET is turned on (pFETs are slow) + CpFET_off + ApFET_off + mov A, #NFETON_DELAY ; Set full delay + djnz ACC, $ + CnFET_on ; Switch nFETs + AnFET_off + ajmp t0_int_pwm_on_exit + +t0_int_pwm_on_exit: + ; Set timer for coming on cycle length + mov A, Current_Pwm_Limited ; Load current pwm + cpl A ; cpl is 255-x + mov TL0, A ; Write start point for timer + ; Set other variables + mov TL1, #0 ; Reset timer1 + mov Pwm_On_Cnt, #0 ; Reset pwm on event counter + setb Flags0.PWM_ON ; Set pwm on flag +t0_int_pwm_on_exit_no_timer_update: + ; Exit interrupt + pop ACC ; Restore preserved registers + pop PSW + setb EA ; Enable all interrupts + reti + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Timer2 interrupt routine +; +; No assumptions +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +t2_int: ; Happens every 128us for low byte and every 32ms for high byte + clr EA ; Disable all interrupts + push PSW ; Preserve registers through interrupt + push ACC + setb PSW.3 ; Select register bank 1 for interrupt routines + ; Clear low byte interrupt flag + clr TF2L ; Clear interrupt flag + ; Check RC pulse timeout counter + mov A, Rcp_Timeout_Cnt ; RC pulse timeout count zero? + jz t2_int_pulses_absent ; Yes - pulses are absent + + ; Decrement timeout counter (if PWM) +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) +; anl A, Flags3 ; Check pwm frequency flags +; jz t2_int_skip_start ; If no flag is set (PPM) - branch + +; dec Rcp_Timeout_Cnt ; No - decrement + ajmp t2_int_skip_start + +t2_int_pulses_absent: + ; Timeout counter has reached zero, pulses are absent + mov Temp1, #RCP_MIN ; RCP_MIN as default + mov Temp2, #RCP_MIN + Read_Rcp_Int ; Look at value of Rcp_In + jnb ACC.Rcp_In, ($+5) ; Is it high? + mov Temp1, #RCP_MAX ; Yes - set RCP_MAX + Rcp_Int_First ; Set interrupt trig to first again + Rcp_Clear_Int_Flag ; Clear interrupt flag + clr Flags2.RCP_EDGE_NO ; Set first edge flag + Read_Rcp_Int ; Look once more at value of Rcp_In + jnb ACC.Rcp_In, ($+5) ; Is it high? + mov Temp2, #RCP_MAX ; Yes - set RCP_MAX + clr C + mov A, Temp1 + subb A, Temp2 ; Compare the two readings of Rcp_In + jnz t2_int_pulses_absent ; Go back if they are not equal + + jnb Flags0.RCP_MEAS_PWM_FREQ, ($+6) ; Is measure RCP pwm frequency flag set? + + mov Rcp_Timeout_Cnt, #RCP_TIMEOUT ; Yes - set timeout count to start value +; mov Pgm_Pulse_Cnt, #0 ;清除PCA中断计数 +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) +; anl A, Flags3 ; Check pwm frequency flags +; jz t2_int_ppm_timeout_set ; If no flag is set (PPM) - branch + +; mov Rcp_Timeout_Cnt, #RCP_TIMEOUT ; For PWM, set timeout count to start value + +t2_int_ppm_timeout_set: + mov New_Rcp, Temp1 ; Store new pulse length + setb Flags2.RCP_UPDATED ; Set updated flag + + +t2_int_skip_start: + ; Check RC pulse skip counter + mov A, Rcp_Skip_Cnt + jz t2_int_skip_end ; If RC pulse skip count is zero - end skipping RC pulse detection + + ; Decrement skip counter (only if edge counter is zero) + dec Rcp_Skip_Cnt ; Decrement + ajmp t2_int_rcp_update_start + +t2_int_skip_end: +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) +; anl A, Flags3 ; Check pwm frequency flags +; jz t2_int_rcp_update_start ; If no flag is set (PPM) - branch + + ; Skip counter has reached zero, start looking for RC pulses again +; Rcp_Int_Enable ; Enable RC pulse interrupt +; Rcp_Clear_Int_Flag ; Clear interrupt flag + +t2_int_rcp_update_start: + ; Process updated RC pulse + jb Flags2.RCP_UPDATED, ($+5) ; Is there an updated RC pulse available? + ajmp t2_int_pwm_exit ; No - exit + + mov A, New_Rcp ; Load new pulse value + mov Temp1, A + clr Flags2.RCP_UPDATED ; Flag that pulse has been evaluated + ; Use a gain of 1.0625x for pwm input if not governor mode +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) +; anl A, Flags3 ; Check pwm frequency flags +; jz t2_int_pwm_min_run ; If no flag is set (PPM) - branch +;;***************************************************** +;; Closed loop mode 选择 +;;***************************************************** +;IF MODE <= 1 ; Main or tail +; mov Temp2, #Pgm_Gov_Mode ; Governor mode? +; cjne @Temp2, #4, t2_int_pwm_min_run; Yes - branch +;ENDIF +;IF MODE == 2 ; Multi +;;读取Closed loop mode +; mov Temp2, #Pgm_Gov_Mode ; Closed loop mode? +; cjne @Temp2, #4, t2_int_pwm_min_run; Yes - branch +;ENDIF + +; ; Limit the maximum value to avoid wrap when scaled to pwm range +;;Closed loop mode = 第1项 Off +; clr C +; mov A, Temp1 +; subb A, #240 ; 240 = (255/1.0625) Needs to be updated according to multiplication factor below +; jc t2_int_rcp_update_mult + +; mov A, #240 ; Set requested pwm to max +; mov Temp1, A + +;t2_int_rcp_update_mult: +; ; Multiply by 1.0625 (optional adjustment gyro gain) +; mov A, Temp1 +; swap A ; After this "0.0625" +; anl A, #0Fh +; add A, Temp1 +; mov Temp1, A +; ; Adjust tail gain +;;读取 Multi gain +; mov Temp2, #Pgm_Motor_Gain +; cjne @Temp2, #3, ($+5) ; Is gain 1? +; ajmp t2_int_pwm_min_run ; Yes - skip adjustment +;;Multi gain = 第2选项 +; clr C +; rrc A ; After this "0.5" +; clr C +; rrc A ; After this "0.25" +; mov Bit_Access_Int, @Temp2 ; (Temp2 has #Pgm_Motor_Gain) +; jb Bit_Access_Int.0, t2_int_rcp_gain_corr ; Branch if bit 0 in gain is set +;;Multi gain = 第4选项 +; clr C +; rrc A ; After this "0.125" + +;t2_int_rcp_gain_corr: +; jb Bit_Access_Int.2, t2_int_rcp_gain_pos ; Branch if bit 2 in gain is set +;;Multi gain = 第1选项 +; clr C +; xch A, Temp1 +; subb A, Temp1 ; Apply negative correction +; mov Temp1, A +; ajmp t2_int_pwm_min_run +;;Multi gain = 第5选项 +;t2_int_rcp_gain_pos: +; add A, Temp1 ; Apply positive correction +; mov Temp1, A +; jnc t2_int_pwm_min_run ; Above max? + +; mov A, #0FFh ; Yes - limit +; mov Temp1, A +;;Multi gain = 第3选项 +t2_int_pwm_min_run: + ; Limit minimum pwm + clr C + mov A, Temp1 + subb A, Pwm_Motor_Idle ; Is requested pwm lower than minimum? + jnc t2_int_pwm_update ; No - branch + + mov A, Pwm_Motor_Idle ; Yes - limit pwm to Pwm_Motor_Idle + mov Temp1, A + +t2_int_pwm_update: + ; Check if any startup phase flags are set + mov A, Flags1 + anl A, #((1 SHL SETTLE_PHASE)+(1 SHL STEPPER_PHASE)) + jnz t2_int_pwm_exit ; Exit if any startup phase set (pwm controlled by set_startup_pwm) + + ; Update requested_pwm + mov Requested_Pwm, Temp1 ; Set requested pwm + ; Limit pwm during direct start + jnb Flags1.DIRECT_STARTUP_PHASE, t2_int_current_pwm_update + + clr C + mov A, Requested_Pwm ; Limit pwm during direct start + subb A, Pwm_Limit + jc ($+7) + + mov Requested_Pwm, Pwm_Limit + ajmp t2_int_current_pwm_update + + mov A, Requested_Pwm + add A, #11 + mov Requested_Pwm, A + +t2_int_current_pwm_update: +IF MODE <= 1 ; Main or tail + mov Temp1, #Pgm_Gov_Mode ; Governor mode? + cjne @Temp1, #4, t2_int_pwm_exit ; Yes - branch +ENDIF +IF MODE == 2 ; Multi +;读取Closed loop mode + mov Temp1, #Pgm_Gov_Mode ; Closed loop mode? + cjne @Temp1, #1, t2_int_pwm_exit ; Yes - branch +ENDIF + + ; Update current pwm, with limited throttle change rate +;Closed loop mode = Off + clr C + mov A, Requested_Pwm + subb A, Current_Pwm ; Is requested pwm larger than current pwm? + jc t2_int_set_current_pwm ; No - proceed + + mov Temp1, #Pgm_Throttle_Rate_Decoded + subb A, @Temp1 ; Is difference larger than throttle change rate? + jc t2_int_set_current_pwm ; No - proceed + + mov A, Current_Pwm ; Increase current pwm by throttle change rate + add A, @Temp1 + mov Current_Pwm, A + jnc t2_int_current_pwm_done ; Is result above max? + + mov Current_Pwm, #0FFh ; Yes - limit + jmp t2_int_current_pwm_done + +t2_int_set_current_pwm: + mov Current_Pwm, Requested_Pwm ; Set equal as default +t2_int_current_pwm_done: +IF MODE == 1 ; Tail + ; If tail, then set current_pwm_limited + mov Current_Pwm_Limited, Current_Pwm ; Default not limited + clr C + mov A, Current_Pwm ; Check against limit + subb A, Pwm_Limit + jc ($+5) ; If current pwm below limit - branch + + mov Current_Pwm_Limited, Pwm_Limit ; Limit pwm +ENDIF +IF MODE == 2 ; Multi + ; If multi, then set current_pwm_limited + mov Current_Pwm_Limited, Current_Pwm ; Default not limited + clr C + mov A, Current_Pwm ; Check against limit + subb A, Pwm_Limit + jc ($+5) ; If current pwm below limit - branch + + mov Current_Pwm_Limited, Pwm_Limit ; Limit pwm +ENDIF + +t2_int_pwm_exit: + ; Check if high byte flag is set + jb TF2H, t2h_int + pop ACC ; Restore preserved registers + pop PSW + clr PSW.3 ; Select register bank 0 for main program routines + setb EA ; Enable all interrupts + reti + +t2h_int: + ; High byte interrupt (happens every 32ms) + clr TF2H ; Clear interrupt flag + mov Temp1, #GOV_SPOOLRATE ; Load governor spool rate + ; Check RC pulse timeout counter (used here for PPM only) + mov A, Rcp_Timeout_Cnt ; RC pulse timeout count zero? + jz t2h_int_rcp_stop_check ; Yes - do not decrement + + ; Decrement timeout counter (if PPM) +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) +; anl A, Flags3 ; Check pwm frequency flags +; jnz t2h_int_rcp_stop_check ; If a flag is set (PWM) - branch + + dec Rcp_Timeout_Cnt ; No flag set (PPM) - decrement + +t2h_int_rcp_stop_check: + ; Check RC pulse against stop value + clr C + mov A, New_Rcp ; Load new pulse value + subb A, #RCP_STOP +; subb A, #(RCP_STOP+2) ; Check if pulse is below stop value + jc t2h_int_rcp_stop + + ; RC pulse higher than stop value, reset stop counter + mov Rcp_Stop_Cnt, #0 ; Reset rcp stop counter + ajmp t2h_int_rcp_gov_pwm + +t2h_int_rcp_stop: + ; RC pulse less than stop value, increment stop counter + mov A, Rcp_Stop_Cnt ; Load rcp stop counter + inc A ; Check if counter is max + jz t2h_int_rcp_gov_pwm ; Branch if counter is equal to max + + inc Rcp_Stop_Cnt ; Increment stop counter + +t2h_int_rcp_gov_pwm: +IF MODE == 0 ; Main + mov Temp2, #Pgm_Gov_Mode ; Governor target by arm mode? + cjne @Temp2, #2, t2h_int_rcp_gov_by_setup ; No - branch + + mov A, Gov_Active ; Is governor active? + jz t2h_int_rcp_gov_by_tx ; No - branch (this ensures soft spoolup by tx) + + clr C + mov A, Requested_Pwm + subb A, #50 ; Is requested pwm below 20%? + jc t2h_int_rcp_gov_by_tx ; Yes - branch (this enables a soft spooldown) + + mov Requested_Pwm, Gov_Arm_Target ; Yes - load arm target + +t2h_int_rcp_gov_by_setup: + mov Temp2, #Pgm_Gov_Mode ; Governor target by setup mode? + cjne @Temp2, #3, t2h_int_rcp_gov_by_tx ; No - branch + + mov A, Gov_Active ; Is governor active? + jz t2h_int_rcp_gov_by_tx ; No - branch (this ensures soft spoolup by tx) + + clr C + mov A, Requested_Pwm + subb A, #50 ; Is requested pwm below 20%? + jc t2h_int_rcp_gov_by_tx ; Yes - branch (this enables a soft spooldown) +;读取Pgm_Gov_Setup_Target + mov Temp2, #Pgm_Gov_Setup_Target ; Gov by setup - load setup target + mov Requested_Pwm, @Temp2 + +t2h_int_rcp_gov_by_tx: + clr C + mov A, Governor_Req_Pwm + subb A, Requested_Pwm ; Is governor requested pwm equal to requested pwm? + jz t2h_int_rcp_gov_pwm_done ; Yes - branch + + jc t2h_int_rcp_gov_pwm_inc ; No - if lower, then increment + + dec Governor_Req_Pwm ; No - if higher, then decrement + ajmp t2h_int_rcp_gov_pwm_done + +t2h_int_rcp_gov_pwm_inc: + inc Governor_Req_Pwm ; Increment + +t2h_int_rcp_gov_pwm_done: + djnz Temp1, t2h_int_rcp_gov_pwm ; If not number of steps processed - go back + + inc Spoolup_Limit_Cnt ; Increment spoolup count + + jnz ($+4) ; Wrapped? + + dec Spoolup_Limit_Cnt ; Yes - decrement + + djnz Spoolup_Limit_Skip, t2h_int_rcp_exit ; Jump if skip count is not reached + + mov Spoolup_Limit_Skip, #1 ; Reset skip count. Default is fast spoolup + mov Temp1, #3 ; Default fast increase + + clr C + mov A, Spoolup_Limit_Cnt + subb A, #(3*MAIN_SPOOLUP_TIME) ; No spoolup until "30"*32ms + jc t2h_int_rcp_exit + + clr C + mov A, Spoolup_Limit_Cnt + subb A, #(10*MAIN_SPOOLUP_TIME) ; Slow spoolup until "100"*32ms + jnc t2h_int_rcp_limit_middle_ramp + + mov Temp1, #1 ; Slow initial spoolup + mov Spoolup_Limit_Skip, #3 + jmp t2h_int_rcp_set_limit + +t2h_int_rcp_limit_middle_ramp: + clr C + mov A, Spoolup_Limit_Cnt + subb A, #(15*MAIN_SPOOLUP_TIME) ; Faster spoolup until "150"*32ms + jnc t2h_int_rcp_set_limit + + mov Temp1, #1 ; Faster middle spoolup + mov Spoolup_Limit_Skip, #1 + +t2h_int_rcp_set_limit: + mov A, Pwm_Limit_Spoolup ; Increment spoolup pwm + clr C + add A, Temp1 + jnc t2h_int_rcp_no_limit ; If below 255 - branch + + mov Pwm_Limit_Spoolup, #0FFh + ajmp t2h_int_rcp_exit + +t2h_int_rcp_no_limit: + mov Pwm_Limit_Spoolup, A +ENDIF +IF MODE == 2 ; Multi + mov A, Pwm_Limit_Spoolup ; Increment spoolup pwm, for a 0.8 seconds spoolup + clr C + add A, #10 + jnc t2h_int_rcp_no_limit ; If below 255 - branch + + mov Pwm_Limit_Spoolup, #0FFh + ajmp t2h_int_rcp_exit + +t2h_int_rcp_no_limit: + mov Pwm_Limit_Spoolup, A +ENDIF + +t2h_int_rcp_exit: + pop ACC ; Restore preserved registers + pop PSW + clr PSW.3 ; Select register bank 0 for main program routines + setb EA ; Enable all interrupts + reti + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Timer3 interrupt routine +; +; No assumptions +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +t3_int: ; Used for commutation timing + clr EA ; Disable all interrupts + anl TMR3CN, #07Fh ; Clear interrupt flag + clr Flags0.T3_PENDING ; Flag that timer has wrapped + setb EA ; Enable all interrupts + reti + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; PCA interrupt routine +; +; No assumptions +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +pca_int: ; Used for RC pulse timing + clr EA ; Disable all interrupts + push PSW ; Preserve registers through interrupt + push ACC + push B + setb PSW.3 ; Select register bank 1 for interrupt routines + +; mov A, Pca_First_Int +; cjne A, #1, ($+5) +; inc Pgm_Pulse_Cnt + ; Get PCA0 capture values + mov Temp1, PCA0CPL0 + mov Temp2, PCA0CPH0 + ; Clear interrupt flag + Rcp_Clear_Int_Flag + ; Check which edge it is + jnb Flags2.RCP_EDGE_NO, ($+5) ; Is it a first edge trig? + ajmp pca_int_second_meas_pwm_freq ; No - branch to second + + Rcp_Int_Second ; Yes - set second edge trig + setb Flags2.RCP_EDGE_NO ; Set second edge flag + ; Read RC signal level + Read_Rcp_Int + ; Test RC signal level + jb ACC.Rcp_In, ($+5) ; Is it high? + ajmp pca_int_fail_minimum ; No - jump to fail minimum + + ; RC pulse was high, store RC pulse start timestamp + mov Rcp_Prev_Edge_L, Temp1 + mov Rcp_Prev_Edge_H, Temp2 + ajmp pca_int_exit ; Exit + +pca_int_fail_minimum: + ; Prepare for next interrupt + Rcp_Int_First ; Set interrupt trig to first again + Rcp_Clear_Int_Flag ; Clear interrupt flag + clr Flags2.RCP_EDGE_NO ; Set first edge flag +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) + mov A, #(1 SHL RCP_PWM_FREQ_1KHZ) + anl A, Flags3 ; Check pwm frequency flags + jnz ($+4) ; If a flag is set (PWM) - proceed + + ajmp pca_int_set_timeout ; If PPM - ignore trig as noise + + mov Temp1, #RCP_MIN ; Set RC pulse value to minimum + Read_Rcp_Int ; Test RC signal level again + jnb ACC.Rcp_In, ($+5) ; Is it high? + ajmp pca_int_set_timeout ; Yes - set new timeout and exit + + mov New_Rcp, Temp1 ; Store new pulse length + ajmp pca_int_set_timeout ; Set new timeout and exit + +pca_int_second_meas_pwm_freq: + ; Prepare for next interrupt + Rcp_Int_First ; Set first edge trig + clr Flags2.RCP_EDGE_NO ; Set first edge flag + ; Check if pwm frequency shall be measured + jb Flags0.RCP_MEAS_PWM_FREQ, ($+5) ; Is measure RCP pwm frequency flag set? + ajmp pca_int_fall ; No - skip measurements + + ; Set second edge trig only during pwm frequency measurement + Rcp_Int_Second ; Set second edge trig + Rcp_Clear_Int_Flag ; Clear interrupt flag + setb Flags2.RCP_EDGE_NO ; Set second edge flag + ; Store edge data to RAM + mov Rcp_Edge_L, Temp1 + mov Rcp_Edge_H, Temp2 + ; Calculate pwm frequency + clr C + mov A, Temp1 + subb A, Rcp_PrePrev_Edge_L + mov Temp1, A ;存低字节差 + mov A, Temp2 + subb A, Rcp_PrePrev_Edge_H + mov Temp2, A ;存高字节差 +; clr A +; mov Temp4, A + ; Check if pwm frequency is 8kHz + mov Temp3, #250 ; Set default period tolerance requirement +; clr C +; mov A, Temp1 +; subb A, #low(360) ; If below 180us, 8kHz pwm is assumed +; mov A, Temp2 +; subb A, #high(360) +; jnc pca_int_check_4kHz + +; clr A +; setb ACC.RCP_PWM_FREQ_8KHZ +; mov Temp4, A +; mov Temp3, #15 ; Set period tolerance requirement +; ajmp pca_int_restore_edge + +;pca_int_check_4kHz: +; ; Check if pwm frequency is 4kHz +; clr C +; mov A, Temp1 +; subb A, #low(720) ; If below 360us, 4kHz pwm is assumed +; mov A, Temp2 +; subb A, #high(720) +; jnc pca_int_check_2kHz + +; clr A +; setb ACC.RCP_PWM_FREQ_4KHZ +; mov Temp4, A +; mov Temp3, #30 ; Set period tolerance requirement +; ajmp pca_int_restore_edge + +;pca_int_check_2kHz: +; ; Check if pwm frequency is 2kHz +; clr C +; mov A, Temp1 +; subb A, #low(1440) ; If below 720us, 2kHz pwm is assumed +; mov A, Temp2 +; subb A, #high(1440) +; jnc pca_int_check_1kHz + +; clr A +; setb ACC.RCP_PWM_FREQ_2KHZ +; mov Temp4, A +; mov Temp3, #60 ; Set period tolerance requirement +; ajmp pca_int_restore_edge + +;pca_int_check_1kHz: + ; Check if pwm frequency is 1kHz + clr C + mov A, Temp1 + subb A, #low(2200) ; If below 1100us, 1kHz pwm is assumed + mov A, Temp2 + subb A, #high(2200) + jnc pca_int_restore_edge + + ajmp pca_int_set_timeout +; clr A +; setb ACC.RCP_PWM_FREQ_1KHZ +; mov Temp4, A +; mov Temp3, #120 ; Set period tolerance requirement + +pca_int_restore_edge: + ; Calculate difference between this period and previous period + clr C + mov A, Temp1 + subb A, Rcp_Prev_Period_L + mov Temp5, A + mov A, Temp2 + subb A, Rcp_Prev_Period_H + mov Temp6, A + ; Make positive + jnb ACC.7, pca_int_check_diff + mov A, Temp5 + cpl A + add A, #1 + mov Temp5, A + mov A, Temp6 + cpl A + mov Temp6, A + +pca_int_check_diff: + ; Check difference + mov Rcp_Period_Diff_Accepted, #0 ; Set not accepted as default + jnz pca_int_store_data ; Check if high byte is zero + + clr C + mov A, Temp5 + subb A, Temp3 ; Check difference + jnc pca_int_store_data + + mov Rcp_Period_Diff_Accepted, #1 ; Set accepted + +pca_int_store_data: + ; Store previous period + mov Rcp_Prev_Period_L, Temp1 + mov Rcp_Prev_Period_H, Temp2 + ; Restore edge data from RAM + mov Temp1, Rcp_Edge_L + mov Temp2, Rcp_Edge_H + ; Store pre previous edge + mov Rcp_PrePrev_Edge_L, Temp1 + mov Rcp_PrePrev_Edge_H, Temp2 + +pca_int_fall: + clr A + mov Temp4, A + ; RC pulse edge was second, calculate new pulse length + clr C + mov A, Temp1 + subb A, Rcp_Prev_Edge_L + mov Temp1, A + mov A, Temp2 + subb A, Rcp_Prev_Edge_H + mov Temp2, A + + jb Flags0.RCP_MEAS_PWM_FREQ, rcp_skip_oneshot ;........................ + mov A, Temp2 + mov Temp6, A + mov A, Temp1 + mov Temp5, A + + ; Check if 270us or above (in order to ignore false pulses) + clr C + mov A, Temp5 + subb A, #28 + mov A, Temp6 + subb A, #2 + jc pca_int_ppm_below_full_range_oneshot + ajmp rcp_skip_oneshot + +pca_int_ppm_below_full_range_oneshot: + ; Check if below 110us (in order to ignore false pulses) + mov A, Temp6 + jnz pca_int_ppm_check_full_range + clr C + mov A, Temp5 + subb A,#220 + jnc pca_int_ppm_check_full_range + +rcp_skip_oneshot: ; ...................................... + +; jnb Flags3.RCP_PWM_FREQ_8KHZ, ($+5) ; Is RC input pwm frequency 8kHz? +; ajmp pca_int_pwm_divide_done ; Yes - branch forward + +; jnb Flags3.RCP_PWM_FREQ_4KHZ, ($+5) ; Is RC input pwm frequency 4kHz? +; ajmp pca_int_pwm_divide ; Yes - branch forward + mov OS_FLAG, #0 ;................................... + mov A, Temp2 ; No - 2kHz. Divide by 2 again + clr C + rrc A + mov Temp2, A + mov A, Temp1 + rrc A + mov Temp1, A + +; jnb Flags3.RCP_PWM_FREQ_2KHZ, ($+5) ; Is RC input pwm frequency 2kHz? +; ajmp pca_int_pwm_divide ; Yes - branch forward + + mov A, Temp2 ; No - 1kHz. Divide by 2 again + clr C + rrc A + mov Temp2, A + mov A, Temp1 + rrc A + mov Temp1, A + + clr C + mov A, Temp1 + subb A, #250 ; If below 500us, + mov A, Temp2 + subb A, #0 + jnc ($+9) + + mov A, Initial_Arm + cjne A, #1, ($+5) + + ajmp pca_int_pwm_divide + +; jnb Flags3.RCP_PWM_FREQ_1KHZ, ($+5) ; Is RC input pwm frequency 1kHz? +; ajmp pca_int_pwm_divide ; Yes - branch forward + + mov A, Temp2 ; No - PPM. Divide by 2 (to bring range to 256) and move to Temp5/6 + clr C + rrc A + mov Temp6, A + mov A, Temp1 + rrc A + mov Temp5, A + ; Skip range limitation if pwm frequency measurement + jb Flags0.RCP_MEAS_PWM_FREQ, pca_int_ppm_check_full_range + + ; Check if 2160us or above (in order to ignore false pulses) + clr C + mov A, Temp5 ; Is pulse 2160us or higher? + subb A, #28 + mov A, Temp6 + subb A, #2 + jc ($+5) ; No - proceed + + ljmp pca_int_set_timeout ; Yes - ignore pulse + + ; Check if below 800us (in order to ignore false pulses) + mov A, Temp6 + jnz pca_int_ppm_check_full_range + + clr C + mov A, Temp5 ; Is pulse below 800us? + subb A, #200 + jnc pca_int_ppm_check_full_range ; No - proceed + + jmp pca_int_exit ; Yes - ignore pulse + +pca_int_ppm_check_full_range: + ; Calculate "1000us" plus throttle minimum + mov A, #0 ; Set 1000us as default minimum + jb Flags3.FULL_THROTTLE_RANGE, pca_int_ppm_calculate ; Check if full range is chosen + + mov Temp1, #Pgm_Ppm_Min_Throttle ; Min throttle value is in 4us units + mov A, @Temp1 + +pca_int_ppm_calculate: + add A, #250 + mov Temp7, A + clr A + addc A, #0 + mov Temp8, A + + clr C + mov A, Temp5 ; Subtract minimum + subb A, Temp7 + mov Temp5, A + mov A, Temp6 + subb A, Temp8 + mov Temp6, A + jnc pca_int_ppm_neg_checked ; Is result negative? + + mov Temp1, #RCP_MIN ; Yes - set to minimum + mov Temp2, #0 + ajmp pca_int_pwm_divide_done + +pca_int_ppm_neg_checked: + clr C ; Check that RC pulse is within legal range (1000+4*255=2020) + mov A, Temp5 + subb A, #RCP_MAX + mov A, Temp6 + subb A, #0 + jc pca_int_ppm_max_checked + + mov Temp1, #RCP_MAX + mov Temp2, #0 + ajmp pca_int_pwm_divide_done + +pca_int_ppm_max_checked: + mov A, Temp5 ; Multiply throttle value by gain + mov B, Ppm_Throttle_Gain + mul AB + xch A, B + mov C, B.7 ; Multiply result by 2 (unity gain is 128) + rlc A + mov Temp1, A ; Transfer to Temp1/2 + mov Temp2, #0 + jc pca_int_ppm_limit_after_mult + + jmp pca_int_limited + +pca_int_ppm_limit_after_mult: + mov Temp1, #RCP_MAX + mov Temp2, #0 + jmp pca_int_limited + +pca_int_pwm_divide: + mov A, OS_FLAG + jnz pca_int_pwm_divide_done ;............................. +;480us + clr C + mov A, Temp1 + subb A, #240 + mov A, Temp2 + subb A, #0 + jnc pca_int_set_timeout +;40us + clr C + mov A, Temp1 + subb A, #20 + mov A, Temp2 + subb A, #0 + jc pca_int_set_timeout + + clr A + setb ACC.RCP_PWM_FREQ_1KHZ + mov Temp4, A +; mov Rcp_Period_Diff_Accepted, #1 + ajmp pca_int_limited + +pca_int_pwm_divide_done: + ; Check that RC pulse is within legal range + clr C + mov A, Temp1 + subb A, #RCP_MAX + mov A, Temp2 + subb A, #0 + jc pca_int_limited + + mov Temp1, #RCP_MAX + +pca_int_limited: + ; RC pulse value accepted + mov New_Rcp, Temp1 ; Store new pulse length + setb Flags2.RCP_UPDATED ; Set updated flag +; jb Flags0.RCP_MEAS_PWM_FREQ, ($+5) ; Is measure RCP pwm frequency flag set? +; ajmp pca_int_set_timeout ; No - skip measurements + +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) + mov A, #(1 SHL RCP_PWM_FREQ_1KHZ) + cpl A + anl A, Flags3 ; Clear all pwm frequency flags + orl A, Temp4 ; Store pwm frequency value in flags + mov Flags3, A + +pca_int_set_timeout: + mov Rcp_Timeout_Cnt, #RCP_TIMEOUT ; Set timeout count to start value +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) + mov A, #(1 SHL RCP_PWM_FREQ_1KHZ) + anl A, Flags3 ; Check pwm frequency flags + jnz pca_int_ppm_timeout_set ; If a flag is set - branch + + mov Rcp_Timeout_Cnt, #RCP_TIMEOUT_PPM ; No flag set means PPM. Set timeout count + +pca_int_ppm_timeout_set: +; jnb Flags0.RCP_MEAS_PWM_FREQ, ($+5) ; Is measure RCP pwm frequency flag set? +; ajmp pca_int_exit ; Yes - exit + +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) +; anl A, Flags3 ; Check pwm frequency flags +; jz pca_int_exit ; If no flag is set (PPM) - branch + +; Rcp_Int_Disable ; Disable RC pulse interrupt + +pca_int_exit: ; Exit interrupt routine + mov Rcp_Skip_Cnt, #RCP_SKIP_RATE ; Load number of skips +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) + mov A, #(1 SHL RCP_PWM_FREQ_1KHZ) + anl A, Flags3 ; Check pwm frequency flags + jnz ($+5) ; If a flag is set (PWM) - branch + + mov Rcp_Skip_Cnt, #10 ; Load number of skips + + pop B ; Restore preserved registers + pop ACC + pop PSW + clr PSW.3 ; Select register bank 0 for main program routines + setb EA ; Enable all interrupts + reti + + + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Wait xms ~(x*4*250) (Different entry points) +; +; No assumptions +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +wait1ms: + mov Temp2, #1 + ajmp waitxms_o + +wait3ms: + mov Temp2, #3 + ajmp waitxms_o + +wait10ms: + mov Temp2, #10 + ajmp waitxms_o + +wait30ms: + mov Temp2, #30 + ajmp waitxms_o + +wait100ms: + mov Temp2, #100 + ajmp waitxms_o + +wait200ms: + mov Temp2, #200 + ajmp waitxms_o + +waitxms_o: ; Outer loop + mov Temp1, #23 +waitxms_m: ; Middle loop + clr A + djnz ACC, $ ; Inner loop (42.7us - 1024 cycles) + djnz Temp1, waitxms_m + djnz Temp2, waitxms_o + ret +;**;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Wait 1 second routine +; +; No assumptions +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +wait1s: + mov Temp5, #5 +wait1s_loop: + call wait200ms + djnz Temp5, wait1s_loop + ret + + +;**;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Success beep routine +; +; No assumptions +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +success_beep: + clr EA ; Disable all interrupts + call beep_f1 + call beep_f2 + call beep_f3 +; call beep_f4 + call wait10ms + call beep_f1 + call beep_f2 + call beep_f3 +; call beep_f4 +; call wait10ms +; call beep_f1 +; call beep_f2 +; call beep_f3 +; call beep_f4 + setb EA ; Enable all interrupts + ret + + +;**;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Success beep inverted routine +; +; No assumptions +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +success_beep_inverted: + clr EA ; Disable all interrupts +; call beep_f4 + call beep_f3 + call beep_f2 + call beep_f1 + call wait10ms +; call beep_f4 + call beep_f3 + call beep_f2 + call beep_f1 + call wait10ms +; call beep_f4 +; call beep_f3 +; call beep_f2 +; call beep_f1 + setb EA ; Enable all interrupts + ret + + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Function and parameter value beep routine +; +; No assumptions +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +function_paraval_beep: + mov Temp7, Tx_Pgm_Func_No ; Function no + mov Temp8, Tx_Pgm_Paraval_No ; Parameter value no + clr EA ; Disable all interrupts +;//////function_beep: +;////// call beep_f1 +;////// call beep_f1 +;////// call beep_f1 +;////// call wait10ms +;////// djnz Temp7, function_beep +;//////paraval_beep: +;////// call beep_f4 +;////// call wait10ms +;////// djnz Temp8, paraval_beep +;function_beep: + jnb Flags0.PROGRAM_FUNC_FLAG,paraval_beep ;跳到参数选项叫 +function_no_6: + clr C + mov A,Temp7 + subb A,#5 + jc function_below_beep + jz function_beep + mov Temp7, A +function_beep: + call beep_f3 + call beep_f3 + call beep_f3 + call beep_f3 + call beep_f3 + call wait30ms + + cjne Temp7,#5,($+5) + ajmp fun_par_end + + +function_below_beep: + call beep_f1 + call beep_f1 + call beep_f1 + call wait100ms + djnz Temp7, function_below_beep + ajmp fun_par_end + +paraval_beep: + clr A + mov Temp7,A + + clr C + mov A,Temp8 + subb A,#10 ;参数数 - 10 + jc paraval_no_7 ;<10 + inc Temp7 ;>=10 + inc Temp7 + jz paraval_below_beep + mov Temp8,A ;>=存差 + ajmp paraval_below_beep + +paraval_no_7: + clr C + mov A,Temp8 + subb A,#5 + jc paraval_no_below + inc Temp7 + jz paraval_below_beep + mov Temp8,A ;>=存差 + +paraval_below_beep: + call beep_f2 + call beep_f2 + call beep_f2 + call wait30ms + djnz Temp7,paraval_below_beep + cjne Temp8,#10,($+5) + ajmp fun_par_end + cjne Temp8,#5,($+5) + ajmp fun_par_end + +paraval_no_below: + call wait100ms + call beep_f4 + call wait100ms + djnz Temp8, paraval_no_below +fun_par_end: + setb EA ; Enable all interrupts + + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; 接收一个数据（一个字节） +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +receive_a_byte: + mov Temp8,#0 + mov Commu_Data_Buffer, #0 + mov Temp4, #8 +wait_high: + + Read_Rcp_Int + ; Test RC signal level + jb ACC.Rcp_In,start_t2 + + jnb TF1, wait_high + +; setb Flags3.ERRO_DATA +; ajmp receive_exit + +start_t2: + mov TL0, #00h + mov TH0, #00h + setb TR0 ;启动T0 +wait_low: + Read_Rcp_Int + ; Test RC signal level + jnb ACC.Rcp_In, measure_wide + + jnb TF1, wait_low + +; setb Flags3.ERRO_DATA +; ajmp receive_exit + +measure_wide: + clr TR0 ;停止T0 +; mov Temp1, TL0 +; mov Temp2, TH0 + +; mov New_Rcp, Temp1 + mov New_Rcp, TL0 + clr C + mov A, New_Rcp + subb A, #20 ;40us + jc receive_a_erro_byte ;<80us 错误字节 + + clr C + mov A, New_Rcp + subb A, #78 ;156us + jnc receive_bit_one ;>160us 进行位1判断 + + mov A, Temp8 ;取0 + rl A + mov Temp8, A + ajmp receive_a_byte_exit + +receive_bit_one: + clr C + mov A, New_Rcp + subb A, #102 ;240us + jc receive_a_erro_byte ;160< New_Rcp <240us 错误字节 + + clr C + mov A, New_Rcp + subb A, #204 ;400us + jnc receive_a_erro_byte ;>400us 错误字节 + + mov A, Temp8 ;取1 + rl A + inc A + mov Temp8, A + ajmp receive_a_byte_exit + +receive_a_erro_byte: + setb Flags3.ERRO_DATA + +receive_a_byte_exit: + jnb TF1, ($+7) + + setb Flags3.ERRO_DATA + ajmp ($+6) + + djnz Temp4, wait_high + mov Commu_Data_Buffer, Temp8 +receive_exit: + ret + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; 接收一组数据 +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +receive_bytes: + mov Temp5, #48 + mov Temp2, #Tag_Temporary_Storage ; Set RAM address + mov Commu_Data_Buffer, #0 +wait_receive_bytes: + call receive_a_byte + mov @Temp2, Commu_Data_Buffer + inc Temp2 + djnz Temp5, wait_receive_bytes + + ret + +store_tags: +; call store_tags + mov Temp2, #Tag_Temporary_Storage ; Set RAM address + mov Temp5, #48 +store_tag: + cjne Temp5, #45, store_two + mov Temp1, #Pgm_Gov_Mode + jmp store_eep +store_two: + cjne Temp5, #44, store_three + mov Temp1, #Pgm_Low_Voltage_Lim + jmp store_eep +store_three: + cjne Temp5, #43, store_four + mov Temp1, #Pgm_Low_Voltage_Ctl + jmp store_eep +store_four: + cjne Temp5, #40, store_five + mov Temp1, #Pgm_Startup_Pwr + jmp store_eep +store_five: + cjne Temp5, #39, store_six + mov Temp1, #Pgm_Pwm_Freq + jmp store_eep +store_six: + cjne Temp5, #28, store_seven + mov Temp1, #Pgm_Comm_Timing + jmp store_eep +store_seven: + cjne Temp5, #27, store_next + mov Temp1, #Pgm_Damping_Force + +store_eep: + mov Commu_Data_Buffer, @Temp2 +;setb P1.7 + mov @Temp1, Commu_Data_Buffer + + call erase_and_store_all_in_eeprom + clr EA + +store_next: + inc Temp2 + djnz Temp5, store_tag + + ret + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; 发送一个数据（一个字节） +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +send_a_byte: + clr EA + setb TR0 ;启动T0 + mov Temp4, #8 + mov A, Commu_Data_Buffer ;读取要发送的数据 +wait_send_a_byte: + setb P0.7 ;发送高电平 + +; mov A, Commu_Data_Buffer ;读取要发送的数据 + jnb ACC.7, low_value ;($+8) + + mov TL0, #067h ;发送1脉宽长 + ajmp high_value ;($+5) + +low_value: + mov TL0, #0CDh ;发送0脉宽长 + +high_value: + mov TH0, #0FFh + + clr TF0 ;清T0溢出标志 + jnb TF0, $ ;等待T0溢出 高电平发送完成 + + clr P0.7 ;发送低电平 + mov TL0, #0CDh ; + mov TH0, #0FFh + + clr TF0 ;清T0溢出标志 + jnb TF0, $ ;等待T0溢出 低电平发送完成 + + rl A ;发送下一位 +; mov Commu_Data_Buffer, A + djnz Temp4, wait_send_a_byte + + ret + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; 发送一组数据 +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +send_bytes: + clr P0.7 +; mov A, P0MDOUT +; orl A, #080h + mov P0MDOUT, #80h ;设置成推挽输出 + mov Temp5, #48 +; mov Temp2, #Tag_Temporary_Storage ; Set RAM address + mov Temp1, #Pgm_Fir_Key +wait_send_bytes: + mov Commu_Data_Buffer, @Temp1 + inc Temp1 + + call send_a_byte ;逐个字节发送 + + djnz Temp5, wait_send_bytes + +; mov A, P0MDOUT +; anl A, #07Fh + mov P0MDOUT, #7Fh ;设置成漏极输出 + setb P0.7 + + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; 编程卡校验和 +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +get_commu_buffers_sum: + mov Commu_Sum, #0 + mov Temp5, #48 + mov Temp2, #Tag_Temporary_Storage ; Set RAM address +wait_get_commu_buffers_sum: + mov A, @Temp2 + clr C + addc A, Commu_Sum + mov Commu_Sum, A + inc Temp2 + djnz Temp5, wait_get_commu_buffers_sum + + ret + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; 清零校验和 +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +set_commu_sum_to_zero: + mov Temp8, #0 + mov Temp5, #47 + mov Temp2, #Tag_Temporary_Storage ; Set RAM address +wait_set_commu_sum_to_zero: + mov A, @Temp2 + clr C + addc A, Temp8 + mov Temp8, A + inc Temp2 + + djnz Temp5, wait_set_commu_sum_to_zero + + cpl A + inc A + mov @Temp2, A ;写最后一个字节 + + ret + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; 读取EEPROM参数 +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +;check_parameters: +; mov Temp5, #48 ; Number of tags +; mov Temp2, #Tag_Temporary_Storage ; Set RAM address +; mov Temp1, #Bit_Access +; mov DPTR, #Eep_ESC_Layout ; Set flash address +;read_eep: +; call read_eeprom_byte +; mov A, Bit_Access +; mov @Temp2, A ; Write to RAM +; inc Temp2 +; inc DPTR +; djnz Temp5, read_eep + +; call get_commu_buffers_sum + +; mov A, Commu_Sum +; jnz set_parameters_default +;;读取首字节 +; mov Temp2, #Tag_Temporary_Storage ; Set RAM address +; mov Temp1, #Bit_Access +; mov DPTR, #Eep_ESC_Layout ; Set flash address +; call read_eeprom_byte +; mov A, Bit_Access +; cjne A, #66h, set_parameters_default +;;设置默认参数 +;set_parameters_default: +; mov Temp5, #48 +; mov Temp2, #Tag_Temporary_Storage ; Set RAM address +; mov DPTR, #FINISH_KEYWORD_TABLE +;wait_set_parameters_default: +; clr A +; movc A, @A+DPTR +; mov @Temp2, A +; inc Temp2 +; inc DPTR +; djnz Temp5, wait_set_parameters_default + +; call set_commu_sum_to_zero + +; ret + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; 编程卡编程函数 +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +program_by_card: +; clr EA + mov Commu_Data_Buffer, #0 + call wait10ms + clr Flags3.ERRO_DATA ;清错误字节标志 + + mov TL1, #00h + mov TH1, #00h + setb TR1 ; 启动T1 + clr TF1 ;清T1溢出标志 + + call receive_a_byte ;读取一个字节（关键字） + + jb Flags3.ERRO_DATA, program_by_card ;接收到错误字节，则重新接收 + + mov A, Commu_Data_Buffer ;读接收到的字节 + cjne A, #05h, receive_next_keyword ;判断是否关键字 0x05 + call wait3ms + + call send_bytes + + ljmp program_by_card + +receive_next_keyword: + + cjne A, #09h, program_by_card ;判断是否关键字 0x09 + mov Commu_Data_Buffer, #0 + mov TL1, #00h + mov TH1, #00h + setb TR1 ;启动T1 + clr TF1 ;清T1溢出标志 + + call receive_bytes ;读取一组数据 + + call get_commu_buffers_sum + mov A, Commu_Sum ;读取校验和 + jnz program_by_card ;判断校验和 + + mov Temp2, #Tag_Temporary_Storage ; Set RAM address + mov A, @Temp2 ;读取首字节 + cjne A, #66h, program_by_card ;判断首字节 + + call wait1ms + clr P0.7 +; mov A, P0MDOUT +; orl A, #080h ;设置成推挽输出 + mov P0MDOUT, #080h + mov Commu_Data_Buffer, #088h + + call send_a_byte ;逐个字节发送 +; mov A, P0MDOUT +; anl A, #07Fh ;设置成漏极输出 + mov P0MDOUT, #07Fh + setb P0.7 + call store_tags ;存一组数据 +; call erase_and_store_all_in_eeprom +; clr EA +;clr P1.7 + ljmp program_by_card + + ret + + + + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Beeper routines (4 different entry points) +; +; No assumptions +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +beep_f1: ; Entry point 1, load beeper frequency 1 settings + mov Temp3, #18 ; Off wait loop length + mov Temp4, #100 ; Number of beep pulses Mos开关次数 + ajmp beep + +beep_f2: ; Entry point 2, load beeper frequency 2 settings + mov Temp3, #14 + mov Temp4, #120 ;Mos开关次数 + ajmp beep + +beep_f3: ; Entry point 3, load beeper frequency 3 settings + mov Temp3, #12 + mov Temp4, #160 ;Mos开关次数 + ajmp beep + +beep_f4: ; Entry point 4, load beeper frequency 4 settings + mov Temp3, #9 + mov Temp4, #180 ;Mos开关次数 + ajmp beep + +beep: ; Beep loop start + mov Temp5, Current_Pwm_Limited ; Store value + mov Current_Pwm_Limited, #1 ; Set to a nonzero value + mov Temp2, #2 ; Must be an even number (or direction will change) +beep_onoff: + cpl Flags3.PGM_DIR_REV ; Toggle between using A fet and C fet 切换A、C up导通 + clr A + BpFET_off ; BpFET off + djnz ACC, $ ; Allow some time after pfet is turned off + BnFET_on ; BnFET on (in order to charge the driver of the BpFET) + djnz ACC, $ ; Let the nfet be turned on a while + BnFET_off ; BnFET off again + djnz ACC, $ ; Allow some time after nfet is turned off + BpFET_on ; BpFET on + djnz ACC, $ ; Allow some time after pfet is turned on + ; Turn on nfet + AnFET_on ; AnFET on + mov A, Beep_Strength + djnz ACC, $ + ; Turn off nfet + AnFET_off ; AnFET off + mov A, #100 ; 25祍 off + djnz ACC, $ + djnz Temp2, beep_onoff + ; Copy variable + mov A, Temp3 + mov Temp1, A +beep_off: ; Fets off loop + djnz ACC, $ + djnz Temp1, beep_off + +beep_recharge_done: + djnz Temp4, beep + BpFET_off ; BpFET off + mov Current_Pwm_Limited, Temp5 ; Restore value + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Division 16bit unsigned by 16bit unsigned +; +; Dividend shall be in Temp2/Temp1, divisor in Temp4/Temp3 +; Result will be in Temp2/Temp1 +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +div_u16_by_u16: + clr C + mov Temp5, #0 + mov Temp6, #0 + mov B, #0 +div_u16_by_u16_div1: + inc B ; Increment counter for each left shift + mov A, Temp3 ; Shift left the divisor + rlc A + mov Temp3, A + mov A, Temp4 + rlc A + mov Temp4, A + jnc div_u16_by_u16_div1 ; Repeat until carry flag is set from high-byte +div_u16_by_u16_div2: + mov A, Temp4 ; Shift right the divisor + rrc A + mov Temp4, A + mov A, Temp3 + rrc A + mov Temp3, A + clr C + mov A, Temp2 ; Make a safe copy of the dividend + mov Temp8, A + mov A, Temp1 + mov Temp7, A + mov A, Temp1 ; Move low-byte of dividend into accumulator + subb A, Temp3 ; Dividend - shifted divisor = result bit (no factor, only 0 or 1) + mov Temp1, A ; Save updated dividend + mov A, Temp2 ; Move high-byte of dividend into accumulator + subb A, Temp4 ; Subtract high-byte of divisor (all together 16-bit substraction) + mov Temp2, A ; Save updated high-byte back in high-byte of divisor + jnc div_u16_by_u16_div3 ; If carry flag is NOT set, result is 1 + mov A, Temp8 ; Otherwise result is 0, save copy of divisor to undo subtraction + mov Temp2, A + mov A, Temp7 + mov Temp1, A +div_u16_by_u16_div3: + cpl C ; Invert carry, so it can be directly copied into result + mov A, Temp5 + rlc A ; Shift carry flag into temporary result + mov Temp5, A + mov A, Temp6 + rlc A + mov Temp6,A + djnz B, div_u16_by_u16_div2 ;Now count backwards and repeat until "B" is zero + mov A, Temp6 ; Move result to Temp2/Temp1 + mov Temp2, A + mov A, Temp5 + mov Temp1, A + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Multiplication 16bit signed by 8bit unsigned +; +; Multiplicand shall be in Temp2/Temp1, multiplicator in Temp3 +; Result will be in Temp2/Temp1. Result will divided by 16 +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +mult_s16_by_u8_div_16: + mov A, Temp1 ; Read input to math registers + mov B, Temp2 + mov Bit_Access, Temp3 + setb PSW.4 ; Select register bank 2 for math routines + mov Temp1, A ; Store in math registers + mov Temp2, B + mov Temp4, #0 ; Set sign in Temp4 and test sign + jnb B.7, mult_s16_by_u8_positive + + mov Temp4, #0FFh + cpl A + add A, #1 + mov Temp1, A + mov A, Temp2 + cpl A + addc A, #0 + mov Temp2, A +mult_s16_by_u8_positive: + mov A, Temp1 ; Multiply LSB with multiplicator + mov B, Bit_Access + mul AB + mov Temp6, B ; Place MSB in Temp6 + mov Temp1, A ; Place LSB in Temp1 (result) + mov A, Temp2 ; Multiply MSB with multiplicator + mov B, Bit_Access + mul AB + mov Temp8, B ; Place in Temp8/7 + mov Temp7, A + mov A, Temp6 ; Add up + add A, Temp7 + mov Temp2, A + mov A, #0 + addc A, Temp8 + mov Temp3, A + mov Temp5, #4 ; Set number of divisions +mult_s16_by_u8_div_loop: + clr C ; Rotate right + mov A, Temp3 + rrc A + mov Temp3, A + mov A, Temp2 + rrc A + mov Temp2, A + mov A, Temp1 + rrc A + mov Temp1, A + djnz Temp5, mult_s16_by_u8_div_loop + + mov B, Temp4 ; Test sign + jnb B.7, mult_s16_by_u8_exit + + mov A, Temp1 + cpl A + add A, #1 + mov Temp1, A + mov A, Temp2 + cpl A + addc A, #0 + mov Temp2, A + +mult_s16_by_u8_exit: + mov A, Temp1 ; Store output + mov B, Temp2 + clr PSW.4 ; Select normal register bank + mov Temp1, A + mov Temp2, B + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Calculate governor routines +; +; No assumptions +; +; Governs headspeed based upon the Comm_Period4x variable and pwm +; The governor task is split into several routines in order to distribute processing time +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; First governor routine - calculate governor target +IF MODE <= 1 ; Main or tail +calc_governor_target: + mov Temp1, #Pgm_Gov_Mode ; Governor mode? + cjne @Temp1, #4, governor_speed_check ; Yes + jmp calc_governor_target_exit ; No + +governor_speed_check: + ; Stop governor for stop RC pulse + clr C + mov A, New_Rcp ; Check RC pulse against stop value + subb A, #RCP_STOP ; Is pulse below stop value? + jc governor_deactivate ; Yes - deactivate + + ; Skip speed check if governor is alrady active + mov A, Gov_Active + jnz governor_target_calc + + ; Check speed (do not run governor for low speeds) + mov Temp1, #05h ; Default high range activation limit value (~62500 eRPM) + clr C + mov Temp2, #Pgm_Gov_Range + cjne @Temp2, #2, ($+5) + + mov Temp1, #12h ; Low range activation limit value (~17400 eRPM) + + clr C + mov A, Comm_Period4x_L + subb A, #00h + mov A, Comm_Period4x_H + subb A, Temp1 + jc governor_activate ; If speed above min limit - run governor + +governor_deactivate: + mov Current_Pwm, Requested_Pwm ; Set current pwm to requested + clr A + mov Gov_Target_L, A ; Set target to zero + mov Gov_Target_H, A + mov Gov_Integral_L, A ; Set integral to zero + mov Gov_Integral_H, A + mov Gov_Integral_X, A + mov Gov_Active, A + jmp calc_governor_target_exit + +governor_activate: + mov Gov_Active, #1 +governor_target_calc: + ; Governor calculations + clr C + mov Temp2, #Pgm_Gov_Range + mov A, @Temp2 ; Check high or low range (Temp2 has #Pgm_Gov_Range) + subb A, #2 + jz calc_governor_target_low + + mov A, Governor_Req_Pwm ; Load governor requested pwm + cpl A ; Calculate 255-pwm (invert pwm) + ; Calculate comm period target (1 + 2*((255-Requested_Pwm)/256) - 0.25) + rlc A ; Msb to carry + rlc A ; To bit0 + mov Temp2, A ; Now 1 lsb is valid for H + rrc A + mov Temp1, A ; Now 7 msbs are valid for L + mov A, Temp2 + anl A, #01h ; Calculate H byte + inc A ; Add 1 + mov Temp2, A + mov A, Temp1 + anl A, #0FEh ; Calculate L byte + clr C + subb A, #40h ; Subtract 0.25 + mov Temp1, A + mov A, Temp2 + subb A, #0 + mov Temp2, A + jmp calc_governor_store_target + +calc_governor_target_low: + mov A, Governor_Req_Pwm ; Load governor requested pwm + cpl A ; Calculate 255-pwm (invert pwm) + ; Calculate comm period target (2 + 8*((255-Requested_Pwm)/256) - 0.25) + rlc A ; Msb to carry + rlc A ; To bit0 + rlc A ; To bit1 + rlc A ; To bit2 + mov Temp2, A ; Now 3 lsbs are valid for H + rrc A + mov Temp1, A ; Now 4 msbs are valid for L + mov A, Temp2 + anl A, #07h ; Calculate H byte + inc A ; Add 1 + inc A ; Add 1 more + mov Temp2, A + mov A, Temp1 + anl A, #0F8h ; Calculate L byte + clr C + subb A, #40h ; Subtract 0.25 + mov Temp1, A + mov A, Temp2 + subb A, #0 + mov Temp2, A + +calc_governor_store_target: + ; Store governor target + mov Gov_Target_L, Temp1 + mov Gov_Target_H, Temp2 +calc_governor_target_exit: + ret +ENDIF +IF MODE == 2 ; Multi +calc_governor_target: +;读取 Closed loop mode + mov Temp1, #Pgm_Gov_Mode ; Closed loop mode? + cjne @Temp1, #1, governor_target_calc ; Yes - branch +;Closed loop mode = 第4项 Off + jmp calc_governor_target_exit ; No + +governor_target_calc: + ; Stop governor for stop RC pulse + clr C + mov A, New_Rcp ; Check RC pulse against stop value + subb A, #RCP_STOP ; Is pulse below stop value? + jc governor_deactivate ; Yes - deactivate + + jmp governor_activate ; No - activate + +governor_deactivate: + mov Current_Pwm, Requested_Pwm ; Set current pwm to requested + clr A + mov Gov_Target_L, A ; Set target to zero + mov Gov_Target_H, A + mov Gov_Integral_L, A ; Set integral to zero + mov Gov_Integral_H, A + mov Gov_Integral_X, A + mov Gov_Active, A + jmp calc_governor_target_exit + +governor_activate: + mov Temp1, #Pgm_Gov_Mode ; Store gov mode + mov A, @Temp1 + mov Temp5, A + mov Gov_Active, #1 + mov A, Requested_Pwm ; Load requested pwm + mov Governor_Req_Pwm, A ; Set governor requested pwm + ; Calculate comm period target 2*(51000/Requested_Pwm) + mov Temp1, #38h ; Load 51000 + mov Temp2, #0C7h + mov Temp3, Comm_Period4x_L ; Load comm period + mov Temp4, Comm_Period4x_H + ; Set speed range. Bare Comm_Period4x corresponds to 400k rpm, because it is 500n units +;Closed loop mode = 第1项 HiRange + clr C + mov A, Temp4 + rrc A + mov Temp4, A + mov A, Temp3 + rrc A + mov Temp3, A ; 200k eRPM range here + ; Check range + mov A, Temp5 + dec A + dec A + dec A + dec A + jz governor_activate_range_set ; 200k eRPM? - branch +;Closed loop mode = 第2项 MidRange +governor_activate_100k: + clr C + mov A, Temp4 + rrc A + mov Temp4, A + mov A, Temp3 + rrc A + mov Temp3, A ; 100k eRPM range here + mov A, Temp5 ; Check range again + dec A + dec A + dec A + jz governor_activate_range_set ; 100k eRPM? - branch +;Closed loop mode = 第3项 LoRange +governor_activate_50k: + clr C + mov A, Temp4 + rrc A + mov Temp4, A + mov A, Temp3 + rrc A + mov Temp3, A ; 50k eRPM range here +governor_activate_range_set: + call div_u16_by_u16 + ; Store governor target + mov Gov_Target_L, Temp1 + mov Gov_Target_H, Temp2 +calc_governor_target_exit: + ret +ENDIF + +; Second governor routine - calculate governor proportional error +calc_governor_prop_error: + ; Exit if governor is inactive + mov A, Gov_Active + jz calc_governor_prop_error_exit + +IF MODE <= 1 ; Main or tail + ; Load comm period and divide by 2 + clr C + mov A, Comm_Period4x_H + rrc A + mov Temp2, A + mov A, Comm_Period4x_L + rrc A + mov Temp1, A + ; Calculate error + clr C + mov A, Gov_Target_L + subb A, Temp1 + mov Temp1, A + mov A, Gov_Target_H + subb A, Temp2 + mov Temp2, A +ENDIF +IF MODE == 2 ; Multi + ; Calculate error + clr C + mov A, Gov_Target_L + subb A, Governor_Req_Pwm + mov Temp1, A + mov A, Gov_Target_H + subb A, #0 + mov Temp2, A +ENDIF + ; Check error and limit + jnc governor_check_prop_limit_pos ; Check carry + + clr C + mov A, Temp1 + subb A, #80h ; Is error too negative? + mov A, Temp2 + subb A, #0FFh + jc governor_limit_prop_error_neg ; Yes - limit + jmp governor_store_prop_error + +governor_check_prop_limit_pos: + clr C + mov A, Temp1 + subb A, #7Fh ; Is error too positive? + mov A, Temp2 + subb A, #00h + jnc governor_limit_prop_error_pos ; Yes - limit + jmp governor_store_prop_error + +governor_limit_prop_error_pos: + mov Temp1, #7Fh ; Limit to max positive (2's complement) + mov Temp2, #00h + jmp governor_store_prop_error + +governor_limit_prop_error_neg: + mov Temp1, #80h ; Limit to max negative (2's complement) + mov Temp2, #0FFh + +governor_store_prop_error: + ; Store proportional + mov Gov_Proportional_L, Temp1 + mov Gov_Proportional_H, Temp2 +calc_governor_prop_error_exit: + ret + + +; Third governor routine - calculate governor integral error +calc_governor_int_error: + ; Exit if governor is inactive + mov A, Gov_Active + jz calc_governor_int_error_exit + + ; Add proportional to integral + mov A, Gov_Proportional_L + add A, Gov_Integral_L + mov Temp1, A + mov A, Gov_Proportional_H + addc A, Gov_Integral_H + mov Temp2, A + mov Bit_Access, Gov_Proportional_H ; Sign extend high byte + clr A + jnb Bit_Access.7, ($+4) + cpl A + addc A, Gov_Integral_X + mov Temp3, A + ; Check integral and limit + jnb ACC.7, governor_check_int_limit_pos ; Check sign bit + + clr C + mov A, Temp3 + subb A, #0F0h ; Is error too negative? + jc governor_limit_int_error_neg ; Yes - limit + jmp governor_check_pwm + +governor_check_int_limit_pos: + clr C + mov A, Temp3 + subb A, #0Fh ; Is error too positive? + jnc governor_limit_int_error_pos ; Yes - limit + jmp governor_check_pwm + +governor_limit_int_error_pos: + mov Temp1, #0FFh ; Limit to max positive (2's complement) + mov Temp2, #0FFh + mov Temp3, #0Fh + jmp governor_check_pwm + +governor_limit_int_error_neg: + mov Temp1, #00h ; Limit to max negative (2's complement) + mov Temp2, #00h + mov Temp3, #0F0h + +governor_check_pwm: + ; Check current pwm + clr C + mov A, Current_Pwm + subb A, Pwm_Limit ; Is current pwm at or above pwm limit? + jnc governor_int_max_pwm ; Yes - branch + + mov A, Current_Pwm ; Is current pwm at zero? + jz governor_int_min_pwm ; Yes - branch + + ajmp governor_store_int_error ; No - store integral error + +governor_int_max_pwm: + mov A, Gov_Proportional_H + jb ACC.7, calc_governor_int_error_exit ; Is proportional error negative - branch (high byte is always zero) + ajmp governor_store_int_error ; Positive - store integral error + +governor_int_min_pwm: + mov A, Gov_Proportional_H + jnb ACC.7, calc_governor_int_error_exit ; Is proportional error positive - branch (high byte is always zero) + +governor_store_int_error: + ; Store integral + mov Gov_Integral_L, Temp1 + mov Gov_Integral_H, Temp2 + mov Gov_Integral_X, Temp3 +calc_governor_int_error_exit: + ret + + +; Fourth governor routine - calculate governor proportional correction +calc_governor_prop_correction: + ; Exit if governor is inactive + mov A, Gov_Active + jnz calc_governor_prop_corr + jmp calc_governor_prop_corr_exit + +calc_governor_prop_corr: + ; Load proportional gain + mov Temp1, #Pgm_Gov_P_Gain_Decoded; Load proportional gain + mov A, @Temp1 + mov Temp3, A ; Store in Temp3 + ; Load proportional + clr C + mov A, Gov_Proportional_L ; Nominal multiply by 2 + rlc A + mov Temp1, A + mov A, Gov_Proportional_H + rlc A + mov Temp2, A + ; Apply gain + call mult_s16_by_u8_div_16 + ; Check error and limit (to low byte) + mov A, Temp2 + jnb ACC.7, governor_check_prop_corr_limit_pos ; Check sign bit + + clr C + mov A, Temp1 + subb A, #80h ; Is error too negative? + mov A, Temp2 + subb A, #0FFh + jc governor_limit_prop_corr_neg ; Yes - limit + ajmp governor_apply_prop_corr + +governor_check_prop_corr_limit_pos: + clr C + mov A, Temp1 + subb A, #7Fh ; Is error too positive? + mov A, Temp2 + subb A, #00h + jnc governor_limit_prop_corr_pos ; Yes - limit + ajmp governor_apply_prop_corr + +governor_limit_prop_corr_pos: + mov Temp1, #7Fh ; Limit to max positive (2's complement) + mov Temp2, #00h + ajmp governor_apply_prop_corr + +governor_limit_prop_corr_neg: + mov Temp1, #80h ; Limit to max negative (2's complement) + mov Temp2, #0FFh + +governor_apply_prop_corr: + ; Test proportional sign + mov A, Temp1 + jb ACC.7, governor_corr_neg_prop ; If proportional negative - go to correct negative + + ; Subtract positive proportional + clr C + mov A, Governor_Req_Pwm + subb A, Temp1 + mov Temp1, A + ; Check result + jc governor_corr_prop_min_pwm ; Is result negative? + + clr C + mov A, Temp1 ; Is result below pwm min? + subb A, #1 + jc governor_corr_prop_min_pwm ; Yes + jmp governor_store_prop_corr ; No - store proportional correction + +governor_corr_prop_min_pwm: + mov Temp1, #1 ; Load minimum pwm + jmp governor_store_prop_corr + +governor_corr_neg_prop: + ; Add negative proportional + mov A, Temp1 + cpl A + add A, #1 + add A, Governor_Req_Pwm + mov Temp1, A + ; Check result + jc governor_corr_prop_max_pwm ; Is result above max? + jmp governor_store_prop_corr ; No - store proportional correction + +governor_corr_prop_max_pwm: + mov Temp1, #255 ; Load maximum pwm +governor_store_prop_corr: + ; Store proportional pwm + mov Gov_Prop_Pwm, Temp1 +calc_governor_prop_corr_exit: + ret + + +; Fifth governor routine - calculate governor integral correction +calc_governor_int_correction: + ; Exit if governor is inactive + mov A, Gov_Active + jnz calc_governor_int_corr + jmp calc_governor_int_corr_exit + +calc_governor_int_corr: + ; Load integral gain + mov Temp1, #Pgm_Gov_I_Gain_Decoded; Load integral gain + mov A, @Temp1 + mov Temp3, A ; Store in Temp3 + ; Load integral + mov Temp1, Gov_Integral_H + mov Temp2, Gov_Integral_X + ; Apply gain + call mult_s16_by_u8_div_16 + ; Check integral and limit + mov A, Temp2 + jnb ACC.7, governor_check_int_corr_limit_pos ; Check sign bit + + clr C + mov A, Temp1 + subb A, #01h ; Is integral too negative? + mov A, Temp2 + subb A, #0FFh + jc governor_limit_int_corr_neg ; Yes - limit + jmp governor_apply_int_corr + +governor_check_int_corr_limit_pos: + clr C + mov A, Temp1 + subb A, #0FFh ; Is integral too positive? + mov A, Temp2 + subb A, #00h + jnc governor_limit_int_corr_pos ; Yes - limit + jmp governor_apply_int_corr + +governor_limit_int_corr_pos: + mov Temp1, #0FFh ; Limit to max positive (2's complement) + mov Temp2, #00h + jmp governor_apply_int_corr + +governor_limit_int_corr_neg: + mov Temp1, #01h ; Limit to max negative (2's complement) + mov Temp2, #0FFh + +governor_apply_int_corr: + ; Test integral sign + mov A, Temp2 + jb ACC.7, governor_corr_neg_int ; If integral negative - go to correct negative + + ; Subtract positive integral + clr C + mov A, Gov_Prop_Pwm + subb A, Temp1 + mov Temp1, A + ; Check result + jc governor_corr_int_min_pwm ; Is result negative? + + clr C + mov A, Temp1 ; Is result below pwm min? + subb A, #1 + jc governor_corr_int_min_pwm ; Yes + jmp governor_store_int_corr ; No - store correction + +governor_corr_int_min_pwm: + mov Temp1, #0 ; Load minimum pwm + jmp governor_store_int_corr + +governor_corr_neg_int: + ; Add negative integral + mov A, Temp1 + cpl A + add A, #1 + add A, Gov_Prop_Pwm + mov Temp1, A + ; Check result + jc governor_corr_int_max_pwm ; Is result above max? + jmp governor_store_int_corr ; No - store correction + +governor_corr_int_max_pwm: + mov Temp1, #255 ; Load maximum pwm +governor_store_int_corr: + ; Store current pwm + mov Current_Pwm, Temp1 +calc_governor_int_corr_exit: + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Measure lipo cells +; +; No assumptions +; +; Measure voltage and calculate lipo cells +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +measure_lipo_cells: +IF MODE == 1 ; Tail + ; If tail, then exit + jmp measure_lipo_exit +ENDIF +measure_lipo_start: + ; Load programmed low voltage limit +;读取Low voltage limit + mov Lipo_Cell_Count, #0 ;2013.8.23 防止接收机拿掉后再接上时，电池检测出错 + mov Temp1, #Pgm_Low_Voltage_Lim ; Load limit + mov A, @Temp1 + mov Bit_Access, A ; Store in Bit_Access + ; Set commutation to BpFET on +; call comm5comm6 + CnFET_off ; Cn off + BnFET_on ; Bn on + Set_Comp_Phase_C ; Set comparator to phase C + mov Comm_Phase, #6 + ; Start adc + Start_Adc + ; Wait for ADC conversion to complete + Get_Adc_Status + jb AD0BUSY, measure_lipo_cells + ; Read ADC result + Read_Adc_Result + ; Stop ADC + Stop_Adc + ; Switch power off + call switch_power_off + ; Set limit step + mov Lipo_Adc_Limit_L, #ADC_LIMIT_L + mov Lipo_Adc_Limit_H, #ADC_LIMIT_H + clr C + mov A, #ADC_LIMIT_H ; Divide 2.8V value by 2 + rrc A + mov Temp6, A + mov A, #ADC_LIMIT_L + rrc A + mov Temp5, A + mov A, #ADC_LIMIT_L ; Calculate 1.4+2.8V=4.2V value + add A, Temp5 + add A, #3 ;加宽一节电池电压范围 4.3V 2013.8.16 + mov Temp5, A + mov A, #ADC_LIMIT_H + addc A, Temp6 + mov Temp6, A + mov A, Temp5 ; Copy step + mov Temp3, A + mov A, Temp6 + mov Temp4, A +;*************************************************** +; Lipo节数检测 +;*************************************************** +measure_lipo_cell_loop: + ; Check voltage against xS lower limit + inc Lipo_Cell_Count ;增加电池节数累加 2013.5.29 + clr C + mov A, Temp1 + subb A, Temp3 ; Voltage above limit? + mov A, Temp2 + subb A, Temp4 + jc measure_lipo_adjust ; No - branch 读取到电压范围 + + ; Set xS voltage limit + mov A, Lipo_Adc_Limit_L + add A, #ADC_LIMIT_L + mov Lipo_Adc_Limit_L, A + mov A, Lipo_Adc_Limit_H + addc A, #ADC_LIMIT_H + mov Lipo_Adc_Limit_H, A + ; Set (x+1)S lower limit + mov A, Temp3 + add A, Temp5 ; Add step + mov Temp3, A + mov A, Temp4 + addc A, Temp6 + mov Temp4, A + jmp measure_lipo_cell_loop ; Check for one more battery cell + +measure_lipo_adjust: + mov Temp7, Lipo_Adc_Limit_L + mov Temp8, Lipo_Adc_Limit_H + ; Calculate 3.125% + clr C + mov A, Lipo_Adc_Limit_H + rrc A + mov Temp2, A + mov A, Lipo_Adc_Limit_L + rrc A + mov Temp1, A ; After this 50% + clr C + mov A, Temp2 + rrc A + mov Temp2, A + mov A, Temp1 + rrc A + mov Temp1, A ; After this 25% +; mov A, Lipo_Adc_Limit_L ; Set adc reference for voltage compensation +; add A, Temp1 ;低压低字节+低压低字节偏移量 +; mov Lipo_Adc_Reference_L, A ;存低压低字节 +; mov A, Lipo_Adc_Limit_H +; addc A, Temp2 ;低压高字节+低压高字节偏移量 +; mov Lipo_Adc_Reference_H, A ;存低压高字节 +; ; Divide three times to get to 3.125% 6.25% 2013.5.30 + mov Temp3, #2 ;3 +measure_lipo_divide_loop: + clr C + mov A, Temp2 + rrc A + mov Temp2, A + mov A, Temp1 + rrc A + mov Temp1, A + djnz Temp3, measure_lipo_divide_loop + + ; Add the programmed number of 0.1V (or 3.125% increments) + mov Temp3, Bit_Access ; Load programmed limit (Bit_Access has Pgm_Low_Voltage_Lim) +; dec Temp3 +; mov A,Temp3 ;增加判断指令，如果没有这条指令能判断？？？？ +; jnz measure_lipo_limit_on ; Is low voltage limiting on? + + cjne Temp3, #4, measure_lipo_limit_on + + mov Lipo_Adc_Limit_L, #0 ; No - set limit to zero + mov Lipo_Adc_Limit_H, #0 + jmp measure_lipo_exit + +measure_lipo_limit_on: + dec Temp3 + mov A, Temp3 + jz measure_lipo_update +;Bit_Access>2时，低压保护值=基值+偏移量 +;Low voltage limit > 第2项时 +measure_lipo_add_loop: + clr C + mov A, Temp7 ; Add 3.125% + add A, Temp1 + mov Temp7, A + mov A, Temp8 + addc A, Temp2 + mov Temp8, A +; clr C +; mov A, Temp7 ; Add 3.125% +; add A, Temp1 +; mov Temp7, A +; mov A, Temp8 +; addc A, Temp2 +; mov Temp8, A + djnz Temp3, measure_lipo_add_loop +;存低压保护值 +;Low voltage limit = 第2项 3.0V +measure_lipo_update: + ; Set ADC limit + mov Lipo_Adc_Limit_L, Temp7 + mov Lipo_Adc_Limit_H, Temp8 +measure_lipo_exit: + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Start ADC conversion +; +; No assumptions +; +; Start conversion used for measuring power supply voltage +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +start_adc_conversion: + ; Start adc + Start_Adc + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Check temperature, power supply voltage and limit power +; +; No assumptions +; +; Used to limit main motor power in order to maintain the required voltage +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +check_temp_voltage_and_limit_power: + ; Load programmed low voltage limit + mov Temp1, #Pgm_Low_Voltage_Lim ;读低压保护选项 + mov A, @Temp1 + mov Temp8, A ; Store in Temp8 + ; Wait for ADC conversion to complete + Get_Adc_Status + jb AD0BUSY, check_temp_voltage_and_limit_power + ; Read ADC result + Read_Adc_Result ;读AD值 + ; Stop ADC + Stop_Adc ;禁止AD转换 + + inc Adc_Conversion_Cnt ; Increment conversion counter + clr C + mov A, Adc_Conversion_Cnt ; Is conversion count equal to temp rate? + subb A, #TEMP_CHECK_RATE + jc check_voltage_start ; No - check voltage AD转换次数 < 8次 ？ + + mov Adc_Conversion_Cnt, #0 ; Yes - temperature check. Reset counter + mov A, Temp2 ; Is temperature reading below 256? + jnz temp_average_inc_dec ; No - proceed + + mov A, Current_Average_Temp ; Yes - decrement average + jz temp_average_updated ; Already zero - no change + jmp temp_average_dec ; Decrement + +temp_average_inc_dec: + clr C + mov A, Temp1 ; Check if current temperature is above or below average + subb A, Current_Average_Temp + jz temp_average_updated_load_acc ; Equal - no change + + mov A, Current_Average_Temp ; Above - increment average + jnc temp_average_inc + + jz temp_average_updated ; Below - decrement average if average is not already zero +temp_average_dec: + dec A ; Decrement average + jmp temp_average_updated + +temp_average_inc: + inc A ; Increment average + jz temp_average_dec + jmp temp_average_updated + +temp_average_updated_load_acc: + mov A, Current_Average_Temp +temp_average_updated: + mov Current_Average_Temp, A + clr C + subb A, #TEMP_LIMIT ; Is temperature below first limit? + mov Temp1, A + jc temp_check_exit ; Yes - exit + + mov Pwm_Limit, #192 ; No - limit pwm + + clr C + mov A, Temp1 ; Is temperature below second limit + subb A, #TEMP_LIMIT_STEP + mov Temp1, A + jc temp_check_exit ; Yes - exit + + mov Pwm_Limit, #128 ; No - limit pwm + + clr C + mov A, Temp1 ; Is temperature below third limit + subb A, #TEMP_LIMIT_STEP + mov Temp1, A + jc temp_check_exit ; Yes - exit + + mov Pwm_Limit, #64 ; No - limit pwm + + clr C + mov A, Temp1 ; Is temperature below final limit + subb A, #TEMP_LIMIT_STEP + mov Temp1, A + jc temp_check_exit ; Yes - exit + + mov Pwm_Limit, #16 ; No - limit pwm 2013.8.5最高温度时速度由0改成16 + +temp_check_exit: + Set_Adc_Ip_Volt ; Select adc input for next conversion + ret + +check_voltage_start: +IF MODE == 0 OR MODE == 2 ; Main or multi + ; Check if low voltage limiting is enabled + mov A, Temp8 + clr C + subb A, #4 ; Is low voltage limit disabled? + jz check_voltage_good ; Yes - voltage declared good + + ; Check if ADC is saturated + clr C + mov A, Temp1 ;读取AD值低字节 + subb A, #0FFh + mov A, Temp2 ;读取AD值高字节 + subb A, #03h ;判断AD是否超出10位AD范围 + jnc check_voltage_good ; ADC saturated, can not make judgement + + ; Check voltage against limit + clr C + mov A, Temp1 + subb A, Lipo_Adc_Limit_L + mov A, Temp2 + subb A, Lipo_Adc_Limit_H + jnc check_voltage_good ; If voltage above limit - branch 电压值 > 低压设置值 + + mov Temp1, #Pgm_Low_Voltage_Ctl ;读低压保护控制方式选项 + mov A, @Temp1 + clr C + subb A, #1 ; + jz check_voltage_next_way + + mov A, Pwm_Limit + subb A, #5 + jnc check_limit_count + setb LOW_LIMIT_STOP ;设置低压保护控制方式2 低压停止标志 + ljmp run_to_wait_for_power_on + +check_voltage_next_way: + ; Decrease pwm limit + mov A, Pwm_Limit + subb A, #80 ;更改低压保护方式 2013.05.31 +;////// jz check_voltage_lim ; If limit zero - branch + jc check_voltage_lim ; If limit <100 - branch +check_limit_count: + mov A, Limit_Count + jz ($+6) + dec Limit_Count ;递减低压保护变量 2013.7.5 + ajmp check_voltage_lim + + mov Limit_Count, #5 ; 2013.7.5 + dec Pwm_Limit ; Decrement limit + jmp check_voltage_lim +;Low voltage limit = 第1项 +check_voltage_good: + ; Increase pwm limit + mov Limit_Count, #5 ; 2013.7.5 + mov A, Pwm_Limit + cpl A + jz check_voltage_lim ; If limit max - branch + + inc Pwm_Limit ; Increment limit + +check_voltage_lim: + mov Temp1, Pwm_Limit ; Set limit + clr C + mov A, Current_Pwm + subb A, Temp1 + jnc check_voltage_spoolup_lim ; If current pwm above limit - branch and limit + + mov Temp1, Current_Pwm ; Set current pwm (no limiting) + +check_voltage_spoolup_lim: + mov Current_Pwm_Limited, Temp1 ;取小值 + ; Slow spoolup + clr C + mov A, Current_Pwm_Limited + subb A, Pwm_Limit_Spoolup + jc check_voltage_exit ; If current pwm below limit - branch + + mov Current_Pwm_Limited, Pwm_Limit_Spoolup ;取小值 + mov A, Pwm_Limit_Spoolup ; Check if spoolup limit is max + cpl A + jz check_voltage_exit ; If max - branch + + mov Pwm_Limit, Pwm_Limit_Spoolup ; Set pwm limit to spoolup limit during ramp (to avoid governor integral buildup) + +check_voltage_exit: +ENDIF + ; Set adc mux for next conversion + clr C + mov A, Adc_Conversion_Cnt ; Is next conversion for temperature? + cjne A, #(TEMP_CHECK_RATE-1), ($+6) + + Set_Adc_Ip_Temp ; Select temp sensor for next conversion + + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Set startup PWM routine +; +; No assumptions +; +; Used for pwm control during startup +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +set_startup_pwm: + ; Set pwm values according to startup phase flags + jnb Flags1.SETTLE_PHASE, ($+5) ; Is it motor start settle phase? 是否启动定位？ + mov Temp1, #PWM_SETTLE ; Yes - set settle power + jnb Flags1.STEPPER_PHASE, ($+5) ; Is it motor start stepper phase? 是否步进启动？ + mov Temp1, #PWM_STEPPER ; Yes - set stepper power + + ; Update pwm variables if any startup phase flag is set + mov A, Flags1 + anl A, #((1 SHL SETTLE_PHASE)+(1 SHL STEPPER_PHASE)) + jz startup_pwm_exit ; If no startup phase set - exit + + ; Adjust startup power + mov A, Temp1 ; Multiply startup power by programmed value + mov Temp2, #Pgm_Startup_Pwr_Decoded + mov B, @Temp2 + mul AB + xch A, B + mov C, B.7 ; Multiply result by 2 (unity gain is 128) + rlc A + mov Temp1, A ; Transfer to Temp1 + clr C + mov A, Temp1 ; Check against limit + subb A, Pwm_Limit + jc startup_pwm_set_pwm ; If pwm below limit - branch + + mov Temp1, Pwm_Limit ; Limit pwm + +startup_pwm_set_pwm: + ; Set pwm variables + mov Requested_Pwm, Temp1 ; Update requested pwm + mov Current_Pwm, Temp1 ; Update current pwm + mov Current_Pwm_Limited, Temp1 ; Update limited version of current pwm + jnb Flags1.SETTLE_PHASE, startup_pwm_exit ; Is it motor start settle phase? + + clr C ; Yes - update spoolup beginning pwm (will use PWM_SETTLE or PWM_SETTLE/2) + mov A, Temp1 + mov Pwm_Spoolup_Beg, A + +startup_pwm_exit: + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Initialize all timings routine +; +; No assumptions +; +; Part of initialization before motor start +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +initialize_all_timings: + ; Load programmed startup rpm +;读取Startup rpm +; mov Temp1, #Pgm_Startup_Rpm ; Load startup rpm +; mov A, @Temp1 +; mov Temp8, A ; Store in Temp8 + ; Check startup rpm setting and set step accordingly +; clr C +; mov A, Temp8 +; subb A, #5 +; jnc stepper_step_high +; clr C +; mov A, Temp8 +; subb A, #4 +; jnc stepper_step_med_high +; clr C +; mov A, Temp8 +; subb A, #3 +; jnc stepper_step_med +; clr C +; mov A, Temp8 +; subb A, #2 +; jnc stepper_step_med_low +; clr C +; mov A, Temp8 +; subb A, #1 +; jnc stepper_step_low +;Startup_Rpm = 第5项 +;stepper_step_high: +; mov Stepper_Step_Beg_L, #low(2000 SHL 1) +; mov Stepper_Step_Beg_H, #high(2000 SHL 1) +; mov Stepper_Step_End_L, #low(670 SHL 1) +; mov Stepper_Step_End_H, #high(670 SHL 1) +; ajmp stepper_step_set +;Startup_Rpm = 第4项 +;stepper_step_med_high: +; mov Stepper_Step_Beg_L, #low(2400 SHL 1) +; mov Stepper_Step_Beg_H, #high(2400 SHL 1) +; mov Stepper_Step_End_L, #low(800 SHL 1) +; mov Stepper_Step_End_H, #high(800 SHL 1) +; ajmp stepper_step_set +;Startup_Rpm = 第3项 +;stepper_step_med: +; mov Stepper_Step_Beg_L, #low(3000 SHL 1) ; ~3300 eRPM +; mov Stepper_Step_Beg_H, #high(3000 SHL 1) +; mov Stepper_Step_End_L, #low(1000 SHL 1) ; ~10000 eRPM +; mov Stepper_Step_End_H, #high(1000 SHL 1) +; ajmp stepper_step_set +;Startup_Rpm = 第2项 +;stepper_step_med_low: +; mov Stepper_Step_Beg_L, #low(3750 SHL 1) +; mov Stepper_Step_Beg_H, #high(3750 SHL 1) +; mov Stepper_Step_End_L, #low(1250 SHL 1) +; mov Stepper_Step_End_H, #high(1250 SHL 1) +; ajmp stepper_step_set +;Startup_Rpm = 第1项 +;stepper_step_low: +; mov Stepper_Step_Beg_L, #low(4500 SHL 1) +; mov Stepper_Step_Beg_H, #high(4500 SHL 1) +; mov Stepper_Step_End_L, #low(1500 SHL 1) +; mov Stepper_Step_End_H, #high(1500 SHL 1) +; ajmp stepper_step_set + +;stepper_step_set: +; mov Wt_Stepper_Step_L, Stepper_Step_Beg_L ; Initialize stepper step time +; mov Wt_Stepper_Step_H, Stepper_Step_Beg_H + mov Comm_Period4x_L, #00h ; Set commutation period registers + mov Comm_Period4x_H, #08h + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Calculate next commutation timing routine +; +; No assumptions +; +; Called immediately after each commutation +; Also sets up timer 1 to wait advance timing +; Two entry points are used +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +;calc_next_comm_timing_start: ; Entry point for startup +; mov Temp1, Wt_Stepper_Step_L ; Set up stepper step wait +; mov Temp2, Wt_Stepper_Step_H +; jmp read_timer + +calc_next_comm_timing: ; Entry point for run phase + mov Temp1, Wt_Advance_L ; Set up advance timing wait + mov Temp2, Wt_Advance_H +read_timer: + ; Set up next wait + mov TMR3CN, #00h ; Timer3 disabled + clr C + clr A + subb A, Temp1 ; Set wait to zero cross scan value + mov TMR3L, A + clr A + subb A, Temp2 + mov TMR3H, A + mov TMR3CN, #04h ; Timer3 enabled + setb Flags0.T3_PENDING + ; Read commutation time + mov TMR2CN, #20h ; Timer2 disabled + mov Temp1, TMR2L ; Load timer value + mov Temp2, TMR2H + mov TMR2CN, #24h ; Timer2 enabled + ; Calculate this commutation time + mov Temp3, Prev_Comm_L + mov Temp4, Prev_Comm_H + mov Prev_Comm_L, Temp1 ; Store timestamp as previous commutation + mov Prev_Comm_H, Temp2 + clr C + mov A, Temp1 + subb A, Temp3 ; Calculate the new commutation time + mov Temp1, A + mov A, Temp2 + subb A, Temp4 + mov Temp2, A + ; Calculate next zero cross scan timeout + mov Temp3, Comm_Period4x_L ; Comm_Period4x(-l-h-x) holds the time of 4 commutations + mov Temp4, Comm_Period4x_H + clr C + mov A, Temp4 + rrc A ; Divide by 2 + mov Temp6, A + mov A, Temp3 + rrc A + mov Temp5, A + clr C + mov A, Temp6 + rrc A ; Divide by 2 again + mov Temp6, A + mov A, Temp5 + rrc A + mov Temp5, A + clr C + mov A, Temp3 + subb A, Temp5 ; Subtract a quarter + mov Temp3, A + mov A, Temp4 + subb A, Temp6 + mov Temp4, A + + mov A, Temp3 + add A, Temp1 ; Add the new time + mov Temp3, A + mov A, Temp4 + addc A, Temp2 + mov Temp4, A + mov Comm_Period4x_L, Temp3 ; Store Comm_Period4x_X + mov Comm_Period4x_H, Temp4 + jc calc_next_comm_slow ; If period larger than 0xffff - go to slow case + + ret + +calc_next_comm_slow: + mov Comm_Period4x_L, #0FFh ; Set commutation period registers to very slow timing (0xffff) + mov Comm_Period4x_H, #0FFh + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Wait advance timing routine +; +; No assumptions +; +; Waits for the advance timing to elapse +; Also sets up timer 1 to wait the zero cross scan wait time +; And has a separate entry point for just setting up zero cross scan wait +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +wait_advance_timing: + jnb Flags0.T3_PENDING, ($+5) + ajmp wait_advance_timing + +setup_zc_scan_wait: + mov TMR3CN, #00h ; Timer3 disabled + clr C + clr A + subb A, Wt_Zc_Scan_L ; Set wait to zero cross scan value + mov TMR3L, A + clr A + subb A, Wt_Zc_Scan_H + mov TMR3H, A + mov TMR3CN, #04h ; Timer3 enabled + setb Flags0.T3_PENDING + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Calculate new wait times routine +; +; No assumptions +; +; Calculates new wait times +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +calc_new_wait_times: + ; Load programmed commutation timing +;读取Commutation timing + mov Temp1, #Pgm_Comm_Timing ; Load timing setting + mov A, @Temp1 + mov Temp8, A ; Store in Temp8 + mov Temp7, #(COMM_TIME_RED SHL 1) + jnb Flags0.DEMAG_DETECTED, calc_new_wait_check_startup ; Demag detected? + + mov Temp8, #5 ; Yes - set high timing + +calc_new_wait_check_startup: + jnb Flags1.DIRECT_STARTUP_PHASE, calc_new_wait_dir_start_set ; Set timing for direct start + + mov Temp8, #3 ; Set medium timing + mov Temp7, #0 ; Set no comm time reduction + +calc_new_wait_dir_start_set: + ; Load current commutation timing + mov Temp2, Comm_Period4x_H ; Load Comm_Period4x + mov Temp1, Comm_Period4x_L + mov Temp3, #4 ; Divide 4 times +divide_wait_times: + clr C + mov A, Temp2 + rrc A ; Divide by 2 + mov Temp2, A + mov A, Temp1 + rrc A + mov Temp1, A + djnz Temp3, divide_wait_times + + clr C + mov A, Temp1 + subb A, Temp7 + mov Temp1, A + mov A, Temp2 + subb A, #0 + mov Temp2, A + jc load_min_time ; Check that result is still positive + + clr C + mov A, Temp1 + subb A, #(COMM_TIME_MIN SHL 1) + mov A, Temp2 + subb A, #0 + jnc adjust_timing ; Check that result is still above minumum + +load_min_time: + mov Temp1, #(COMM_TIME_MIN SHL 1) + clr A + mov Temp2, A + +adjust_timing: + mov A, Temp2 ; Copy values + mov Temp4, A + mov A, Temp1 + mov Temp3, A + clr C + mov A, Temp2 + rrc A ; Divide by 2 + mov Temp6, A + mov A, Temp1 + rrc A + mov Temp5, A + clr C + mov A, Temp8 ; (Temp8 has Pgm_Comm_Timing) + subb A, #3 ; Is timing normal? + jz store_times_decrease ; Yes - branch + + mov A, Temp8 + jb ACC.0, adjust_timing_two_steps ; If an odd number - branch +;Commutation timing = 第2、4项 + mov A, Temp1 ; Add 7.5?and store in Temp1/2 + add A, Temp5 + mov Temp1, A + mov A, Temp2 + addc A, Temp6 + mov Temp2, A + mov A, Temp5 ; Store 7.5?in Temp3/4 + mov Temp3, A + mov A, Temp6 + mov Temp4, A + jmp store_times_up_or_down +;Commutation timing = 第1、5项 +adjust_timing_two_steps: + mov A, Temp1 ; Add 15?and store in Temp1/2 + add A, Temp1 + mov Temp1, A + mov A, Temp2 + addc A, Temp2 + mov Temp2, A + mov Temp3, #(COMM_TIME_MIN SHL 1) ; Store minimum time in Temp3/4 + clr A + mov Temp4, A + +store_times_up_or_down: + clr C + mov A, Temp8 + subb A, #3 ; Is timing higher than normal? + jc store_times_decrease ; No - branch +;Commutation timing = 第4、5项 +store_times_increase: + mov Wt_Comm_L, Temp3 ; Now commutation time (~60? divided by 4 (~15?nominal) + mov Wt_Comm_H, Temp4 + mov Wt_Advance_L, Temp1 ; New commutation advance time (~15?nominal) + mov Wt_Advance_H, Temp2 + mov Wt_Zc_Scan_L, Temp5 ; Use this value for zero cross scan delay (7.5? + mov Wt_Zc_Scan_H, Temp6 + ret +;Commutation timing = 第1、2、3项 +store_times_decrease: + mov Wt_Comm_L, Temp1 ; Now commutation time (~60? divided by 4 (~15?nominal) + mov Wt_Comm_H, Temp2 + mov Wt_Advance_L, Temp3 ; New commutation advance time (~15?nominal) + mov Wt_Advance_H, Temp4 + mov Wt_Zc_Scan_L, Temp5 ; Use this value for zero cross scan delay (7.5? + mov Wt_Zc_Scan_H, Temp6 + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Wait before zero cross scan routine +; +; No assumptions +; +; Waits for the zero cross scan wait time to elapse +; Also sets up timer 1 to wait the zero cross scan timeout time +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +wait_before_zc_scan: + jnb Flags0.T3_PENDING, ($+5) + ajmp wait_before_zc_scan + + mov TMR3CN, #00h ; Timer3 disabled + clr C + clr A + subb A, Comm_Period4x_L ; Set wait to zero comm period 4x value + mov TMR3L, A + clr A + subb A, Comm_Period4x_H + mov TMR3H, A + mov TMR3CN, #04h ; Timer3 enabled + setb Flags0.T3_PENDING + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Wait for comparator to go low/high routines +; +; No assumptions +; +; Waits for the zero cross scan wait time to elapse +; Then scans for comparator going low/high +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +wait_for_comp_out_low: + mov Comp_Wait_Reads, #0 + mov Bit_Access, #00h ; Desired comparator output + jmp wait_for_comp_out_start + +wait_for_comp_out_high: + mov Comp_Wait_Reads, #0 + mov Bit_Access, #40h ; Desired comparator output + +wait_for_comp_out_start: + setb EA ; Enable interrupts + inc Comp_Wait_Reads + jb Flags0.T3_PENDING, ($+4) ; Has zero cross scan timeout elapsed? + ret ; Yes - return + + ; Set default comparator response times + mov CPT0MD, #0 ; Set fast response (100ns) as default +IF COMP1_USED==1 + mov CPT1MD, #0 ; Set fast response (100ns) as default +ENDIF + ; Select number of comparator readings based upon current rotation speed + mov A, Comm_Period4x_H ; Load rotation period + clr C + rrc A ; Divide by 4 + clr C + rrc A + mov Temp1, A + inc Temp1 ; Add one to be sure it is always larger than zero + jz comp_wait_on_comp_able ; If minimum number of readings - jump directly to reading + ; For damped mode, do fewer comparator readings (since comparator info is primarily only available in the pwm on period) + jnb Flags2.PGM_PWMOFF_DAMPED, comp_wait_set_max_readings + + clr C + rrc A ; Divide by 4 again + clr C + rrc A + mov Temp1, A + inc Temp1 ; Add one to be sure it is always larger than zero + +comp_wait_set_max_readings: + clr C + mov A, Temp1 ; Limit to a max of 10 + subb A, #10 + jc ($+4) + + mov Temp1, #10 + + jnb Flags2.PGM_PWM_HIGH_FREQ, comp_wait_set_response_time ; Jump if pwm frequency is low + + clr C + mov A, Temp1 ; Limit to a max of 4 + subb A, #4 + jc ($+4) + + mov Temp1, #4 + +comp_wait_set_response_time: + clr C + mov A, Comm_Period4x_H ; Is Comm_Period4x_H less than 1ms? + subb A, #8 + jc comp_wait_on_comp_able ; Yes - jump + + mov CPT0MD, #2 ; Set medium response (300ns) +IF COMP1_USED==1 + mov CPT1MD, #2 ; Set medium response (300ns) +ENDIF + clr C + mov A, Comm_Period4x_H ; Is Comm_Period4x_H less than 2ms? + subb A, #16 + jc comp_wait_on_comp_able ; Yes - jump + + mov CPT0MD, #3 ; Set slow response (1000ns) +IF COMP1_USED==1 + mov CPT1MD, #3 ; Set slow response (1000ns) +ENDIF + +comp_wait_on_comp_able: + jb Flags0.T3_PENDING, ($+6) ; Has zero cross scan timeout elapsed? + setb EA ; Enable interrupts + ret ; Yes - return + + mov Temp2, #COMP_PWM_HIGH_ON_DELAY ; Wait time after pwm has been switched on (motor wire electrical settling) + jb Flags2.PGM_PWM_HIGH_FREQ, ($+5) + mov Temp2, #COMP_PWM_LOW_ON_DELAY + setb EA ; Enable interrupts + nop ; Allocate only just enough time to capture interrupt + nop + clr EA ; Disable interrupts + jb Flags0.PWM_ON, pwm_wait_startup ; If pwm on - proceed + + mov Temp2, #COMP_PWM_HIGH_OFF_DELAY ; Wait time after pwm has been switched off (motor wire electrical settling) + jb Flags2.PGM_PWM_HIGH_FREQ, ($+5) + mov Temp2, #COMP_PWM_LOW_OFF_DELAY + jnb Flags2.CURR_PWMOFF_COMP_ABLE, comp_wait_on_comp_able ; If comparator is not usable in pwm off - go back + +pwm_wait_startup: + jnb Flags1.DIRECT_STARTUP_PHASE, pwm_wait ; Set a long delay from pwm on/off events during direct startup + + mov Temp2, #120 +pwm_wait: + clr C + mov A, TL1 + subb A, Temp2 +IF DAMPED_MODE_ENABLE==0 + jc comp_wait_on_comp_able ; Re-evaluate pwm cycle for slower escs +ENDIF +IF DAMPED_MODE_ENABLE==1 ; Assume same pwm cycle for fast escs + jb Flags1.DIRECT_STARTUP_PHASE, ($+5) + jc pwm_wait + jc comp_wait_on_comp_able ; Re-evaluate pwm cycle during direct start +ENDIF + +comp_read: + Read_Comp_Out ; Read comparator output + cpl A + anl A, #40h + cjne A, Bit_Access, ($+5) ; If comparator output is correct - proceed + + ajmp wait_for_comp_out_start ; If comparator output is not correct - go back and restart + + djnz Temp1, comp_wait_on_comp_able ; Decrement readings counter - repeat comparator reading if not zero + + setb EA ; Enable interrupts + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Evaluate comparator integrity +; +; No assumptions +; +; Checks comparator signal behaviour versus expected behaviour +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +evaluate_comparator_integrity: + clr Flags0.DEMAG_DETECTED ; Clear demag detected flag + ; Check if demag compensation is enabled +;读取Demag compensation + mov Temp1, #Pgm_Demag_Comp ; Load programmed demag compensation + mov A, @Temp1 + dec A + jz eval_comp_no_demag + + ; Check if a demag situation has occurred + mov A, Comp_Wait_Reads ; Check if there were no waits (there shall be some). If none a demag situation has occurred + dec A + jnz eval_comp_no_demag +;Demag compensation = 第2项 + setb Flags0.DEMAG_DETECTED ; Set demag detected flag +;Demag compensation = 第1项 +eval_comp_no_demag: + jnb Flags1.DIRECT_STARTUP_PHASE, eval_comp_check_timeout + + inc Direct_Startup_Ok_Cnt ; Increment ok counter + jb Flags0.T3_PENDING, eval_comp_exit + + mov Direct_Startup_Ok_Cnt, #0 ; Reset ok counter + jmp eval_comp_exit + +eval_comp_check_timeout: + jb Flags0.T3_PENDING, eval_comp_exit ; Has timeout elapsed? + dec SP ; Routine exit without "ret" command + dec SP + ljmp run_to_wait_for_power_on ; Yes - exit run mode + +eval_comp_exit: + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Setup commutation timing routine +; +; No assumptions +; +; Sets up and starts wait from commutation to zero cross +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +setup_comm_wait: + mov TMR3CN, #00h ; Timer3 disabled + clr C + clr A + subb A, Wt_Comm_L ; Set wait commutation value + mov TMR3L, A + clr A + subb A, Wt_Comm_H + mov TMR3H, A + mov TMR3CN, #04h ; Timer3 enabled + setb Flags0.T3_PENDING + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Wait for commutation routine +; +; No assumptions +; +; Waits from zero cross to commutation +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +wait_for_comm: + ; Store motor power + mov Temp7, Current_Pwm_Limited + ; Check if a demag situation has occurred + jnb Flags0.DEMAG_DETECTED, wait_for_comm_wait; Demag detected? + + ; Load programmed demag compensation + mov Temp1, #Pgm_Demag_Comp_Power_Decoded ; Yes - load programmed demag compensation power decoded + mov A, @Temp1 + mov Temp8, A ; Store in Temp8 + + ; Check for first power off + cjne Temp8, #1, wait_for_comm_blind + + setb Flags0.DEMAG_CUT_POWER ; Turn off motor power + mov Current_Pwm_Limited, #0 + All_nFETs_off + + ; Wait a blind wait +wait_for_comm_blind: + call setup_zc_scan_wait ; Setup a zero cross scan wait (7.5 deg) +wait_demag_default_zc1: + jnb Flags0.DEMAG_CUT_POWER, ($+6) ; Cut motor power if set + mov Current_Pwm_Limited, #0 + jnb Flags0.T3_PENDING, ($+5) + ajmp wait_demag_default_zc1 + + ; Check for second power off + cjne Temp8, #2, wait_for_comm_second_blind_wait + + setb Flags0.DEMAG_CUT_POWER ; Turn off motor power + mov Current_Pwm_Limited, #0 + All_nFETs_off + + ; Check for another blind wait +wait_for_comm_second_blind_wait: + mov Temp1, #Pgm_Demag_Comp_Wait_Decoded ; Yes - load programmed demag compensation wait decoded + cjne @Temp1, #1, wait_for_comm_power3 + + call setup_zc_scan_wait ; Setup a zero cross scan wait (7.5 deg) +wait_demag_default_zc2: + jnb Flags0.DEMAG_CUT_POWER, ($+6) ; Cut motor power if set + mov Current_Pwm_Limited, #0 + jnb Flags0.T3_PENDING, ($+5) + ajmp wait_demag_default_zc2 + +wait_for_comm_power3: + ; Check for third power off + cjne Temp8, #3, wait_for_comm_setup + + setb Flags0.DEMAG_CUT_POWER ; Turn off motor power + mov Current_Pwm_Limited, #0 + All_nFETs_off + +wait_for_comm_setup: + call setup_comm_wait ; Setup commutation wait +wait_for_comm_wait: + jnb Flags0.DEMAG_CUT_POWER, ($+6) ; Cut motor power if set + mov Current_Pwm_Limited, #0 + jnb Flags0.T3_PENDING, ($+5) + ajmp wait_for_comm_wait + + jnb Flags0.DEMAG_DETECTED, wait_for_comm_exit ; Was there a demag situation? + + clr Flags0.DEMAG_CUT_POWER ; Restore motor power again + mov Current_Pwm_Limited, Temp7 + +wait_for_comm_exit: + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Commutation routines +; +; No assumptions +; +; Performs commutation switching +; Damped routines uses all pfets on when in pwm off to dampen the motor +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +;****************************************************** +; 关B down 并 开A down或Cdown +;****************************************************** +comm1comm2: + clr EA ; Disable all interrupts + BpFET_off ; Bp off + jb Flags2.PGM_PWMOFF_DAMPED, comm12_damp + jmp comm12_nondamp +comm12_damp: +IF DAMPED_MODE_ENABLE == 0 + mov DPTR, #pwm_cnfet_apfet_on_fast + jb Flags2.PGM_PWMOFF_DAMPED_LIGHT, comm12_nondamp +ENDIF +IF DAMPED_MODE_ENABLE == 1 + mov DPTR, #pwm_cnfet_apfet_on_safe +ENDIF + jnb Flags2.CURR_PWMOFF_DAMPED, comm12_nondamp ; If pwm off not damped - branch + CpFET_off + mov A, #NFETON_DELAY ; Delay + djnz ACC, $ +comm12_nondamp: + ApFET_on ; Ap on + Set_Comp_Phase_B ; Set comparator to phase B + mov Comm_Phase, #2 + setb EA ; Enable all interrupts + ret + +comm2comm3: + clr EA ; Disable all interrupts + jb Flags2.PGM_PWMOFF_DAMPED, comm23_damp + jmp comm23_nondamp +comm23_damp: +IF DAMPED_MODE_ENABLE == 0 + mov DPTR, #pwm_bnfet_apfet_on_fast +ENDIF +IF DAMPED_MODE_ENABLE == 1 + mov DPTR, #pwm_bnfet_apfet_on_safe +ENDIF + jnb Flags2.CURR_PWMOFF_DAMPED, comm23_nfet ; If pwm off not damped - branch + BpFET_off + CpFET_off + mov A, #NFETON_DELAY ; Delay + djnz ACC, $ + jmp comm23_nfet +comm23_nondamp: + mov DPTR, #pwm_bfet_on +comm23_nfet: + CnFET_off ; Cn off + jnb Flags0.PWM_ON, comm23_cp ; Is pwm on? + BnFET_on ; Yes - Bn on +comm23_cp: + Set_Comp_Phase_C ; Set comparator to phase C + mov Comm_Phase, #3 + setb EA ; Enable all interrupts + ret + +comm3comm4: + clr EA ; Disable all interrupts + ApFET_off ; Ap off + jb Flags2.PGM_PWMOFF_DAMPED, comm34_damp + jmp comm34_nondamp +comm34_damp: +IF DAMPED_MODE_ENABLE == 0 + mov DPTR, #pwm_bnfet_cpfet_on_fast + jb Flags2.PGM_PWMOFF_DAMPED_LIGHT, comm34_nondamp +ENDIF +IF DAMPED_MODE_ENABLE == 1 + mov DPTR, #pwm_bnfet_cpfet_on_safe +ENDIF + jnb Flags2.CURR_PWMOFF_DAMPED, comm34_nondamp ; If pwm off not damped - branch + BpFET_off + mov A, #NFETON_DELAY ; Delay + djnz ACC, $ +comm34_nondamp: + CpFET_on ; Cp on + Set_Comp_Phase_A ; Set comparator to phase A + mov Comm_Phase, #4 + setb EA ; Enable all interrupts + ret + +comm4comm5: + clr EA ; Disable all interrupts + jb Flags2.PGM_PWMOFF_DAMPED, comm45_damp + jmp comm45_nondamp +comm45_damp: +IF DAMPED_MODE_ENABLE == 0 + mov DPTR, #pwm_anfet_cpfet_on_fast +ENDIF +IF DAMPED_MODE_ENABLE == 1 + mov DPTR, #pwm_anfet_cpfet_on_safe +ENDIF + jnb Flags2.CURR_PWMOFF_DAMPED, comm45_nfet ; If pwm off not damped - branch + ApFET_off + BpFET_off + mov A, #NFETON_DELAY ; Delay + djnz ACC, $ + jmp comm45_nfet +comm45_nondamp: + mov DPTR, #pwm_afet_on +comm45_nfet: + BnFET_off ; Bn off + jnb Flags0.PWM_ON, comm45_cp ; Is pwm on? + AnFET_on ; Yes - An on +comm45_cp: + Set_Comp_Phase_B ; Set comparator to phase B + mov Comm_Phase, #5 + setb EA ; Enable all interrupts + ret + +comm5comm6: + clr EA ; Disable all interrupts + CpFET_off ; Cp off + jb Flags2.PGM_PWMOFF_DAMPED, comm56_damp + jmp comm56_nondamp +comm56_damp: +IF DAMPED_MODE_ENABLE == 0 + mov DPTR, #pwm_anfet_bpfet_on_fast + jb Flags2.PGM_PWMOFF_DAMPED_LIGHT, comm56_nondamp +ENDIF +IF DAMPED_MODE_ENABLE == 1 + mov DPTR, #pwm_anfet_bpfet_on_safe +ENDIF + jnb Flags2.CURR_PWMOFF_DAMPED, comm56_nondamp ; If pwm off not damped - branch + ApFET_off + mov A, #NFETON_DELAY ; Delay + djnz ACC, $ +comm56_nondamp: + BpFET_on ; Bp on + Set_Comp_Phase_C ; Set comparator to phase C + mov Comm_Phase, #6 + setb EA ; Enable all interrupts + ret + +comm6comm1: + clr EA ; Disable all interrupts + jb Flags2.PGM_PWMOFF_DAMPED, comm61_damp + jmp comm61_nondamp +comm61_damp: +IF DAMPED_MODE_ENABLE == 0 + mov DPTR, #pwm_cnfet_bpfet_on_fast +ENDIF +IF DAMPED_MODE_ENABLE == 1 + mov DPTR, #pwm_cnfet_bpfet_on_safe +ENDIF + jnb Flags2.CURR_PWMOFF_DAMPED, comm61_nfet ; If pwm off not damped - branch + ApFET_off + CpFET_off + mov A, #NFETON_DELAY ; Delay + djnz ACC, $ + jmp comm61_nfet +comm61_nondamp: + mov DPTR, #pwm_cfet_on +comm61_nfet: + AnFET_off ; An off + jnb Flags0.PWM_ON, comm61_cp ; Is pwm on? + CnFET_on ; Yes - Cn on +comm61_cp: + Set_Comp_Phase_A ; Set comparator to phase A + mov Comm_Phase, #1 + setb EA ; Enable all interrupts + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Switch power off routine +; +; No assumptions +; +; Switches all fets off +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +switch_power_off: + mov DPTR, #pwm_nofet_on ; Set DPTR register to pwm_nofet_on label + All_pFETs_Off ; Turn off all pfets + All_nFETs_Off ; Turn off all nfets + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Decrement stepper step routine +; +; No assumptions +; +; Decrements the stepper step +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +;decrement_stepper_step: +; clr C +; mov A, Wt_Stepper_Step_L +; subb A, Stepper_Step_End_L ; Minimum Stepper_Step_End +; mov A, Wt_Stepper_Step_H +; subb A, Stepper_Step_End_H +; jnc decrement_step ; Branch if same or higher than minimum +; ret + +;decrement_step: + ; Load programmed startup acceleration +;读取Startup acceleration +; mov Temp1, #Pgm_Startup_Accel ; Load startup accel +; mov A, @Temp1 +; mov Temp8, A ; Store in Temp8 + ; Check acceleration setting and set step size accordingly +; clr C +; mov A, Temp8 +; subb A, #5 +; jnc dec_step_high +; clr C +; mov A, Temp8 +; subb A, #4 +; jnc dec_step_med_high +; clr C +; mov A, Temp8 +; subb A, #3 +; jnc dec_step_med +; clr C +; mov A, Temp8 +; subb A, #2 +; jnc dec_step_med_low +; clr C +; mov A, Temp8 +; subb A, #1 +; jnc dec_step_low +;Start acceleration = 第5项 +;dec_step_high: +; clr C +; mov A, Wt_Stepper_Step_L +; subb A, #low(30 SHL 1) +; mov Temp1, A +; jmp decrement_step_exit +;Start acceleration = 第4项 +;dec_step_med_high: +; clr C +; mov A, Wt_Stepper_Step_L +; subb A, #low(20 SHL 1) +; mov Temp1, A +; jmp decrement_step_exit +;Start acceleration = 第3项 +;dec_step_med: +; clr C +; mov A, Wt_Stepper_Step_L +; subb A, #low(13 SHL 1) +; mov Temp1, A +; jmp decrement_step_exit +;Start acceleration = 第2项 +;dec_step_med_low: +; clr C +; mov A, Wt_Stepper_Step_L +; subb A, #low(9 SHL 1) +; mov Temp1, A +; jmp decrement_step_exit +;Start acceleration = 第1项 +;dec_step_low: +; clr C +; mov A, Wt_Stepper_Step_L +; subb A, #low(5 SHL 1) +; mov Temp1, A +; jmp decrement_step_exit + +;decrement_step_exit: +; mov A, Wt_Stepper_Step_H +; subb A, #0 +; mov Temp2, A +; mov Wt_Stepper_Step_L, Temp1 +; mov Wt_Stepper_Step_H, Temp2 +; ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Stepper timer wait +; +; No assumptions +; +; Waits for the stepper step timer to elapse +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +;stepper_timer_wait: +; jnb Flags0.T3_PENDING, ($+6) ; Timer pending? +; ljmp stepper_timer_wait ; Yes, go back +; ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Set default parameters +; +; No assumptions +; +; Sets default programming parameters +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +set_default_parameters: +IF MODE == 0 ; Main + mov Temp1, #Pgm_Gov_P_Gain + mov @Temp1, #DEFAULT_PGM_MAIN_P_GAIN + mov Temp1, #Pgm_Gov_I_Gain + mov @Temp1, #DEFAULT_PGM_MAIN_I_GAIN + mov Temp1, #Pgm_Gov_Mode + mov @Temp1, #DEFAULT_PGM_MAIN_GOVERNOR_MODE + mov Temp1, #Pgm_Gov_Range + mov @Temp1, #DEFAULT_PGM_MAIN_GOVERNOR_RANGE + mov Temp1, #Pgm_Low_Voltage_Lim + mov @Temp1, #DEFAULT_PGM_MAIN_LOW_VOLTAGE_LIM + mov Temp1, #Pgm_Startup_Pwr + mov @Temp1, #DEFAULT_PGM_MAIN_STARTUP_PWR + mov Temp1, #Pgm_Startup_Rpm + mov @Temp1, #DEFAULT_PGM_MAIN_STARTUP_RPM + mov Temp1, #Pgm_Startup_Accel + mov @Temp1, #DEFAULT_PGM_MAIN_STARTUP_ACCEL + mov Temp1, #Pgm_Startup_Method + mov @Temp1, #DEFAULT_PGM_MAIN_STARTUP_METHOD + mov Temp1, #Pgm_Comm_Timing + mov @Temp1, #DEFAULT_PGM_MAIN_COMM_TIMING + mov Temp1, #Pgm_Throttle_Rate + mov @Temp1, #DEFAULT_PGM_MAIN_THROTTLE_RATE + mov Temp1, #Pgm_Damping_Force + mov @Temp1, #DEFAULT_PGM_MAIN_DAMPING_FORCE + mov Temp1, #Pgm_Pwm_Freq + mov @Temp1, #DEFAULT_PGM_MAIN_PWM_FREQ + mov Temp1, #Pgm_Demag_Comp + mov @Temp1, #DEFAULT_PGM_MAIN_DEMAG_COMP + mov Temp1, #Pgm_Direction_Rev + mov @Temp1, #DEFAULT_PGM_MAIN_DIRECTION_REV + mov Temp1, #Pgm_Input_Pol + mov @Temp1, #DEFAULT_PGM_MAIN_RCP_PWM_POL + mov Temp1, #Pgm_Motor_Idle + mov @Temp1, #0 +ENDIF +IF MODE == 1 ; Tail + mov Temp1, #Pgm_Motor_Gain + mov @Temp1, #DEFAULT_PGM_TAIL_GAIN + mov Temp1, #Pgm_Motor_Idle + mov @Temp1, #DEFAULT_PGM_TAIL_IDLE_SPEED + mov Temp1, #Pgm_Startup_Pwr + mov @Temp1, #DEFAULT_PGM_TAIL_STARTUP_PWR + mov Temp1, #Pgm_Startup_Rpm + mov @Temp1, #DEFAULT_PGM_TAIL_STARTUP_RPM + mov Temp1, #Pgm_Startup_Accel + mov @Temp1, #DEFAULT_PGM_TAIL_STARTUP_ACCEL + mov Temp1, #Pgm_Startup_Method + mov @Temp1, #DEFAULT_PGM_TAIL_STARTUP_METHOD + mov Temp1, #Pgm_Comm_Timing + mov @Temp1, #DEFAULT_PGM_TAIL_COMM_TIMING + mov Temp1, #Pgm_Throttle_Rate + mov @Temp1, #DEFAULT_PGM_TAIL_THROTTLE_RATE + mov Temp1, #Pgm_Damping_Force + mov @Temp1, #DEFAULT_PGM_TAIL_DAMPING_FORCE + mov Temp1, #Pgm_Pwm_Freq + mov @Temp1, #DEFAULT_PGM_TAIL_PWM_FREQ + mov Temp1, #Pgm_Demag_Comp + mov @Temp1, #DEFAULT_PGM_TAIL_DEMAG_COMP + mov Temp1, #Pgm_Direction_Rev + mov @Temp1, #DEFAULT_PGM_TAIL_DIRECTION_REV + mov Temp1, #Pgm_Input_Pol + mov @Temp1, #DEFAULT_PGM_TAIL_RCP_PWM_POL + mov Temp1, #Pgm_Gov_Mode + mov @Temp1, #4 +ENDIF +IF MODE == 2 ; Multi + mov Temp1, #Pgm_Fir_Key ;2013.8.27 增加关键字默认值 + mov @Temp1, #DEFAULT_PGM_MULTI_FIRST_KEYWORD + mov Temp1, #Pgm_Gov_P_Gain + mov @Temp1, #DEFAULT_PGM_MAIN_P_GAIN ;Pgm_Gov_P_Gain=9 P增益 + mov Temp1, #Pgm_Gov_I_Gain + mov @Temp1, #DEFAULT_PGM_MAIN_I_GAIN ;Pgm_Gov_I_Gain=9 I增益 + mov Temp1, #Pgm_Gov_Mode + mov @Temp1, #DEFAULT_PGM_MAIN_GOVERNOR_MODE ;Pgm_Gov_Mode=4 + mov Temp1, #Pgm_Motor_Gain + mov @Temp1, #DEFAULT_PGM_MULTI_GAIN ;gm_Motor_Gain=3 + mov Temp1, #Pgm_Low_Voltage_Lim + mov @Temp1, #DEFAULT_PGM_MULTI_LOW_VOLTAGE_LIM ;Pgm_Low_Voltage_Lim=1 低压保护：1=Off 2=3.0V/c 3=3.1V/c 4=3.2V/c 5=3.3V/c 6=3.4V/c + mov Temp1, #Pgm_Low_Voltage_Ctl + mov @Temp1, #DEFAULT_PGM_MULTI_LOW_VOLTAGE_CTL ;Pgm_Low_Voltage_Ctl=1 低压保护: 1=功率逐渐降低到31% 2=关闭输出 + mov Temp1, #Pgm_Startup_Pwr + mov @Temp1, #DEFAULT_PGM_MULTI_STARTUP_PWR ;Pgm_Startup_Pwr=10 + mov Temp1, #Pgm_Startup_Rpm + mov @Temp1, #DEFAULT_PGM_MULTI_STARTUP_RPM ;Pgm_Startup_Rpm=1 启动转速：1=0.67 2=0.8 3=1.00 4=1.25 5=1.5 + mov Temp1, #Pgm_Startup_Accel + mov @Temp1, #DEFAULT_PGM_MULTI_STARTUP_ACCEL ;Pgm_Startup_Accel=5 +; mov Temp1, #Pgm_Startup_Method +; mov @Temp1, #DEFAULT_PGM_MULTI_STARTUP_METHOD ;Pgm_Startup_Method=2 启动方式：1=步进启动 2=直接启动 + mov Temp1, #Pgm_Comm_Timing + mov @Temp1, #DEFAULT_PGM_MULTI_COMM_TIMING ;Pgm_Comm_Timing=3 进角时间：1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=High + mov Temp1, #Pgm_Throttle_Rate + mov @Temp1, #DEFAULT_PGM_MULTI_THROTTLE_RATE ;Pgm_Throttle_Rate=13 油门范围：1=2 2=3 3=4 4=6 5=8 6=12 7=16 8=24 9=32 10=48 11=64 12=128 13=255 + mov Temp1, #Pgm_Damping_Force + mov @Temp1, #DEFAULT_PGM_MULTI_DAMPING_FORCE ;Pgm_Damping_Force=6 刹车力度：1=VeryLow 2=Low 3=MediumLow 4=MediumHigh 5=High 6=Highest + mov Temp1, #Pgm_Pwm_Freq + mov @Temp1, #DEFAULT_PGM_MULTI_PWM_FREQ ;Pgm_Pwm_Freq=1 1=Low 2=High 3=DampedLight 4=Damped + mov Temp1, #Pgm_Demag_Comp + mov @Temp1, #DEFAULT_PGM_MULTI_DEMAG_COMP ;Pgm_Demag_Comp=2 1=Disabled 2=15/0 3=15/7.5 4=7.5/7.5 5=7.5/15 (degrees blind advance / power off) + mov Temp1, #Pgm_Direction_Rev + mov @Temp1, #DEFAULT_PGM_MULTI_DIRECTION_REV ;Pgm_Direction_Rev=1 方向设置：1=正转 2=反转 + mov Temp1, #Pgm_Input_Pol + mov @Temp1, #DEFAULT_PGM_MULTI_RCP_PWM_POL ;Pgm_Input_Pol=1 接收信号极性设置：1=Positive（正） 2=Negative（负） + mov Temp1, #Pgm_Motor_Idle + mov @Temp1, #0 ;Pgm_Motor_Idle=0 +ENDIF + mov Temp1, #Pgm_Enable_TX_Program + mov @Temp1, #DEFAULT_PGM_ENABLE_TX_PROGRAM ;Pgm_Enable_TX_Program=1 1=Enabled 0=Disabled + mov Temp1, #Pgm_Main_Rearm_Start + mov @Temp1, #DEFAULT_PGM_MAIN_REARM_START ;Pgm_Main_Rearm_Start=0 1=Enabled 0=Disabled + mov Temp1, #Pgm_Gov_Setup_Target + mov @Temp1, #DEFAULT_PGM_MAIN_GOV_SETUP_TARGET ;Pgm_Gov_Setup_Target=180 Target for governor in setup mode. Corresponds to 70% throttle + mov Temp1, #Pgm_Ppm_Min_Throttle + mov @Temp1, #DEFAULT_PGM_PPM_MIN_THROTTLE ;Pgm_Ppm_Min_Throttle=37 最小油门：4*37+1000=1148 + mov Temp1, #Pgm_Ppm_Max_Throttle + mov @Temp1, #DEFAULT_PGM_PPM_MAX_THROTTLE ;Pgm_Ppm_Max_Throttle=208 最大油门：4*208+1000=1832 + mov Temp1, #Pgm_Beep_Strength + mov @Temp1, #DEFAULT_PGM_MAIN_BEEP_STRENGTH ;Pgm_Beep_Strength=120 声强设置 + mov Temp1, #Pgm_Beacon_Strength + mov @Temp1, #DEFAULT_PGM_MAIN_BEACON_STRENGTH ;Pgm_Beacon_Strength=200 灯亮长Beacon strength？？？？ + mov Temp1, #Pgm_Beacon_Delay + mov @Temp1, #DEFAULT_PGM_MAIN_BEACON_DELAY ;Pgm_Beacon_Delay=4 灯亮时间？？？？1=30s 2=1m 3=2m 4=3m 5=Infinite + mov Temp1, #Pgm_BEC_Voltage_High + mov @Temp1, #DEFAULT_PGM_BEC_VOLTAGE_HIGH ;Pgm_BEC_Voltage_High=0 0=Low 1= High + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Decode parameters +; +; No assumptions +; +; Decodes programming parameters +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +decode_parameters: + ; Load programmed damping force +;读取Damping force + mov Temp1, #Pgm_Damping_Force; Load damping force + mov A, @Temp1 + mov Temp8, A ; Store in Temp8 + ; Decode damping +;Damping force = 第1项 + clr Flags2.PGM_PWMOFF_DAMPED + clr Flags2.PGM_PWMOFF_DAMPED_LIGHT + clr Flags2.PGM_PWMOFF_DAMPED_FULL + clr Flags2.CURR_PWMOFF_DAMPED + setb Flags2.CURR_PWMOFF_COMP_ABLE +; mov Damping_Period, #9 ; Set default +; mov Damping_On, #1 + mov Rcp_Stop_Limit,#40 + clr C + cjne Temp8, #1, decode_damping_2 ; Look for 2 + jmp decode_damping_done + +;Damping force = 第2项 +decode_damping_2: + setb Flags2.PGM_PWMOFF_DAMPED +; setb Flags2.PGM_PWMOFF_DAMPED_LIGHT +; clr Flags2.PGM_PWMOFF_DAMPED_FULL + setb Flags2.CURR_PWMOFF_DAMPED + clr Flags2.CURR_PWMOFF_COMP_ABLE + mov Damping_Period, #5 + mov Damping_On, #1 + mov Rcp_Stop_Limit,#60 + clr C + cjne Temp8, #2, decode_damping_3 ; Look for 3 + jmp decode_damping_done + +;Damping force = 第3项 +decode_damping_3: +; setb Flags2.PGM_PWMOFF_DAMPED + setb Flags2.PGM_PWMOFF_DAMPED_LIGHT +; clr Flags2.PGM_PWMOFF_DAMPED_FULL +; setb Flags2.CURR_PWMOFF_DAMPED +; clr Flags2.CURR_PWMOFF_COMP_ABLE + mov Damping_Period, #5 + mov Damping_On, #2 + mov Rcp_Stop_Limit,#30 + clr C + cjne Temp8, #3, decode_damping_4 ; Look for 4 + jmp decode_damping_done + +;Damping force = 第4项 +decode_damping_4: +; setb Flags2.PGM_PWMOFF_DAMPED +; setb Flags2.PGM_PWMOFF_DAMPED_LIGHT +; clr Flags2.PGM_PWMOFF_DAMPED_FULL +; setb Flags2.CURR_PWMOFF_DAMPED +; clr Flags2.CURR_PWMOFF_COMP_ABLE + mov Damping_Period, #5 + mov Damping_On, #3 + mov Rcp_Stop_Limit,#25 + clr C + cjne Temp8, #4, decode_damping_5 ; Look for 5 + jmp decode_damping_done + +;Damping force = 第5项 +decode_damping_5: +; setb Flags2.PGM_PWMOFF_DAMPED +; setb Flags2.PGM_PWMOFF_DAMPED_LIGHT + setb Flags2.PGM_PWMOFF_DAMPED_FULL +; setb Flags2.CURR_PWMOFF_DAMPED +; clr Flags2.CURR_PWMOFF_COMP_ABLE + mov Damping_Period, #5 + mov Damping_On, #4 + mov Rcp_Stop_Limit,#20 + clr C + cjne Temp8, #5, decode_damping_6; Look for 6 + jmp decode_damping_done + +;Damping force = 第6项 +decode_damping_6: +; setb Flags2.PGM_PWMOFF_DAMPED +; setb Flags2.PGM_PWMOFF_DAMPED_LIGHT +; setb Flags2.PGM_PWMOFF_DAMPED_FULL +; setb Flags2.CURR_PWMOFF_DAMPED +; clr Flags2.CURR_PWMOFF_COMP_ABLE + mov Damping_Period, #5 + mov Damping_On, #5 + mov Rcp_Stop_Limit,#10 + +;*********************************************** +; 读取编程PWM频率(1) +;带硬刹/软刹 +;*********************************************** +decode_damping_done: + ; Load programmed pwm frequency +;读取Pwm frequency + mov Temp1, #Pgm_Pwm_Freq ; Load pwm freq + mov A, @Temp1 + mov Temp8, A ; Store in Temp8 +IF MODE == 0 ; Main + clr Flags2.PGM_PWMOFF_DAMPED_LIGHT + clr C + cjne Temp8, #3, ($+5) + setb Flags2.PGM_PWMOFF_DAMPED_LIGHT + clr Flags2.PGM_PWMOFF_DAMPED_FULL +ENDIF +IF MODE >= 1 ; Tail or multi +;////// clr Flags2.PGM_PWMOFF_DAMPED_LIGHT +;////// clr C +;////// cjne Temp8, #3, ($+5) +;Pwm frequency = 第3项 +;////// setb Flags2.PGM_PWMOFF_DAMPED_LIGHT +;////// clr Flags2.PGM_PWMOFF_DAMPED_FULL +;////// clr C +;////// cjne Temp8, #4, ($+5) +;Pwm frequency = 第4项 +;////// setb Flags2.PGM_PWMOFF_DAMPED_FULL +ENDIF +;Pwm frequency = 第1项 +;////// clr Flags2.PGM_PWMOFF_DAMPED ; Set damped flag if fully damped or damped light is set +;////// mov A, #((1 SHL PGM_PWMOFF_DAMPED_FULL)+(1 SHL PGM_PWMOFF_DAMPED_LIGHT)) +;////// anl A, Flags2 ; Check if any damped mode is set +;////// jz ($+4) +;Pwm frequency = 第2项 +;////// setb Flags2.PGM_PWMOFF_DAMPED +;Pwm frequency = 第1项 +;////// clr Flags2.CURR_PWMOFF_DAMPED ; Set non damped status as start +;////// jz ($+4) +;Pwm frequency = 第2项 +;////// setb Flags2.CURR_PWMOFF_DAMPED ; Set non damped status as start if damped +;Pwm frequency = 第1项 +;////// setb Flags2.CURR_PWMOFF_COMP_ABLE ; Set comparator usable status +;////// jz ($+4) +;Pwm frequency = 第2项 +;////// clr Flags2.CURR_PWMOFF_COMP_ABLE ; Set comparator not usable status if damped +;*********************************************** +; 读取编程电机旋转方向 +;*********************************************** +;读取Rotation direction +;Rotation direction = 第1项 + clr Flags3.PGM_DIR_REV + mov Temp1, #Pgm_Direction_Rev + mov A, @Temp1 + jnb ACC.1, ($+5) +;Rotation direction = 第2项 + setb Flags3.PGM_DIR_REV +;*********************************************** +; 读取编程输入信号RC PWM脉冲有效极性 +;*********************************************** +;读取Input pwm polarity +;Input pwm polarity = 第1项 + clr Flags3.PGM_RCP_PWM_POL + mov Temp1, #Pgm_Input_Pol + mov A, @Temp1 + jnb ACC.1, ($+5) +;Input pwm polarity = 第2项 + setb Flags3.PGM_RCP_PWM_POL +;*********************************************** +; 读取编程PWM频率(2) +;不带硬刹/软刹 +;*********************************************** + clr C + mov A, Temp8 + subb A, #1 + jz decode_pwm_freq_low +;Pwm frequency = 第1项 + mov CKCON, #01h ; Timer0 set for clk/4 (22kHz pwm) + setb Flags2.PGM_PWM_HIGH_FREQ + jmp decode_pwm_freq_end +;Pwm frequency = 第2项 +decode_pwm_freq_low: + mov CKCON, #00h ; Timer0 set for clk/12 (8kHz pwm) + clr Flags2.PGM_PWM_HIGH_FREQ + +decode_pwm_freq_end: + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Decode governor gain +; +; No assumptions +; +; Decodes governor gains +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +decode_governor_gains: + ; Decode governor gains +;读取 Closed loop P gain + mov Temp1, #Pgm_Gov_P_Gain ; Decode governor P gain + mov A, @Temp1 + dec A + mov DPTR, #GOV_GAIN_TABLE + movc A, @A+DPTR + mov Temp1, #Pgm_Gov_P_Gain_Decoded + mov @Temp1, A +;读取 Closed loop I gain + mov Temp1, #Pgm_Gov_I_Gain ; Decode governor P gain + mov A, @Temp1 + dec A + mov DPTR, #GOV_GAIN_TABLE + movc A, @A+DPTR + mov Temp1, #Pgm_Gov_I_Gain_Decoded + mov @Temp1, A + call switch_power_off ; Reset DPTR + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Decode throttle rate +; +; No assumptions +; +; Decodes throttle rate +; +;读取中点油门范围 +;**** **** **** **** **** **** **** **** **** **** **** **** **** +decode_throttle_rate: + ; Decode throttle rate +;读取 Throttle change rate + mov Temp1, #Pgm_Throttle_Rate + mov A, @Temp1 + dec A + mov DPTR, #THROTTLE_RATE_TABLE + movc A, @A+DPTR + mov Temp1, #Pgm_Throttle_Rate_Decoded + mov @Temp1, A + call switch_power_off ; Reset DPTR + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Decode startup power +; +; No assumptions +; +; Decodes startup power +; +;编程启动力度 +;**** **** **** **** **** **** **** **** **** **** **** **** **** +decode_startup_power: + ; Decode startup power +;读取 Startup power + mov Temp1, #Pgm_Startup_Pwr + mov A, @Temp1 + dec A + mov DPTR, #STARTUP_POWER_TABLE + movc A, @A+DPTR + mov Temp1, #Pgm_Startup_Pwr_Decoded + mov @Temp1, A + call switch_power_off ; Reset DPTR + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Decode demag compensation +; +; No assumptions +; +; Decodes throttle rate +; +;编程demag compensation参数 +;**** **** **** **** **** **** **** **** **** **** **** **** **** +decode_demag_comp: + ; Decode demag compensation +;读取Demag compensation + mov Temp1, #Pgm_Demag_Comp + mov A, @Temp1 + dec A + mov DPTR, #DEMAG_WAIT_TABLE + movc A, @A+DPTR + mov Temp1, #Pgm_Demag_Comp_Wait_Decoded + mov @Temp1, A + mov Temp1, #Pgm_Demag_Comp + mov A, @Temp1 + dec A + mov DPTR, #DEMAG_POWER_TABLE + movc A, @A+DPTR + mov Temp1, #Pgm_Demag_Comp_Power_Decoded + mov @Temp1, A + call switch_power_off ; Reset DPTR + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Set BEC voltage +; +; No assumptions +; +; Sets the BEC output voltage low or high +; +;读取 BEC +;**** **** **** **** **** **** **** **** **** **** **** **** **** +set_bec_voltage: + ; Set bec voltage +IF DUAL_BEC_VOLTAGE == 1 + Set_BEC_Lo ; Set default to low +;读取 Pgm_BEC_Voltage_High + mov Temp1, #Pgm_BEC_Voltage_High + mov A, @Temp1 + jz set_bec_voltage_exit + + Set_BEC_Hi ; Set to high + +set_bec_voltage_exit: +ENDIF + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Find throttle gain +; +; The difference between max and min throttle must be more than 520us (a Pgm_Ppm_xxx_Throttle difference of 130) +; +; Finds throttle gain from throttle calibration values +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +find_throttle_gain: + ; Load programmed minimum and maximum throttle +;读取Pgm_Ppm_Min_Throttle和Pgm_Ppm_Max_Throttle增量 + mov Temp1, #Pgm_Ppm_Min_Throttle + mov A, @Temp1 + mov Temp3, A + mov Temp1, #Pgm_Ppm_Max_Throttle + mov A, @Temp1 + mov Temp4, A + ; Check if full range is chosen + jnb Flags3.FULL_THROTTLE_RANGE, find_throttle_gain_calculate + + mov Temp3, #0 + mov Temp4, #255 + +find_throttle_gain_calculate: + ; Calculate difference + clr C + mov A, Temp4 + subb A, Temp3 + mov Temp5, A + ; Check that difference is minimum 130 + clr C + subb A, #130 + jnc ($+4) + + mov Temp5, #130 + + ; Find gain + mov Ppm_Throttle_Gain, #0 +test_gain: + inc Ppm_Throttle_Gain + mov A, Temp5 + mov B, Ppm_Throttle_Gain ; A has difference, B has gain + mul AB + clr C + mov A, B + subb A, #128 + jc test_gain + ret + + + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +;**** **** **** **** **** **** **** **** **** **** **** **** **** +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Main program start +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +;**** **** **** **** **** **** **** **** **** **** **** **** **** +;**** **** **** **** **** **** **** **** **** **** **** **** **** + +reset: + ; Select register bank 0 for main program routines + clr PSW.3 ; Select register bank 0 for main program routines + ; Disable the WDT. + anl PCA0MD, #NOT(40h) ; Clear watchdog enable bit + ; Initialize stack + mov SP, #0c0h ; Stack = 64 upper bytes of RAM + ; Initialize VDD monitor + orl VDM0CN, #080h ; Enable the VDD monitor + + mov Temp2, #3 +wait_100us: + djnz ACC, $ + djnz Temp2,wait_100us + +; call wait1ms ; Wait at least 100us + mov RSTSRC, #02h ; Set VDD monitor as a reset source (PORSF) + ; Set clock frequency + orl OSCICN, #03h ; Set clock divider to 1 + mov A, OSCICL + add A, #04h ; 24.5MHz to 24MHz (~0.5% per step) + jc reset_cal_done ; Is carry set? - skip next instruction + + mov OSCICL, A + +reset_cal_done: + ; Ports initialization + mov P0, #P0_INIT + mov P0MDOUT, #P0_PUSHPULL + mov P0MDIN, #P0_DIGITAL + mov P0SKIP, #P0_SKIP + mov P1, #P1_INIT + mov P1MDOUT, #P1_PUSHPULL + mov P1MDIN, #P1_DIGITAL + mov P1SKIP, #P1_SKIP +IF PORT3_EXIST == 1 + mov P2, #P2_INIT +ENDIF + mov P2MDOUT, #P2_PUSHPULL +IF PORT3_EXIST == 1 + mov P2MDIN, #P2_DIGITAL + mov P2SKIP, #P2_SKIP + mov P3, #P3_INIT + mov P3MDOUT, #P3_PUSHPULL + mov P3MDIN, #P3_DIGITAL +ENDIF + mov XBR1, #41h ; Xbar enabled, CEX0 routed to pin Rcp_In + ; Switch power off + call switch_power_off + ; Clear RAM + clr A ; Clear accumulator + mov Temp1, A ; Clear Temp1 +clear_ram: + mov @Temp1, A ; Clear RAM + djnz Temp1, clear_ram ; Is A not zero? - jump + ; Set default programmed parameters + call set_default_parameters + ; Read all programmed parameters + call read_all_eeprom_parameters + ; Decode parameters + call decode_parameters + ; Decode governor gains + call decode_governor_gains + ; Decode throttle rate + call decode_throttle_rate + ; Decode startup power + call decode_startup_power + ; Decode demag compensation + call decode_demag_comp + ; Set BEC voltage + call set_bec_voltage + ; Find throttle gain from stored min and max settings + call find_throttle_gain + ; Set beep strength +;读取超强设置 + mov Temp1, #Pgm_Beep_Strength + mov Beep_Strength, @Temp1 + ; Switch power off + call switch_power_off + ; Timer control + mov TCON, #50h ; Timer0 and timer1 enabled + ; Timer mode + mov TMOD, #02h ; Timer0 as 8bit + ; Timer2: clk/12 for 128us and 32ms interrupts + mov TMR2CN, #24h ; Timer2 enabled, low counter interrups enabled + ; Timer3: clk/12 for commutation timing + mov TMR3CN, #04h ; Timer3 enabled + ; PCA + mov PCA0CN, #40h ; PCA enabled + ; Initializing beep 上电叫3声 + call wait1s + call wait1s + + ; Enable interrupts + mov IE, #22h ; Enable timer0 and timer2 interrupts + mov IP, #02h ; High priority to timer0 interrupts + mov EIE1, #90h ; Enable timer3 and PCA0 interrupts + mov EIP1, #10h ; High priority to PCA interrupts + ; Initialize comparator + mov CPT0CN, #80h ; Comparator enabled, no hysteresis + mov CPT0MD, #03h ; Comparator response time 1us +IF COMP1_USED==1 + mov CPT1CN, #80h ; Comparator enabled, no hysteresis + mov CPT1MD, #03h ; Comparator response time 1us +ENDIF + ; Initialize ADC + Initialize_Adc ; Initialize ADC operation + call wait1ms + setb EA ; Enable all interrupts + ; Measure number of lipo cells + call Measure_Lipo_Cells ; Measure number of lipo cells + ; Initialize rc pulse + Rcp_Int_Enable ; Enable interrupt + Rcp_Int_First ; Enable interrupt and set to first edge + Rcp_Clear_Int_Flag ; Clear interrupt flag + clr Flags2.RCP_EDGE_NO ; Set first edge flag + call wait200ms + ; Set initial arm variable + mov Initial_Arm, #1 + mov OS_FLAG, #1 +; mov Pca_First_Int, #1 + + ; Measure PWM frequency +measure_pwm_freq_init: + setb Flags0.RCP_MEAS_PWM_FREQ ; Set measure pwm frequency flag +; clr C ;2014.08.08 信号滤波 +; mov A, Pgm_Pulse_Cnt ;2014.08.08 信号滤波 +; subb A, #5 ;2014.08.08 信号滤波 +; jc measure_pwm_freq_init ;2014.08.08 信号滤波 +; mov Pca_First_Int, #0 ;2014.08.08 信号滤波 +; call wait200ms +measure_pwm_freq_start: + mov Temp3, #5 ; Number of pulses to measure +; mov Temp3, #1 +measure_pwm_freq_loop: + ; Check if period diff was accepted + mov A, Rcp_Period_Diff_Accepted +; jnz ($+4) + + jnz measure_pwm + call wait3ms + mov A, New_RCP + clr C + subb A,#230 + jc ($+7) + mov Pgm_Pulse_Cnt, #0 +; mov Temp3, #5 + ajmp measure_pwm_freq_start + + mov A, New_RCP + clr C + subb A,#30 + jnc ($+7) + mov Pgm_Pulse_Cnt, #0 +; mov Temp3, #5 + ajmp measure_pwm_freq_start + + inc Pgm_Pulse_Cnt + mov A, Pgm_Pulse_Cnt + clr C + subb A, #5 + jnc ($+6) + mov Temp3, #5 ; Reset number of pulses to measure + ajmp measure_pwm_freq_loop +; mov Temp3, #1 +measure_pwm: + call wait3ms ; Wait for next pulse (NB: Uses Temp1/2!) + mov A, New_Rcp ; Load value + clr C + subb A, #RCP_VALIDATE ; Higher than validate level? + jc measure_pwm_freq_start ; No - start over + + mov A, Flags3 ; Check pwm frequency flags +; anl A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) + anl A, #(1 SHL RCP_PWM_FREQ_1KHZ) + mov Prev_Rcp_Pwm_Freq, Curr_Rcp_Pwm_Freq ; Store as previous flags for next pulse + mov Curr_Rcp_Pwm_Freq, A ; Store current flags for next pulse + cjne A, Prev_Rcp_Pwm_Freq, measure_pwm_freq_start ; Go back if new flags not same as previous + + djnz Temp3, measure_pwm_freq_loop ; Go back if not required number of pulses seen + + ; Clear measure pwm frequency flag + clr Flags0.RCP_MEAS_PWM_FREQ + ; Set up RC pulse interrupts after pwm frequency measurement + Rcp_Int_First ; Enable interrupt and set to first edge + Rcp_Clear_Int_Flag ; Clear interrupt flag + clr Flags2.RCP_EDGE_NO ; Set first edge flag + call wait100ms ; Wait for new RC pulse + + ; Validate RC pulse +;Pgm_Main_Rearm_Start = 第1项 enabled +validate_rcp_start: + call wait3ms ; Wait for next pulse (NB: Uses Temp1/2!) + mov Temp1, #RCP_VALIDATE ; Set validate level as default +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) + mov A, #(1 SHL RCP_PWM_FREQ_1KHZ) + anl A, Flags3 ; Check pwm frequency flags + jnz ($+4) ; If a flag is set (PWM) - branch + + mov Temp1, #0 ; Set level to zero for PPM (any level will be accepted) + + clr C + mov A, New_Rcp ; Load value + subb A, Temp1 ; Higher than validate level? + jc validate_rcp_start ; No - start over + + ; Beep arm sequence start signal +;上电检测到信号后叫第1声（信号准备就绪） +;////// clr EA ; Disable all interrupts +;////// call beep_f1 ; Signal that RC pulse is ready +;////// call beep_f1 +;////// call beep_f1 +;////// setb EA ; Enable all interrupts + call wait200ms + + ; Arming sequence start + mov Gov_Arm_Target, #0 ; Clear governor arm target +arming_start: + call wait3ms +;读取Pgm_Enable_TX_Program + mov Temp1, #Pgm_Enable_TX_Program; Yes - start programming mode entry if enabled + mov A, @Temp1 + clr C + subb A, #1 ; Is TX programming enabled? + jnc arming_initial_arm_check ; Yes - proceed + + jmp program_by_tx_checked ; No - branch +;Pgm_Enable_TX_Program = 1 enabled +arming_initial_arm_check: + mov A, Initial_Arm ; Yes - check if it is initial arm sequence + clr C + subb A, #1 ; Is it the initial arm sequence? + jnc arming_ppm_check ; Yes - proceed + + jmp wait_for_power_on ; No - branch + +arming_ppm_check: + mov Temp1, #Pgm_Ppm_Min_Throttle ; Store + mov A, @Temp1 + mov Min_Throttle, A +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) + mov A, #(1 SHL RCP_PWM_FREQ_1KHZ) + anl A, Flags3 ; Check pwm frequency flags + jz throttle_high_cal_start ; If no flag is set (PPM) - branch + + ; PWM tx program entry +; clr C +; mov A, New_Rcp ; Load new RC pulse value +; subb A, #200 ; Is RC pulse max? +; jc program_by_tx_entry_wait_pwm ; Yes - proceed +; subb A, #RCP_MAX ; Is RC pulse max? +; jnc program_by_tx_entry_pwm ; Yes - proceed + +; jmp program_by_tx_checked ; No - branch + +;program_by_tx_entry_pwm: +; clr EA ; Disable all interrupts +; call beep_f4 +; setb EA ; Enable all interrupts +; call wait100ms +; clr C +; mov A, New_Rcp ; Load new RC pulse value +; subb A, #RCP_STOP ; Below stop? +; subb A, #200 +; jnc program_by_tx_entry_pwm ; No - start over + +;program_by_tx_entry_wait_pwm: + + clr EA + call beep_f2 + call beep_f3 + call beep_f1 + + mov TMOD, #11h ;T0/T1 工作方式1 + mov CKCON, #02h ;T0/T1 48分频 + mov P0MDIN, #80h + mov P0SKIP, #0FFh + call program_by_card ; Yes - enter programming mode +; ajmp program_by_card_entry + +;program_by_tx_entry_wait_pwm: +; clr EA ; Disable all interrupts +; call beep_f1 +; call wait10ms +; call beep_f1 +; setb EA ; Enable all interrupts +; call wait100ms +; clr C +; mov A, New_Rcp ; Load new RC pulse value +; subb A, #RCP_MAX ; At or above max? +; jc program_by_tx_entry_wait_pwm ; No - start over + +; call program_by_tx ; Yes - enter programming mode +; jmp program_by_tx_checked + + ; PPM throttle calibration and tx program entry +;计算PPM最大，最小油门并存储 +throttle_high_cal_start: + mov Temp8, #5 ; Set 3 seconds wait time +throttle_high_cal: + setb Flags3.FULL_THROTTLE_RANGE ; Set range to 1000-2020us + call Find_Throttle_Gain ; Set throttle gain + call wait100ms ; Wait for new throttle value + clr EA ; Disable interrupts (freeze New_Rcp value) + clr Flags3.FULL_THROTTLE_RANGE ; Set programmed range + call Find_Throttle_Gain ; Set throttle gain + clr C + mov A, New_Rcp ; Load new RC pulse value + subb A, #220 ; Is RC pulse above midstick? 1840 + setb EA ; Enable interrupts + jc ($+6) + djnz Temp8, throttle_high_cal + ajmp throttle_high_cal_save + + clr C + mov A, New_Rcp ; Load new RC pulse value + subb A, Min_Throttle ; Is RC pulse above midstick? 1148 + jnc ($+4) + ajmp arm_target_updated ; No - branch 检测低油门 + + clr EA + call beep_f1; + call wait100ms + setb EA + ajmp throttle_high_cal_start + +throttle_high_cal_save: +; call wait1s + clr C + mov A, New_Rcp ; Limit to max 250 + subb A, #5 ; Subtract about 2% and ensure that it is 250 or lower + mov Temp1, #Pgm_Ppm_Max_Throttle ; Store + mov @Temp1, A + call set_commu_sum_to_zero ;////////////////////////////////////************************** + call erase_and_store_all_in_eeprom + clr EA + call beep_f1;/////////////////////////////////////////成功存储最大油门叫声 + call beep_f1;/////////////////////////////////////////成功存储最大油门叫声 + call wait30ms;/////////////////////////////////////////成功存储最大油门叫声 + call beep_f4;/////////////////////////////////////////成功存储最大油门叫声 + call beep_f4;/////////////////////////////////////////成功存储最大油门叫声 + setb EA + + call wait1s + call wait1s +wait_loop_1: + clr C + mov A, New_Rcp ; Load new RC pulse value + subb A, #215 ;Is RC pulse above midstick? + jnc program_by_tx_entry_wait_ppm + clr C + mov A, New_Rcp ; Load new RC pulse value + subb A, #60 ;Is RC pulse above midstick? + jnc wait_loop_1 + ajmp throttle_low_cal_start + + ;PPM编程模式 +program_by_tx_entry_wait_ppm: + call program_by_tx ; Yes - enter programming mode 成功进入编程模式 + +;最小油门设置及退出 +throttle_low_cal_start: + mov Temp8, #5 ; Set 3 seconds wait time +throttle_low_cal: +; call wait1s + setb Flags3.FULL_THROTTLE_RANGE ; Set range to 1000-2020us + call Find_Throttle_Gain ; Set throttle gain + call wait100ms + clr EA ; Disable interrupts (freeze New_Rcp value) + clr Flags3.FULL_THROTTLE_RANGE ; Set programmed range + call Find_Throttle_Gain ; Set throttle gain + clr C + mov A, New_Rcp ; Load new RC pulse value + subb A, #60 ; Below midstick? + setb EA ; Enable interrupts + jnc throttle_low_cal_start ; No - start over 在中点油门以上，则继续等待，直到检测到在中点油门以下 + djnz Temp8, throttle_low_cal ; Yes - continue to wait +; call wait3ms + mov A, New_Rcp + add A, #5 ; Add about 2% + mov Temp1, #Pgm_Ppm_Min_Throttle ; Store + mov @Temp1, A + call set_commu_sum_to_zero ;////////////////////////////////////************************** + call erase_and_store_all_in_eeprom + +; clr EA ; Disable all interrupts +; mov RSTSRC, #12h ; Generate hardware reset 强制复位系统 + + ; Set default programmed parameters +; call set_default_parameters + ; Read all programmed parameters + call read_all_eeprom_parameters + ; Decode parameters + call decode_parameters + ; Decode governor gains + call decode_governor_gains + ; Decode throttle rate + call decode_throttle_rate + ; Decode startup power + call decode_startup_power + ; Decode demag compensation + call decode_demag_comp + ; Set BEC voltage + call set_bec_voltage + ; Find throttle gain from stored min and max settings + call find_throttle_gain + +program_by_tx_checked: + clr C + mov A, New_Rcp ; Load new RC pulse value + subb A, Gov_Arm_Target ; Is RC pulse larger than arm target? + jc arm_target_updated ; No - do not update + + mov Gov_Arm_Target, New_Rcp ; Yes - update arm target + +arm_target_updated: + call wait100ms ; Wait for new throttle value + clr C + mov A, New_Rcp ; Load new RC pulse value + subb A, #RCP_STOP ; Below stop? + jc arm_end_beep ; Yes - proceed ;最小油门计算好 及 编程结束叫声 + jmp arm_target_updated ; No - start over + +;上电检测到信号后叫第2声（信号检测结束） +arm_end_beep: +;改变上电叫方式 + clr EA ; Disable all interrupts + call beep_f1 + call beep_f1 + call beep_f1 + call wait1s + ; Beep arm sequence end signal + +;************************************************* +; 增加Lipo节数叫声 2013.5.29 +;************************************************* + mov Temp8,Lipo_Cell_Count ; +lipo_cell_beep: + call beep_f1 + call beep_f1 + call wait30ms + djnz Temp8,lipo_cell_beep + call wait200ms + call wait200ms +;///// call beep_f4 ; Signal that rcpulse is ready +;///// call beep_f4 +;///// call beep_f4 +;改变信号检测OK叫声方式 2013.5.29 + call beep_f1 + call wait30ms + call wait30ms + call beep_f2 + call wait30ms + call wait30ms + call beep_f3 + call wait30ms + call wait30ms + setb EA ; Enable all interrupts + call wait100ms + + ; Measure number of lipo cells + call Measure_Lipo_Cells ; Measure number of lipo cells + + ; Clear initial arm variable + mov Initial_Arm, #0 + + ; Armed and waiting for power on +wait_for_power_on: + clr LOW_LIMIT_STOP ; 清低压保护控制方式2 停止标志 +;////// clr A +;////// mov Power_On_Wait_Cnt_L, A ; Clear wait counter +;////// mov Power_On_Wait_Cnt_H, A +wait_for_power_on_loop: + mov Temp8, #50 +;////// inc Power_On_Wait_Cnt_L ; Increment low wait counter +;////// mov A, Power_On_Wait_Cnt_L +;////// cpl A +;////// jnz wait_for_power_on_no_beep; Counter wrapping (about 1 sec)? + +;////// inc Power_On_Wait_Cnt_H ; Increment high wait counter +;******************************************************** +; 读取油门最低未操作等待叫声时间 +;******************************************************** +;读取Pgm_Beacon_Delay +;////// mov Temp1, #Pgm_Beacon_Delay +;////// mov A, @Temp1 +;////// mov Temp1, #40 ; Approximately 30 sec +;////// dec A +;////// jz beep_delay_set + +;////// mov Temp1, #80 ; Approximately 1 min +;////// dec A +;////// jz beep_delay_set + +;////// mov Temp1, #160 ; Approximately 2 min +;////// dec A +;////// jz beep_delay_set + +;////// mov Temp1, #240 ; Approximately 3 min +;////// dec A +;////// jz beep_delay_set + +;////// mov Power_On_Wait_Cnt_H, #0 ; Reset counter for infinite delay + +;//////beep_delay_set: +;////// clr C +;////// mov A, Power_On_Wait_Cnt_H +;////// subb A, Temp1 ; Check against chosen delay +;////// jc wait_for_power_on_no_beep; Has delay elapsed? +;******************************************************** +; 读取油门最低未操作等待叫声声强设置 +;******************************************************** +;////// dec Power_On_Wait_Cnt_H ; Decrement high wait counter +;读取Pgm_Beacon_Strength +;////// mov Temp1, #Pgm_Beacon_Strength +;////// mov Beep_Strength, @Temp1 +;////// clr EA ; Disable all interrupts +;////// All_pFETs_Off;///////////////////////////////////////***************************** +;////// call beep_f4 ; Signal that there is no signal +;////// setb EA ; Enable all interrupts +;读取Pgm_Beep_Strength +;////// mov Temp1, #Pgm_Beep_Strength +;////// mov Beep_Strength, @Temp1 +;////// call wait100ms ; Wait for new RC pulse to be measured + +wait_for_power_on_no_beep: + call wait3ms + mov A, Rcp_Timeout_Cnt ; Load RC pulse timeout counter value + jnz wait_for_power_on_ppm_not_missing ; If it is not zero - proceed + +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) +; anl A, Flags3 ; Check pwm frequency flags +; jnz wait_for_power_on_ppm_not_missing ; If a flag is set (PWM) - branch + + jmp measure_pwm_freq_init ; If ppm and pulses missing - go back to measure pwm frequency + +wait_for_power_on_ppm_not_missing: + clr C + mov A, New_Rcp ; Load new RC pulse value +; subb A, #RCP_STOP ; Higher than stop (plus some hysteresis)? + subb A, #(RCP_STOP+7) ; Higher than stop (plus some hysteresis)? + jc wait_for_power_on ; No - start over + djnz Temp8, wait_for_power_on_no_beep + jb LOW_LIMIT_STOP, wait_for_power_on_loop + call wait100ms ; Wait to see if start pulse was only a glitch + mov A, Rcp_Timeout_Cnt ; Load RC pulse timeout counter value + jnz ($+5) ; If it is not zero - proceed + + ljmp measure_pwm_freq_init ; If it is zero (pulses missing) - go back to measure pwm frequency + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Start entry point +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +init_start: + setb ET0 + call switch_power_off + clr A + mov Requested_Pwm, A ; Set requested pwm to zero + mov Governor_Req_Pwm, A ; Set governor requested pwm to zero + mov Current_Pwm, A ; Set current pwm to zero + mov Current_Pwm_Limited, A ; Set limited current pwm to zero + mov Pwm_Limit, #0FFh ; Set pwm limit to max + mov Temp1, #Pgm_Motor_Idle + mov Pwm_Motor_Idle, @Temp1 ; Set idle pwm to programmed value + mov Gov_Target_L, A ; Set target to zero + mov Gov_Target_H, A + mov Gov_Integral_L, A ; Set integral to zero + mov Gov_Integral_H, A + mov Gov_Integral_X, A + mov Adc_Conversion_Cnt, A + mov Gov_Active, A + mov Flags0, A ; Clear flags0 + mov Flags1, A ; Clear flags1 + mov Rcp_Stop_Cnt, A ; Set RC pulse stop count to zero + mov Rcp_Stop_Limit, A + mov Lipo_Cell_Count, A + mov Limit_Count, A + mov New_Rcp, #Rcp_MIN + call initialize_all_timings ; Initialize timing + ;**** **** **** **** **** + ; Settle phase beginning + ;**** **** **** **** **** + mov Adc_Conversion_Cnt, #TEMP_CHECK_RATE ; Make sure a temp reading is done + Set_Adc_Ip_Temp + call wait1ms + call start_adc_conversion +read_initial_temp: + Get_Adc_Status + jb AD0BUSY, read_initial_temp + Read_Adc_Result ; Read initial temperature + mov A, Temp2 + jnz ($+3) ; Is reading below 256? + + mov Temp1, A ; Yes - set average temperature value to zero + + mov Current_Average_Temp, Temp1 ; Set initial average temperature + call check_temp_voltage_and_limit_power + mov Adc_Conversion_Cnt, #TEMP_CHECK_RATE ; Make sure a temp reading is done next time + Set_Adc_Ip_Temp + + ; Go to the desired startup mode +; mov Temp1, #Pgm_Startup_Method +; mov A, @Temp1 +; jnb ACC.0, direct_method_start + +; jmp stepper_method_start + +direct_method_start: + ; Set up start operating conditions + mov Temp1, #Pgm_Pwm_Freq + mov A, @Temp1 + mov Temp7, A ; Store setting in Temp7 + mov @Temp1, #1 ; Set nondamped low frequency pwm mode + call decode_parameters ; (Decode_parameters uses Temp1 and Temp8) + mov Temp1, #Pgm_Pwm_Freq + mov A, Temp7 + mov @Temp1, A ; Restore settings + ; Set max allowed power + setb Flags1.SETTLE_PHASE ; Set settle phase power as max + clr EA ; Disable interrupts to avoid that Requested_Pwm is overwritten + call set_startup_pwm + mov Pwm_Limit, Requested_Pwm + mov Pwm_Limit_Spoolup, Requested_Pwm + setb EA + clr Flags1.SETTLE_PHASE + mov Current_Pwm_Limited, #0 ; Set low pwm again after calling set_startup_pwm + mov Spoolup_Limit_Cnt, #0 + mov Spoolup_Limit_Skip, #1 + ; Begin startup sequence + setb Flags1.MOTOR_SPINNING ; Set motor spinning flag + setb Flags1.DIRECT_STARTUP_PHASE ; Set direct startup phase flag + mov Direct_Startup_Ok_Cnt, #0 ; Reset ok counter + call comm5comm6 ; Initialize commutation + call comm6comm1 + call calc_next_comm_timing ; Set virtual commutation point + call initialize_all_timings ; Initialize timing + jmp run1 + + +;stepper_method_start: +; ; Set up start operating conditions +; mov Temp1, #Pgm_Pwm_Freq +; mov A, @Temp1 +; mov Temp7, A ; Store setting in Temp7 +; mov @Temp1, #3 ; Set damped light mode +; mov Temp1, #Pgm_Damping_Force +; mov A, @Temp1 +; mov Temp6, A ; Store setting in Temp6 +; mov @Temp1, #5 ; Set high damping force +; call decode_parameters ; (Decode_parameters uses Temp1 and Temp8) +; mov Temp1, #Pgm_Pwm_Freq +; mov A, Temp7 +; mov @Temp1, A ; Restore settings +; mov Temp1, #Pgm_Damping_Force +; mov A, Temp6 +; mov @Temp1, A + ; Begin startup sequence +; setb Flags1.MOTOR_SPINNING ; Set motor spinning flag +; setb Flags1.SETTLE_PHASE ; Set motor start settling phase flag +; setb Flags2.CURR_PWMOFF_DAMPED; Set damped status, in order to ensure that pfets will be turned off in an initial pwm on +; call comm5comm6 ; Initialize commutation +; call comm6comm1 +; call set_startup_pwm +; call wait1ms +; call comm1comm2 +; call wait1ms +; call wait1ms +; call comm2comm3 +; call wait3ms +; call comm3comm4 +; call wait3ms +; call wait3ms +; call comm4comm5 +; call wait10ms ; Settle rotor +; call comm5comm6 +; call wait3ms +; call wait1ms +; clr Flags1.SETTLE_PHASE ; Clear settling phase flag +; setb Flags1.STEPPER_PHASE ; Set motor start stepper phase flag + +; ;**** **** **** **** **** +; ; Stepper phase beginning +; ;**** **** **** **** **** +;stepper_rot_beg: +; call start_adc_conversion +; call check_temp_voltage_and_limit_power +; call set_startup_pwm +; mov Adc_Conversion_Cnt, #TEMP_CHECK_RATE ; Make sure a temp reading is done next time +; Set_Adc_Ip_Temp + +; call comm6comm1 ; Commutate +; call calc_next_comm_timing_start ; Update timing and set timer +; call calc_new_wait_times +; call decrement_stepper_step +; call stepper_timer_wait + +; call comm1comm2 +; call calc_next_comm_timing_start +; call calc_new_wait_times +; call decrement_stepper_step +; call stepper_timer_wait + +; call comm2comm3 +; call calc_next_comm_timing_start +; call calc_new_wait_times +; call decrement_stepper_step +; call stepper_timer_wait + +; call comm3comm4 +; call calc_next_comm_timing_start +; call calc_new_wait_times +; call decrement_stepper_step +; call stepper_timer_wait + +; call comm4comm5 +; call calc_next_comm_timing_start +; call calc_new_wait_times +; call decrement_stepper_step +; call stepper_timer_wait + +; call comm5comm6 +; call calc_next_comm_timing_start +; call calc_new_wait_times +; call decrement_stepper_step +; ; Check stepper step versus end criteria +; clr C +; mov A, Wt_Stepper_Step_L +; subb A, Stepper_Step_End_L ; Minimum Stepper_Step_End +; mov A, Wt_Stepper_Step_H +; subb A, Stepper_Step_End_H +; jc stepper_rot_exit ; Branch if lower than minimum + +; ; Wait for step +; call stepper_timer_wait +; clr C +; mov A, New_Rcp ; Load new pulse value +; subb A, #RCP_STOP ; Check if pulse is below stop value +; jnc stepper_rot_beg + +; jmp run_to_wait_for_power_on + +;stepper_rot_exit: +; ; Wait for step +; call stepper_timer_wait +; ; Clear stepper phase +; clr Flags1.STEPPER_PHASE ; Clear motor start stepper phase flag +; ; Set dondamped low pwm frequency +; mov Temp1, #Pgm_Pwm_Freq +; mov A, @Temp1 +; mov Temp7, A ; Store setting in Temp7 +; mov @Temp1, #2 ; Set nondamped low frequency pwm mode +; call decode_parameters ; (Decode_parameters uses Temp1 and Temp8) +; mov Temp1, #Pgm_Pwm_Freq +; mov A, Temp7 +; mov @Temp1, A ; Restore settings +; ; Set spoolup power variables (power is now controlled from RCP) +; mov Pwm_Limit, Requested_Pwm +; mov Pwm_Limit_Spoolup, Pwm_Spoolup_Beg +; mov Current_Pwm_Limited, Pwm_Spoolup_Beg +; mov Spoolup_Limit_Cnt, #0 +; mov Spoolup_Limit_Skip, #1 +; ; Set direct startup phase to aquire sync quickly +; setb Flags1.DIRECT_STARTUP_PHASE ; Set direct startup phase flag +; mov Direct_Startup_Ok_Cnt, #0 ; Reset ok counter +; clr EA ; Disable interrupts +; ApFET_off ; Ap off. Turn off unused pfets (to turn off damping fets that might be on) +; CpFET_off ; Cp off +; mov A, #45 ; 8us delay for pfets to go off +; djnz ACC, $ +; setb EA ; Enable interrupts +; call comm6comm1 +; call calc_next_comm_timing ; Calculate next timing and start advance timing wait +; call wait_advance_timing ; Wait advance timing and start zero cross wait +; call calc_new_wait_times +; call wait_before_zc_scan ; Wait zero cross wait and start zero cross timeout +; mov Adc_Conversion_Cnt, #0 ; Make sure a voltage reading is done next time +; Set_Adc_Ip_Volt ; Set adc measurement to voltage +; jmp run1 + + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Run entry point +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +damped_transition: + ; Transition from nondamped to damped if applicable + call decode_parameters ; Set programmed parameters + call comm6comm1 + mov Adc_Conversion_Cnt, #0 ; Make sure a voltage reading is done next time + Set_Adc_Ip_Volt ; Set adc measurement to voltage + +; Run 1 = B(p-on) + C(n-pwm) - comparator A evaluated +; Out_cA changes from high to low +run1: + call wait_for_comp_out_high ; Wait zero cross wait and wait for high + call evaluate_comparator_integrity ; Check whether comparator reading has been normal + call setup_comm_wait ; Setup wait time from zero cross to commutation + call calc_governor_target ; Calculate governor target + call wait_for_comm ; Wait from zero cross to commutation + call comm1comm2 ; Commutate + call calc_next_comm_timing ; Calculate next timing and start advance timing wait + call wait_advance_timing ; Wait advance timing and start zero cross wait + call calc_new_wait_times + call wait_before_zc_scan ; Wait zero cross wait and start zero cross timeout + +; Run 2 = A(p-on) + C(n-pwm) - comparator B evaluated +; Out_cB changes from low to high +run2: + call wait_for_comp_out_low + call evaluate_comparator_integrity + call setup_comm_wait + call calc_governor_prop_error + call wait_for_comm + call comm2comm3 + call calc_next_comm_timing + call wait_advance_timing + call calc_new_wait_times + call wait_before_zc_scan + +; Run 3 = A(p-on) + B(n-pwm) - comparator C evaluated +; Out_cC changes from high to low +run3: + call wait_for_comp_out_high + call evaluate_comparator_integrity + call setup_comm_wait + call calc_governor_int_error + call wait_for_comm + call comm3comm4 + call calc_next_comm_timing + call wait_advance_timing + call calc_new_wait_times + call wait_before_zc_scan + +; Run 4 = C(p-on) + B(n-pwm) - comparator A evaluated +; Out_cA changes from low to high +run4: + call wait_for_comp_out_low + call evaluate_comparator_integrity + call setup_comm_wait + call calc_governor_prop_correction + call wait_for_comm + call comm4comm5 + call calc_next_comm_timing + call wait_advance_timing + call calc_new_wait_times + call wait_before_zc_scan + +; Run 5 = C(p-on) + A(n-pwm) - comparator B evaluated +; Out_cB changes from high to low +run5: + call wait_for_comp_out_high + call evaluate_comparator_integrity + call setup_comm_wait + call calc_governor_int_correction + call wait_for_comm + call comm5comm6 + call calc_next_comm_timing + call wait_advance_timing + call calc_new_wait_times + call wait_before_zc_scan + +; Run 6 = B(p-on) + A(n-pwm) - comparator C evaluated +; Out_cC changes from low to high +run6: + call wait_for_comp_out_low + call start_adc_conversion + call evaluate_comparator_integrity + call setup_comm_wait + call check_temp_voltage_and_limit_power + call wait_for_comm + call comm6comm1 + call calc_next_comm_timing + call wait_advance_timing + call calc_new_wait_times + call wait_before_zc_scan + + ; Check if it is direct startup + jnb Flags1.DIRECT_STARTUP_PHASE, normal_run_checks + + ; Set spoolup power variables +IF MODE == 0 OR MODE == 2 + mov Pwm_Limit, Pwm_Spoolup_Beg ; Set initial max power + mov Pwm_Limit_Spoolup, Pwm_Spoolup_Beg ; Set initial slow spoolup power +ENDIF +IF MODE == 1 + mov Pwm_Limit, #0FFh ; Allow full power + mov Pwm_Limit_Spoolup, #0FFh +ENDIF + mov Spoolup_Limit_Cnt, #0 + mov Spoolup_Limit_Skip, #1 + ; Check startup ok counter + clr C + mov A, Direct_Startup_Ok_Cnt ; Load ok counter + subb A, #100 ; Is counter above requirement? + jc direct_start_check_rcp ; No - proceed + + clr Flags1.DIRECT_STARTUP_PHASE ; Clear direct startup phase flag + setb Flags1.INITIAL_RUN_PHASE ; Set initial run phase flag + mov Startup_Rot_Cnt, #20 ; Set startup rotation count + jmp normal_run_checks + +direct_start_check_rcp: + clr C + mov A, New_Rcp ; Load new pulse value + subb A, #RCP_STOP ; Check if pulse is below stop value + jc ($+5) + + ljmp run1 ; Continue to run + + jmp run_to_wait_for_power_on + + +normal_run_checks: + ; Check if it is initial run phase + jnb Flags1.INITIAL_RUN_PHASE, initial_run_phase_done; If not initial run phase - branch + + ; Decrement startup rotaton count + mov A, Startup_Rot_Cnt + dec A + ; Check number of nondamped rotations + jnz normal_run_check_startup_rot ; Branch if counter is not zero + + clr Flags1.INITIAL_RUN_PHASE ; Clear initial run phase flag + ljmp damped_transition ; Do damped transition if counter is zero + +normal_run_check_startup_rot: + mov Startup_Rot_Cnt, A ; Not zero - store counter + + clr C + mov A, New_Rcp ; Load new pulse value + subb A, #RCP_STOP ; Check if pulse is below stop value + jc ($+5) + + ljmp run1 ; Continue to run + + jmp run_to_wait_for_power_on + + +initial_run_phase_done: + ; Exit run loop after a given time + clr C + mov A, Rcp_Stop_Cnt ; Load stop RC pulse counter value +;///// subb A, #RCP_STOP_LIMIT ; Is number of stop RC pulses above limit? + subb A, Rcp_Stop_Limit ; Is number of stop RC pulses above limit? + jnc run_to_wait_for_power_on ; Yes, go back to wait for poweron + +run6_check_rcp_timeout: +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) +; anl A, Flags3 ; Check pwm frequency flags +; jnz run6_check_speed ; If a flag is set (PWM) - branch + + mov A, Rcp_Timeout_Cnt ; Load RC pulse timeout counter value + jz run_to_wait_for_power_on ; If it is zero - go back to wait for poweron + +run6_check_speed: + clr C + mov A, Comm_Period4x_H ; Is Comm_Period4x more than 32ms (~1220 eRPM)? + subb A, #0F0h + jnc run_to_wait_for_power_on ; Yes - go back to motor start + jmp run1 ; Go back to run 1 + + +run_to_wait_for_power_on: + + call switch_power_off + mov Temp1, #Pgm_Pwm_Freq + mov A, @Temp1 + mov Temp7, A ; Store setting in Temp7 + mov @Temp1, #1 ; Set low pwm mode (in order to turn off damping) + call decode_parameters ; (Decode_parameters uses Temp1 and Temp8) + mov Temp1, #Pgm_Pwm_Freq + mov A, Temp7 + mov @Temp1, A ; Restore settings + clr A + mov Requested_Pwm, A ; Set requested pwm to zero + mov Governor_Req_Pwm, A ; Set governor requested pwm to zero + mov Current_Pwm, A ; Set current pwm to zero + mov Current_Pwm_Limited, A ; Set limited current pwm to zero + mov Pwm_Motor_Idle, A ; Set motor idle to zero + clr Flags1.MOTOR_SPINNING ; Clear motor spinning flag + call wait1ms ; Wait for pwm to be stopped + call switch_power_off + call wait10ms + clr ET0 + jb Flags2.PGM_PWMOFF_DAMPED, light_damp + ajmp damp_exit + +light_damp: + jb Flags2.PGM_PWMOFF_DAMPED_LIGHT, ($+7) + setb P1.BnFET + ajmp damp_exit + jb Flags2.PGM_PWMOFF_DAMPED_FULL, high_damp + A_B_nFETs_On + ajmp damp_exit +high_damp: + All_nFETs_On +damp_exit: + +IF MODE == 0 ; Main + mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) + anl A, Flags3 ; Check pwm frequency flags + jnz run_to_next_state_main ; If a flag is set (PWM) - branch + + mov A, Rcp_Timeout_Cnt ; Load RC pulse timeout counter value + jnz run_to_next_state_main ; If it is not zero - branch + + jmp measure_pwm_freq_init ; If it is zero (pulses missing) - go back to measure pwm frequency + +run_to_next_state_main: + mov Temp1, #Pgm_Startup_Method ; Check if it is stepped startup + mov A, @Temp1 + jnb ACC.0, run_to_next_state_main_wait_done ; If direct startup - jump + + call wait1s ; 3 second delay before new startup (in stepped mode) + call wait1s + call wait1s +run_to_next_state_main_wait_done: +;读取Pgm_Main_Rearm_Start + mov Temp1, #Pgm_Main_Rearm_Start + mov A, @Temp1 + clr C + subb A, #1 ; Is re-armed start enabled? + jc jmp_wait_for_power_on ; No - do like tail and start immediately + + jmp validate_rcp_start ; Yes - go back to validate RC pulse +;Pgm_Main_Rearm_Start = 第2项 disabled +jmp_wait_for_power_on: + jmp wait_for_power_on ; Go back to wait for power on +ENDIF +IF MODE >= 1 ; Tail or multi +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) +; anl A, Flags3 ; Check pwm frequency flags +; jnz jmp_wait_for_power_on ; If a flag is set (PWM) - branch + + mov A, Rcp_Timeout_Cnt ; Load RC pulse timeout counter value + jnz jmp_wait_for_power_on ; If it is not zero - go back to wait for poweron + + jmp measure_pwm_freq_init ; If it is zero (pulses missing) - go back to measure pwm frequency + +jmp_wait_for_power_on: + jmp wait_for_power_on_loop ; Go back to wait for power on +ENDIF + +;**** **** **** **** **** **** **** **** **** **** **** **** **** + +$include (BLHeliTxPgm.inc) ; Include source code for programming the ESC with the TX + +;**** **** **** **** **** **** **** **** **** **** **** **** **** + + + + +END$NOMOD51 +;**** **** **** **** **** +; +; BLHeli program for controlling brushless motors in helicopters +; +; Copyright 2011, 2012 Steffen Skaug +; This program is distributed under the terms of the GNU General Public License +; +; This file is part of BLHeli. +; +; BLHeli is free software: you can redistribute it and/or modify +; it under the terms of the GNU General Public License as published by +; the Free Software Foundation, either version 3 of the License, or +; (at your option) any later version. +; +; BLHeli is distributed in the hope that it will be useful, +; but WITHOUT ANY WARRANTY; without even the implied warranty of +; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the +; GNU General Public License for more details. +; +; You should have received a copy of the GNU General Public License +; along with BLHeli. If not, see . +; +;**** **** **** **** **** +; +; The software was initially designed for use with Eflite mCP X, but is now adapted to a copters/planes in general +; +; The software was inspired by and started from from Bernard Konze's BLMC: http://home.versanet.de/~bkonze/blc_6a/blc_6a.htm +; And also Simon Kirby's TGY: https://github.com/sim-/tgy +; +; This file is best viewed with tab width set to 5 +; +; The input signal can be positive 1kHz, 2kHz, 4kHz or 8kHz PWM (taken from the "resistor tap" on mCPx) +; And the input signal can be PPM (1-2ms) at rates up to several hundred Hz. +; The code adapts itself to the various input modes/frequencies +; The code ESC can also be programmed to accept inverted input signal. +; +; The first lines of the software must be modified according to the chosen environment: +; Uncomment the selected ESC and main/tail/multi mode +; BESC EQU "ESC"_"mode" +; +;**** **** **** **** **** +; Revision history: +; - Rev1.0: Initial revision based upon BLHeli for AVR controllers +; - Rev2.0: Changed "Eeprom" initialization, layout and defaults +; Various changes and improvements to comparator reading. Now using timer1 for time from pwm on/off +; Beeps are made louder +; Added programmable low voltage limit +; Added programmable damped tail mode (only for 1S ESCs) +; Added programmable motor rotation direction +; - Rev2.1: (minor changes by 4712) +; Added Disable TX Programming by PC Setup Application +; therfore changed EEPROM_LAYOUT_REVISION = 8 +; Added Vdd Monitor as reset source when writing to "EEProm" +; Changed for use of batch file to assemble, link and make hex files +; - Rev2.2: (minor changes by 4712) +; Added Disable Throttle Re-Arming every motor start by PC Setup Application +; - Rev2.3: (minor changes by 4712) +; Added bugfixed (2x CLR C before j(n)c operations)thx Steffen! +; - Rev2.4: Revisions 2.1 to 2.3 integrated +; - Rev3.0: Added PPM (1050us-1866us) as accepted input signal +; Added startup rpm as a programming parameter +; Added startup acceleration as a programming parameter +; Added option for using voltage measurements to compensate motor power +; Added governor target by setup as a governor mode option +; Governor is kept active regardless of rpm +; Smooth governor spoolup/down in arm and setup modes +; Increased governor P and I gain programming ranges +; Increased and changed low voltage limit programming range +; Disabled tx programming entry for all but the first arming sequence after power on +; Made it possible to skip parameters in tx programming by setting throttle midstick +; Made it default not to rearm for every restart +; - Rev3.1: Fixed bug that prevented chosen parameter to be set in tx programming +; - Rev3.2: ...also updated the EEPROM revision parameter +; - Rev3.3: Fixed negative number bug in voltage compensation +; Fixed bug in startup power calculation for non-default power +; Prevented possibility for voltage compensation fighting low voltage limiting +; Applied overall spoolup control to ensure soft spoolup in any mode +; Added a delay of 3 seconds from initiation of main motor stop until new startup is allowed +; Reduced beep power to reduce power consumption for very strong motors/ESCs +; - Rev3.4: Fixed bug that prevented full power in governor arm and setup modes +; Increased NFETON_DELAY for XP_7A and XP_12A to allow for more powerful fets +; Increased initial spoolup power, and linked to startup power +; - Rev4.0: Fixed bug that made tail tx program beeps very weak +; Added thermal protection feature +; Governor P and I gain ranges are extended up to 8.0x gain +; Startup sequence is aborted upon zero throttle +; Avoided voltage compensation function induced latency for tail when voltage compensation is not enabled +; Improved input signal frequency detection robustness +; - Rev4.1: Increased thermal protection temperature limits +; - Rev5.0: Added multi(copter) operating mode. TAIL define changed to MODE with three modes: MAIN, TAIL and MULTI +; Added programmable commutation timing +; Added a damped light mode that has less damping, but that can be used with all escs +; Added programmable damping force +; Added thermal protection for startup too +; Added wait beeps when waiting more than 30 sec for throttle above zero (after having been armed) +; Modified tail idling to provide option for very low speeds +; Changed PPM range to 1150-1830us +; Arming sequence is dropped for PPM input, unless it is governor arm mode +; Loss of input signal will immediately stop the motor for PPM input +; Bug corrected in Turnigy Plush 6A voltage measurement setup +; FET switching delays are set for original fets. Stronger/doubled/tripled etc fets may require faster pfet off switching +; Miscellaneous other changes +; - Rev6.0: Reverted comparator reading routine to rev5.0 equivalent, in order to avoid tail motor stops +; Added governor range programmability +; Implemented startup retry sequence with varying startup power for multi mode +; In damped light mode, damping is now applied to the active nfet phase for fully damped capable ESCs +; - Rev6.1: Added input signal qualification criteria for PPM, to avoid triggering on noise spikes (fix for plush hardware) +; Changed main and multi mode stop criteria. Will now be in run mode, even if RC pulse input is zero +; Fixed bug in commutation that caused rough running in damped light mode +; Miscellaneous other changes +; - Rev7.0 Added direct startup mode programmability +; Added throttle calibration. Min>=1000us and Max<=2000us. Difference must be >520us, otherwise max is shifted so that difference=520us +; Added programmable throttle change rate +; Added programmable beep strength, beacon strength and beacon delay +; Reduced power step to full power significantly +; Miscellaneous other changes +; - Rev8.0 Added a 2 second delay after power up, to wait for receiver initialization +; Added a programming option for disabling low voltage limit, and made it default for MULTI +; Added programable demag compensation, using the concept of SimonK +; Improved robustness against noisy input signal +; Refined direct startup +; Removed voltage compensation +; Miscellaneous other changes +; - Rev9.0 Increased programming range for startup power, and made it's default ESC dependent +; Made default startup method ESC dependent +; Even more smooth and gentle spoolup for MAIN, to suit larger helis +; Improved transition from stepped startup to run +; Refined direct startup +; - Rev9.1 Fixed bug that changed FW revision after throttle calibration or TX programming +; - Rev9.2 Altered timing of throttle calibration in order to work with MultiWii calibration firmware +; Reduced main spoolup time to around 5 seconds +; Changed default beacon delay to 3 minutes +; - Rev9.3 Fixed bug in Plush 60/80A temperature reading, that caused failure in operation above 4S +; Corrected temperature limit for HiModel cool 22/33/41A, RCTimer 6A, Skywalker 20/40A, Turnigy AE45A, Plush 40/60/80A. Limit was previously set too high +; - Rev9.4 Improved timing for increased maximum rpm limit +; - Rev10.0 Added closed loop mode for multi +; Added high/low BEC voltage option (for the ESCs where HW supports it) +; Added method of resetting all programmed parameter values to defaults by TX programming +; Added Turnigy K-force 40A and Turnigy K-force 120A HV ESCs +; Enabled fully damped mode for several ESCs +; Extended startup power range downwards to enable very smooth start for large heli main motors +; Extended damping force with a highest setting +; Corrected temperature limits for F310 chips (Plush 40A and AE 45A) +; Implemented temperature reading average in order to avoid problems with ADC noise on Skywalkers +; Increased switching delays for XP 7A fast, in order to avoid cross conduction of N and P fets +; Miscellaneous other changes +; - Rev10.1 Relaxed RC signal jitter requirement during frequency measurement +; Corrected bug that prevented using governor low +; Enabled vdd monitor always, in order to reduce likelihood of accidental overwriting of adjustments +; Fixed bug that caused stop for PPM input above 2048us, and moved upper accepted limit to 2160us +; - Rev10.2 Corrected temperature limit for AE20-30/XP7-25, where limit was too high +; Corrected temperature limit for 120HV, where limit was too low +; Fixed bug that caused AE20/25/30A not to run in reverse +; +; +;**** **** **** **** **** +; Up to 8K Bytes of In-System Self-Programmable Flash +; 768 Bytes Internal SRAM +; +;**** **** **** **** **** +; Master clock is internal 24MHz oscillator +; Timer 0 (167/500ns counts) always counts up and is used for +; - PWM generation +; Timer 1 (167/500ns counts) always counts up and is not used +; - Time from pwm on/off event +; Timer 2 (500ns counts) always counts up and is used for +; - RC pulse timeout/skip counts and commutation times +; Timer 3 (500ns counts) always counts up and is used for +; - Commutation timeouts +; PCA0 (500ns counts) always counts up and is used for +; - RC pulse measurement +; +;**** **** **** **** **** +; Interrupt handling +; The F330/2 does not disable interrupts when entering an interrupt routine. +; Also some interrupt flags need to be cleared by software +; The code disables interrupts in interrupt routines, in order to avoid nested interrupts +; - Interrupts are disabled during beeps, to avoid audible interference from interrupts +; - RC pulse interrupts are periodically disabled in order to reduce interference with pwm interrupts. +; +;**** **** **** **** **** +; Motor control: +; - Brushless motor control with 6 states for each electrical 360 degrees +; - An advance timing of 0deg has zero cross 30deg after one commutation and 30deg before the next +; - Timing advance in this implementation is set to 15deg nominally +; - "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. +; Motor sequence starting from zero crossing: +; - Timer wait: Wt_Comm 15deg ; Time to wait from zero cross to actual commutation +; - Timer wait: Wt_Advance 15deg ; Time to wait for timing advance. Nominal commutation point is after this +; - Timer wait: Wt_Zc_Scan 7.5deg ; Time to wait before looking for zero cross +; - Scan for zero cross 22.5deg , Nominal, with some motor variations +; +; Motor startup in stepper mode: +; Initial motor rotations are done with the motor controlled as a stepper motor. +; In this stepper motor mode comparator information is not used. +; Settle phase is the first, where there are a few commutations with increasing step length, in order to settle the motor in a predefined position. +; Stepper phase comes next, where there is a step length decrease sequence. +; Direct startup is the last phase, for synchronization before normal bemf commutation run begins. +; Motor startup in direct mode: +; Direct startup is the only phase, before normal bemf commutation run begins. +; +;**** **** **** **** **** +; List of enumerated supported ESCs and modes (main, tail or multi) +XP_3A_Main EQU 1 +XP_3A_Tail EQU 2 +XP_3A_Multi EQU 3 +XP_7A_Main EQU 4 +XP_7A_Tail EQU 5 +XP_7A_Multi EQU 6 +XP_7A_Fast_Main EQU 7 +XP_7A_Fast_Tail EQU 8 +XP_7A_Fast_Multi EQU 9 +XP_12A_Main EQU 10 +XP_12A_Tail EQU 11 +XP_12A_Multi EQU 12 +XP_18A_Main EQU 13 +XP_18A_Tail EQU 14 +XP_18A_Multi EQU 15 +XP_25A_Main EQU 16 +XP_25A_Tail EQU 17 +XP_25A_Multi EQU 18 +DP_3A_Main EQU 19 +DP_3A_Tail EQU 20 +DP_3A_Multi EQU 21 +Supermicro_3p5A_Main EQU 22 +Supermicro_3p5A_Tail EQU 23 +Supermicro_3p5A_Multi EQU 24 +Turnigy_Plush_6A_Main EQU 25 +Turnigy_Plush_6A_Tail EQU 26 +Turnigy_Plush_6A_Multi EQU 27 +Turnigy_Plush_10A_Main EQU 28 +Turnigy_Plush_10A_Tail EQU 29 +Turnigy_Plush_10A_Multi EQU 30 +Turnigy_Plush_12A_Main EQU 31 +Turnigy_Plush_12A_Tail EQU 32 +Turnigy_Plush_12A_Multi EQU 33 +Turnigy_Plush_18A_Main EQU 34 +Turnigy_Plush_18A_Tail EQU 35 +Turnigy_Plush_18A_Multi EQU 36 +Turnigy_Plush_25A_Main EQU 37 +Turnigy_Plush_25A_Tail EQU 38 +Turnigy_Plush_25A_Multi EQU 39 +Turnigy_Plush_30A_Main EQU 40 +Turnigy_Plush_30A_Tail EQU 41 +Turnigy_Plush_30A_Multi EQU 42 +Turnigy_Plush_40A_Main EQU 43 +Turnigy_Plush_40A_Tail EQU 44 +Turnigy_Plush_40A_Multi EQU 45 +Turnigy_Plush_60A_Main EQU 46 +Turnigy_Plush_60A_Tail EQU 47 +Turnigy_Plush_60A_Multi EQU 48 +Turnigy_Plush_80A_Main EQU 49 +Turnigy_Plush_80A_Tail EQU 50 +Turnigy_Plush_80A_Multi EQU 51 +Turnigy_AE_20A_Main EQU 52 +Turnigy_AE_20A_Tail EQU 53 +Turnigy_AE_20A_Multi EQU 54 +Turnigy_AE_25A_Main EQU 55 +Turnigy_AE_25A_Tail EQU 56 +Turnigy_AE_25A_Multi EQU 57 +Turnigy_AE_30A_Main EQU 58 +Turnigy_AE_30A_Tail EQU 59 +Turnigy_AE_30A_Multi EQU 60 +Turnigy_AE_45A_Main EQU 61 +Turnigy_AE_45A_Tail EQU 62 +Turnigy_AE_45A_Multi EQU 63 +Turnigy_KForce_40A_Main EQU 64 +Turnigy_KForce_40A_Tail EQU 65 +Turnigy_KForce_40A_Multi EQU 66 +Turnigy_KForce_120A_HV_Main EQU 67 +Turnigy_KForce_120A_HV_Tail EQU 68 +Turnigy_KForce_120A_HV_Multi EQU 69 +Skywalker_20A_Main EQU 70 +Skywalker_20A_Tail EQU 71 +Skywalker_20A_Multi EQU 72 +Skywalker_40A_Main EQU 73 +Skywalker_40A_Tail EQU 74 +Skywalker_40A_Multi EQU 75 +HiModel_Cool_22A_Main EQU 76 +HiModel_Cool_22A_Tail EQU 77 +HiModel_Cool_22A_Multi EQU 78 +HiModel_Cool_33A_Main EQU 79 +HiModel_Cool_33A_Tail EQU 80 +HiModel_Cool_33A_Multi EQU 81 +HiModel_Cool_41A_Main EQU 82 +HiModel_Cool_41A_Tail EQU 83 +HiModel_Cool_41A_Multi EQU 84 +RCTimer_6A_Main EQU 85 +RCTimer_6A_Tail EQU 86 +RCTimer_6A_Multi EQU 87 +Align_RCE_BL15X_Main EQU 88 +Align_RCE_BL15X_Tail EQU 89 +Align_RCE_BL15X_Multi EQU 90 +Align_RCE_BL35X_Main EQU 91 +Align_RCE_BL35X_Tail EQU 92 +Align_RCE_BL35X_Multi EQU 93 +Align_RCE_BL35P_Main EQU 94 +Align_RCE_BL35P_Tail EQU 95 +Align_RCE_BL35P_Multi EQU 96 +H_King_10A_Main EQU 97 +H_King_10A_Tail EQU 98 +H_King_10A_Multi EQU 99 + +;**** **** **** **** **** +; Select the ESC and mode to use (or unselect all for use with external batch compile file) +;BESC EQU XP_3A_Main +;BESC EQU XP_3A_Tail +;BESC EQU XP_3A_Multi +;BESC EQU XP_7A_Main +;BESC EQU XP_7A_Tail +;BESC EQU XP_7A_Multi +;BESC EQU XP_7A_Fast_Main +;BESC EQU XP_7A_Fast_Tail +;BESC EQU XP_7A_Fast_Multi +;BESC EQU XP_12A_Main +;BESC EQU XP_12A_Tail +;BESC EQU XP_12A_Multi +;BESC EQU XP_18A_Main +;BESC EQU XP_18A_Tail +BESC EQU XP_18A_Multi +;BESC EQU XP_25A_Main +;BESC EQU XP_25A_Tail +;BESC EQU XP_25A_Multi +;BESC EQU DP_3A_Main +;BESC EQU DP_3A_Tail +;BESC EQU DP_3A_Multi +;BESC EQU Supermicro_3p5A_Main +;BESC EQU Supermicro_3p5A_Tail +;BESC EQU Supermicro_3p5A_Multi +;BESC EQU Turnigy_Plush_6A_Main +;BESC EQU Turnigy_Plush_6A_Tail +;BESC EQU Turnigy_Plush_6A_Multi +;BESC EQU Turnigy_Plush_10A_Main +;BESC EQU Turnigy_Plush_10A_Tail +;BESC EQU Turnigy_Plush_10A_Multi +;BESC EQU Turnigy_Plush_12A_Main +;BESC EQU Turnigy_Plush_12A_Tail +;BESC EQU Turnigy_Plush_12A_Multi +;BESC EQU Turnigy_Plush_18A_Main +;BESC EQU Turnigy_Plush_18A_Tail +;BESC EQU Turnigy_Plush_18A_Multi +;BESC EQU Turnigy_Plush_25A_Main +;BESC EQU Turnigy_Plush_25A_Tail +;BESC EQU Turnigy_Plush_25A_Multi +;BESC EQU Turnigy_Plush_30A_Main +;BESC EQU Turnigy_Plush_30A_Tail +;BESC EQU Turnigy_Plush_30A_Multi +;BESC EQU Turnigy_Plush_40A_Main +;BESC EQU Turnigy_Plush_40A_Tail +;BESC EQU Turnigy_Plush_40A_Multi +;BESC EQU Turnigy_Plush_60A_Main +;BESC EQU Turnigy_Plush_60A_Tail +;BESC EQU Turnigy_Plush_60A_Multi +;BESC EQU Turnigy_Plush_80A_Main +;BESC EQU Turnigy_Plush_80A_Tail +;BESC EQU Turnigy_Plush_80A_Multi +;BESC EQU Turnigy_AE_20A_Main +;BESC EQU Turnigy_AE_20A_Tail +;BESC EQU Turnigy_AE_20A_Multi +;BESC EQU Turnigy_AE_25A_Main +;BESC EQU Turnigy_AE_25A_Tail +;BESC EQU Turnigy_AE_25A_Multi +;BESC EQU Turnigy_AE_30A_Main +;BESC EQU Turnigy_AE_30A_Tail +;BESC EQU Turnigy_AE_30A_Multi +;BESC EQU Turnigy_AE_45A_Main +;BESC EQU Turnigy_AE_45A_Tail +;BESC EQU Turnigy_AE_45A_Multi +;BESC EQU Turnigy_KForce_40A_Main +;BESC EQU Turnigy_KForce_40A_Tail +;BESC EQU Turnigy_KForce_40A_Multi +;BESC EQU Turnigy_KForce_120A_HV_Main +;BESC EQU Turnigy_KForce_120A_HV_Tail +;BESC EQU Turnigy_KForce_120A_HV_Multi +;BESC EQU Skywalker_20A_Main +;BESC EQU Skywalker_20A_Tail +;BESC EQU Skywalker_20A_Multi +;BESC EQU Skywalker_40A_Main +;BESC EQU Skywalker_40A_Tail +;BESC EQU Skywalker_40A_Multi +;BESC EQU HiModel_Cool_22A_Main +;BESC EQU HiModel_Cool_22A_Tail +;BESC EQU HiModel_Cool_22A_Multi +;BESC EQU HiModel_Cool_33A_Main +;BESC EQU HiModel_Cool_33A_Tail +;BESC EQU HiModel_Cool_33A_Multi +;BESC EQU HiModel_Cool_41A_Main +;BESC EQU HiModel_Cool_41A_Tail +;BESC EQU HiModel_Cool_41A_Multi +;BESC EQU RCTimer_6A_Main +;BESC EQU RCTimer_6A_Tail +;BESC EQU RCTimer_6A_Multi +;BESC EQU Align_RCE_BL15X_Main +;BESC EQU Align_RCE_BL15X_Tail +;BESC EQU Align_RCE_BL15X_Multi +;BESC EQU Align_RCE_BL35X_Main +;BESC EQU Align_RCE_BL35X_Tail +;BESC EQU Align_RCE_BL35X_Multi +;BESC EQU Align_RCE_BL35P_Main +;BESC EQU Align_RCE_BL35P_Tail +;BESC EQU Align_RCE_BL35P_Multi +;BESC EQU H_King_10A_Main +;BESC EQU H_King_10A_Tail +;BESC EQU H_King_10A_Multi + + +;**** **** **** **** **** +; ESC selection statements +IF BESC == XP_3A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (XP_3A.inc) ; Select XP 3A pinout +ENDIF + +IF BESC == XP_3A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (XP_3A.inc) ; Select XP 3A pinout +ENDIF + +IF BESC == XP_3A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (XP_3A.inc) ; Select XP 3A pinout +ENDIF + +IF BESC == XP_7A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (XP_7A.inc) ; Select XP 7A pinout +ENDIF + +IF BESC == XP_7A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (XP_7A.inc) ; Select XP 7A pinout +ENDIF + +IF BESC == XP_7A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (XP_7A.inc) ; Select XP 7A pinout +ENDIF + +IF BESC == XP_7A_Fast_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (XP_7A_Fast.inc) ; Select XP 7A Fast pinout +ENDIF + +IF BESC == XP_7A_Fast_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (XP_7A_Fast.inc) ; Select XP 7A Fast pinout +ENDIF + +IF BESC == XP_7A_Fast_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (XP_7A_Fast.inc) ; Select XP 7A Fast pinout +ENDIF + +IF BESC == XP_12A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (XP_12A.inc) ; Select XP 12A pinout +ENDIF + +IF BESC == XP_12A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (XP_12A.inc) ; Select XP 12A pinout +ENDIF + +IF BESC == XP_12A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (XP_12A.inc) ; Select XP 12A pinout +ENDIF + +IF BESC == XP_18A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (XP_18A.inc) ; Select XP 18A pinout +ENDIF + +IF BESC == XP_18A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (XP_18A.inc) ; Select XP 18A pinout +ENDIF + +IF BESC == XP_18A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (XP_18A.inc) ; Select XP 18A pinout +ENDIF + +IF BESC == XP_25A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (XP_25A.inc) ; Select XP 25A pinout +ENDIF + +IF BESC == XP_25A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (XP_25A.inc) ; Select XP 25A pinout +ENDIF + +IF BESC == XP_25A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (XP_25A.inc) ; Select XP 25A pinout +ENDIF + +IF BESC == DP_3A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (DP_3A.inc) ; Select DP 3A pinout +ENDIF + +IF BESC == DP_3A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (DP_3A.inc) ; Select DP 3A pinout +ENDIF + +IF BESC == DP_3A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (DP_3A.inc) ; Select DP 3A pinout +ENDIF + +IF BESC == Supermicro_3p5A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Supermicro_3p5A.inc) ; Select Supermicro 3.5A pinout +ENDIF + +IF BESC == Supermicro_3p5A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Supermicro_3p5A.inc) ; Select Supermicro 3.5A pinout +ENDIF + +IF BESC == Supermicro_3p5A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Supermicro_3p5A.inc) ; Select Supermicro 3.5A pinout +ENDIF + +IF BESC == Turnigy_Plush_6A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_Plush_6A.inc) ; Select Turnigy Plush 6A pinout +ENDIF + +IF BESC == Turnigy_Plush_6A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_Plush_6A.inc) ; Select Turnigy Plush 6A pinout +ENDIF + +IF BESC == Turnigy_Plush_6A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_Plush_6A.inc) ; Select Turnigy Plush 6A pinout +ENDIF + +IF BESC == Turnigy_Plush_10A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_Plush_10A.inc) ; Select Turnigy Plush 10A pinout +ENDIF + +IF BESC == Turnigy_Plush_10A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_Plush_10A.inc) ; Select Turnigy Plush 10A pinout +ENDIF + +IF BESC == Turnigy_Plush_10A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_Plush_10A.inc) ; Select Turnigy Plush 10A pinout +ENDIF + +IF BESC == Turnigy_Plush_12A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_Plush_12A.inc) ; Select Turnigy Plush 12A pinout +ENDIF + +IF BESC == Turnigy_Plush_12A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_Plush_12A.inc) ; Select Turnigy Plush 12A pinout +ENDIF + +IF BESC == Turnigy_Plush_12A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_Plush_12A.inc) ; Select Turnigy Plush 12A pinout +ENDIF + +IF BESC == Turnigy_Plush_18A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_Plush_18A.inc) ; Select Turnigy Plush 18A pinout +ENDIF + +IF BESC == Turnigy_Plush_18A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_Plush_18A.inc) ; Select Turnigy Plush 18A pinout +ENDIF + +IF BESC == Turnigy_Plush_18A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_Plush_18A.inc) ; Select Turnigy Plush 18A pinout +ENDIF + +IF BESC == Turnigy_Plush_25A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_Plush_25A.inc) ; Select Turnigy Plush 25A pinout +ENDIF + +IF BESC == Turnigy_Plush_25A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_Plush_25A.inc) ; Select Turnigy Plush 25A pinout +ENDIF + +IF BESC == Turnigy_Plush_25A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_Plush_25A.inc) ; Select Turnigy Plush 25A pinout +ENDIF + +IF BESC == Turnigy_Plush_30A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_Plush_30A.inc) ; Select Turnigy Plush 30A pinout +ENDIF + +IF BESC == Turnigy_Plush_30A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_Plush_30A.inc) ; Select Turnigy Plush 30A pinout +ENDIF + +IF BESC == Turnigy_Plush_30A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_Plush_30A.inc) ; Select Turnigy Plush 30A pinout +ENDIF + +IF BESC == Turnigy_Plush_40A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_Plush_40A.inc) ; Select Turnigy Plush 40A pinout +ENDIF + +IF BESC == Turnigy_Plush_40A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_Plush_40A.inc) ; Select Turnigy Plush 40A pinout +ENDIF + +IF BESC == Turnigy_Plush_40A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_Plush_40A.inc) ; Select Turnigy Plush 40A pinout +ENDIF + +IF BESC == Turnigy_Plush_60A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_Plush_60A.inc) ; Select Turnigy Plush 60A pinout +ENDIF + +IF BESC == Turnigy_Plush_60A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_Plush_60A.inc) ; Select Turnigy Plush 60A pinout +ENDIF + +IF BESC == Turnigy_Plush_60A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_Plush_60A.inc) ; Select Turnigy Plush 60A pinout +ENDIF + +IF BESC == Turnigy_Plush_80A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_Plush_80A.inc) ; Select Turnigy Plush 80A pinout +ENDIF + +IF BESC == Turnigy_Plush_80A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_Plush_80A.inc) ; Select Turnigy Plush 80A pinout +ENDIF + +IF BESC == Turnigy_Plush_80A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_Plush_80A.inc) ; Select Turnigy Plush 80A pinout +ENDIF + +IF BESC == Turnigy_AE_20A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_AE_20A.inc) ; Select Turnigy AE-20A pinout +ENDIF + +IF BESC == Turnigy_AE_20A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_AE_20A.inc) ; Select Turnigy AE-20A pinout +ENDIF + +IF BESC == Turnigy_AE_20A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_AE_20A.inc) ; Select Turnigy AE-20A pinout +ENDIF + +IF BESC == Turnigy_AE_25A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_AE_25A.inc) ; Select Turnigy AE-25A pinout +ENDIF + +IF BESC == Turnigy_AE_25A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_AE_25A.inc) ; Select Turnigy AE-25A pinout +ENDIF + +IF BESC == Turnigy_AE_25A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_AE_25A.inc) ; Select Turnigy AE-25A pinout +ENDIF + +IF BESC == Turnigy_AE_30A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_AE_30A.inc) ; Select Turnigy AE-30A pinout +ENDIF + +IF BESC == Turnigy_AE_30A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_AE_30A.inc) ; Select Turnigy AE-30A pinout +ENDIF + +IF BESC == Turnigy_AE_30A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_AE_30A.inc) ; Select Turnigy AE-30A pinout +ENDIF + +IF BESC == Turnigy_AE_45A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_AE_45A.inc) ; Select Turnigy AE-45A pinout +ENDIF + +IF BESC == Turnigy_AE_45A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_AE_45A.inc) ; Select Turnigy AE-45A pinout +ENDIF + +IF BESC == Turnigy_AE_45A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_AE_45A.inc) ; Select Turnigy AE-45A pinout +ENDIF + +IF BESC == Turnigy_KForce_40A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_KForce_40A.inc) ; Select Turnigy KForce 40A pinout +ENDIF + +IF BESC == Turnigy_KForce_40A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_KForce_40A.inc) ; Select Turnigy KForce 40A pinout +ENDIF + +IF BESC == Turnigy_KForce_40A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_KForce_40A.inc) ; Select Turnigy KForce 40A pinout +ENDIF + +IF BESC == Turnigy_KForce_120A_HV_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Turnigy_KForce_120A_HV.inc) ; Select Turnigy KForce 120A HV pinout +ENDIF + +IF BESC == Turnigy_KForce_120A_HV_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Turnigy_KForce_120A_HV.inc) ; Select Turnigy KForce 120A HV pinout +ENDIF + +IF BESC == Turnigy_KForce_120A_HV_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Turnigy_KForce_120A_HV.inc) ; Select Turnigy KForce 120A HV pinout +ENDIF + +IF BESC == Skywalker_20A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Skywalker_20A.inc) ; Select Skywalker 20A pinout +ENDIF + +IF BESC == Skywalker_20A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Skywalker_20A.inc) ; Select Skywalker 20A pinout +ENDIF + +IF BESC == Skywalker_20A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Skywalker_20A.inc) ; Select Skywalker 20A pinout +ENDIF + +IF BESC == Skywalker_40A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Skywalker_40A.inc) ; Select Skywalker 40A pinout +ENDIF + +IF BESC == Skywalker_40A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Skywalker_40A.inc) ; Select Skywalker 40A pinout +ENDIF + +IF BESC == Skywalker_40A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Skywalker_40A.inc) ; Select Skywalker 40A pinout +ENDIF + +IF BESC == HiModel_Cool_22A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (HiModel_Cool_22A.inc) ; Select HiModel Cool 22A pinout +ENDIF + +IF BESC == HiModel_Cool_22A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (HiModel_Cool_22A.inc) ; Select HiModel Cool 22A pinout +ENDIF + +IF BESC == HiModel_Cool_22A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (HiModel_Cool_22A.inc) ; Select HiModel Cool 22A pinout +ENDIF + +IF BESC == HiModel_Cool_33A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (HiModel_Cool_33A.inc) ; Select HiModel Cool 33A pinout +ENDIF + +IF BESC == HiModel_Cool_33A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (HiModel_Cool_33A.inc) ; Select HiModel Cool 33A pinout +ENDIF + +IF BESC == HiModel_Cool_33A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (HiModel_Cool_33A.inc) ; Select HiModel Cool 33A pinout +ENDIF + +IF BESC == HiModel_Cool_41A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (HiModel_Cool_41A.inc) ; Select HiModel Cool 41A pinout +ENDIF + +IF BESC == HiModel_Cool_41A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (HiModel_Cool_41A.inc) ; Select HiModel Cool 41A pinout +ENDIF + +IF BESC == HiModel_Cool_41A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (HiModel_Cool_41A.inc) ; Select HiModel Cool 41A pinout +ENDIF + +IF BESC == RCTimer_6A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (RCTimer_6A.inc) ; Select RC Timer 6A pinout +ENDIF + +IF BESC == RCTimer_6A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (RCTimer_6A.inc) ; Select RC Timer 6A pinout +ENDIF + +IF BESC == RCTimer_6A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (RCTimer_6A.inc) ; Select RC Timer 6A pinout +ENDIF + +IF BESC == Align_RCE_BL15X_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Align_RCE_BL15X.inc) ; Select Align RCE-BL15X pinout +ENDIF + +IF BESC == Align_RCE_BL15X_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Align_RCE_BL15X.inc) ; Select Align RCE-BL15X pinout +ENDIF + +IF BESC == Align_RCE_BL15X_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Align_RCE_BL15X.inc) ; Select Align RCE-BL15X pinout +ENDIF + +IF BESC == Align_RCE_BL35X_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Align_RCE_BL35X.inc) ; Select Align RCE-BL35X pinout +ENDIF + +IF BESC == Align_RCE_BL35X_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Align_RCE_BL35X.inc) ; Select Align RCE-BL35X pinout +ENDIF + +IF BESC == Align_RCE_BL35X_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Align_RCE_BL35X.inc) ; Select Align RCE-BL35X pinout +ENDIF + +IF BESC == Align_RCE_BL35P_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (Align_RCE_BL35P.inc) ; Select Align RCE-BL35P pinout +ENDIF + +IF BESC == Align_RCE_BL35P_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (Align_RCE_BL35P.inc) ; Select Align RCE-BL35P pinout +ENDIF + +IF BESC == Align_RCE_BL35P_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (Align_RCE_BL35P.inc) ; Select Align RCE-BL35P pinout +ENDIF + +IF BESC == H_King_10A_Main +MODE EQU 0 ; Choose mode. Set to 0 for main motor +$include (H_King_10A.inc) ; Select H-King 10A pinout +ENDIF + +IF BESC == H_King_10A_Tail +MODE EQU 1 ; Choose mode. Set to 1 for tail motor +$include (H_King_10A.inc) ; Select H-King 10A pinout +ENDIF + +IF BESC == H_King_10A_Multi +MODE EQU 2 ; Choose mode. Set to 2 for multirotor +$include (H_King_10A.inc) ; Select H-King 10A pinout +ENDIF + +;**** **** **** **** **** +; TX programming defaults +; +; Parameter dependencies: +; - Governor P gain, I gain and Range is only used if one of the three governor modes is selected +; - Governor setup target is only used if Setup governor mode is selected (or closed loop mode is on for multi) +; - Startup rpm and startup accel is only used if stepped startup method is selected +; - Damping force is only used if DampedLight or Damped is selected +; +; Main +DEFAULT_PGM_MAIN_P_GAIN EQU 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 +DEFAULT_PGM_MAIN_I_GAIN EQU 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 +DEFAULT_PGM_MAIN_GOVERNOR_MODE EQU 1 ; 1=Tx 2=Arm 3=Setup 4=Off +DEFAULT_PGM_MAIN_GOVERNOR_RANGE EQU 1 ; 1=High 2=Low +DEFAULT_PGM_MAIN_LOW_VOLTAGE_LIM EQU 4 ; 1=Off 2=3.0V/c 3=3.1V/c 4=3.2V/c 5=3.3V/c 6=3.4V/c +DEFAULT_PGM_MAIN_STARTUP_RPM EQU 3 ; 1=0.67 2=0.8 3=1.00 4=1.25 5=1.5 +DEFAULT_PGM_MAIN_STARTUP_ACCEL EQU 1 ; 1=0.4 2=0.7 3=1.0 4=1.5 5=2.3 +DEFAULT_PGM_MAIN_COMM_TIMING EQU 3 ; 1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=High +DEFAULT_PGM_MAIN_THROTTLE_RATE EQU 13 ; 1=2 2=3 3=4 4=6 5=8 6=12 7=16 8=24 9=32 10=48 11=64 12=128 13=255 +DEFAULT_PGM_MAIN_DAMPING_FORCE EQU 1 ; 1=VeryLow 2=Low 3=MediumLow 4=MediumHigh 5=High 6=Highest +DEFAULT_PGM_MAIN_PWM_FREQ EQU 2 ; 1=High 2=Low 3=DampedLight +DEFAULT_PGM_MAIN_DEMAG_COMP EQU 1 ; 1=Disabled 2=15/0 3=15/7.5 4=7.5/7.5 5=7.5/15 (degrees blind advance / power off) +DEFAULT_PGM_MAIN_DIRECTION_REV EQU 1 ; 1=Normal 2=Reversed +DEFAULT_PGM_MAIN_RCP_PWM_POL EQU 1 ; 1=Positive 2=Negative +DEFAULT_PGM_MAIN_GOV_SETUP_TARGET EQU 180 ; Target for governor in setup mode. Corresponds to 70% throttle +DEFAULT_PGM_MAIN_REARM_START EQU 0 ; 1=Enabled 0=Disabled +DEFAULT_PGM_MAIN_BEEP_STRENGTH EQU 80 ; Beep strength +DEFAULT_PGM_MAIN_BEACON_STRENGTH EQU 100 ; Beacon strength +DEFAULT_PGM_MAIN_BEACON_DELAY EQU 2 ; 1=30s 2=1m 3=2m 4=3m 5=Infinite +; Tail +DEFAULT_PGM_TAIL_GAIN EQU 3 ; 1=0.75 2=0.88 3=1.00 4=1.12 5=1.25 +DEFAULT_PGM_TAIL_IDLE_SPEED EQU 4 ; 1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=High +DEFAULT_PGM_TAIL_STARTUP_RPM EQU 3 ; 1=0.67 2=0.8 3=1.00 4=1.25 5=1.5 +DEFAULT_PGM_TAIL_STARTUP_ACCEL EQU 5 ; 1=0.4 2=0.7 3=1.0 4=1.5 5=2.3 +DEFAULT_PGM_TAIL_COMM_TIMING EQU 3 ; 1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=High +DEFAULT_PGM_TAIL_THROTTLE_RATE EQU 13 ; 1=2 2=3 3=4 4=6 5=8 6=12 7=16 8=24 9=32 10=48 11=64 12=128 13=255 +DEFAULT_PGM_TAIL_DAMPING_FORCE EQU 5 ; 1=VeryLow 2=Low 3=MediumLow 4=MediumHigh 5=High 6=Highest +IF DAMPED_MODE_ENABLE == 1 +DEFAULT_PGM_TAIL_PWM_FREQ EQU 1 ; 1=High 2=Low 3=DampedLight 4=Damped +ELSE +DEFAULT_PGM_TAIL_PWM_FREQ EQU 1 ; 1=High 2=Low 3=DampedLight +ENDIF +DEFAULT_PGM_TAIL_DEMAG_COMP EQU 1 ; 1=Disabled 2=15/0 3=15/7.5 4=7.5/7.5 5=7.5/15 (degrees blind advance / power off) +DEFAULT_PGM_TAIL_DIRECTION_REV EQU 1 ; 1=Normal 2=Reversed +DEFAULT_PGM_TAIL_RCP_PWM_POL EQU 1 ; 1=Positive 2=Negative +DEFAULT_PGM_TAIL_BEEP_STRENGTH EQU 250 ; Beep strength +DEFAULT_PGM_TAIL_BEACON_STRENGTH EQU 250 ; Beacon strength +DEFAULT_PGM_TAIL_BEACON_DELAY EQU 4 ; 1=30s 2=1m 3=2m 4=3m 5=Infinite +; Multi +DEFAULT_PGM_MULTI_FIRST_KEYWORD EQU 66h ;增加首个关键字 2013.8.27 +DEFAULT_PGM_MULTI_P_GAIN EQU 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 +DEFAULT_PGM_MULTI_I_GAIN EQU 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 +DEFAULT_PGM_MULTI_GOVERNOR_MODE EQU 1 ; 1=Off 2=LoRange 3=MidRange 4=HiRange +DEFAULT_PGM_MULTI_GAIN EQU 3 ; 1=0.75 2=0.88 3=1.00 4=1.12 5=1.25 +DEFAULT_PGM_MULTI_LOW_VOLTAGE_LIM EQU 2 +;DEFAULT_PGM_MULTI_LOW_VOLTAGE_LIM EQU 1 ; 1=Off 2=3.0V/c 3=3.1V/c 4=3.2V/c 5=3.3V/c 6=3.4V/c +DEFAULT_PGM_MULTI_LOW_VOLTAGE_CTL EQU 1 ;增加低压保护控制方式默认值 1=功率逐渐降到31% 2=关闭输出 +DEFAULT_PGM_MULTI_STARTUP_RPM EQU 1 ; 1=0.67 2=0.8 3=1.00 4=1.25 5=1.5 +DEFAULT_PGM_MULTI_STARTUP_ACCEL EQU 5 ; 1=0.4 2=0.7 3=1.0 4=1.5 5=2.3 +DEFAULT_PGM_MULTI_COMM_TIMING EQU 3 ; 1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=High +DEFAULT_PGM_MULTI_THROTTLE_RATE EQU 13 ; 1=2 2=3 3=4 4=6 5=8 6=12 7=16 8=24 9=32 10=48 11=64 12=128 13=255 +DEFAULT_PGM_MULTI_DAMPING_FORCE EQU 1 ; 1=VeryLow 2=Low 3=MediumLow 4=MediumHigh 5=High 6=Highest +IF DAMPED_MODE_ENABLE == 1 +DEFAULT_PGM_MULTI_PWM_FREQ EQU 1 ; 1=Low 2=High +ELSE +DEFAULT_PGM_MULTI_PWM_FREQ EQU 1 ; 1=Low 2=High +ENDIF +DEFAULT_PGM_MULTI_DEMAG_COMP EQU 2 ; 1=Disabled 2=15/0 3=15/7.5 4=7.5/7.5 5=7.5/15 (degrees blind advance / power off) +DEFAULT_PGM_MULTI_DIRECTION_REV EQU 1 ; 1=Normal 2=Reversed +DEFAULT_PGM_MULTI_RCP_PWM_POL EQU 1 ; 1=Positive 2=Negative +;DEFAULT_PGM_MULTI_BEEP_STRENGTH EQU 40 ; Beep strength +;DEFAULT_PGM_MULTI_BEACON_STRENGTH EQU 80 ; Beacon strength 等待摇杆操作声强 +;DEFAULT_PGM_MULTI_BEACON_DELAY EQU 4 ; 1=30s 2=1m 3=2m 4=3m 5=Infinite +DEFAULT_PGM_MULTI_BEEP_STRENGTH EQU 80 ; Beep strength 上电到一次信号拉到最低声强 +DEFAULT_PGM_MULTI_BEACON_STRENGTH EQU 100 ; Beacon strength 等待摇杆操作声强 +DEFAULT_PGM_MULTI_BEACON_DELAY EQU 2 ; 1=30s 2=1m 3=2m 4=3m 5=Infinite 等待摇杆操作时间 +; Common +DEFAULT_PGM_ENABLE_TX_PROGRAM EQU 1 ; 1=Enabled 0=Disabled +DEFAULT_PGM_PPM_MIN_THROTTLE EQU 37 ; 4*37+1000=1148 +DEFAULT_PGM_PPM_MAX_THROTTLE EQU 208 ; 4*208+1000=1832 +DEFAULT_PGM_BEC_VOLTAGE_HIGH EQU 0 ; 0=Low 1= High + +;**** **** **** **** **** +; Constant definitions for main +IF MODE == 0 + +GOV_SPOOLRATE EQU 1 ; Number of steps for governor requested pwm per 32ms + +RCP_TIMEOUT_PPM EQU 10 ; Number of timer2H overflows (about 32ms) before considering rc pulse lost +RCP_TIMEOUT EQU 64 ; Number of timer2L overflows (about 128us) before considering rc pulse lost +RCP_SKIP_RATE EQU 32 ; Number of timer2L overflows (about 128us) before reenabling rc pulse detection +RCP_MIN EQU 0 ; This is minimum RC pulse length +RCP_MAX EQU 254 ; This is maximum RC pulse length +RCP_VALIDATE EQU 2 ; Require minimum this pulse length to validate RC pulse +RCP_STOP EQU 1 ; Stop motor at or below this pulse length +RCP_STOP_LIMIT EQU 3 ; Stop motor if this many timer2H overflows (~32ms) are below stop limit + +PWM_SETTLE EQU 50 ; PWM used when in start settling phase (also max power during direct start) +PWM_STEPPER EQU 80 ; PWM used when in start stepper phase + +COMM_TIME_RED EQU 8 ; Fixed reduction (in us) for commutation wait (to account for fixed delays) +COMM_TIME_MIN EQU 1 ; Minimum time (in us) for commutation wait + +TEMP_CHECK_RATE EQU 8 ; Number of adc conversions for each check of temperature (the other conversions are used for voltage) + +ENDIF +; Constant definitions for tail +IF MODE == 1 + +GOV_SPOOLRATE EQU 1 ; Number of steps for governor requested pwm per 32ms +RCP_TIMEOUT_PPM EQU 10 ; Number of timer2H overflows (about 32ms) before considering rc pulse lost +RCP_TIMEOUT EQU 24 ; Number of timer2L overflows (about 128us) before considering rc pulse lost +RCP_SKIP_RATE EQU 6 ; Number of timer2L overflows (about 128us) before reenabling rc pulse detection +RCP_MIN EQU 0 ; This is minimum RC pulse length +RCP_MAX EQU 255 ; This is maximum RC pulse length +RCP_VALIDATE EQU 2 ; Require minimum this pulse length to validate RC pulse +RCP_STOP EQU 1 ; Stop motor at or below this pulse length +RCP_STOP_LIMIT EQU 130 ; Stop motor if this many timer2H overflows (~32ms) are below stop limit + +PWM_SETTLE EQU 50 ; PWM used when in start settling phase (also max power during direct start) +PWM_STEPPER EQU 120 ; PWM used when in start stepper phase + +COMM_TIME_RED EQU 8 ; Fixed reduction (in us) for commutation wait (to account for fixed delays) +COMM_TIME_MIN EQU 1 ; Minimum time (in us) for commutation wait + +TEMP_CHECK_RATE EQU 8 ; Number of adc conversions for each check of temperature (the other conversions are used for voltage) + +ENDIF +; Constant definitions for multi +IF MODE == 2 + +GOV_SPOOLRATE EQU 1 ; Number of steps for governor requested pwm per 32ms + +RCP_TIMEOUT_PPM EQU 64 ; Number of timer2H overflows (about 32ms) before considering rc pulse lost +RCP_TIMEOUT EQU 40 ; Number of timer2L overflows (about 128us) before considering rc pulse lost +RCP_SKIP_RATE EQU 6 ; Number of timer2L overflows (about 128us) before reenabling rc pulse detection +RCP_MIN EQU 0 ; This is minimum RC pulse length +RCP_MAX EQU 255 ; This is maximum RC pulse length +RCP_VALIDATE EQU 2 ; Require minimum this pulse length to validate RC pulse +RCP_STOP EQU 1 ; Stop motor at or below this pulse length +;//////RCP_STOP_LIMIT EQU 20 ; Stop motor if this many timer2H overflows (~32ms) are below stop limit + +PWM_SETTLE EQU 50 ; PWM used when in start settling phase (also max power during direct start) +PWM_STEPPER EQU 120 ; PWM used when in start stepper phase + +COMM_TIME_RED EQU 8 ; Fixed reduction (in us) for commutation wait (to account for fixed delays) +COMM_TIME_MIN EQU 1 ; Minimum time (in us) for commutation wait + +TEMP_CHECK_RATE EQU 8 ; Number of adc conversions for each check of temperature (the other conversions are used for voltage) + +ENDIF + +;**** **** **** **** **** +; Temporary register definitions +Temp1 EQU R0 +Temp2 EQU R1 +Temp3 EQU R2 +Temp4 EQU R3 +Temp5 EQU R4 +Temp6 EQU R5 +Temp7 EQU R6 +Temp8 EQU R7 + +;**** **** **** **** **** +; Register definitions +DSEG AT 20h ; Variables segment + +Bit_Access: DS 1 ; Variable at bit accessible address (for non interrupt routines) +Bit_Access_Int: DS 1 ; Variable at bit accessible address (for interrupts) + +Requested_Pwm: DS 1 ; Requested pwm (from RC pulse value) +Governor_Req_Pwm: DS 1 ; Governor requested pwm (sets governor target) +Current_Pwm: DS 1 ; Current pwm +Current_Pwm_Limited: DS 1 ; Current pwm that is limited (applied to the motor output) +Rcp_Prev_Edge_L: DS 1 ; RC pulse previous edge timer3 timestamp (lo byte) +Rcp_Prev_Edge_H: DS 1 ; RC pulse previous edge timer3 timestamp (hi byte) +Rcp_Timeout_Cnt: DS 1 ; RC pulse timeout counter (decrementing) +Rcp_Skip_Cnt: DS 1 ; RC pulse skip counter (decrementing) +Rcp_Edge_Cnt: DS 1 ; RC pulse edge counter +Rcp_Stop_Limit: DS 1 ;增加刹车速度 + +Flags0: DS 1 ; State flags. Reset upon init_start +T3_PENDING EQU 0 ; Timer3 pending flag +RCP_MEAS_PWM_FREQ EQU 1 ; Measure RC pulse pwm frequency +PWM_ON EQU 2 ; Set in on part of pwm cycle +DEMAG_DETECTED EQU 3 ; Set when excessive demag time is detected +DEMAG_CUT_POWER EQU 4 ; Set when demag compensation cuts power +; EQU 5 +PROGRAM_FUNC_FLAG EQU 6 +SUCCESS_BEEP_FLAG EQU 7 + +Flags1: DS 1 ; State flags. Reset upon init_start +MOTOR_SPINNING EQU 0 ; Set when in motor is spinning +SETTLE_PHASE EQU 1 ; Set when in motor start settling phase +STEPPER_PHASE EQU 2 ; Set when in motor start stepper motor phase +DIRECT_STARTUP_PHASE EQU 3 ; Set when in direct startup phase +INITIAL_RUN_PHASE EQU 4 ; Set when in initial run phase, before synchronized run is achieved +; EQU 5 +; EQU 6 +LOW_LIMIT_STOP EQU 7 + +Flags2: DS 1 ; State flags. NOT reset upon init_start +RCP_UPDATED EQU 0 ; New RC pulse length value available +RCP_EDGE_NO EQU 1 ; RC pulse edge no. 0=rising, 1=falling +PGM_PWMOFF_DAMPED EQU 2 ; Programmed pwm off damped mode. Set when fully damped or damped light mode is selected +PGM_PWMOFF_DAMPED_FULL EQU 3 ; Programmed pwm off fully damped mode. Set when all pfets shall be on in pwm_off period +PGM_PWMOFF_DAMPED_LIGHT EQU 4 ; Programmed pwm off damped light mode. Set when only 2 pfets shall be on in pwm_off period +PGM_PWM_HIGH_FREQ EQU 5 ; Progremmed pwm high frequency +CURR_PWMOFF_DAMPED EQU 6 ; Currently running pwm off cycle is damped +CURR_PWMOFF_COMP_ABLE EQU 7 ; Currently running pwm off cycle is usable for comparator + +Flags3: DS 1 ; State flags. NOT reset upon init_start +RCP_PWM_FREQ_1KHZ EQU 0 ; RC pulse pwm frequency is 1kHz +RCP_PWM_FREQ_2KHZ EQU 1 ; RC pulse pwm frequency is 2kHz +RCP_PWM_FREQ_4KHZ EQU 2 ; RC pulse pwm frequency is 4kHz +RCP_PWM_FREQ_8KHZ EQU 3 ; RC pulse pwm frequency is 8kHz +PGM_DIR_REV EQU 4 ; Programmed direction. 0=normal, 1=reversed +PGM_RCP_PWM_POL EQU 5 ; Programmed RC pulse pwm polarity. 0=positive, 1=negative +FULL_THROTTLE_RANGE EQU 6 ; When set full throttle range is used (1000-2000us) and stored calibration values are ignored +ERRO_DATA EQU 7 ;增加编程卡数据错误标志 2013.8.27 +;**** **** **** **** **** +; RAM definitions +DSEG AT 30h ; Ram data segment, direct addressing +OS_FLAG: DS 1 +Initial_Arm: DS 1 ; Variable that is set during the first arm sequence after power on +;Pca_First_Int: DS 1 +Power_On_Wait_Cnt_L: DS 1 ; Power on wait counter (lo byte) +Power_On_Wait_Cnt_H: DS 1 ; Power on wait counter (hi byte) +Pgm_Pulse_Cnt: DS 1 +;Stepper_Step_Beg_L: DS 1 ; Stepper phase step time at the beginning (lo byte) +;Stepper_Step_Beg_H: DS 1 ; Stepper phase step time at the beginning (hi byte) +;Stepper_Step_End_L: DS 1 ; Stepper phase step time at the end (lo byte) +;Stepper_Step_End_H: DS 1 ; Stepper phase step time at the end (hi byte) +Startup_Rot_Cnt: DS 1 ; Startup phase rotations counter +Direct_Startup_Ok_Cnt: DS 1 ; Direct startup phase ok comparator waits counter (incrementing) + +Prev_Comm_L: DS 1 ; Previous commutation timer3 timestamp (lo byte) +Prev_Comm_H: DS 1 ; Previous commutation timer3 timestamp (hi byte) +Comm_Period4x_L: DS 1 ; Timer3 counts between the last 4 commutations (lo byte) +Comm_Period4x_H: DS 1 ; Timer3 counts between the last 4 commutations (hi byte) +Comm_Phase: DS 1 ; Current commutation phase +Comp_Wait_Reads: DS 1 ; Comparator wait comparator reads + +Gov_Target_L: DS 1 ; Governor target (lo byte) +Gov_Target_H: DS 1 ; Governor target (hi byte) +Gov_Integral_L: DS 1 ; Governor integral error (lo byte) +Gov_Integral_H: DS 1 ; Governor integral error (hi byte) +Gov_Integral_X: DS 1 ; Governor integral error (ex byte) +Gov_Proportional_L: DS 1 ; Governor proportional error (lo byte) +Gov_Proportional_H: DS 1 ; Governor proportional error (hi byte) +Gov_Prop_Pwm: DS 1 ; Governor calculated new pwm based upon proportional error +Gov_Arm_Target: DS 1 ; Governor arm target value +Gov_Active: DS 1 ; Governor active (enabled when speed is above minimum) + +Wt_Advance_L: DS 1 ; Timer3 counts for commutation advance timing (lo byte) +Wt_Advance_H: DS 1 ; Timer3 counts for commutation advance timing (hi byte) +Wt_Zc_Scan_L: DS 1 ; Timer3 counts from commutation to zero cross scan (lo byte) +Wt_Zc_Scan_H: DS 1 ; Timer3 counts from commutation to zero cross scan (hi byte) +Wt_Comm_L: DS 1 ; Timer3 counts from zero cross to commutation (lo byte) +Wt_Comm_H: DS 1 ; Timer3 counts from zero cross to commutation (hi byte) +;Wt_Stepper_Step_L: DS 1 ; Timer3 counts for stepper step (lo byte) +;Wt_Stepper_Step_H: DS 1 ; Timer3 counts for stepper step (hi byte) + +Rcp_PrePrev_Edge_L: DS 1 ; RC pulse pre previous edge pca timestamp (lo byte) +Rcp_PrePrev_Edge_H: DS 1 ; RC pulse pre previous edge pca timestamp (hi byte) +Rcp_Edge_L: DS 1 ; RC pulse edge pca timestamp (lo byte) +Rcp_Edge_H: DS 1 ; RC pulse edge pca timestamp (hi byte) +Rcp_Prev_Period_L: DS 1 ; RC pulse previous period (lo byte) +Rcp_Prev_Period_H: DS 1 ; RC pulse previous period (hi byte) +Rcp_Period_Diff_Accepted: DS 1 ; RC pulse period difference acceptable +New_Rcp: DS 1 ; New RC pulse value in pca counts +Prev_Rcp_Pwm_Freq: DS 1 ; Previous RC pulse pwm frequency (used during pwm frequency measurement) +Curr_Rcp_Pwm_Freq: DS 1 ; Current RC pulse pwm frequency (used during pwm frequency measurement) +Rcp_Stop_Cnt: DS 1 ; Counter for RC pulses below stop value + +Pwm_Limit: DS 1 ; Maximum allowed pwm +Pwm_Limit_Spoolup: DS 1 ; Maximum allowed pwm during spoolup of main +Pwm_Spoolup_Beg: DS 1 ; Pwm to begin main spoolup with +Pwm_Motor_Idle: DS 1 ; Motor idle speed pwm +Pwm_On_Cnt: DS 1 ; Pwm on event counter (used to increase pwm off time for low pwm) +Pwm_Off_Cnt: DS 1 ; Pwm off event counter (used to run some pwm cycles without damping) + +Spoolup_Limit_Cnt: DS 1 ; Interrupt count for spoolup limit +Spoolup_Limit_Skip: DS 1 ; Interrupt skips for spoolup limit increment (0=no skips, 1=skip one etc) + +Damping_Period: DS 1 ; Damping on/off period +Damping_On: DS 1 ; Damping on part of damping period + +;Lipo_Adc_Reference_L: DS 1 ; Voltage reference adc value (lo byte) +;Lipo_Adc_Reference_H: DS 1 ; Voltage reference adc value (hi byte) +Lipo_Adc_Limit_L: DS 1 ; Low voltage limit adc value (lo byte) +Lipo_Adc_Limit_H: DS 1 ; Low voltage limit adc value (hi byte) +Adc_Conversion_Cnt: DS 1 ; Adc conversion counter +Lipo_Cell_Count: DS 1 ;增加电池节数累计变量 2013.5.29 +Limit_Count: DS 1 ;增加逐渐递减计数变量 2013.7.5 + +Current_Average_Temp: DS 1 ; Current average temperature (lo byte ADC reading, assuming hi byte is 1) + +Ppm_Throttle_Gain: DS 1 ; Gain to be applied to RCP value for PPM input +Beep_Strength: DS 1 ; Strength of beeps + +Tx_Pgm_Func_No: DS 1 ; Function number when doing programming by tx +Tx_Pgm_Paraval_No: DS 1 ; Parameter value number when doing programming by tx +Tx_Pgm_Beep_No: DS 1 ; Beep number when doing programming by tx +Commu_Data_Buffer: DS 1 ;编程卡接收，发送数据暂存 2013.8.27 +Commu_Sum: DS 1 ;增加编程卡校验和 2013.8.27 +Min_Throttle: DS 1 + +; Indirect addressing data segment +ISEG AT 080h +Pgm_Fir_Key: DS 1 ;增加首个关键字 2013.8.27 +Pgm_Gov_P_Gain: DS 1 ; Programmed governor P gain +Pgm_Gov_I_Gain: DS 1 ; Programmed governor I gain +Pgm_Gov_Mode: DS 1 ; Programmed governor mode + +Pgm_Low_Voltage_Lim: DS 1 ; Programmed low voltage limit +Pgm_Low_Voltage_Ctl: DS 1 ;增加低压保护控制方式编程项 2013.7.5 +Pgm_Motor_Gain: DS 1 ; Programmed motor gain +Pgm_Motor_Idle: DS 1 ; Programmed motor idle speed +Pgm_Startup_Pwr: DS 1 ; Programmed startup power +Pgm_Pwm_Freq: DS 1 ; Programmed pwm frequency +Pgm_Direction_Rev: DS 1 ; Programmed rotation direction +Pgm_Input_Pol: DS 1 ; Programmed input pwm polarity +Initialized_L_Dummy: DS 1 ; Place holder +Initialized_H_Dummy: DS 1 ; Place holder +Pgm_Enable_TX_Program: DS 1 ; Programmed enable/disable value for TX programming +Pgm_Main_Rearm_Start: DS 1 ; Programmed enable/disable re-arming main every start +Pgm_Gov_Setup_Target: DS 1 ; Programmed main governor setup target +Pgm_Startup_Rpm: DS 1 ; Programmed startup rpm +Pgm_Startup_Accel: DS 1 ; Programmed startup acceleration +Pgm_Volt_Comp_Dummy: DS 1 ; Place holder +Pgm_Comm_Timing: DS 1 ; Programmed commutation timing +Pgm_Damping_Force: DS 1 ; Programmed damping force +Pgm_Gov_Range: DS 1 ; Programmed governor range +;Pgm_Startup_Method: DS 1 ; Programmed startup method +Pgm_Ppm_Min_Throttle: DS 1 ; Programmed throttle minimum +Pgm_Ppm_Max_Throttle: DS 1 ; Programmed throttle maximum +Pgm_Beep_Strength: DS 1 ; Programmed beep strength +Pgm_Beacon_Strength: DS 1 ; Programmed beacon strength +Pgm_Beacon_Delay: DS 1 ; Programmed beacon delay +Pgm_Throttle_Rate: DS 1 ; Programmed throttle rate +Pgm_Demag_Comp: DS 1 ; Programmed demag compensation +Pgm_BEC_Voltage_High: DS 1 ; Programmed BEC voltage + +Pgm_Gov_P_Gain_Decoded: DS 1 ; Programmed governor decoded P gain +Pgm_Gov_I_Gain_Decoded: DS 1 ; Programmed governor decoded I gain +Pgm_Throttle_Rate_Decoded: DS 1 ; Programmed throttle rate decoded +Pgm_Startup_Pwr_Decoded: DS 1 ; Programmed startup power decoded +Pgm_Demag_Comp_Wait_Decoded: DS 1 ; Programmed demag compensation wait decoded +Pgm_Demag_Comp_Power_Decoded: DS 1 ; Programmed demag compensation power cut decoded + + +; Indirect addressing data segment +ISEG AT 0D0h +Tag_Temporary_Storage: DS 48 ; Temporary storage for tags when updating "Eeprom" + + +;**** **** **** **** **** +CSEG AT 1A00h ; "Eeprom" segment +EEPROM_FW_MAIN_REVISION EQU 10 ; Main revision of the firmware +EEPROM_FW_SUB_REVISION EQU 2 ; Sub revision of the firmware +EEPROM_LAYOUT_REVISION EQU 16 ; Revision of the EEPROM layout + +Eep_FW_Main_Revision: DB EEPROM_FW_MAIN_REVISION ; EEPROM firmware main revision number +Eep_FW_Sub_Revision: DB EEPROM_FW_SUB_REVISION ; EEPROM firmware sub revision number +Eep_Layout_Revision: DB EEPROM_LAYOUT_REVISION ; EEPROM layout revision number + +IF MODE == 0 +Eep_Pgm_Gov_P_Gain: DB DEFAULT_PGM_MAIN_P_GAIN ; EEPROM copy of programmed governor P gain +Eep_Pgm_Gov_I_Gain: DB DEFAULT_PGM_MAIN_I_GAIN ; EEPROM copy of programmed governor I gain +Eep_Pgm_Gov_Mode: DB DEFAULT_PGM_MAIN_GOVERNOR_MODE ; EEPROM copy of programmed governor mode + +Eep_Pgm_Low_Voltage_Lim: DB DEFAULT_PGM_MAIN_LOW_VOLTAGE_LIM ; EEPROM copy of programmed low voltage limit +_Eep_Pgm_Motor_Gain: DB 0FFh +_Eep_Pgm_Motor_Idle: DB 0FFh +Eep_Pgm_Startup_Pwr: DB DEFAULT_PGM_MAIN_STARTUP_PWR ; EEPROM copy of programmed startup power +Eep_Pgm_Pwm_Freq: DB DEFAULT_PGM_MAIN_PWM_FREQ ; EEPROM copy of programmed pwm frequency +Eep_Pgm_Direction_Rev: DB DEFAULT_PGM_MAIN_DIRECTION_REV ; EEPROM copy of programmed rotation direction +Eep_Pgm_Input_Pol: DB DEFAULT_PGM_MAIN_RCP_PWM_POL ; EEPROM copy of programmed input polarity +Eep_Initialized_L: DB 0A5h ; EEPROM initialized signature low byte +Eep_Initialized_H: DB 05Ah ; EEPROM initialized signature high byte +Eep_Enable_TX_Program: DB DEFAULT_PGM_ENABLE_TX_PROGRAM ; EEPROM TX programming enable +Eep_Main_Rearm_Start: DB DEFAULT_PGM_MAIN_REARM_START ; EEPROM re-arming main enable +Eep_Pgm_Gov_Setup_Target: DB DEFAULT_PGM_MAIN_GOV_SETUP_TARGET ; EEPROM main governor setup target +Eep_Pgm_Startup_Rpm: DB DEFAULT_PGM_MAIN_STARTUP_RPM ; EEPROM copy of programmed startup rpm +Eep_Pgm_Startup_Accel: DB DEFAULT_PGM_MAIN_STARTUP_ACCEL ; EEPROM copy of programmed startup acceleration +_Eep_Pgm_Volt_Comp: DB 0FFh +Eep_Pgm_Comm_Timing: DB DEFAULT_PGM_MAIN_COMM_TIMING ; EEPROM copy of programmed commutation timing +Eep_Pgm_Damping_Force: DB DEFAULT_PGM_MAIN_DAMPING_FORCE ; EEPROM copy of programmed damping force +Eep_Pgm_Gov_Range: DB DEFAULT_PGM_MAIN_GOVERNOR_RANGE ; EEPROM copy of programmed governor range +Eep_Pgm_Startup_Method: DB DEFAULT_PGM_MAIN_STARTUP_METHOD ; EEPROM copy of programmed startup method +Eep_Pgm_Ppm_Min_Throttle: DB DEFAULT_PGM_PPM_MIN_THROTTLE ; EEPROM copy of programmed minimum throttle (final value is 4x+1000=1148) +Eep_Pgm_Ppm_Max_Throttle: DB DEFAULT_PGM_PPM_MAX_THROTTLE ; EEPROM copy of programmed minimum throttle (final value is 4x+1000=1832) +Eep_Pgm_Beep_Strength: DB DEFAULT_PGM_MAIN_BEEP_STRENGTH ; EEPROM copy of programmed beep strength +Eep_Pgm_Beacon_Strength: DB DEFAULT_PGM_MAIN_BEACON_STRENGTH ; EEPROM copy of programmed beacon strength +Eep_Pgm_Beacon_Delay: DB DEFAULT_PGM_MAIN_BEACON_DELAY ; EEPROM copy of programmed beacon delay +Eep_Pgm_Throttle_Rate: DB DEFAULT_PGM_MAIN_THROTTLE_RATE ; EEPROM copy of programmed throttle rate +Eep_Pgm_Demag_Comp: DB DEFAULT_PGM_MAIN_DEMAG_COMP ; EEPROM copy of programmed demag compensation +Eep_Pgm_BEC_Voltage_High: DB DEFAULT_PGM_BEC_VOLTAGE_HIGH ; EEPROM copy of programmed BEC voltage +ENDIF + +IF MODE == 1 +_Eep_Pgm_Gov_P_Gain: DB 0FFh +_Eep_Pgm_Gov_I_Gain: DB 0FFh +_Eep_Pgm_Gov_Mode: DB 0FFh + +_Eep_Pgm_Low_Voltage_Lim: DB 0FFh +Eep_Pgm_Motor_Gain: DB DEFAULT_PGM_TAIL_GAIN ; EEPROM copy of programmed tail gain +Eep_Pgm_Motor_Idle: DB DEFAULT_PGM_TAIL_IDLE_SPEED ; EEPROM copy of programmed tail idle speed +Eep_Pgm_Startup_Pwr: DB DEFAULT_PGM_TAIL_STARTUP_PWR ; EEPROM copy of programmed startup power +Eep_Pgm_Pwm_Freq: DB DEFAULT_PGM_TAIL_PWM_FREQ ; EEPROM copy of programmed pwm frequency +Eep_Pgm_Direction_Rev: DB DEFAULT_PGM_TAIL_DIRECTION_REV ; EEPROM copy of programmed rotation direction +Eep_Pgm_Input_Pol: DB DEFAULT_PGM_TAIL_RCP_PWM_POL ; EEPROM copy of programmed input polarity +Eep_Initialized_L: DB 05Ah ; EEPROM initialized signature low byte +Eep_Initialized_H: DB 0A5h ; EEPROM initialized signature high byte +Eep_Enable_TX_Program: DB DEFAULT_PGM_ENABLE_TX_PROGRAM ; EEPROM TX programming enable +_Eep_Main_Rearm_Start: DB 0FFh +_Eep_Pgm_Gov_Setup_Target: DB 0FFh +Eep_Pgm_Startup_Rpm: DB DEFAULT_PGM_TAIL_STARTUP_RPM ; EEPROM copy of programmed startup rpm +Eep_Pgm_Startup_Accel: DB DEFAULT_PGM_TAIL_STARTUP_ACCEL ; EEPROM copy of programmed startup acceleration +_Eep_Pgm_Volt_Comp: DB 0FFh +Eep_Pgm_Comm_Timing: DB DEFAULT_PGM_TAIL_COMM_TIMING ; EEPROM copy of programmed commutation timing +Eep_Pgm_Damping_Force: DB DEFAULT_PGM_TAIL_DAMPING_FORCE ; EEPROM copy of programmed damping force +_Eep_Pgm_Gov_Range: DB 0FFh +Eep_Pgm_Startup_Method: DB DEFAULT_PGM_TAIL_STARTUP_METHOD ; EEPROM copy of programmed startup method +Eep_Pgm_Ppm_Min_Throttle: DB DEFAULT_PGM_PPM_MIN_THROTTLE ; EEPROM copy of programmed minimum throttle (final value is 4x+1000=1148) +Eep_Pgm_Ppm_Max_Throttle: DB DEFAULT_PGM_PPM_MAX_THROTTLE ; EEPROM copy of programmed minimum throttle (final value is 4x+1000=1832) +Eep_Pgm_Beep_Strength: DB DEFAULT_PGM_TAIL_BEEP_STRENGTH ; EEPROM copy of programmed beep strength +Eep_Pgm_Beacon_Strength: DB DEFAULT_PGM_TAIL_BEACON_STRENGTH ; EEPROM copy of programmed beacon strength +Eep_Pgm_Beacon_Delay: DB DEFAULT_PGM_TAIL_BEACON_DELAY ; EEPROM copy of programmed beacon delay +Eep_Pgm_Throttle_Rate: DB DEFAULT_PGM_TAIL_THROTTLE_RATE ; EEPROM copy of programmed throttle rate +Eep_Pgm_Demag_Comp: DB DEFAULT_PGM_TAIL_DEMAG_COMP ; EEPROM copy of programmed demag compensation +Eep_Pgm_BEC_Voltage_High: DB DEFAULT_PGM_BEC_VOLTAGE_HIGH ; EEPROM copy of programmed BEC voltage +ENDIF + +IF MODE == 2 +Eep_Pgm_Fir_Key: DB DEFAULT_PGM_MULTI_FIRST_KEYWORD ;增加首个关键字 2013.8.27 +Eep_Pgm_Gov_P_Gain: DB DEFAULT_PGM_MULTI_P_GAIN ; EEPROM copy of programmed closed loop P gain +Eep_Pgm_Gov_I_Gain: DB DEFAULT_PGM_MULTI_I_GAIN ; EEPROM copy of programmed closed loop I gain +Eep_Pgm_Gov_Mode: DB DEFAULT_PGM_MULTI_GOVERNOR_MODE ; EEPROM copy of programmed closed loop mode + +Eep_Pgm_Low_Voltage_Lim: DB DEFAULT_PGM_MULTI_LOW_VOLTAGE_LIM ; EEPROM copy of programmed low voltage limit +Eep_Pgm_Low_Voltage_Ctl: DB DEFAULT_PGM_MULTI_LOW_VOLTAGE_CTL ; 增加低压保护控制编程 +Eep_Pgm_Motor_Gain: DB DEFAULT_PGM_MULTI_GAIN ; EEPROM copy of programmed tail gain +_Eep_Pgm_Motor_Idle: DB 0FFh ; EEPROM copy of programmed tail idle speed +Eep_Pgm_Startup_Pwr: DB DEFAULT_PGM_MULTI_STARTUP_PWR ; EEPROM copy of programmed startup power +Eep_Pgm_Pwm_Freq: DB DEFAULT_PGM_MULTI_PWM_FREQ ; EEPROM copy of programmed pwm frequency +Eep_Pgm_Direction_Rev: DB DEFAULT_PGM_MULTI_DIRECTION_REV ; EEPROM copy of programmed rotation direction +Eep_Pgm_Input_Pol: DB DEFAULT_PGM_MULTI_RCP_PWM_POL ; EEPROM copy of programmed input polarity +Eep_Initialized_L: DB 055h ; EEPROM initialized signature low byte +Eep_Initialized_H: DB 0AAh ; EEPROM initialized signature high byte +Eep_Enable_TX_Program: DB DEFAULT_PGM_ENABLE_TX_PROGRAM ; EEPROM TX programming enable +_Eep_Main_Rearm_Start: DB 0FFh +_Eep_Pgm_Gov_Setup_Target: DB 0FFh +Eep_Pgm_Startup_Rpm: DB DEFAULT_PGM_MULTI_STARTUP_RPM ; EEPROM copy of programmed startup rpm +Eep_Pgm_Startup_Accel: DB DEFAULT_PGM_MULTI_STARTUP_ACCEL ; EEPROM copy of programmed startup acceleration +_Eep_Pgm_Volt_Comp: DB 0FFh +Eep_Pgm_Comm_Timing: DB DEFAULT_PGM_MULTI_COMM_TIMING ; EEPROM copy of programmed commutation timing +Eep_Pgm_Damping_Force: DB DEFAULT_PGM_MULTI_DAMPING_FORCE ; EEPROM copy of programmed damping force +_Eep_Pgm_Gov_Range: DB 0FFh +;Eep_Pgm_Startup_Method: DB DEFAULT_PGM_MULTI_STARTUP_METHOD ; EEPROM copy of programmed startup method +Eep_Pgm_Ppm_Min_Throttle: DB DEFAULT_PGM_PPM_MIN_THROTTLE ; EEPROM copy of programmed minimum throttle (final value is 4x+1000=1148) +Eep_Pgm_Ppm_Max_Throttle: DB DEFAULT_PGM_PPM_MAX_THROTTLE ; EEPROM copy of programmed minimum throttle (final value is 4x+1000=1832) +Eep_Pgm_Beep_Strength: DB DEFAULT_PGM_MULTI_BEEP_STRENGTH ; EEPROM copy of programmed beep strength +Eep_Pgm_Beacon_Strength: DB DEFAULT_PGM_MULTI_BEACON_STRENGTH ; EEPROM copy of programmed beacon strength +Eep_Pgm_Beacon_Delay: DB DEFAULT_PGM_MULTI_BEACON_DELAY ; EEPROM copy of programmed beacon delay +Eep_Pgm_Throttle_Rate: DB DEFAULT_PGM_MULTI_THROTTLE_RATE ; EEPROM copy of programmed throttle rate +Eep_Pgm_Demag_Comp: DB DEFAULT_PGM_MULTI_DEMAG_COMP ; EEPROM copy of programmed demag compensation +Eep_Pgm_BEC_Voltage_High: DB DEFAULT_PGM_BEC_VOLTAGE_HIGH ; EEPROM copy of programmed BEC voltage +ENDIF + + +Eep_Dummy: DB 0FFh ; EEPROM address for safety reason + +CSEG AT 1A60h +Eep_Name: DB " " ; Name tag (16 Bytes) + +;**** **** **** **** **** + Interrupt_Table_Definition ; SiLabs interrupts +CSEG AT 80h ; Code segment after interrupt vectors + +;**** **** **** **** **** + +; Table definitions +GOV_GAIN_TABLE: DB 02h, 03h, 04h, 06h, 08h, 0Ch, 10h, 18h, 20h, 30h, 40h, 60h, 80h +THROTTLE_RATE_TABLE: DB 02h, 03h, 04h, 06h, 08h, 0Ch, 10h, 18h, 20h, 30h, 40h, 80h, 0FFh +STARTUP_POWER_TABLE: DB 04h, 06h, 08h, 0Ch, 10h, 18h, 20h, 30h, 40h, 60h, 80h, 0A0h, 0C0h +DEMAG_WAIT_TABLE: DB 0, 0, 0, 1, 1 +DEMAG_POWER_TABLE: DB 0, 3, 2, 2, 1 + +;FINISH_KEYWORD_TABLE: DB 066h, 08h, 08h, 03h, 01h, 00h, 02h, 00h +; DB 09h, 01h, 00h, 00h, 00h, 00h, 00h, 00h +; DB 0B4h, 00h, 04h, 00h, 02h, 00h, 00h, 025h +; DB 0D0h, 050h, 064h, 01h, 0Dh, 01h, 00h, 00h ;增加编程默认值表 2013.7.30 ;////////////////////////////////////////// +; DB 00h, 00h, 00h, 00h, 00h, 00h, 00h, 00h +; DB 00h, 00h, 00h, 00h, 00h, 00h, 00h, 00h + +IF MODE == 0 +TX_PGM_PARAMS_MAIN: DB 13, 13, 4, 2, 6, 2, 13, 5, 5, 5, 13, 6, 3, 5, 2, 2 +ENDIF +IF MODE == 1 + IF DAMPED_MODE_ENABLE == 1 +TX_PGM_PARAMS_TAIL: DB 5, 5, 2, 13, 5, 5, 5, 13, 6, 4, 5, 2, 2 + ENDIF + IF DAMPED_MODE_ENABLE == 0 +TX_PGM_PARAMS_TAIL: DB 5, 5, 2, 13, 5, 5, 5, 13, 6, 3, 5, 2, 2 + ENDIF +ENDIF +IF MODE == 2 + IF DAMPED_MODE_ENABLE == 1 +;///////TX_PGM_PARAMS_MULTI: DB 13, 13, 4, 5, 6, 2, 13, 5, 5, 5, 13, 6, 4, 5, 2, 2 + TX_PGM_PARAMS_MULTI: DB 6, 5, 13, 4, 2, 4, 2 + ENDIF + IF DAMPED_MODE_ENABLE == 0 +;///////TX_PGM_PARAMS_MULTI: DB 13, 13, 4, 5, 6, 2, 13, 5, 5, 5, 13, 6, 3, 5, 2, 2 + TX_PGM_PARAMS_MULTI: DB 6, 5, 13, 4, 2, 4, 2 + ENDIF +ENDIF + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Timer0 interrupt routine +; +; Assumptions: DPTR register must be set to desired pwm_nfet_on label +; Requirements: Temp variables can NOT be used since PWSW.3 is not set +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +t0_int: ; Used for pwm control + clr EA ; Disable all interrupts + push PSW ; Preserve registers through interrupt + push ACC + ; Check if pwm is on + jb Flags0.PWM_ON, t0_int_pwm_off ; Is pwm on? + + ; Pwm on cycle. + jnb Current_Pwm_Limited.7, t0_int_pwm_on_low_pwm ; Jump for low pwm (<50%) + +t0_int_pwm_on_execute: + clr A + jmp @A+DPTR ; No - jump to pwm on routines. DPTR should be set to one of the pwm_nfet_on labels + +t0_int_pwm_on_low_pwm: + ; Do not execute pwm when stopped + jnb Flags1.MOTOR_SPINNING, t0_int_pwm_on_stopped + +IF MODE == 0 OR MODE == 2 ; Main or multi + jmp t0_int_pwm_on_execute +ENDIF +IF MODE == 1 ; Tail + ; Skip pwm on cycles for very low pwm + inc Pwm_On_Cnt ; Increment event counter + clr C + mov A, #5 ; Only skip for very low pwm + subb A, Current_Pwm_Limited ; Check skipping shall be done (for low pwm only) + jc t0_int_pwm_on_execute + + subb A, Pwm_On_Cnt ; Check if on cycle is to be skipped + jc t0_int_pwm_on_execute + + mov TL0, #120 ; Write start point for timer + mov A, Current_Pwm_Limited + jnz ($+5) + mov TL0, #0 ; Write start point for timer (long time for zero pwm) + jmp t0_int_pwm_on_exit_no_timer_update +ENDIF + +t0_int_pwm_on_stopped: + jmp t0_int_pwm_on_exit + + +t0_int_pwm_off: + jnb Flags1.DIRECT_STARTUP_PHASE, t0_int_pwm_off_start_checked + All_nFETs_Off ; Switch off all nfets early during direct start, for a smooth start +t0_int_pwm_off_start_checked: + ; Pwm off cycle + mov TL0, Current_Pwm_Limited ; Load new timer setting + ; Clear pwm on flag + clr Flags0.PWM_ON + ; 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 + mov A, Current_Pwm_Limited ; Load current pwm + cpl A ; Full pwm? + jnz ($+4) ; No - branch + ajmp t0_int_pwm_off_fullpower_exit ; Yes - exit + + inc Pwm_Off_Cnt ; Increment event counter + ; Do not execute pwm when stopped + jnb Flags1.MOTOR_SPINNING, t0_int_pwm_off_stopped + + ; If damped operation, set pFETs on in pwm_off + jb Flags2.PGM_PWMOFF_DAMPED, t0_int_pwm_off_damped ; Damped operation? + + ; Separate exit commands here for minimum delay + mov TL1, #0 ; Reset timer1 + pop ACC ; Restore preserved registers + pop PSW + All_nFETs_Off ; Switch off all nfets + setb EA ; Enable all interrupts + reti + +t0_int_pwm_off_stopped: + All_nFETs_Off ; Switch off all nfets + jmp t0_int_pwm_off_exit + +t0_int_pwm_off_damped: + setb Flags2.CURR_PWMOFF_DAMPED ; Set damped status + clr Flags2.CURR_PWMOFF_COMP_ABLE ; Set comparator unusable status + mov A, Damping_On + jz t0_int_pwm_off_do_damped ; Highest damping - apply damping always + + clr C + mov A, Pwm_Off_Cnt ; Is damped on number reached? + dec A + subb A, Damping_On + jc t0_int_pwm_off_do_damped ; No - apply damping + + clr Flags2.CURR_PWMOFF_DAMPED ; Set non damped status + setb Flags2.CURR_PWMOFF_COMP_ABLE ; Set comparator usable status + clr C + mov A, Pwm_Off_Cnt + subb A, Damping_Period ; Is damped period number reached? + jnc t0_int_pwm_off_clr_cnt ; Yes - Proceed + + jmp t0_int_pwm_off_exit ; No - Branch + +t0_int_pwm_off_clr_cnt: + mov Pwm_Off_Cnt, #0 ; Yes - clear counter + jmp t0_int_pwm_off_exit ; Not damped cycle - exit + +t0_int_pwm_off_do_damped: + ; Delay to allow nFETs to go off before pFETs are turned on (only in full damped mode) + jb Flags2.PGM_PWMOFF_DAMPED_LIGHT, t0_int_pwm_off_damped_light ; If damped light operation - branch + +; All_pFETs_Off ; Switch off all nfets +; mov A, #PFETON_DELAY +; djnz ACC, $ +; All_nFETs_On ; Switch on all pfets + jmp t0_int_pwm_off_exit + +t0_int_pwm_off_damped_light: +IF DAMPED_MODE_ENABLE == 1 + setb Flags2.CURR_PWMOFF_COMP_ABLE ; Set comparator usable status always for damped light mode on fully damped capable escs +ENDIF + All_nFETs_Off ; Switch off all nfets + mov A, Comm_Phase ; Turn on pfets according to commutation phase + jb ACC.2, t0_int_pwm_off_comm_4_5_6 + jb ACC.1, t0_int_pwm_off_comm_2_3 + +IF DAMPED_MODE_ENABLE == 0 + AnFET_On ; Comm phase 1 - turn on A +ELSE + mov A, #PFETON_DELAY + djnz ACC, $ + CnFET_On ; Comm phase 1 - turn on C +ENDIF + jmp t0_int_pwm_off_exit + +t0_int_pwm_off_comm_2_3: + jb ACC.0, t0_int_pwm_off_comm_3 +IF DAMPED_MODE_ENABLE == 0 + BnFET_On ; Comm phase 2 - turn on B +ELSE + mov A, #PFETON_DELAY + djnz ACC, $ + CnFET_On ; Comm phase 2 - turn on C +ENDIF + jmp t0_int_pwm_off_exit + +t0_int_pwm_off_comm_3: +IF DAMPED_MODE_ENABLE == 0 + CnFET_On ; Comm phase 3 - turn on C +ELSE + mov A, #PFETON_DELAY + djnz ACC, $ + BnFET_On ; Comm phase 3 - turn on B +ENDIF + jmp t0_int_pwm_off_exit + +t0_int_pwm_off_comm_4_5_6: + jb ACC.1, t0_int_pwm_off_comm_6 + jb ACC.0, t0_int_pwm_off_comm_5 + +IF DAMPED_MODE_ENABLE == 0 + AnFET_On ; Comm phase 4 - turn on A +ELSE + mov A, #PFETON_DELAY + djnz ACC, $ + BnFET_On ; Comm phase 4 - turn on B +ENDIF + jmp t0_int_pwm_off_exit + +t0_int_pwm_off_comm_5: +IF DAMPED_MODE_ENABLE == 0 + BnFET_On ; Comm phase 5 - turn on B +ELSE + mov A, #PFETON_DELAY + djnz ACC, $ + AnFET_On ; Comm phase 5 - turn on A +ENDIF + jmp t0_int_pwm_off_exit + +t0_int_pwm_off_comm_6: +IF DAMPED_MODE_ENABLE == 0 + CnFET_On ; Comm phase 6 - turn on C +ELSE + mov A, #PFETON_DELAY + djnz ACC, $ + AnFET_On ; Comm phase 6 - turn on A +ENDIF + +t0_int_pwm_off_exit: ; Exit from pwm off cycle + mov TL1, #0 ; Reset timer1 + pop ACC ; Restore preserved registers + pop PSW + All_nFETs_Off ; Switch off all nfets + setb EA ; Enable all interrupts + reti + +t0_int_pwm_off_fullpower_exit: ; Exit from pwm off cycle, leaving power on + pop ACC ; Restore preserved registers + pop PSW + setb EA ; Enable all interrupts + reti + + + +pwm_nofet_on: ; Dummy pwm on cycle + ajmp t0_int_pwm_on_exit + +pwm_afet_on: ; Pwm on cycle afet on (bfet off) + AnFET_on + BnFET_off + ajmp t0_int_pwm_on_exit + +pwm_bfet_on: ; Pwm on cycle bfet on (cfet off) + BnFET_on + CnFET_off + ajmp t0_int_pwm_on_exit + +pwm_cfet_on: ; Pwm on cycle cfet on (afet off) + CnFET_on + AnFET_off + ajmp t0_int_pwm_on_exit + +pwm_anfet_bpfet_on_fast: ; Pwm on cycle anfet on (bnfet off) and bpfet on (used in damped state 6) + ApFET_off + AnFET_on ; Switch nFETs + CpFET_off + BnFET_off + ajmp t0_int_pwm_on_exit +pwm_anfet_bpfet_on_safe: ; Pwm on cycle anfet on (bnfet off) and bpfet on (used in damped state 6) + ; Delay from pFETs are turned off (only in damped mode) until nFET is turned on (pFETs are slow) + ApFET_off + CpFET_off + mov A, #NFETON_DELAY ; Set full delay + djnz ACC, $ + AnFET_on ; Switch nFETs + BnFET_off + ajmp t0_int_pwm_on_exit + +pwm_anfet_cpfet_on_fast: ; Pwm on cycle anfet on (bnfet off) and cpfet on (used in damped state 5) + ApFET_off + AnFET_on ; Switch nFETs + BpFET_off + BnFET_off + ajmp t0_int_pwm_on_exit +pwm_anfet_cpfet_on_safe: ; Pwm on cycle anfet on (bnfet off) and cpfet on (used in damped state 5) + ; Delay from pFETs are turned off (only in damped mode) until nFET is turned on (pFETs are slow) + ApFET_off + BpFET_off + mov A, #NFETON_DELAY ; Set full delay + djnz ACC, $ + AnFET_on ; Switch nFETs + BnFET_off + ajmp t0_int_pwm_on_exit + +pwm_bnfet_cpfet_on_fast: ; Pwm on cycle bnfet on (cnfet off) and cpfet on (used in damped state 4) + BpFET_off + BnFET_on ; Switch nFETs + ApFET_off + CnFET_off + ajmp t0_int_pwm_on_exit +pwm_bnfet_cpfet_on_safe: ; Pwm on cycle bnfet on (cnfet off) and cpfet on (used in damped state 4) + ; Delay from pFETs are turned off (only in damped mode) until nFET is turned on (pFETs are slow) + BpFET_off + ApFET_off + mov A, #NFETON_DELAY ; Set full delay + djnz ACC, $ + BnFET_on ; Switch nFETs + CnFET_off + ajmp t0_int_pwm_on_exit + +pwm_bnfet_apfet_on_fast: ; Pwm on cycle bnfet on (cnfet off) and apfet on (used in damped state 3) + BpFET_off + BnFET_on ; Switch nFETs + CpFET_off + CnFET_off + ajmp t0_int_pwm_on_exit +pwm_bnfet_apfet_on_safe: ; Pwm on cycle bnfet on (cnfet off) and apfet on (used in damped state 3) + ; Delay from pFETs are turned off (only in damped mode) until nFET is turned on (pFETs are slow) + BpFET_off + CpFET_off + mov A, #NFETON_DELAY ; Set full delay + djnz ACC, $ + BnFET_on ; Switch nFETs + CnFET_off + ajmp t0_int_pwm_on_exit + +pwm_cnfet_apfet_on_fast: ; Pwm on cycle cnfet on (anfet off) and apfet on (used in damped state 2) + CpFET_off + CnFET_on ; Switch nFETs + BpFET_off + AnFET_off + ajmp t0_int_pwm_on_exit +pwm_cnfet_apfet_on_safe: ; Pwm on cycle cnfet on (anfet off) and apfet on (used in damped state 2) + ; Delay from pFETs are turned off (only in damped mode) until nFET is turned on (pFETs are slow) + CpFET_off + BpFET_off + mov A, #NFETON_DELAY ; Set full delay + djnz ACC, $ + CnFET_on ; Switch nFETs + AnFET_off + ajmp t0_int_pwm_on_exit + +pwm_cnfet_bpfet_on_fast: ; Pwm on cycle cnfet on (anfet off) and bpfet on (used in damped state 1) + CpFET_off + CnFET_on ; Switch nFETs + ApFET_off + AnFET_off + ajmp t0_int_pwm_on_exit +pwm_cnfet_bpfet_on_safe: ; Pwm on cycle cnfet on (anfet off) and bpfet on (used in damped state 1) + ; Delay from pFETs are turned off (only in damped mode) until nFET is turned on (pFETs are slow) + CpFET_off + ApFET_off + mov A, #NFETON_DELAY ; Set full delay + djnz ACC, $ + CnFET_on ; Switch nFETs + AnFET_off + ajmp t0_int_pwm_on_exit + +t0_int_pwm_on_exit: + ; Set timer for coming on cycle length + mov A, Current_Pwm_Limited ; Load current pwm + cpl A ; cpl is 255-x + mov TL0, A ; Write start point for timer + ; Set other variables + mov TL1, #0 ; Reset timer1 + mov Pwm_On_Cnt, #0 ; Reset pwm on event counter + setb Flags0.PWM_ON ; Set pwm on flag +t0_int_pwm_on_exit_no_timer_update: + ; Exit interrupt + pop ACC ; Restore preserved registers + pop PSW + setb EA ; Enable all interrupts + reti + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Timer2 interrupt routine +; +; No assumptions +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +t2_int: ; Happens every 128us for low byte and every 32ms for high byte + clr EA ; Disable all interrupts + push PSW ; Preserve registers through interrupt + push ACC + setb PSW.3 ; Select register bank 1 for interrupt routines + ; Clear low byte interrupt flag + clr TF2L ; Clear interrupt flag + ; Check RC pulse timeout counter + mov A, Rcp_Timeout_Cnt ; RC pulse timeout count zero? + jz t2_int_pulses_absent ; Yes - pulses are absent + + ; Decrement timeout counter (if PWM) +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) +; anl A, Flags3 ; Check pwm frequency flags +; jz t2_int_skip_start ; If no flag is set (PPM) - branch + +; dec Rcp_Timeout_Cnt ; No - decrement + ajmp t2_int_skip_start + +t2_int_pulses_absent: + ; Timeout counter has reached zero, pulses are absent + mov Temp1, #RCP_MIN ; RCP_MIN as default + mov Temp2, #RCP_MIN + Read_Rcp_Int ; Look at value of Rcp_In + jnb ACC.Rcp_In, ($+5) ; Is it high? + mov Temp1, #RCP_MAX ; Yes - set RCP_MAX + Rcp_Int_First ; Set interrupt trig to first again + Rcp_Clear_Int_Flag ; Clear interrupt flag + clr Flags2.RCP_EDGE_NO ; Set first edge flag + Read_Rcp_Int ; Look once more at value of Rcp_In + jnb ACC.Rcp_In, ($+5) ; Is it high? + mov Temp2, #RCP_MAX ; Yes - set RCP_MAX + clr C + mov A, Temp1 + subb A, Temp2 ; Compare the two readings of Rcp_In + jnz t2_int_pulses_absent ; Go back if they are not equal + + jnb Flags0.RCP_MEAS_PWM_FREQ, ($+6) ; Is measure RCP pwm frequency flag set? + + mov Rcp_Timeout_Cnt, #RCP_TIMEOUT ; Yes - set timeout count to start value +; mov Pgm_Pulse_Cnt, #0 ;清除PCA中断计数 +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) +; anl A, Flags3 ; Check pwm frequency flags +; jz t2_int_ppm_timeout_set ; If no flag is set (PPM) - branch + +; mov Rcp_Timeout_Cnt, #RCP_TIMEOUT ; For PWM, set timeout count to start value + +t2_int_ppm_timeout_set: + mov New_Rcp, Temp1 ; Store new pulse length + setb Flags2.RCP_UPDATED ; Set updated flag + + +t2_int_skip_start: + ; Check RC pulse skip counter + mov A, Rcp_Skip_Cnt + jz t2_int_skip_end ; If RC pulse skip count is zero - end skipping RC pulse detection + + ; Decrement skip counter (only if edge counter is zero) + dec Rcp_Skip_Cnt ; Decrement + ajmp t2_int_rcp_update_start + +t2_int_skip_end: +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) +; anl A, Flags3 ; Check pwm frequency flags +; jz t2_int_rcp_update_start ; If no flag is set (PPM) - branch + + ; Skip counter has reached zero, start looking for RC pulses again +; Rcp_Int_Enable ; Enable RC pulse interrupt +; Rcp_Clear_Int_Flag ; Clear interrupt flag + +t2_int_rcp_update_start: + ; Process updated RC pulse + jb Flags2.RCP_UPDATED, ($+5) ; Is there an updated RC pulse available? + ajmp t2_int_pwm_exit ; No - exit + + mov A, New_Rcp ; Load new pulse value + mov Temp1, A + clr Flags2.RCP_UPDATED ; Flag that pulse has been evaluated + ; Use a gain of 1.0625x for pwm input if not governor mode +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) +; anl A, Flags3 ; Check pwm frequency flags +; jz t2_int_pwm_min_run ; If no flag is set (PPM) - branch +;;***************************************************** +;; Closed loop mode 选择 +;;***************************************************** +;IF MODE <= 1 ; Main or tail +; mov Temp2, #Pgm_Gov_Mode ; Governor mode? +; cjne @Temp2, #4, t2_int_pwm_min_run; Yes - branch +;ENDIF +;IF MODE == 2 ; Multi +;;读取Closed loop mode +; mov Temp2, #Pgm_Gov_Mode ; Closed loop mode? +; cjne @Temp2, #4, t2_int_pwm_min_run; Yes - branch +;ENDIF + +; ; Limit the maximum value to avoid wrap when scaled to pwm range +;;Closed loop mode = 第1项 Off +; clr C +; mov A, Temp1 +; subb A, #240 ; 240 = (255/1.0625) Needs to be updated according to multiplication factor below +; jc t2_int_rcp_update_mult + +; mov A, #240 ; Set requested pwm to max +; mov Temp1, A + +;t2_int_rcp_update_mult: +; ; Multiply by 1.0625 (optional adjustment gyro gain) +; mov A, Temp1 +; swap A ; After this "0.0625" +; anl A, #0Fh +; add A, Temp1 +; mov Temp1, A +; ; Adjust tail gain +;;读取 Multi gain +; mov Temp2, #Pgm_Motor_Gain +; cjne @Temp2, #3, ($+5) ; Is gain 1? +; ajmp t2_int_pwm_min_run ; Yes - skip adjustment +;;Multi gain = 第2选项 +; clr C +; rrc A ; After this "0.5" +; clr C +; rrc A ; After this "0.25" +; mov Bit_Access_Int, @Temp2 ; (Temp2 has #Pgm_Motor_Gain) +; jb Bit_Access_Int.0, t2_int_rcp_gain_corr ; Branch if bit 0 in gain is set +;;Multi gain = 第4选项 +; clr C +; rrc A ; After this "0.125" + +;t2_int_rcp_gain_corr: +; jb Bit_Access_Int.2, t2_int_rcp_gain_pos ; Branch if bit 2 in gain is set +;;Multi gain = 第1选项 +; clr C +; xch A, Temp1 +; subb A, Temp1 ; Apply negative correction +; mov Temp1, A +; ajmp t2_int_pwm_min_run +;;Multi gain = 第5选项 +;t2_int_rcp_gain_pos: +; add A, Temp1 ; Apply positive correction +; mov Temp1, A +; jnc t2_int_pwm_min_run ; Above max? + +; mov A, #0FFh ; Yes - limit +; mov Temp1, A +;;Multi gain = 第3选项 +t2_int_pwm_min_run: + ; Limit minimum pwm + clr C + mov A, Temp1 + subb A, Pwm_Motor_Idle ; Is requested pwm lower than minimum? + jnc t2_int_pwm_update ; No - branch + + mov A, Pwm_Motor_Idle ; Yes - limit pwm to Pwm_Motor_Idle + mov Temp1, A + +t2_int_pwm_update: + ; Check if any startup phase flags are set + mov A, Flags1 + anl A, #((1 SHL SETTLE_PHASE)+(1 SHL STEPPER_PHASE)) + jnz t2_int_pwm_exit ; Exit if any startup phase set (pwm controlled by set_startup_pwm) + + ; Update requested_pwm + mov Requested_Pwm, Temp1 ; Set requested pwm + ; Limit pwm during direct start + jnb Flags1.DIRECT_STARTUP_PHASE, t2_int_current_pwm_update + + clr C + mov A, Requested_Pwm ; Limit pwm during direct start + subb A, Pwm_Limit + jc ($+7) + + mov Requested_Pwm, Pwm_Limit + ajmp t2_int_current_pwm_update + + mov A, Requested_Pwm + add A, #11 + mov Requested_Pwm, A + +t2_int_current_pwm_update: +IF MODE <= 1 ; Main or tail + mov Temp1, #Pgm_Gov_Mode ; Governor mode? + cjne @Temp1, #4, t2_int_pwm_exit ; Yes - branch +ENDIF +IF MODE == 2 ; Multi +;读取Closed loop mode + mov Temp1, #Pgm_Gov_Mode ; Closed loop mode? + cjne @Temp1, #1, t2_int_pwm_exit ; Yes - branch +ENDIF + + ; Update current pwm, with limited throttle change rate +;Closed loop mode = Off + clr C + mov A, Requested_Pwm + subb A, Current_Pwm ; Is requested pwm larger than current pwm? + jc t2_int_set_current_pwm ; No - proceed + + mov Temp1, #Pgm_Throttle_Rate_Decoded + subb A, @Temp1 ; Is difference larger than throttle change rate? + jc t2_int_set_current_pwm ; No - proceed + + mov A, Current_Pwm ; Increase current pwm by throttle change rate + add A, @Temp1 + mov Current_Pwm, A + jnc t2_int_current_pwm_done ; Is result above max? + + mov Current_Pwm, #0FFh ; Yes - limit + jmp t2_int_current_pwm_done + +t2_int_set_current_pwm: + mov Current_Pwm, Requested_Pwm ; Set equal as default +t2_int_current_pwm_done: +IF MODE == 1 ; Tail + ; If tail, then set current_pwm_limited + mov Current_Pwm_Limited, Current_Pwm ; Default not limited + clr C + mov A, Current_Pwm ; Check against limit + subb A, Pwm_Limit + jc ($+5) ; If current pwm below limit - branch + + mov Current_Pwm_Limited, Pwm_Limit ; Limit pwm +ENDIF +IF MODE == 2 ; Multi + ; If multi, then set current_pwm_limited + mov Current_Pwm_Limited, Current_Pwm ; Default not limited + clr C + mov A, Current_Pwm ; Check against limit + subb A, Pwm_Limit + jc ($+5) ; If current pwm below limit - branch + + mov Current_Pwm_Limited, Pwm_Limit ; Limit pwm +ENDIF + +t2_int_pwm_exit: + ; Check if high byte flag is set + jb TF2H, t2h_int + pop ACC ; Restore preserved registers + pop PSW + clr PSW.3 ; Select register bank 0 for main program routines + setb EA ; Enable all interrupts + reti + +t2h_int: + ; High byte interrupt (happens every 32ms) + clr TF2H ; Clear interrupt flag + mov Temp1, #GOV_SPOOLRATE ; Load governor spool rate + ; Check RC pulse timeout counter (used here for PPM only) + mov A, Rcp_Timeout_Cnt ; RC pulse timeout count zero? + jz t2h_int_rcp_stop_check ; Yes - do not decrement + + ; Decrement timeout counter (if PPM) +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) +; anl A, Flags3 ; Check pwm frequency flags +; jnz t2h_int_rcp_stop_check ; If a flag is set (PWM) - branch + + dec Rcp_Timeout_Cnt ; No flag set (PPM) - decrement + +t2h_int_rcp_stop_check: + ; Check RC pulse against stop value + clr C + mov A, New_Rcp ; Load new pulse value + subb A, #RCP_STOP +; subb A, #(RCP_STOP+2) ; Check if pulse is below stop value + jc t2h_int_rcp_stop + + ; RC pulse higher than stop value, reset stop counter + mov Rcp_Stop_Cnt, #0 ; Reset rcp stop counter + ajmp t2h_int_rcp_gov_pwm + +t2h_int_rcp_stop: + ; RC pulse less than stop value, increment stop counter + mov A, Rcp_Stop_Cnt ; Load rcp stop counter + inc A ; Check if counter is max + jz t2h_int_rcp_gov_pwm ; Branch if counter is equal to max + + inc Rcp_Stop_Cnt ; Increment stop counter + +t2h_int_rcp_gov_pwm: +IF MODE == 0 ; Main + mov Temp2, #Pgm_Gov_Mode ; Governor target by arm mode? + cjne @Temp2, #2, t2h_int_rcp_gov_by_setup ; No - branch + + mov A, Gov_Active ; Is governor active? + jz t2h_int_rcp_gov_by_tx ; No - branch (this ensures soft spoolup by tx) + + clr C + mov A, Requested_Pwm + subb A, #50 ; Is requested pwm below 20%? + jc t2h_int_rcp_gov_by_tx ; Yes - branch (this enables a soft spooldown) + + mov Requested_Pwm, Gov_Arm_Target ; Yes - load arm target + +t2h_int_rcp_gov_by_setup: + mov Temp2, #Pgm_Gov_Mode ; Governor target by setup mode? + cjne @Temp2, #3, t2h_int_rcp_gov_by_tx ; No - branch + + mov A, Gov_Active ; Is governor active? + jz t2h_int_rcp_gov_by_tx ; No - branch (this ensures soft spoolup by tx) + + clr C + mov A, Requested_Pwm + subb A, #50 ; Is requested pwm below 20%? + jc t2h_int_rcp_gov_by_tx ; Yes - branch (this enables a soft spooldown) +;读取Pgm_Gov_Setup_Target + mov Temp2, #Pgm_Gov_Setup_Target ; Gov by setup - load setup target + mov Requested_Pwm, @Temp2 + +t2h_int_rcp_gov_by_tx: + clr C + mov A, Governor_Req_Pwm + subb A, Requested_Pwm ; Is governor requested pwm equal to requested pwm? + jz t2h_int_rcp_gov_pwm_done ; Yes - branch + + jc t2h_int_rcp_gov_pwm_inc ; No - if lower, then increment + + dec Governor_Req_Pwm ; No - if higher, then decrement + ajmp t2h_int_rcp_gov_pwm_done + +t2h_int_rcp_gov_pwm_inc: + inc Governor_Req_Pwm ; Increment + +t2h_int_rcp_gov_pwm_done: + djnz Temp1, t2h_int_rcp_gov_pwm ; If not number of steps processed - go back + + inc Spoolup_Limit_Cnt ; Increment spoolup count + + jnz ($+4) ; Wrapped? + + dec Spoolup_Limit_Cnt ; Yes - decrement + + djnz Spoolup_Limit_Skip, t2h_int_rcp_exit ; Jump if skip count is not reached + + mov Spoolup_Limit_Skip, #1 ; Reset skip count. Default is fast spoolup + mov Temp1, #3 ; Default fast increase + + clr C + mov A, Spoolup_Limit_Cnt + subb A, #(3*MAIN_SPOOLUP_TIME) ; No spoolup until "30"*32ms + jc t2h_int_rcp_exit + + clr C + mov A, Spoolup_Limit_Cnt + subb A, #(10*MAIN_SPOOLUP_TIME) ; Slow spoolup until "100"*32ms + jnc t2h_int_rcp_limit_middle_ramp + + mov Temp1, #1 ; Slow initial spoolup + mov Spoolup_Limit_Skip, #3 + jmp t2h_int_rcp_set_limit + +t2h_int_rcp_limit_middle_ramp: + clr C + mov A, Spoolup_Limit_Cnt + subb A, #(15*MAIN_SPOOLUP_TIME) ; Faster spoolup until "150"*32ms + jnc t2h_int_rcp_set_limit + + mov Temp1, #1 ; Faster middle spoolup + mov Spoolup_Limit_Skip, #1 + +t2h_int_rcp_set_limit: + mov A, Pwm_Limit_Spoolup ; Increment spoolup pwm + clr C + add A, Temp1 + jnc t2h_int_rcp_no_limit ; If below 255 - branch + + mov Pwm_Limit_Spoolup, #0FFh + ajmp t2h_int_rcp_exit + +t2h_int_rcp_no_limit: + mov Pwm_Limit_Spoolup, A +ENDIF +IF MODE == 2 ; Multi + mov A, Pwm_Limit_Spoolup ; Increment spoolup pwm, for a 0.8 seconds spoolup + clr C + add A, #10 + jnc t2h_int_rcp_no_limit ; If below 255 - branch + + mov Pwm_Limit_Spoolup, #0FFh + ajmp t2h_int_rcp_exit + +t2h_int_rcp_no_limit: + mov Pwm_Limit_Spoolup, A +ENDIF + +t2h_int_rcp_exit: + pop ACC ; Restore preserved registers + pop PSW + clr PSW.3 ; Select register bank 0 for main program routines + setb EA ; Enable all interrupts + reti + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Timer3 interrupt routine +; +; No assumptions +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +t3_int: ; Used for commutation timing + clr EA ; Disable all interrupts + anl TMR3CN, #07Fh ; Clear interrupt flag + clr Flags0.T3_PENDING ; Flag that timer has wrapped + setb EA ; Enable all interrupts + reti + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; PCA interrupt routine +; +; No assumptions +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +pca_int: ; Used for RC pulse timing + clr EA ; Disable all interrupts + push PSW ; Preserve registers through interrupt + push ACC + push B + setb PSW.3 ; Select register bank 1 for interrupt routines + +; mov A, Pca_First_Int +; cjne A, #1, ($+5) +; inc Pgm_Pulse_Cnt + ; Get PCA0 capture values + mov Temp1, PCA0CPL0 + mov Temp2, PCA0CPH0 + ; Clear interrupt flag + Rcp_Clear_Int_Flag + ; Check which edge it is + jnb Flags2.RCP_EDGE_NO, ($+5) ; Is it a first edge trig? + ajmp pca_int_second_meas_pwm_freq ; No - branch to second + + Rcp_Int_Second ; Yes - set second edge trig + setb Flags2.RCP_EDGE_NO ; Set second edge flag + ; Read RC signal level + Read_Rcp_Int + ; Test RC signal level + jb ACC.Rcp_In, ($+5) ; Is it high? + ajmp pca_int_fail_minimum ; No - jump to fail minimum + + ; RC pulse was high, store RC pulse start timestamp + mov Rcp_Prev_Edge_L, Temp1 + mov Rcp_Prev_Edge_H, Temp2 + ajmp pca_int_exit ; Exit + +pca_int_fail_minimum: + ; Prepare for next interrupt + Rcp_Int_First ; Set interrupt trig to first again + Rcp_Clear_Int_Flag ; Clear interrupt flag + clr Flags2.RCP_EDGE_NO ; Set first edge flag +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) + mov A, #(1 SHL RCP_PWM_FREQ_1KHZ) + anl A, Flags3 ; Check pwm frequency flags + jnz ($+4) ; If a flag is set (PWM) - proceed + + ajmp pca_int_set_timeout ; If PPM - ignore trig as noise + + mov Temp1, #RCP_MIN ; Set RC pulse value to minimum + Read_Rcp_Int ; Test RC signal level again + jnb ACC.Rcp_In, ($+5) ; Is it high? + ajmp pca_int_set_timeout ; Yes - set new timeout and exit + + mov New_Rcp, Temp1 ; Store new pulse length + ajmp pca_int_set_timeout ; Set new timeout and exit + +pca_int_second_meas_pwm_freq: + ; Prepare for next interrupt + Rcp_Int_First ; Set first edge trig + clr Flags2.RCP_EDGE_NO ; Set first edge flag + ; Check if pwm frequency shall be measured + jb Flags0.RCP_MEAS_PWM_FREQ, ($+5) ; Is measure RCP pwm frequency flag set? + ajmp pca_int_fall ; No - skip measurements + + ; Set second edge trig only during pwm frequency measurement + Rcp_Int_Second ; Set second edge trig + Rcp_Clear_Int_Flag ; Clear interrupt flag + setb Flags2.RCP_EDGE_NO ; Set second edge flag + ; Store edge data to RAM + mov Rcp_Edge_L, Temp1 + mov Rcp_Edge_H, Temp2 + ; Calculate pwm frequency + clr C + mov A, Temp1 + subb A, Rcp_PrePrev_Edge_L + mov Temp1, A ;存低字节差 + mov A, Temp2 + subb A, Rcp_PrePrev_Edge_H + mov Temp2, A ;存高字节差 +; clr A +; mov Temp4, A + ; Check if pwm frequency is 8kHz + mov Temp3, #250 ; Set default period tolerance requirement +; clr C +; mov A, Temp1 +; subb A, #low(360) ; If below 180us, 8kHz pwm is assumed +; mov A, Temp2 +; subb A, #high(360) +; jnc pca_int_check_4kHz + +; clr A +; setb ACC.RCP_PWM_FREQ_8KHZ +; mov Temp4, A +; mov Temp3, #15 ; Set period tolerance requirement +; ajmp pca_int_restore_edge + +;pca_int_check_4kHz: +; ; Check if pwm frequency is 4kHz +; clr C +; mov A, Temp1 +; subb A, #low(720) ; If below 360us, 4kHz pwm is assumed +; mov A, Temp2 +; subb A, #high(720) +; jnc pca_int_check_2kHz + +; clr A +; setb ACC.RCP_PWM_FREQ_4KHZ +; mov Temp4, A +; mov Temp3, #30 ; Set period tolerance requirement +; ajmp pca_int_restore_edge + +;pca_int_check_2kHz: +; ; Check if pwm frequency is 2kHz +; clr C +; mov A, Temp1 +; subb A, #low(1440) ; If below 720us, 2kHz pwm is assumed +; mov A, Temp2 +; subb A, #high(1440) +; jnc pca_int_check_1kHz + +; clr A +; setb ACC.RCP_PWM_FREQ_2KHZ +; mov Temp4, A +; mov Temp3, #60 ; Set period tolerance requirement +; ajmp pca_int_restore_edge + +;pca_int_check_1kHz: + ; Check if pwm frequency is 1kHz + clr C + mov A, Temp1 + subb A, #low(2200) ; If below 1100us, 1kHz pwm is assumed + mov A, Temp2 + subb A, #high(2200) + jnc pca_int_restore_edge + + ajmp pca_int_set_timeout +; clr A +; setb ACC.RCP_PWM_FREQ_1KHZ +; mov Temp4, A +; mov Temp3, #120 ; Set period tolerance requirement + +pca_int_restore_edge: + ; Calculate difference between this period and previous period + clr C + mov A, Temp1 + subb A, Rcp_Prev_Period_L + mov Temp5, A + mov A, Temp2 + subb A, Rcp_Prev_Period_H + mov Temp6, A + ; Make positive + jnb ACC.7, pca_int_check_diff + mov A, Temp5 + cpl A + add A, #1 + mov Temp5, A + mov A, Temp6 + cpl A + mov Temp6, A + +pca_int_check_diff: + ; Check difference + mov Rcp_Period_Diff_Accepted, #0 ; Set not accepted as default + jnz pca_int_store_data ; Check if high byte is zero + + clr C + mov A, Temp5 + subb A, Temp3 ; Check difference + jnc pca_int_store_data + + mov Rcp_Period_Diff_Accepted, #1 ; Set accepted + +pca_int_store_data: + ; Store previous period + mov Rcp_Prev_Period_L, Temp1 + mov Rcp_Prev_Period_H, Temp2 + ; Restore edge data from RAM + mov Temp1, Rcp_Edge_L + mov Temp2, Rcp_Edge_H + ; Store pre previous edge + mov Rcp_PrePrev_Edge_L, Temp1 + mov Rcp_PrePrev_Edge_H, Temp2 + +pca_int_fall: + clr A + mov Temp4, A + ; RC pulse edge was second, calculate new pulse length + clr C + mov A, Temp1 + subb A, Rcp_Prev_Edge_L + mov Temp1, A + mov A, Temp2 + subb A, Rcp_Prev_Edge_H + mov Temp2, A + + jb Flags0.RCP_MEAS_PWM_FREQ, rcp_skip_oneshot ;........................ + mov A, Temp2 + mov Temp6, A + mov A, Temp1 + mov Temp5, A + + ; Check if 270us or above (in order to ignore false pulses) + clr C + mov A, Temp5 + subb A, #28 + mov A, Temp6 + subb A, #2 + jc pca_int_ppm_below_full_range_oneshot + ajmp rcp_skip_oneshot + +pca_int_ppm_below_full_range_oneshot: + ; Check if below 110us (in order to ignore false pulses) + mov A, Temp6 + jnz pca_int_ppm_check_full_range + clr C + mov A, Temp5 + subb A,#220 + jnc pca_int_ppm_check_full_range + +rcp_skip_oneshot: ; ...................................... + +; jnb Flags3.RCP_PWM_FREQ_8KHZ, ($+5) ; Is RC input pwm frequency 8kHz? +; ajmp pca_int_pwm_divide_done ; Yes - branch forward + +; jnb Flags3.RCP_PWM_FREQ_4KHZ, ($+5) ; Is RC input pwm frequency 4kHz? +; ajmp pca_int_pwm_divide ; Yes - branch forward + mov OS_FLAG, #0 ;................................... + mov A, Temp2 ; No - 2kHz. Divide by 2 again + clr C + rrc A + mov Temp2, A + mov A, Temp1 + rrc A + mov Temp1, A + +; jnb Flags3.RCP_PWM_FREQ_2KHZ, ($+5) ; Is RC input pwm frequency 2kHz? +; ajmp pca_int_pwm_divide ; Yes - branch forward + + mov A, Temp2 ; No - 1kHz. Divide by 2 again + clr C + rrc A + mov Temp2, A + mov A, Temp1 + rrc A + mov Temp1, A + + clr C + mov A, Temp1 + subb A, #250 ; If below 500us, + mov A, Temp2 + subb A, #0 + jnc ($+9) + + mov A, Initial_Arm + cjne A, #1, ($+5) + + ajmp pca_int_pwm_divide + +; jnb Flags3.RCP_PWM_FREQ_1KHZ, ($+5) ; Is RC input pwm frequency 1kHz? +; ajmp pca_int_pwm_divide ; Yes - branch forward + + mov A, Temp2 ; No - PPM. Divide by 2 (to bring range to 256) and move to Temp5/6 + clr C + rrc A + mov Temp6, A + mov A, Temp1 + rrc A + mov Temp5, A + ; Skip range limitation if pwm frequency measurement + jb Flags0.RCP_MEAS_PWM_FREQ, pca_int_ppm_check_full_range + + ; Check if 2160us or above (in order to ignore false pulses) + clr C + mov A, Temp5 ; Is pulse 2160us or higher? + subb A, #28 + mov A, Temp6 + subb A, #2 + jc ($+5) ; No - proceed + + ljmp pca_int_set_timeout ; Yes - ignore pulse + + ; Check if below 800us (in order to ignore false pulses) + mov A, Temp6 + jnz pca_int_ppm_check_full_range + + clr C + mov A, Temp5 ; Is pulse below 800us? + subb A, #200 + jnc pca_int_ppm_check_full_range ; No - proceed + + jmp pca_int_exit ; Yes - ignore pulse + +pca_int_ppm_check_full_range: + ; Calculate "1000us" plus throttle minimum + mov A, #0 ; Set 1000us as default minimum + jb Flags3.FULL_THROTTLE_RANGE, pca_int_ppm_calculate ; Check if full range is chosen + + mov Temp1, #Pgm_Ppm_Min_Throttle ; Min throttle value is in 4us units + mov A, @Temp1 + +pca_int_ppm_calculate: + add A, #250 + mov Temp7, A + clr A + addc A, #0 + mov Temp8, A + + clr C + mov A, Temp5 ; Subtract minimum + subb A, Temp7 + mov Temp5, A + mov A, Temp6 + subb A, Temp8 + mov Temp6, A + jnc pca_int_ppm_neg_checked ; Is result negative? + + mov Temp1, #RCP_MIN ; Yes - set to minimum + mov Temp2, #0 + ajmp pca_int_pwm_divide_done + +pca_int_ppm_neg_checked: + clr C ; Check that RC pulse is within legal range (1000+4*255=2020) + mov A, Temp5 + subb A, #RCP_MAX + mov A, Temp6 + subb A, #0 + jc pca_int_ppm_max_checked + + mov Temp1, #RCP_MAX + mov Temp2, #0 + ajmp pca_int_pwm_divide_done + +pca_int_ppm_max_checked: + mov A, Temp5 ; Multiply throttle value by gain + mov B, Ppm_Throttle_Gain + mul AB + xch A, B + mov C, B.7 ; Multiply result by 2 (unity gain is 128) + rlc A + mov Temp1, A ; Transfer to Temp1/2 + mov Temp2, #0 + jc pca_int_ppm_limit_after_mult + + jmp pca_int_limited + +pca_int_ppm_limit_after_mult: + mov Temp1, #RCP_MAX + mov Temp2, #0 + jmp pca_int_limited + +pca_int_pwm_divide: + mov A, OS_FLAG + jnz pca_int_pwm_divide_done ;............................. +;480us + clr C + mov A, Temp1 + subb A, #240 + mov A, Temp2 + subb A, #0 + jnc pca_int_set_timeout +;40us + clr C + mov A, Temp1 + subb A, #20 + mov A, Temp2 + subb A, #0 + jc pca_int_set_timeout + + clr A + setb ACC.RCP_PWM_FREQ_1KHZ + mov Temp4, A +; mov Rcp_Period_Diff_Accepted, #1 + ajmp pca_int_limited + +pca_int_pwm_divide_done: + ; Check that RC pulse is within legal range + clr C + mov A, Temp1 + subb A, #RCP_MAX + mov A, Temp2 + subb A, #0 + jc pca_int_limited + + mov Temp1, #RCP_MAX + +pca_int_limited: + ; RC pulse value accepted + mov New_Rcp, Temp1 ; Store new pulse length + setb Flags2.RCP_UPDATED ; Set updated flag +; jb Flags0.RCP_MEAS_PWM_FREQ, ($+5) ; Is measure RCP pwm frequency flag set? +; ajmp pca_int_set_timeout ; No - skip measurements + +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) + mov A, #(1 SHL RCP_PWM_FREQ_1KHZ) + cpl A + anl A, Flags3 ; Clear all pwm frequency flags + orl A, Temp4 ; Store pwm frequency value in flags + mov Flags3, A + +pca_int_set_timeout: + mov Rcp_Timeout_Cnt, #RCP_TIMEOUT ; Set timeout count to start value +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) + mov A, #(1 SHL RCP_PWM_FREQ_1KHZ) + anl A, Flags3 ; Check pwm frequency flags + jnz pca_int_ppm_timeout_set ; If a flag is set - branch + + mov Rcp_Timeout_Cnt, #RCP_TIMEOUT_PPM ; No flag set means PPM. Set timeout count + +pca_int_ppm_timeout_set: +; jnb Flags0.RCP_MEAS_PWM_FREQ, ($+5) ; Is measure RCP pwm frequency flag set? +; ajmp pca_int_exit ; Yes - exit + +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) +; anl A, Flags3 ; Check pwm frequency flags +; jz pca_int_exit ; If no flag is set (PPM) - branch + +; Rcp_Int_Disable ; Disable RC pulse interrupt + +pca_int_exit: ; Exit interrupt routine + mov Rcp_Skip_Cnt, #RCP_SKIP_RATE ; Load number of skips +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) + mov A, #(1 SHL RCP_PWM_FREQ_1KHZ) + anl A, Flags3 ; Check pwm frequency flags + jnz ($+5) ; If a flag is set (PWM) - branch + + mov Rcp_Skip_Cnt, #10 ; Load number of skips + + pop B ; Restore preserved registers + pop ACC + pop PSW + clr PSW.3 ; Select register bank 0 for main program routines + setb EA ; Enable all interrupts + reti + + + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Wait xms ~(x*4*250) (Different entry points) +; +; No assumptions +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +wait1ms: + mov Temp2, #1 + ajmp waitxms_o + +wait3ms: + mov Temp2, #3 + ajmp waitxms_o + +wait10ms: + mov Temp2, #10 + ajmp waitxms_o + +wait30ms: + mov Temp2, #30 + ajmp waitxms_o + +wait100ms: + mov Temp2, #100 + ajmp waitxms_o + +wait200ms: + mov Temp2, #200 + ajmp waitxms_o + +waitxms_o: ; Outer loop + mov Temp1, #23 +waitxms_m: ; Middle loop + clr A + djnz ACC, $ ; Inner loop (42.7us - 1024 cycles) + djnz Temp1, waitxms_m + djnz Temp2, waitxms_o + ret +;**;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Wait 1 second routine +; +; No assumptions +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +wait1s: + mov Temp5, #5 +wait1s_loop: + call wait200ms + djnz Temp5, wait1s_loop + ret + + +;**;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Success beep routine +; +; No assumptions +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +success_beep: + clr EA ; Disable all interrupts + call beep_f1 + call beep_f2 + call beep_f3 +; call beep_f4 + call wait10ms + call beep_f1 + call beep_f2 + call beep_f3 +; call beep_f4 +; call wait10ms +; call beep_f1 +; call beep_f2 +; call beep_f3 +; call beep_f4 + setb EA ; Enable all interrupts + ret + + +;**;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Success beep inverted routine +; +; No assumptions +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +success_beep_inverted: + clr EA ; Disable all interrupts +; call beep_f4 + call beep_f3 + call beep_f2 + call beep_f1 + call wait10ms +; call beep_f4 + call beep_f3 + call beep_f2 + call beep_f1 + call wait10ms +; call beep_f4 +; call beep_f3 +; call beep_f2 +; call beep_f1 + setb EA ; Enable all interrupts + ret + + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Function and parameter value beep routine +; +; No assumptions +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +function_paraval_beep: + mov Temp7, Tx_Pgm_Func_No ; Function no + mov Temp8, Tx_Pgm_Paraval_No ; Parameter value no + clr EA ; Disable all interrupts +;//////function_beep: +;////// call beep_f1 +;////// call beep_f1 +;////// call beep_f1 +;////// call wait10ms +;////// djnz Temp7, function_beep +;//////paraval_beep: +;////// call beep_f4 +;////// call wait10ms +;////// djnz Temp8, paraval_beep +;function_beep: + jnb Flags0.PROGRAM_FUNC_FLAG,paraval_beep ;跳到参数选项叫 +function_no_6: + clr C + mov A,Temp7 + subb A,#5 + jc function_below_beep + jz function_beep + mov Temp7, A +function_beep: + call beep_f3 + call beep_f3 + call beep_f3 + call beep_f3 + call beep_f3 + call wait30ms + + cjne Temp7,#5,($+5) + ajmp fun_par_end + + +function_below_beep: + call beep_f1 + call beep_f1 + call beep_f1 + call wait100ms + djnz Temp7, function_below_beep + ajmp fun_par_end + +paraval_beep: + clr A + mov Temp7,A + + clr C + mov A,Temp8 + subb A,#10 ;参数数 - 10 + jc paraval_no_7 ;<10 + inc Temp7 ;>=10 + inc Temp7 + jz paraval_below_beep + mov Temp8,A ;>=存差 + ajmp paraval_below_beep + +paraval_no_7: + clr C + mov A,Temp8 + subb A,#5 + jc paraval_no_below + inc Temp7 + jz paraval_below_beep + mov Temp8,A ;>=存差 + +paraval_below_beep: + call beep_f2 + call beep_f2 + call beep_f2 + call wait30ms + djnz Temp7,paraval_below_beep + cjne Temp8,#10,($+5) + ajmp fun_par_end + cjne Temp8,#5,($+5) + ajmp fun_par_end + +paraval_no_below: + call wait100ms + call beep_f4 + call wait100ms + djnz Temp8, paraval_no_below +fun_par_end: + setb EA ; Enable all interrupts + + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; 接收一个数据（一个字节） +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +receive_a_byte: + mov Temp8,#0 + mov Commu_Data_Buffer, #0 + mov Temp4, #8 +wait_high: + + Read_Rcp_Int + ; Test RC signal level + jb ACC.Rcp_In,start_t2 + + jnb TF1, wait_high + +; setb Flags3.ERRO_DATA +; ajmp receive_exit + +start_t2: + mov TL0, #00h + mov TH0, #00h + setb TR0 ;启动T0 +wait_low: + Read_Rcp_Int + ; Test RC signal level + jnb ACC.Rcp_In, measure_wide + + jnb TF1, wait_low + +; setb Flags3.ERRO_DATA +; ajmp receive_exit + +measure_wide: + clr TR0 ;停止T0 +; mov Temp1, TL0 +; mov Temp2, TH0 + +; mov New_Rcp, Temp1 + mov New_Rcp, TL0 + clr C + mov A, New_Rcp + subb A, #20 ;40us + jc receive_a_erro_byte ;<80us 错误字节 + + clr C + mov A, New_Rcp + subb A, #78 ;156us + jnc receive_bit_one ;>160us 进行位1判断 + + mov A, Temp8 ;取0 + rl A + mov Temp8, A + ajmp receive_a_byte_exit + +receive_bit_one: + clr C + mov A, New_Rcp + subb A, #102 ;240us + jc receive_a_erro_byte ;160< New_Rcp <240us 错误字节 + + clr C + mov A, New_Rcp + subb A, #204 ;400us + jnc receive_a_erro_byte ;>400us 错误字节 + + mov A, Temp8 ;取1 + rl A + inc A + mov Temp8, A + ajmp receive_a_byte_exit + +receive_a_erro_byte: + setb Flags3.ERRO_DATA + +receive_a_byte_exit: + jnb TF1, ($+7) + + setb Flags3.ERRO_DATA + ajmp ($+6) + + djnz Temp4, wait_high + mov Commu_Data_Buffer, Temp8 +receive_exit: + ret + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; 接收一组数据 +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +receive_bytes: + mov Temp5, #48 + mov Temp2, #Tag_Temporary_Storage ; Set RAM address + mov Commu_Data_Buffer, #0 +wait_receive_bytes: + call receive_a_byte + mov @Temp2, Commu_Data_Buffer + inc Temp2 + djnz Temp5, wait_receive_bytes + + ret + +store_tags: +; call store_tags + mov Temp2, #Tag_Temporary_Storage ; Set RAM address + mov Temp5, #48 +store_tag: + cjne Temp5, #45, store_two + mov Temp1, #Pgm_Gov_Mode + jmp store_eep +store_two: + cjne Temp5, #44, store_three + mov Temp1, #Pgm_Low_Voltage_Lim + jmp store_eep +store_three: + cjne Temp5, #43, store_four + mov Temp1, #Pgm_Low_Voltage_Ctl + jmp store_eep +store_four: + cjne Temp5, #40, store_five + mov Temp1, #Pgm_Startup_Pwr + jmp store_eep +store_five: + cjne Temp5, #39, store_six + mov Temp1, #Pgm_Pwm_Freq + jmp store_eep +store_six: + cjne Temp5, #28, store_seven + mov Temp1, #Pgm_Comm_Timing + jmp store_eep +store_seven: + cjne Temp5, #27, store_next + mov Temp1, #Pgm_Damping_Force + +store_eep: + mov Commu_Data_Buffer, @Temp2 +;setb P1.7 + mov @Temp1, Commu_Data_Buffer + + call erase_and_store_all_in_eeprom + clr EA + +store_next: + inc Temp2 + djnz Temp5, store_tag + + ret + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; 发送一个数据（一个字节） +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +send_a_byte: + clr EA + setb TR0 ;启动T0 + mov Temp4, #8 + mov A, Commu_Data_Buffer ;读取要发送的数据 +wait_send_a_byte: + setb P0.7 ;发送高电平 + +; mov A, Commu_Data_Buffer ;读取要发送的数据 + jnb ACC.7, low_value ;($+8) + + mov TL0, #067h ;发送1脉宽长 + ajmp high_value ;($+5) + +low_value: + mov TL0, #0CDh ;发送0脉宽长 + +high_value: + mov TH0, #0FFh + + clr TF0 ;清T0溢出标志 + jnb TF0, $ ;等待T0溢出 高电平发送完成 + + clr P0.7 ;发送低电平 + mov TL0, #0CDh ; + mov TH0, #0FFh + + clr TF0 ;清T0溢出标志 + jnb TF0, $ ;等待T0溢出 低电平发送完成 + + rl A ;发送下一位 +; mov Commu_Data_Buffer, A + djnz Temp4, wait_send_a_byte + + ret + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; 发送一组数据 +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +send_bytes: + clr P0.7 +; mov A, P0MDOUT +; orl A, #080h + mov P0MDOUT, #80h ;设置成推挽输出 + mov Temp5, #48 +; mov Temp2, #Tag_Temporary_Storage ; Set RAM address + mov Temp1, #Pgm_Fir_Key +wait_send_bytes: + mov Commu_Data_Buffer, @Temp1 + inc Temp1 + + call send_a_byte ;逐个字节发送 + + djnz Temp5, wait_send_bytes + +; mov A, P0MDOUT +; anl A, #07Fh + mov P0MDOUT, #7Fh ;设置成漏极输出 + setb P0.7 + + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; 编程卡校验和 +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +get_commu_buffers_sum: + mov Commu_Sum, #0 + mov Temp5, #48 + mov Temp2, #Tag_Temporary_Storage ; Set RAM address +wait_get_commu_buffers_sum: + mov A, @Temp2 + clr C + addc A, Commu_Sum + mov Commu_Sum, A + inc Temp2 + djnz Temp5, wait_get_commu_buffers_sum + + ret + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; 清零校验和 +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +set_commu_sum_to_zero: + mov Temp8, #0 + mov Temp5, #47 + mov Temp2, #Tag_Temporary_Storage ; Set RAM address +wait_set_commu_sum_to_zero: + mov A, @Temp2 + clr C + addc A, Temp8 + mov Temp8, A + inc Temp2 + + djnz Temp5, wait_set_commu_sum_to_zero + + cpl A + inc A + mov @Temp2, A ;写最后一个字节 + + ret + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; 读取EEPROM参数 +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +;check_parameters: +; mov Temp5, #48 ; Number of tags +; mov Temp2, #Tag_Temporary_Storage ; Set RAM address +; mov Temp1, #Bit_Access +; mov DPTR, #Eep_ESC_Layout ; Set flash address +;read_eep: +; call read_eeprom_byte +; mov A, Bit_Access +; mov @Temp2, A ; Write to RAM +; inc Temp2 +; inc DPTR +; djnz Temp5, read_eep + +; call get_commu_buffers_sum + +; mov A, Commu_Sum +; jnz set_parameters_default +;;读取首字节 +; mov Temp2, #Tag_Temporary_Storage ; Set RAM address +; mov Temp1, #Bit_Access +; mov DPTR, #Eep_ESC_Layout ; Set flash address +; call read_eeprom_byte +; mov A, Bit_Access +; cjne A, #66h, set_parameters_default +;;设置默认参数 +;set_parameters_default: +; mov Temp5, #48 +; mov Temp2, #Tag_Temporary_Storage ; Set RAM address +; mov DPTR, #FINISH_KEYWORD_TABLE +;wait_set_parameters_default: +; clr A +; movc A, @A+DPTR +; mov @Temp2, A +; inc Temp2 +; inc DPTR +; djnz Temp5, wait_set_parameters_default + +; call set_commu_sum_to_zero + +; ret + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; 编程卡编程函数 +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +program_by_card: +; clr EA + mov Commu_Data_Buffer, #0 + call wait10ms + clr Flags3.ERRO_DATA ;清错误字节标志 + + mov TL1, #00h + mov TH1, #00h + setb TR1 ; 启动T1 + clr TF1 ;清T1溢出标志 + + call receive_a_byte ;读取一个字节（关键字） + + jb Flags3.ERRO_DATA, program_by_card ;接收到错误字节，则重新接收 + + mov A, Commu_Data_Buffer ;读接收到的字节 + cjne A, #05h, receive_next_keyword ;判断是否关键字 0x05 + call wait3ms + + call send_bytes + + ljmp program_by_card + +receive_next_keyword: + + cjne A, #09h, program_by_card ;判断是否关键字 0x09 + mov Commu_Data_Buffer, #0 + mov TL1, #00h + mov TH1, #00h + setb TR1 ;启动T1 + clr TF1 ;清T1溢出标志 + + call receive_bytes ;读取一组数据 + + call get_commu_buffers_sum + mov A, Commu_Sum ;读取校验和 + jnz program_by_card ;判断校验和 + + mov Temp2, #Tag_Temporary_Storage ; Set RAM address + mov A, @Temp2 ;读取首字节 + cjne A, #66h, program_by_card ;判断首字节 + + call wait1ms + clr P0.7 +; mov A, P0MDOUT +; orl A, #080h ;设置成推挽输出 + mov P0MDOUT, #080h + mov Commu_Data_Buffer, #088h + + call send_a_byte ;逐个字节发送 +; mov A, P0MDOUT +; anl A, #07Fh ;设置成漏极输出 + mov P0MDOUT, #07Fh + setb P0.7 + call store_tags ;存一组数据 +; call erase_and_store_all_in_eeprom +; clr EA +;clr P1.7 + ljmp program_by_card + + ret + + + + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Beeper routines (4 different entry points) +; +; No assumptions +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +beep_f1: ; Entry point 1, load beeper frequency 1 settings + mov Temp3, #18 ; Off wait loop length + mov Temp4, #100 ; Number of beep pulses Mos开关次数 + ajmp beep + +beep_f2: ; Entry point 2, load beeper frequency 2 settings + mov Temp3, #14 + mov Temp4, #120 ;Mos开关次数 + ajmp beep + +beep_f3: ; Entry point 3, load beeper frequency 3 settings + mov Temp3, #12 + mov Temp4, #160 ;Mos开关次数 + ajmp beep + +beep_f4: ; Entry point 4, load beeper frequency 4 settings + mov Temp3, #9 + mov Temp4, #180 ;Mos开关次数 + ajmp beep + +beep: ; Beep loop start + mov Temp5, Current_Pwm_Limited ; Store value + mov Current_Pwm_Limited, #1 ; Set to a nonzero value + mov Temp2, #2 ; Must be an even number (or direction will change) +beep_onoff: + cpl Flags3.PGM_DIR_REV ; Toggle between using A fet and C fet 切换A、C up导通 + clr A + BpFET_off ; BpFET off + djnz ACC, $ ; Allow some time after pfet is turned off + BnFET_on ; BnFET on (in order to charge the driver of the BpFET) + djnz ACC, $ ; Let the nfet be turned on a while + BnFET_off ; BnFET off again + djnz ACC, $ ; Allow some time after nfet is turned off + BpFET_on ; BpFET on + djnz ACC, $ ; Allow some time after pfet is turned on + ; Turn on nfet + AnFET_on ; AnFET on + mov A, Beep_Strength + djnz ACC, $ + ; Turn off nfet + AnFET_off ; AnFET off + mov A, #100 ; 25祍 off + djnz ACC, $ + djnz Temp2, beep_onoff + ; Copy variable + mov A, Temp3 + mov Temp1, A +beep_off: ; Fets off loop + djnz ACC, $ + djnz Temp1, beep_off + +beep_recharge_done: + djnz Temp4, beep + BpFET_off ; BpFET off + mov Current_Pwm_Limited, Temp5 ; Restore value + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Division 16bit unsigned by 16bit unsigned +; +; Dividend shall be in Temp2/Temp1, divisor in Temp4/Temp3 +; Result will be in Temp2/Temp1 +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +div_u16_by_u16: + clr C + mov Temp5, #0 + mov Temp6, #0 + mov B, #0 +div_u16_by_u16_div1: + inc B ; Increment counter for each left shift + mov A, Temp3 ; Shift left the divisor + rlc A + mov Temp3, A + mov A, Temp4 + rlc A + mov Temp4, A + jnc div_u16_by_u16_div1 ; Repeat until carry flag is set from high-byte +div_u16_by_u16_div2: + mov A, Temp4 ; Shift right the divisor + rrc A + mov Temp4, A + mov A, Temp3 + rrc A + mov Temp3, A + clr C + mov A, Temp2 ; Make a safe copy of the dividend + mov Temp8, A + mov A, Temp1 + mov Temp7, A + mov A, Temp1 ; Move low-byte of dividend into accumulator + subb A, Temp3 ; Dividend - shifted divisor = result bit (no factor, only 0 or 1) + mov Temp1, A ; Save updated dividend + mov A, Temp2 ; Move high-byte of dividend into accumulator + subb A, Temp4 ; Subtract high-byte of divisor (all together 16-bit substraction) + mov Temp2, A ; Save updated high-byte back in high-byte of divisor + jnc div_u16_by_u16_div3 ; If carry flag is NOT set, result is 1 + mov A, Temp8 ; Otherwise result is 0, save copy of divisor to undo subtraction + mov Temp2, A + mov A, Temp7 + mov Temp1, A +div_u16_by_u16_div3: + cpl C ; Invert carry, so it can be directly copied into result + mov A, Temp5 + rlc A ; Shift carry flag into temporary result + mov Temp5, A + mov A, Temp6 + rlc A + mov Temp6,A + djnz B, div_u16_by_u16_div2 ;Now count backwards and repeat until "B" is zero + mov A, Temp6 ; Move result to Temp2/Temp1 + mov Temp2, A + mov A, Temp5 + mov Temp1, A + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Multiplication 16bit signed by 8bit unsigned +; +; Multiplicand shall be in Temp2/Temp1, multiplicator in Temp3 +; Result will be in Temp2/Temp1. Result will divided by 16 +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +mult_s16_by_u8_div_16: + mov A, Temp1 ; Read input to math registers + mov B, Temp2 + mov Bit_Access, Temp3 + setb PSW.4 ; Select register bank 2 for math routines + mov Temp1, A ; Store in math registers + mov Temp2, B + mov Temp4, #0 ; Set sign in Temp4 and test sign + jnb B.7, mult_s16_by_u8_positive + + mov Temp4, #0FFh + cpl A + add A, #1 + mov Temp1, A + mov A, Temp2 + cpl A + addc A, #0 + mov Temp2, A +mult_s16_by_u8_positive: + mov A, Temp1 ; Multiply LSB with multiplicator + mov B, Bit_Access + mul AB + mov Temp6, B ; Place MSB in Temp6 + mov Temp1, A ; Place LSB in Temp1 (result) + mov A, Temp2 ; Multiply MSB with multiplicator + mov B, Bit_Access + mul AB + mov Temp8, B ; Place in Temp8/7 + mov Temp7, A + mov A, Temp6 ; Add up + add A, Temp7 + mov Temp2, A + mov A, #0 + addc A, Temp8 + mov Temp3, A + mov Temp5, #4 ; Set number of divisions +mult_s16_by_u8_div_loop: + clr C ; Rotate right + mov A, Temp3 + rrc A + mov Temp3, A + mov A, Temp2 + rrc A + mov Temp2, A + mov A, Temp1 + rrc A + mov Temp1, A + djnz Temp5, mult_s16_by_u8_div_loop + + mov B, Temp4 ; Test sign + jnb B.7, mult_s16_by_u8_exit + + mov A, Temp1 + cpl A + add A, #1 + mov Temp1, A + mov A, Temp2 + cpl A + addc A, #0 + mov Temp2, A + +mult_s16_by_u8_exit: + mov A, Temp1 ; Store output + mov B, Temp2 + clr PSW.4 ; Select normal register bank + mov Temp1, A + mov Temp2, B + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Calculate governor routines +; +; No assumptions +; +; Governs headspeed based upon the Comm_Period4x variable and pwm +; The governor task is split into several routines in order to distribute processing time +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; First governor routine - calculate governor target +IF MODE <= 1 ; Main or tail +calc_governor_target: + mov Temp1, #Pgm_Gov_Mode ; Governor mode? + cjne @Temp1, #4, governor_speed_check ; Yes + jmp calc_governor_target_exit ; No + +governor_speed_check: + ; Stop governor for stop RC pulse + clr C + mov A, New_Rcp ; Check RC pulse against stop value + subb A, #RCP_STOP ; Is pulse below stop value? + jc governor_deactivate ; Yes - deactivate + + ; Skip speed check if governor is alrady active + mov A, Gov_Active + jnz governor_target_calc + + ; Check speed (do not run governor for low speeds) + mov Temp1, #05h ; Default high range activation limit value (~62500 eRPM) + clr C + mov Temp2, #Pgm_Gov_Range + cjne @Temp2, #2, ($+5) + + mov Temp1, #12h ; Low range activation limit value (~17400 eRPM) + + clr C + mov A, Comm_Period4x_L + subb A, #00h + mov A, Comm_Period4x_H + subb A, Temp1 + jc governor_activate ; If speed above min limit - run governor + +governor_deactivate: + mov Current_Pwm, Requested_Pwm ; Set current pwm to requested + clr A + mov Gov_Target_L, A ; Set target to zero + mov Gov_Target_H, A + mov Gov_Integral_L, A ; Set integral to zero + mov Gov_Integral_H, A + mov Gov_Integral_X, A + mov Gov_Active, A + jmp calc_governor_target_exit + +governor_activate: + mov Gov_Active, #1 +governor_target_calc: + ; Governor calculations + clr C + mov Temp2, #Pgm_Gov_Range + mov A, @Temp2 ; Check high or low range (Temp2 has #Pgm_Gov_Range) + subb A, #2 + jz calc_governor_target_low + + mov A, Governor_Req_Pwm ; Load governor requested pwm + cpl A ; Calculate 255-pwm (invert pwm) + ; Calculate comm period target (1 + 2*((255-Requested_Pwm)/256) - 0.25) + rlc A ; Msb to carry + rlc A ; To bit0 + mov Temp2, A ; Now 1 lsb is valid for H + rrc A + mov Temp1, A ; Now 7 msbs are valid for L + mov A, Temp2 + anl A, #01h ; Calculate H byte + inc A ; Add 1 + mov Temp2, A + mov A, Temp1 + anl A, #0FEh ; Calculate L byte + clr C + subb A, #40h ; Subtract 0.25 + mov Temp1, A + mov A, Temp2 + subb A, #0 + mov Temp2, A + jmp calc_governor_store_target + +calc_governor_target_low: + mov A, Governor_Req_Pwm ; Load governor requested pwm + cpl A ; Calculate 255-pwm (invert pwm) + ; Calculate comm period target (2 + 8*((255-Requested_Pwm)/256) - 0.25) + rlc A ; Msb to carry + rlc A ; To bit0 + rlc A ; To bit1 + rlc A ; To bit2 + mov Temp2, A ; Now 3 lsbs are valid for H + rrc A + mov Temp1, A ; Now 4 msbs are valid for L + mov A, Temp2 + anl A, #07h ; Calculate H byte + inc A ; Add 1 + inc A ; Add 1 more + mov Temp2, A + mov A, Temp1 + anl A, #0F8h ; Calculate L byte + clr C + subb A, #40h ; Subtract 0.25 + mov Temp1, A + mov A, Temp2 + subb A, #0 + mov Temp2, A + +calc_governor_store_target: + ; Store governor target + mov Gov_Target_L, Temp1 + mov Gov_Target_H, Temp2 +calc_governor_target_exit: + ret +ENDIF +IF MODE == 2 ; Multi +calc_governor_target: +;读取 Closed loop mode + mov Temp1, #Pgm_Gov_Mode ; Closed loop mode? + cjne @Temp1, #1, governor_target_calc ; Yes - branch +;Closed loop mode = 第4项 Off + jmp calc_governor_target_exit ; No + +governor_target_calc: + ; Stop governor for stop RC pulse + clr C + mov A, New_Rcp ; Check RC pulse against stop value + subb A, #RCP_STOP ; Is pulse below stop value? + jc governor_deactivate ; Yes - deactivate + + jmp governor_activate ; No - activate + +governor_deactivate: + mov Current_Pwm, Requested_Pwm ; Set current pwm to requested + clr A + mov Gov_Target_L, A ; Set target to zero + mov Gov_Target_H, A + mov Gov_Integral_L, A ; Set integral to zero + mov Gov_Integral_H, A + mov Gov_Integral_X, A + mov Gov_Active, A + jmp calc_governor_target_exit + +governor_activate: + mov Temp1, #Pgm_Gov_Mode ; Store gov mode + mov A, @Temp1 + mov Temp5, A + mov Gov_Active, #1 + mov A, Requested_Pwm ; Load requested pwm + mov Governor_Req_Pwm, A ; Set governor requested pwm + ; Calculate comm period target 2*(51000/Requested_Pwm) + mov Temp1, #38h ; Load 51000 + mov Temp2, #0C7h + mov Temp3, Comm_Period4x_L ; Load comm period + mov Temp4, Comm_Period4x_H + ; Set speed range. Bare Comm_Period4x corresponds to 400k rpm, because it is 500n units +;Closed loop mode = 第1项 HiRange + clr C + mov A, Temp4 + rrc A + mov Temp4, A + mov A, Temp3 + rrc A + mov Temp3, A ; 200k eRPM range here + ; Check range + mov A, Temp5 + dec A + dec A + dec A + dec A + jz governor_activate_range_set ; 200k eRPM? - branch +;Closed loop mode = 第2项 MidRange +governor_activate_100k: + clr C + mov A, Temp4 + rrc A + mov Temp4, A + mov A, Temp3 + rrc A + mov Temp3, A ; 100k eRPM range here + mov A, Temp5 ; Check range again + dec A + dec A + dec A + jz governor_activate_range_set ; 100k eRPM? - branch +;Closed loop mode = 第3项 LoRange +governor_activate_50k: + clr C + mov A, Temp4 + rrc A + mov Temp4, A + mov A, Temp3 + rrc A + mov Temp3, A ; 50k eRPM range here +governor_activate_range_set: + call div_u16_by_u16 + ; Store governor target + mov Gov_Target_L, Temp1 + mov Gov_Target_H, Temp2 +calc_governor_target_exit: + ret +ENDIF + +; Second governor routine - calculate governor proportional error +calc_governor_prop_error: + ; Exit if governor is inactive + mov A, Gov_Active + jz calc_governor_prop_error_exit + +IF MODE <= 1 ; Main or tail + ; Load comm period and divide by 2 + clr C + mov A, Comm_Period4x_H + rrc A + mov Temp2, A + mov A, Comm_Period4x_L + rrc A + mov Temp1, A + ; Calculate error + clr C + mov A, Gov_Target_L + subb A, Temp1 + mov Temp1, A + mov A, Gov_Target_H + subb A, Temp2 + mov Temp2, A +ENDIF +IF MODE == 2 ; Multi + ; Calculate error + clr C + mov A, Gov_Target_L + subb A, Governor_Req_Pwm + mov Temp1, A + mov A, Gov_Target_H + subb A, #0 + mov Temp2, A +ENDIF + ; Check error and limit + jnc governor_check_prop_limit_pos ; Check carry + + clr C + mov A, Temp1 + subb A, #80h ; Is error too negative? + mov A, Temp2 + subb A, #0FFh + jc governor_limit_prop_error_neg ; Yes - limit + jmp governor_store_prop_error + +governor_check_prop_limit_pos: + clr C + mov A, Temp1 + subb A, #7Fh ; Is error too positive? + mov A, Temp2 + subb A, #00h + jnc governor_limit_prop_error_pos ; Yes - limit + jmp governor_store_prop_error + +governor_limit_prop_error_pos: + mov Temp1, #7Fh ; Limit to max positive (2's complement) + mov Temp2, #00h + jmp governor_store_prop_error + +governor_limit_prop_error_neg: + mov Temp1, #80h ; Limit to max negative (2's complement) + mov Temp2, #0FFh + +governor_store_prop_error: + ; Store proportional + mov Gov_Proportional_L, Temp1 + mov Gov_Proportional_H, Temp2 +calc_governor_prop_error_exit: + ret + + +; Third governor routine - calculate governor integral error +calc_governor_int_error: + ; Exit if governor is inactive + mov A, Gov_Active + jz calc_governor_int_error_exit + + ; Add proportional to integral + mov A, Gov_Proportional_L + add A, Gov_Integral_L + mov Temp1, A + mov A, Gov_Proportional_H + addc A, Gov_Integral_H + mov Temp2, A + mov Bit_Access, Gov_Proportional_H ; Sign extend high byte + clr A + jnb Bit_Access.7, ($+4) + cpl A + addc A, Gov_Integral_X + mov Temp3, A + ; Check integral and limit + jnb ACC.7, governor_check_int_limit_pos ; Check sign bit + + clr C + mov A, Temp3 + subb A, #0F0h ; Is error too negative? + jc governor_limit_int_error_neg ; Yes - limit + jmp governor_check_pwm + +governor_check_int_limit_pos: + clr C + mov A, Temp3 + subb A, #0Fh ; Is error too positive? + jnc governor_limit_int_error_pos ; Yes - limit + jmp governor_check_pwm + +governor_limit_int_error_pos: + mov Temp1, #0FFh ; Limit to max positive (2's complement) + mov Temp2, #0FFh + mov Temp3, #0Fh + jmp governor_check_pwm + +governor_limit_int_error_neg: + mov Temp1, #00h ; Limit to max negative (2's complement) + mov Temp2, #00h + mov Temp3, #0F0h + +governor_check_pwm: + ; Check current pwm + clr C + mov A, Current_Pwm + subb A, Pwm_Limit ; Is current pwm at or above pwm limit? + jnc governor_int_max_pwm ; Yes - branch + + mov A, Current_Pwm ; Is current pwm at zero? + jz governor_int_min_pwm ; Yes - branch + + ajmp governor_store_int_error ; No - store integral error + +governor_int_max_pwm: + mov A, Gov_Proportional_H + jb ACC.7, calc_governor_int_error_exit ; Is proportional error negative - branch (high byte is always zero) + ajmp governor_store_int_error ; Positive - store integral error + +governor_int_min_pwm: + mov A, Gov_Proportional_H + jnb ACC.7, calc_governor_int_error_exit ; Is proportional error positive - branch (high byte is always zero) + +governor_store_int_error: + ; Store integral + mov Gov_Integral_L, Temp1 + mov Gov_Integral_H, Temp2 + mov Gov_Integral_X, Temp3 +calc_governor_int_error_exit: + ret + + +; Fourth governor routine - calculate governor proportional correction +calc_governor_prop_correction: + ; Exit if governor is inactive + mov A, Gov_Active + jnz calc_governor_prop_corr + jmp calc_governor_prop_corr_exit + +calc_governor_prop_corr: + ; Load proportional gain + mov Temp1, #Pgm_Gov_P_Gain_Decoded; Load proportional gain + mov A, @Temp1 + mov Temp3, A ; Store in Temp3 + ; Load proportional + clr C + mov A, Gov_Proportional_L ; Nominal multiply by 2 + rlc A + mov Temp1, A + mov A, Gov_Proportional_H + rlc A + mov Temp2, A + ; Apply gain + call mult_s16_by_u8_div_16 + ; Check error and limit (to low byte) + mov A, Temp2 + jnb ACC.7, governor_check_prop_corr_limit_pos ; Check sign bit + + clr C + mov A, Temp1 + subb A, #80h ; Is error too negative? + mov A, Temp2 + subb A, #0FFh + jc governor_limit_prop_corr_neg ; Yes - limit + ajmp governor_apply_prop_corr + +governor_check_prop_corr_limit_pos: + clr C + mov A, Temp1 + subb A, #7Fh ; Is error too positive? + mov A, Temp2 + subb A, #00h + jnc governor_limit_prop_corr_pos ; Yes - limit + ajmp governor_apply_prop_corr + +governor_limit_prop_corr_pos: + mov Temp1, #7Fh ; Limit to max positive (2's complement) + mov Temp2, #00h + ajmp governor_apply_prop_corr + +governor_limit_prop_corr_neg: + mov Temp1, #80h ; Limit to max negative (2's complement) + mov Temp2, #0FFh + +governor_apply_prop_corr: + ; Test proportional sign + mov A, Temp1 + jb ACC.7, governor_corr_neg_prop ; If proportional negative - go to correct negative + + ; Subtract positive proportional + clr C + mov A, Governor_Req_Pwm + subb A, Temp1 + mov Temp1, A + ; Check result + jc governor_corr_prop_min_pwm ; Is result negative? + + clr C + mov A, Temp1 ; Is result below pwm min? + subb A, #1 + jc governor_corr_prop_min_pwm ; Yes + jmp governor_store_prop_corr ; No - store proportional correction + +governor_corr_prop_min_pwm: + mov Temp1, #1 ; Load minimum pwm + jmp governor_store_prop_corr + +governor_corr_neg_prop: + ; Add negative proportional + mov A, Temp1 + cpl A + add A, #1 + add A, Governor_Req_Pwm + mov Temp1, A + ; Check result + jc governor_corr_prop_max_pwm ; Is result above max? + jmp governor_store_prop_corr ; No - store proportional correction + +governor_corr_prop_max_pwm: + mov Temp1, #255 ; Load maximum pwm +governor_store_prop_corr: + ; Store proportional pwm + mov Gov_Prop_Pwm, Temp1 +calc_governor_prop_corr_exit: + ret + + +; Fifth governor routine - calculate governor integral correction +calc_governor_int_correction: + ; Exit if governor is inactive + mov A, Gov_Active + jnz calc_governor_int_corr + jmp calc_governor_int_corr_exit + +calc_governor_int_corr: + ; Load integral gain + mov Temp1, #Pgm_Gov_I_Gain_Decoded; Load integral gain + mov A, @Temp1 + mov Temp3, A ; Store in Temp3 + ; Load integral + mov Temp1, Gov_Integral_H + mov Temp2, Gov_Integral_X + ; Apply gain + call mult_s16_by_u8_div_16 + ; Check integral and limit + mov A, Temp2 + jnb ACC.7, governor_check_int_corr_limit_pos ; Check sign bit + + clr C + mov A, Temp1 + subb A, #01h ; Is integral too negative? + mov A, Temp2 + subb A, #0FFh + jc governor_limit_int_corr_neg ; Yes - limit + jmp governor_apply_int_corr + +governor_check_int_corr_limit_pos: + clr C + mov A, Temp1 + subb A, #0FFh ; Is integral too positive? + mov A, Temp2 + subb A, #00h + jnc governor_limit_int_corr_pos ; Yes - limit + jmp governor_apply_int_corr + +governor_limit_int_corr_pos: + mov Temp1, #0FFh ; Limit to max positive (2's complement) + mov Temp2, #00h + jmp governor_apply_int_corr + +governor_limit_int_corr_neg: + mov Temp1, #01h ; Limit to max negative (2's complement) + mov Temp2, #0FFh + +governor_apply_int_corr: + ; Test integral sign + mov A, Temp2 + jb ACC.7, governor_corr_neg_int ; If integral negative - go to correct negative + + ; Subtract positive integral + clr C + mov A, Gov_Prop_Pwm + subb A, Temp1 + mov Temp1, A + ; Check result + jc governor_corr_int_min_pwm ; Is result negative? + + clr C + mov A, Temp1 ; Is result below pwm min? + subb A, #1 + jc governor_corr_int_min_pwm ; Yes + jmp governor_store_int_corr ; No - store correction + +governor_corr_int_min_pwm: + mov Temp1, #0 ; Load minimum pwm + jmp governor_store_int_corr + +governor_corr_neg_int: + ; Add negative integral + mov A, Temp1 + cpl A + add A, #1 + add A, Gov_Prop_Pwm + mov Temp1, A + ; Check result + jc governor_corr_int_max_pwm ; Is result above max? + jmp governor_store_int_corr ; No - store correction + +governor_corr_int_max_pwm: + mov Temp1, #255 ; Load maximum pwm +governor_store_int_corr: + ; Store current pwm + mov Current_Pwm, Temp1 +calc_governor_int_corr_exit: + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Measure lipo cells +; +; No assumptions +; +; Measure voltage and calculate lipo cells +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +measure_lipo_cells: +IF MODE == 1 ; Tail + ; If tail, then exit + jmp measure_lipo_exit +ENDIF +measure_lipo_start: + ; Load programmed low voltage limit +;读取Low voltage limit + mov Lipo_Cell_Count, #0 ;2013.8.23 防止接收机拿掉后再接上时，电池检测出错 + mov Temp1, #Pgm_Low_Voltage_Lim ; Load limit + mov A, @Temp1 + mov Bit_Access, A ; Store in Bit_Access + ; Set commutation to BpFET on +; call comm5comm6 + CnFET_off ; Cn off + BnFET_on ; Bn on + Set_Comp_Phase_C ; Set comparator to phase C + mov Comm_Phase, #6 + ; Start adc + Start_Adc + ; Wait for ADC conversion to complete + Get_Adc_Status + jb AD0BUSY, measure_lipo_cells + ; Read ADC result + Read_Adc_Result + ; Stop ADC + Stop_Adc + ; Switch power off + call switch_power_off + ; Set limit step + mov Lipo_Adc_Limit_L, #ADC_LIMIT_L + mov Lipo_Adc_Limit_H, #ADC_LIMIT_H + clr C + mov A, #ADC_LIMIT_H ; Divide 2.8V value by 2 + rrc A + mov Temp6, A + mov A, #ADC_LIMIT_L + rrc A + mov Temp5, A + mov A, #ADC_LIMIT_L ; Calculate 1.4+2.8V=4.2V value + add A, Temp5 + add A, #3 ;加宽一节电池电压范围 4.3V 2013.8.16 + mov Temp5, A + mov A, #ADC_LIMIT_H + addc A, Temp6 + mov Temp6, A + mov A, Temp5 ; Copy step + mov Temp3, A + mov A, Temp6 + mov Temp4, A +;*************************************************** +; Lipo节数检测 +;*************************************************** +measure_lipo_cell_loop: + ; Check voltage against xS lower limit + inc Lipo_Cell_Count ;增加电池节数累加 2013.5.29 + clr C + mov A, Temp1 + subb A, Temp3 ; Voltage above limit? + mov A, Temp2 + subb A, Temp4 + jc measure_lipo_adjust ; No - branch 读取到电压范围 + + ; Set xS voltage limit + mov A, Lipo_Adc_Limit_L + add A, #ADC_LIMIT_L + mov Lipo_Adc_Limit_L, A + mov A, Lipo_Adc_Limit_H + addc A, #ADC_LIMIT_H + mov Lipo_Adc_Limit_H, A + ; Set (x+1)S lower limit + mov A, Temp3 + add A, Temp5 ; Add step + mov Temp3, A + mov A, Temp4 + addc A, Temp6 + mov Temp4, A + jmp measure_lipo_cell_loop ; Check for one more battery cell + +measure_lipo_adjust: + mov Temp7, Lipo_Adc_Limit_L + mov Temp8, Lipo_Adc_Limit_H + ; Calculate 3.125% + clr C + mov A, Lipo_Adc_Limit_H + rrc A + mov Temp2, A + mov A, Lipo_Adc_Limit_L + rrc A + mov Temp1, A ; After this 50% + clr C + mov A, Temp2 + rrc A + mov Temp2, A + mov A, Temp1 + rrc A + mov Temp1, A ; After this 25% +; mov A, Lipo_Adc_Limit_L ; Set adc reference for voltage compensation +; add A, Temp1 ;低压低字节+低压低字节偏移量 +; mov Lipo_Adc_Reference_L, A ;存低压低字节 +; mov A, Lipo_Adc_Limit_H +; addc A, Temp2 ;低压高字节+低压高字节偏移量 +; mov Lipo_Adc_Reference_H, A ;存低压高字节 +; ; Divide three times to get to 3.125% 6.25% 2013.5.30 + mov Temp3, #2 ;3 +measure_lipo_divide_loop: + clr C + mov A, Temp2 + rrc A + mov Temp2, A + mov A, Temp1 + rrc A + mov Temp1, A + djnz Temp3, measure_lipo_divide_loop + + ; Add the programmed number of 0.1V (or 3.125% increments) + mov Temp3, Bit_Access ; Load programmed limit (Bit_Access has Pgm_Low_Voltage_Lim) +; dec Temp3 +; mov A,Temp3 ;增加判断指令，如果没有这条指令能判断？？？？ +; jnz measure_lipo_limit_on ; Is low voltage limiting on? + + cjne Temp3, #4, measure_lipo_limit_on + + mov Lipo_Adc_Limit_L, #0 ; No - set limit to zero + mov Lipo_Adc_Limit_H, #0 + jmp measure_lipo_exit + +measure_lipo_limit_on: + dec Temp3 + mov A, Temp3 + jz measure_lipo_update +;Bit_Access>2时，低压保护值=基值+偏移量 +;Low voltage limit > 第2项时 +measure_lipo_add_loop: + clr C + mov A, Temp7 ; Add 3.125% + add A, Temp1 + mov Temp7, A + mov A, Temp8 + addc A, Temp2 + mov Temp8, A +; clr C +; mov A, Temp7 ; Add 3.125% +; add A, Temp1 +; mov Temp7, A +; mov A, Temp8 +; addc A, Temp2 +; mov Temp8, A + djnz Temp3, measure_lipo_add_loop +;存低压保护值 +;Low voltage limit = 第2项 3.0V +measure_lipo_update: + ; Set ADC limit + mov Lipo_Adc_Limit_L, Temp7 + mov Lipo_Adc_Limit_H, Temp8 +measure_lipo_exit: + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Start ADC conversion +; +; No assumptions +; +; Start conversion used for measuring power supply voltage +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +start_adc_conversion: + ; Start adc + Start_Adc + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Check temperature, power supply voltage and limit power +; +; No assumptions +; +; Used to limit main motor power in order to maintain the required voltage +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +check_temp_voltage_and_limit_power: + ; Load programmed low voltage limit + mov Temp1, #Pgm_Low_Voltage_Lim ;读低压保护选项 + mov A, @Temp1 + mov Temp8, A ; Store in Temp8 + ; Wait for ADC conversion to complete + Get_Adc_Status + jb AD0BUSY, check_temp_voltage_and_limit_power + ; Read ADC result + Read_Adc_Result ;读AD值 + ; Stop ADC + Stop_Adc ;禁止AD转换 + + inc Adc_Conversion_Cnt ; Increment conversion counter + clr C + mov A, Adc_Conversion_Cnt ; Is conversion count equal to temp rate? + subb A, #TEMP_CHECK_RATE + jc check_voltage_start ; No - check voltage AD转换次数 < 8次 ？ + + mov Adc_Conversion_Cnt, #0 ; Yes - temperature check. Reset counter + mov A, Temp2 ; Is temperature reading below 256? + jnz temp_average_inc_dec ; No - proceed + + mov A, Current_Average_Temp ; Yes - decrement average + jz temp_average_updated ; Already zero - no change + jmp temp_average_dec ; Decrement + +temp_average_inc_dec: + clr C + mov A, Temp1 ; Check if current temperature is above or below average + subb A, Current_Average_Temp + jz temp_average_updated_load_acc ; Equal - no change + + mov A, Current_Average_Temp ; Above - increment average + jnc temp_average_inc + + jz temp_average_updated ; Below - decrement average if average is not already zero +temp_average_dec: + dec A ; Decrement average + jmp temp_average_updated + +temp_average_inc: + inc A ; Increment average + jz temp_average_dec + jmp temp_average_updated + +temp_average_updated_load_acc: + mov A, Current_Average_Temp +temp_average_updated: + mov Current_Average_Temp, A + clr C + subb A, #TEMP_LIMIT ; Is temperature below first limit? + mov Temp1, A + jc temp_check_exit ; Yes - exit + + mov Pwm_Limit, #192 ; No - limit pwm + + clr C + mov A, Temp1 ; Is temperature below second limit + subb A, #TEMP_LIMIT_STEP + mov Temp1, A + jc temp_check_exit ; Yes - exit + + mov Pwm_Limit, #128 ; No - limit pwm + + clr C + mov A, Temp1 ; Is temperature below third limit + subb A, #TEMP_LIMIT_STEP + mov Temp1, A + jc temp_check_exit ; Yes - exit + + mov Pwm_Limit, #64 ; No - limit pwm + + clr C + mov A, Temp1 ; Is temperature below final limit + subb A, #TEMP_LIMIT_STEP + mov Temp1, A + jc temp_check_exit ; Yes - exit + + mov Pwm_Limit, #16 ; No - limit pwm 2013.8.5最高温度时速度由0改成16 + +temp_check_exit: + Set_Adc_Ip_Volt ; Select adc input for next conversion + ret + +check_voltage_start: +IF MODE == 0 OR MODE == 2 ; Main or multi + ; Check if low voltage limiting is enabled + mov A, Temp8 + clr C + subb A, #4 ; Is low voltage limit disabled? + jz check_voltage_good ; Yes - voltage declared good + + ; Check if ADC is saturated + clr C + mov A, Temp1 ;读取AD值低字节 + subb A, #0FFh + mov A, Temp2 ;读取AD值高字节 + subb A, #03h ;判断AD是否超出10位AD范围 + jnc check_voltage_good ; ADC saturated, can not make judgement + + ; Check voltage against limit + clr C + mov A, Temp1 + subb A, Lipo_Adc_Limit_L + mov A, Temp2 + subb A, Lipo_Adc_Limit_H + jnc check_voltage_good ; If voltage above limit - branch 电压值 > 低压设置值 + + mov Temp1, #Pgm_Low_Voltage_Ctl ;读低压保护控制方式选项 + mov A, @Temp1 + clr C + subb A, #1 ; + jz check_voltage_next_way + + mov A, Pwm_Limit + subb A, #5 + jnc check_limit_count + setb LOW_LIMIT_STOP ;设置低压保护控制方式2 低压停止标志 + ljmp run_to_wait_for_power_on + +check_voltage_next_way: + ; Decrease pwm limit + mov A, Pwm_Limit + subb A, #80 ;更改低压保护方式 2013.05.31 +;////// jz check_voltage_lim ; If limit zero - branch + jc check_voltage_lim ; If limit <100 - branch +check_limit_count: + mov A, Limit_Count + jz ($+6) + dec Limit_Count ;递减低压保护变量 2013.7.5 + ajmp check_voltage_lim + + mov Limit_Count, #5 ; 2013.7.5 + dec Pwm_Limit ; Decrement limit + jmp check_voltage_lim +;Low voltage limit = 第1项 +check_voltage_good: + ; Increase pwm limit + mov Limit_Count, #5 ; 2013.7.5 + mov A, Pwm_Limit + cpl A + jz check_voltage_lim ; If limit max - branch + + inc Pwm_Limit ; Increment limit + +check_voltage_lim: + mov Temp1, Pwm_Limit ; Set limit + clr C + mov A, Current_Pwm + subb A, Temp1 + jnc check_voltage_spoolup_lim ; If current pwm above limit - branch and limit + + mov Temp1, Current_Pwm ; Set current pwm (no limiting) + +check_voltage_spoolup_lim: + mov Current_Pwm_Limited, Temp1 ;取小值 + ; Slow spoolup + clr C + mov A, Current_Pwm_Limited + subb A, Pwm_Limit_Spoolup + jc check_voltage_exit ; If current pwm below limit - branch + + mov Current_Pwm_Limited, Pwm_Limit_Spoolup ;取小值 + mov A, Pwm_Limit_Spoolup ; Check if spoolup limit is max + cpl A + jz check_voltage_exit ; If max - branch + + mov Pwm_Limit, Pwm_Limit_Spoolup ; Set pwm limit to spoolup limit during ramp (to avoid governor integral buildup) + +check_voltage_exit: +ENDIF + ; Set adc mux for next conversion + clr C + mov A, Adc_Conversion_Cnt ; Is next conversion for temperature? + cjne A, #(TEMP_CHECK_RATE-1), ($+6) + + Set_Adc_Ip_Temp ; Select temp sensor for next conversion + + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Set startup PWM routine +; +; No assumptions +; +; Used for pwm control during startup +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +set_startup_pwm: + ; Set pwm values according to startup phase flags + jnb Flags1.SETTLE_PHASE, ($+5) ; Is it motor start settle phase? 是否启动定位？ + mov Temp1, #PWM_SETTLE ; Yes - set settle power + jnb Flags1.STEPPER_PHASE, ($+5) ; Is it motor start stepper phase? 是否步进启动？ + mov Temp1, #PWM_STEPPER ; Yes - set stepper power + + ; Update pwm variables if any startup phase flag is set + mov A, Flags1 + anl A, #((1 SHL SETTLE_PHASE)+(1 SHL STEPPER_PHASE)) + jz startup_pwm_exit ; If no startup phase set - exit + + ; Adjust startup power + mov A, Temp1 ; Multiply startup power by programmed value + mov Temp2, #Pgm_Startup_Pwr_Decoded + mov B, @Temp2 + mul AB + xch A, B + mov C, B.7 ; Multiply result by 2 (unity gain is 128) + rlc A + mov Temp1, A ; Transfer to Temp1 + clr C + mov A, Temp1 ; Check against limit + subb A, Pwm_Limit + jc startup_pwm_set_pwm ; If pwm below limit - branch + + mov Temp1, Pwm_Limit ; Limit pwm + +startup_pwm_set_pwm: + ; Set pwm variables + mov Requested_Pwm, Temp1 ; Update requested pwm + mov Current_Pwm, Temp1 ; Update current pwm + mov Current_Pwm_Limited, Temp1 ; Update limited version of current pwm + jnb Flags1.SETTLE_PHASE, startup_pwm_exit ; Is it motor start settle phase? + + clr C ; Yes - update spoolup beginning pwm (will use PWM_SETTLE or PWM_SETTLE/2) + mov A, Temp1 + mov Pwm_Spoolup_Beg, A + +startup_pwm_exit: + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Initialize all timings routine +; +; No assumptions +; +; Part of initialization before motor start +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +initialize_all_timings: + ; Load programmed startup rpm +;读取Startup rpm +; mov Temp1, #Pgm_Startup_Rpm ; Load startup rpm +; mov A, @Temp1 +; mov Temp8, A ; Store in Temp8 + ; Check startup rpm setting and set step accordingly +; clr C +; mov A, Temp8 +; subb A, #5 +; jnc stepper_step_high +; clr C +; mov A, Temp8 +; subb A, #4 +; jnc stepper_step_med_high +; clr C +; mov A, Temp8 +; subb A, #3 +; jnc stepper_step_med +; clr C +; mov A, Temp8 +; subb A, #2 +; jnc stepper_step_med_low +; clr C +; mov A, Temp8 +; subb A, #1 +; jnc stepper_step_low +;Startup_Rpm = 第5项 +;stepper_step_high: +; mov Stepper_Step_Beg_L, #low(2000 SHL 1) +; mov Stepper_Step_Beg_H, #high(2000 SHL 1) +; mov Stepper_Step_End_L, #low(670 SHL 1) +; mov Stepper_Step_End_H, #high(670 SHL 1) +; ajmp stepper_step_set +;Startup_Rpm = 第4项 +;stepper_step_med_high: +; mov Stepper_Step_Beg_L, #low(2400 SHL 1) +; mov Stepper_Step_Beg_H, #high(2400 SHL 1) +; mov Stepper_Step_End_L, #low(800 SHL 1) +; mov Stepper_Step_End_H, #high(800 SHL 1) +; ajmp stepper_step_set +;Startup_Rpm = 第3项 +;stepper_step_med: +; mov Stepper_Step_Beg_L, #low(3000 SHL 1) ; ~3300 eRPM +; mov Stepper_Step_Beg_H, #high(3000 SHL 1) +; mov Stepper_Step_End_L, #low(1000 SHL 1) ; ~10000 eRPM +; mov Stepper_Step_End_H, #high(1000 SHL 1) +; ajmp stepper_step_set +;Startup_Rpm = 第2项 +;stepper_step_med_low: +; mov Stepper_Step_Beg_L, #low(3750 SHL 1) +; mov Stepper_Step_Beg_H, #high(3750 SHL 1) +; mov Stepper_Step_End_L, #low(1250 SHL 1) +; mov Stepper_Step_End_H, #high(1250 SHL 1) +; ajmp stepper_step_set +;Startup_Rpm = 第1项 +;stepper_step_low: +; mov Stepper_Step_Beg_L, #low(4500 SHL 1) +; mov Stepper_Step_Beg_H, #high(4500 SHL 1) +; mov Stepper_Step_End_L, #low(1500 SHL 1) +; mov Stepper_Step_End_H, #high(1500 SHL 1) +; ajmp stepper_step_set + +;stepper_step_set: +; mov Wt_Stepper_Step_L, Stepper_Step_Beg_L ; Initialize stepper step time +; mov Wt_Stepper_Step_H, Stepper_Step_Beg_H + mov Comm_Period4x_L, #00h ; Set commutation period registers + mov Comm_Period4x_H, #08h + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Calculate next commutation timing routine +; +; No assumptions +; +; Called immediately after each commutation +; Also sets up timer 1 to wait advance timing +; Two entry points are used +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +;calc_next_comm_timing_start: ; Entry point for startup +; mov Temp1, Wt_Stepper_Step_L ; Set up stepper step wait +; mov Temp2, Wt_Stepper_Step_H +; jmp read_timer + +calc_next_comm_timing: ; Entry point for run phase + mov Temp1, Wt_Advance_L ; Set up advance timing wait + mov Temp2, Wt_Advance_H +read_timer: + ; Set up next wait + mov TMR3CN, #00h ; Timer3 disabled + clr C + clr A + subb A, Temp1 ; Set wait to zero cross scan value + mov TMR3L, A + clr A + subb A, Temp2 + mov TMR3H, A + mov TMR3CN, #04h ; Timer3 enabled + setb Flags0.T3_PENDING + ; Read commutation time + mov TMR2CN, #20h ; Timer2 disabled + mov Temp1, TMR2L ; Load timer value + mov Temp2, TMR2H + mov TMR2CN, #24h ; Timer2 enabled + ; Calculate this commutation time + mov Temp3, Prev_Comm_L + mov Temp4, Prev_Comm_H + mov Prev_Comm_L, Temp1 ; Store timestamp as previous commutation + mov Prev_Comm_H, Temp2 + clr C + mov A, Temp1 + subb A, Temp3 ; Calculate the new commutation time + mov Temp1, A + mov A, Temp2 + subb A, Temp4 + mov Temp2, A + ; Calculate next zero cross scan timeout + mov Temp3, Comm_Period4x_L ; Comm_Period4x(-l-h-x) holds the time of 4 commutations + mov Temp4, Comm_Period4x_H + clr C + mov A, Temp4 + rrc A ; Divide by 2 + mov Temp6, A + mov A, Temp3 + rrc A + mov Temp5, A + clr C + mov A, Temp6 + rrc A ; Divide by 2 again + mov Temp6, A + mov A, Temp5 + rrc A + mov Temp5, A + clr C + mov A, Temp3 + subb A, Temp5 ; Subtract a quarter + mov Temp3, A + mov A, Temp4 + subb A, Temp6 + mov Temp4, A + + mov A, Temp3 + add A, Temp1 ; Add the new time + mov Temp3, A + mov A, Temp4 + addc A, Temp2 + mov Temp4, A + mov Comm_Period4x_L, Temp3 ; Store Comm_Period4x_X + mov Comm_Period4x_H, Temp4 + jc calc_next_comm_slow ; If period larger than 0xffff - go to slow case + + ret + +calc_next_comm_slow: + mov Comm_Period4x_L, #0FFh ; Set commutation period registers to very slow timing (0xffff) + mov Comm_Period4x_H, #0FFh + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Wait advance timing routine +; +; No assumptions +; +; Waits for the advance timing to elapse +; Also sets up timer 1 to wait the zero cross scan wait time +; And has a separate entry point for just setting up zero cross scan wait +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +wait_advance_timing: + jnb Flags0.T3_PENDING, ($+5) + ajmp wait_advance_timing + +setup_zc_scan_wait: + mov TMR3CN, #00h ; Timer3 disabled + clr C + clr A + subb A, Wt_Zc_Scan_L ; Set wait to zero cross scan value + mov TMR3L, A + clr A + subb A, Wt_Zc_Scan_H + mov TMR3H, A + mov TMR3CN, #04h ; Timer3 enabled + setb Flags0.T3_PENDING + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Calculate new wait times routine +; +; No assumptions +; +; Calculates new wait times +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +calc_new_wait_times: + ; Load programmed commutation timing +;读取Commutation timing + mov Temp1, #Pgm_Comm_Timing ; Load timing setting + mov A, @Temp1 + mov Temp8, A ; Store in Temp8 + mov Temp7, #(COMM_TIME_RED SHL 1) + jnb Flags0.DEMAG_DETECTED, calc_new_wait_check_startup ; Demag detected? + + mov Temp8, #5 ; Yes - set high timing + +calc_new_wait_check_startup: + jnb Flags1.DIRECT_STARTUP_PHASE, calc_new_wait_dir_start_set ; Set timing for direct start + + mov Temp8, #3 ; Set medium timing + mov Temp7, #0 ; Set no comm time reduction + +calc_new_wait_dir_start_set: + ; Load current commutation timing + mov Temp2, Comm_Period4x_H ; Load Comm_Period4x + mov Temp1, Comm_Period4x_L + mov Temp3, #4 ; Divide 4 times +divide_wait_times: + clr C + mov A, Temp2 + rrc A ; Divide by 2 + mov Temp2, A + mov A, Temp1 + rrc A + mov Temp1, A + djnz Temp3, divide_wait_times + + clr C + mov A, Temp1 + subb A, Temp7 + mov Temp1, A + mov A, Temp2 + subb A, #0 + mov Temp2, A + jc load_min_time ; Check that result is still positive + + clr C + mov A, Temp1 + subb A, #(COMM_TIME_MIN SHL 1) + mov A, Temp2 + subb A, #0 + jnc adjust_timing ; Check that result is still above minumum + +load_min_time: + mov Temp1, #(COMM_TIME_MIN SHL 1) + clr A + mov Temp2, A + +adjust_timing: + mov A, Temp2 ; Copy values + mov Temp4, A + mov A, Temp1 + mov Temp3, A + clr C + mov A, Temp2 + rrc A ; Divide by 2 + mov Temp6, A + mov A, Temp1 + rrc A + mov Temp5, A + clr C + mov A, Temp8 ; (Temp8 has Pgm_Comm_Timing) + subb A, #3 ; Is timing normal? + jz store_times_decrease ; Yes - branch + + mov A, Temp8 + jb ACC.0, adjust_timing_two_steps ; If an odd number - branch +;Commutation timing = 第2、4项 + mov A, Temp1 ; Add 7.5?and store in Temp1/2 + add A, Temp5 + mov Temp1, A + mov A, Temp2 + addc A, Temp6 + mov Temp2, A + mov A, Temp5 ; Store 7.5?in Temp3/4 + mov Temp3, A + mov A, Temp6 + mov Temp4, A + jmp store_times_up_or_down +;Commutation timing = 第1、5项 +adjust_timing_two_steps: + mov A, Temp1 ; Add 15?and store in Temp1/2 + add A, Temp1 + mov Temp1, A + mov A, Temp2 + addc A, Temp2 + mov Temp2, A + mov Temp3, #(COMM_TIME_MIN SHL 1) ; Store minimum time in Temp3/4 + clr A + mov Temp4, A + +store_times_up_or_down: + clr C + mov A, Temp8 + subb A, #3 ; Is timing higher than normal? + jc store_times_decrease ; No - branch +;Commutation timing = 第4、5项 +store_times_increase: + mov Wt_Comm_L, Temp3 ; Now commutation time (~60? divided by 4 (~15?nominal) + mov Wt_Comm_H, Temp4 + mov Wt_Advance_L, Temp1 ; New commutation advance time (~15?nominal) + mov Wt_Advance_H, Temp2 + mov Wt_Zc_Scan_L, Temp5 ; Use this value for zero cross scan delay (7.5? + mov Wt_Zc_Scan_H, Temp6 + ret +;Commutation timing = 第1、2、3项 +store_times_decrease: + mov Wt_Comm_L, Temp1 ; Now commutation time (~60? divided by 4 (~15?nominal) + mov Wt_Comm_H, Temp2 + mov Wt_Advance_L, Temp3 ; New commutation advance time (~15?nominal) + mov Wt_Advance_H, Temp4 + mov Wt_Zc_Scan_L, Temp5 ; Use this value for zero cross scan delay (7.5? + mov Wt_Zc_Scan_H, Temp6 + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Wait before zero cross scan routine +; +; No assumptions +; +; Waits for the zero cross scan wait time to elapse +; Also sets up timer 1 to wait the zero cross scan timeout time +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +wait_before_zc_scan: + jnb Flags0.T3_PENDING, ($+5) + ajmp wait_before_zc_scan + + mov TMR3CN, #00h ; Timer3 disabled + clr C + clr A + subb A, Comm_Period4x_L ; Set wait to zero comm period 4x value + mov TMR3L, A + clr A + subb A, Comm_Period4x_H + mov TMR3H, A + mov TMR3CN, #04h ; Timer3 enabled + setb Flags0.T3_PENDING + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Wait for comparator to go low/high routines +; +; No assumptions +; +; Waits for the zero cross scan wait time to elapse +; Then scans for comparator going low/high +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +wait_for_comp_out_low: + mov Comp_Wait_Reads, #0 + mov Bit_Access, #00h ; Desired comparator output + jmp wait_for_comp_out_start + +wait_for_comp_out_high: + mov Comp_Wait_Reads, #0 + mov Bit_Access, #40h ; Desired comparator output + +wait_for_comp_out_start: + setb EA ; Enable interrupts + inc Comp_Wait_Reads + jb Flags0.T3_PENDING, ($+4) ; Has zero cross scan timeout elapsed? + ret ; Yes - return + + ; Set default comparator response times + mov CPT0MD, #0 ; Set fast response (100ns) as default +IF COMP1_USED==1 + mov CPT1MD, #0 ; Set fast response (100ns) as default +ENDIF + ; Select number of comparator readings based upon current rotation speed + mov A, Comm_Period4x_H ; Load rotation period + clr C + rrc A ; Divide by 4 + clr C + rrc A + mov Temp1, A + inc Temp1 ; Add one to be sure it is always larger than zero + jz comp_wait_on_comp_able ; If minimum number of readings - jump directly to reading + ; For damped mode, do fewer comparator readings (since comparator info is primarily only available in the pwm on period) + jnb Flags2.PGM_PWMOFF_DAMPED, comp_wait_set_max_readings + + clr C + rrc A ; Divide by 4 again + clr C + rrc A + mov Temp1, A + inc Temp1 ; Add one to be sure it is always larger than zero + +comp_wait_set_max_readings: + clr C + mov A, Temp1 ; Limit to a max of 10 + subb A, #10 + jc ($+4) + + mov Temp1, #10 + + jnb Flags2.PGM_PWM_HIGH_FREQ, comp_wait_set_response_time ; Jump if pwm frequency is low + + clr C + mov A, Temp1 ; Limit to a max of 4 + subb A, #4 + jc ($+4) + + mov Temp1, #4 + +comp_wait_set_response_time: + clr C + mov A, Comm_Period4x_H ; Is Comm_Period4x_H less than 1ms? + subb A, #8 + jc comp_wait_on_comp_able ; Yes - jump + + mov CPT0MD, #2 ; Set medium response (300ns) +IF COMP1_USED==1 + mov CPT1MD, #2 ; Set medium response (300ns) +ENDIF + clr C + mov A, Comm_Period4x_H ; Is Comm_Period4x_H less than 2ms? + subb A, #16 + jc comp_wait_on_comp_able ; Yes - jump + + mov CPT0MD, #3 ; Set slow response (1000ns) +IF COMP1_USED==1 + mov CPT1MD, #3 ; Set slow response (1000ns) +ENDIF + +comp_wait_on_comp_able: + jb Flags0.T3_PENDING, ($+6) ; Has zero cross scan timeout elapsed? + setb EA ; Enable interrupts + ret ; Yes - return + + mov Temp2, #COMP_PWM_HIGH_ON_DELAY ; Wait time after pwm has been switched on (motor wire electrical settling) + jb Flags2.PGM_PWM_HIGH_FREQ, ($+5) + mov Temp2, #COMP_PWM_LOW_ON_DELAY + setb EA ; Enable interrupts + nop ; Allocate only just enough time to capture interrupt + nop + clr EA ; Disable interrupts + jb Flags0.PWM_ON, pwm_wait_startup ; If pwm on - proceed + + mov Temp2, #COMP_PWM_HIGH_OFF_DELAY ; Wait time after pwm has been switched off (motor wire electrical settling) + jb Flags2.PGM_PWM_HIGH_FREQ, ($+5) + mov Temp2, #COMP_PWM_LOW_OFF_DELAY + jnb Flags2.CURR_PWMOFF_COMP_ABLE, comp_wait_on_comp_able ; If comparator is not usable in pwm off - go back + +pwm_wait_startup: + jnb Flags1.DIRECT_STARTUP_PHASE, pwm_wait ; Set a long delay from pwm on/off events during direct startup + + mov Temp2, #120 +pwm_wait: + clr C + mov A, TL1 + subb A, Temp2 +IF DAMPED_MODE_ENABLE==0 + jc comp_wait_on_comp_able ; Re-evaluate pwm cycle for slower escs +ENDIF +IF DAMPED_MODE_ENABLE==1 ; Assume same pwm cycle for fast escs + jb Flags1.DIRECT_STARTUP_PHASE, ($+5) + jc pwm_wait + jc comp_wait_on_comp_able ; Re-evaluate pwm cycle during direct start +ENDIF + +comp_read: + Read_Comp_Out ; Read comparator output + cpl A + anl A, #40h + cjne A, Bit_Access, ($+5) ; If comparator output is correct - proceed + + ajmp wait_for_comp_out_start ; If comparator output is not correct - go back and restart + + djnz Temp1, comp_wait_on_comp_able ; Decrement readings counter - repeat comparator reading if not zero + + setb EA ; Enable interrupts + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Evaluate comparator integrity +; +; No assumptions +; +; Checks comparator signal behaviour versus expected behaviour +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +evaluate_comparator_integrity: + clr Flags0.DEMAG_DETECTED ; Clear demag detected flag + ; Check if demag compensation is enabled +;读取Demag compensation + mov Temp1, #Pgm_Demag_Comp ; Load programmed demag compensation + mov A, @Temp1 + dec A + jz eval_comp_no_demag + + ; Check if a demag situation has occurred + mov A, Comp_Wait_Reads ; Check if there were no waits (there shall be some). If none a demag situation has occurred + dec A + jnz eval_comp_no_demag +;Demag compensation = 第2项 + setb Flags0.DEMAG_DETECTED ; Set demag detected flag +;Demag compensation = 第1项 +eval_comp_no_demag: + jnb Flags1.DIRECT_STARTUP_PHASE, eval_comp_check_timeout + + inc Direct_Startup_Ok_Cnt ; Increment ok counter + jb Flags0.T3_PENDING, eval_comp_exit + + mov Direct_Startup_Ok_Cnt, #0 ; Reset ok counter + jmp eval_comp_exit + +eval_comp_check_timeout: + jb Flags0.T3_PENDING, eval_comp_exit ; Has timeout elapsed? + dec SP ; Routine exit without "ret" command + dec SP + ljmp run_to_wait_for_power_on ; Yes - exit run mode + +eval_comp_exit: + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Setup commutation timing routine +; +; No assumptions +; +; Sets up and starts wait from commutation to zero cross +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +setup_comm_wait: + mov TMR3CN, #00h ; Timer3 disabled + clr C + clr A + subb A, Wt_Comm_L ; Set wait commutation value + mov TMR3L, A + clr A + subb A, Wt_Comm_H + mov TMR3H, A + mov TMR3CN, #04h ; Timer3 enabled + setb Flags0.T3_PENDING + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Wait for commutation routine +; +; No assumptions +; +; Waits from zero cross to commutation +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +wait_for_comm: + ; Store motor power + mov Temp7, Current_Pwm_Limited + ; Check if a demag situation has occurred + jnb Flags0.DEMAG_DETECTED, wait_for_comm_wait; Demag detected? + + ; Load programmed demag compensation + mov Temp1, #Pgm_Demag_Comp_Power_Decoded ; Yes - load programmed demag compensation power decoded + mov A, @Temp1 + mov Temp8, A ; Store in Temp8 + + ; Check for first power off + cjne Temp8, #1, wait_for_comm_blind + + setb Flags0.DEMAG_CUT_POWER ; Turn off motor power + mov Current_Pwm_Limited, #0 + All_nFETs_off + + ; Wait a blind wait +wait_for_comm_blind: + call setup_zc_scan_wait ; Setup a zero cross scan wait (7.5 deg) +wait_demag_default_zc1: + jnb Flags0.DEMAG_CUT_POWER, ($+6) ; Cut motor power if set + mov Current_Pwm_Limited, #0 + jnb Flags0.T3_PENDING, ($+5) + ajmp wait_demag_default_zc1 + + ; Check for second power off + cjne Temp8, #2, wait_for_comm_second_blind_wait + + setb Flags0.DEMAG_CUT_POWER ; Turn off motor power + mov Current_Pwm_Limited, #0 + All_nFETs_off + + ; Check for another blind wait +wait_for_comm_second_blind_wait: + mov Temp1, #Pgm_Demag_Comp_Wait_Decoded ; Yes - load programmed demag compensation wait decoded + cjne @Temp1, #1, wait_for_comm_power3 + + call setup_zc_scan_wait ; Setup a zero cross scan wait (7.5 deg) +wait_demag_default_zc2: + jnb Flags0.DEMAG_CUT_POWER, ($+6) ; Cut motor power if set + mov Current_Pwm_Limited, #0 + jnb Flags0.T3_PENDING, ($+5) + ajmp wait_demag_default_zc2 + +wait_for_comm_power3: + ; Check for third power off + cjne Temp8, #3, wait_for_comm_setup + + setb Flags0.DEMAG_CUT_POWER ; Turn off motor power + mov Current_Pwm_Limited, #0 + All_nFETs_off + +wait_for_comm_setup: + call setup_comm_wait ; Setup commutation wait +wait_for_comm_wait: + jnb Flags0.DEMAG_CUT_POWER, ($+6) ; Cut motor power if set + mov Current_Pwm_Limited, #0 + jnb Flags0.T3_PENDING, ($+5) + ajmp wait_for_comm_wait + + jnb Flags0.DEMAG_DETECTED, wait_for_comm_exit ; Was there a demag situation? + + clr Flags0.DEMAG_CUT_POWER ; Restore motor power again + mov Current_Pwm_Limited, Temp7 + +wait_for_comm_exit: + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Commutation routines +; +; No assumptions +; +; Performs commutation switching +; Damped routines uses all pfets on when in pwm off to dampen the motor +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +;****************************************************** +; 关B down 并 开A down或Cdown +;****************************************************** +comm1comm2: + clr EA ; Disable all interrupts + BpFET_off ; Bp off + jb Flags2.PGM_PWMOFF_DAMPED, comm12_damp + jmp comm12_nondamp +comm12_damp: +IF DAMPED_MODE_ENABLE == 0 + mov DPTR, #pwm_cnfet_apfet_on_fast + jb Flags2.PGM_PWMOFF_DAMPED_LIGHT, comm12_nondamp +ENDIF +IF DAMPED_MODE_ENABLE == 1 + mov DPTR, #pwm_cnfet_apfet_on_safe +ENDIF + jnb Flags2.CURR_PWMOFF_DAMPED, comm12_nondamp ; If pwm off not damped - branch + CpFET_off + mov A, #NFETON_DELAY ; Delay + djnz ACC, $ +comm12_nondamp: + ApFET_on ; Ap on + Set_Comp_Phase_B ; Set comparator to phase B + mov Comm_Phase, #2 + setb EA ; Enable all interrupts + ret + +comm2comm3: + clr EA ; Disable all interrupts + jb Flags2.PGM_PWMOFF_DAMPED, comm23_damp + jmp comm23_nondamp +comm23_damp: +IF DAMPED_MODE_ENABLE == 0 + mov DPTR, #pwm_bnfet_apfet_on_fast +ENDIF +IF DAMPED_MODE_ENABLE == 1 + mov DPTR, #pwm_bnfet_apfet_on_safe +ENDIF + jnb Flags2.CURR_PWMOFF_DAMPED, comm23_nfet ; If pwm off not damped - branch + BpFET_off + CpFET_off + mov A, #NFETON_DELAY ; Delay + djnz ACC, $ + jmp comm23_nfet +comm23_nondamp: + mov DPTR, #pwm_bfet_on +comm23_nfet: + CnFET_off ; Cn off + jnb Flags0.PWM_ON, comm23_cp ; Is pwm on? + BnFET_on ; Yes - Bn on +comm23_cp: + Set_Comp_Phase_C ; Set comparator to phase C + mov Comm_Phase, #3 + setb EA ; Enable all interrupts + ret + +comm3comm4: + clr EA ; Disable all interrupts + ApFET_off ; Ap off + jb Flags2.PGM_PWMOFF_DAMPED, comm34_damp + jmp comm34_nondamp +comm34_damp: +IF DAMPED_MODE_ENABLE == 0 + mov DPTR, #pwm_bnfet_cpfet_on_fast + jb Flags2.PGM_PWMOFF_DAMPED_LIGHT, comm34_nondamp +ENDIF +IF DAMPED_MODE_ENABLE == 1 + mov DPTR, #pwm_bnfet_cpfet_on_safe +ENDIF + jnb Flags2.CURR_PWMOFF_DAMPED, comm34_nondamp ; If pwm off not damped - branch + BpFET_off + mov A, #NFETON_DELAY ; Delay + djnz ACC, $ +comm34_nondamp: + CpFET_on ; Cp on + Set_Comp_Phase_A ; Set comparator to phase A + mov Comm_Phase, #4 + setb EA ; Enable all interrupts + ret + +comm4comm5: + clr EA ; Disable all interrupts + jb Flags2.PGM_PWMOFF_DAMPED, comm45_damp + jmp comm45_nondamp +comm45_damp: +IF DAMPED_MODE_ENABLE == 0 + mov DPTR, #pwm_anfet_cpfet_on_fast +ENDIF +IF DAMPED_MODE_ENABLE == 1 + mov DPTR, #pwm_anfet_cpfet_on_safe +ENDIF + jnb Flags2.CURR_PWMOFF_DAMPED, comm45_nfet ; If pwm off not damped - branch + ApFET_off + BpFET_off + mov A, #NFETON_DELAY ; Delay + djnz ACC, $ + jmp comm45_nfet +comm45_nondamp: + mov DPTR, #pwm_afet_on +comm45_nfet: + BnFET_off ; Bn off + jnb Flags0.PWM_ON, comm45_cp ; Is pwm on? + AnFET_on ; Yes - An on +comm45_cp: + Set_Comp_Phase_B ; Set comparator to phase B + mov Comm_Phase, #5 + setb EA ; Enable all interrupts + ret + +comm5comm6: + clr EA ; Disable all interrupts + CpFET_off ; Cp off + jb Flags2.PGM_PWMOFF_DAMPED, comm56_damp + jmp comm56_nondamp +comm56_damp: +IF DAMPED_MODE_ENABLE == 0 + mov DPTR, #pwm_anfet_bpfet_on_fast + jb Flags2.PGM_PWMOFF_DAMPED_LIGHT, comm56_nondamp +ENDIF +IF DAMPED_MODE_ENABLE == 1 + mov DPTR, #pwm_anfet_bpfet_on_safe +ENDIF + jnb Flags2.CURR_PWMOFF_DAMPED, comm56_nondamp ; If pwm off not damped - branch + ApFET_off + mov A, #NFETON_DELAY ; Delay + djnz ACC, $ +comm56_nondamp: + BpFET_on ; Bp on + Set_Comp_Phase_C ; Set comparator to phase C + mov Comm_Phase, #6 + setb EA ; Enable all interrupts + ret + +comm6comm1: + clr EA ; Disable all interrupts + jb Flags2.PGM_PWMOFF_DAMPED, comm61_damp + jmp comm61_nondamp +comm61_damp: +IF DAMPED_MODE_ENABLE == 0 + mov DPTR, #pwm_cnfet_bpfet_on_fast +ENDIF +IF DAMPED_MODE_ENABLE == 1 + mov DPTR, #pwm_cnfet_bpfet_on_safe +ENDIF + jnb Flags2.CURR_PWMOFF_DAMPED, comm61_nfet ; If pwm off not damped - branch + ApFET_off + CpFET_off + mov A, #NFETON_DELAY ; Delay + djnz ACC, $ + jmp comm61_nfet +comm61_nondamp: + mov DPTR, #pwm_cfet_on +comm61_nfet: + AnFET_off ; An off + jnb Flags0.PWM_ON, comm61_cp ; Is pwm on? + CnFET_on ; Yes - Cn on +comm61_cp: + Set_Comp_Phase_A ; Set comparator to phase A + mov Comm_Phase, #1 + setb EA ; Enable all interrupts + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Switch power off routine +; +; No assumptions +; +; Switches all fets off +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +switch_power_off: + mov DPTR, #pwm_nofet_on ; Set DPTR register to pwm_nofet_on label + All_pFETs_Off ; Turn off all pfets + All_nFETs_Off ; Turn off all nfets + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Decrement stepper step routine +; +; No assumptions +; +; Decrements the stepper step +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +;decrement_stepper_step: +; clr C +; mov A, Wt_Stepper_Step_L +; subb A, Stepper_Step_End_L ; Minimum Stepper_Step_End +; mov A, Wt_Stepper_Step_H +; subb A, Stepper_Step_End_H +; jnc decrement_step ; Branch if same or higher than minimum +; ret + +;decrement_step: + ; Load programmed startup acceleration +;读取Startup acceleration +; mov Temp1, #Pgm_Startup_Accel ; Load startup accel +; mov A, @Temp1 +; mov Temp8, A ; Store in Temp8 + ; Check acceleration setting and set step size accordingly +; clr C +; mov A, Temp8 +; subb A, #5 +; jnc dec_step_high +; clr C +; mov A, Temp8 +; subb A, #4 +; jnc dec_step_med_high +; clr C +; mov A, Temp8 +; subb A, #3 +; jnc dec_step_med +; clr C +; mov A, Temp8 +; subb A, #2 +; jnc dec_step_med_low +; clr C +; mov A, Temp8 +; subb A, #1 +; jnc dec_step_low +;Start acceleration = 第5项 +;dec_step_high: +; clr C +; mov A, Wt_Stepper_Step_L +; subb A, #low(30 SHL 1) +; mov Temp1, A +; jmp decrement_step_exit +;Start acceleration = 第4项 +;dec_step_med_high: +; clr C +; mov A, Wt_Stepper_Step_L +; subb A, #low(20 SHL 1) +; mov Temp1, A +; jmp decrement_step_exit +;Start acceleration = 第3项 +;dec_step_med: +; clr C +; mov A, Wt_Stepper_Step_L +; subb A, #low(13 SHL 1) +; mov Temp1, A +; jmp decrement_step_exit +;Start acceleration = 第2项 +;dec_step_med_low: +; clr C +; mov A, Wt_Stepper_Step_L +; subb A, #low(9 SHL 1) +; mov Temp1, A +; jmp decrement_step_exit +;Start acceleration = 第1项 +;dec_step_low: +; clr C +; mov A, Wt_Stepper_Step_L +; subb A, #low(5 SHL 1) +; mov Temp1, A +; jmp decrement_step_exit + +;decrement_step_exit: +; mov A, Wt_Stepper_Step_H +; subb A, #0 +; mov Temp2, A +; mov Wt_Stepper_Step_L, Temp1 +; mov Wt_Stepper_Step_H, Temp2 +; ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Stepper timer wait +; +; No assumptions +; +; Waits for the stepper step timer to elapse +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +;stepper_timer_wait: +; jnb Flags0.T3_PENDING, ($+6) ; Timer pending? +; ljmp stepper_timer_wait ; Yes, go back +; ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Set default parameters +; +; No assumptions +; +; Sets default programming parameters +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +set_default_parameters: +IF MODE == 0 ; Main + mov Temp1, #Pgm_Gov_P_Gain + mov @Temp1, #DEFAULT_PGM_MAIN_P_GAIN + mov Temp1, #Pgm_Gov_I_Gain + mov @Temp1, #DEFAULT_PGM_MAIN_I_GAIN + mov Temp1, #Pgm_Gov_Mode + mov @Temp1, #DEFAULT_PGM_MAIN_GOVERNOR_MODE + mov Temp1, #Pgm_Gov_Range + mov @Temp1, #DEFAULT_PGM_MAIN_GOVERNOR_RANGE + mov Temp1, #Pgm_Low_Voltage_Lim + mov @Temp1, #DEFAULT_PGM_MAIN_LOW_VOLTAGE_LIM + mov Temp1, #Pgm_Startup_Pwr + mov @Temp1, #DEFAULT_PGM_MAIN_STARTUP_PWR + mov Temp1, #Pgm_Startup_Rpm + mov @Temp1, #DEFAULT_PGM_MAIN_STARTUP_RPM + mov Temp1, #Pgm_Startup_Accel + mov @Temp1, #DEFAULT_PGM_MAIN_STARTUP_ACCEL + mov Temp1, #Pgm_Startup_Method + mov @Temp1, #DEFAULT_PGM_MAIN_STARTUP_METHOD + mov Temp1, #Pgm_Comm_Timing + mov @Temp1, #DEFAULT_PGM_MAIN_COMM_TIMING + mov Temp1, #Pgm_Throttle_Rate + mov @Temp1, #DEFAULT_PGM_MAIN_THROTTLE_RATE + mov Temp1, #Pgm_Damping_Force + mov @Temp1, #DEFAULT_PGM_MAIN_DAMPING_FORCE + mov Temp1, #Pgm_Pwm_Freq + mov @Temp1, #DEFAULT_PGM_MAIN_PWM_FREQ + mov Temp1, #Pgm_Demag_Comp + mov @Temp1, #DEFAULT_PGM_MAIN_DEMAG_COMP + mov Temp1, #Pgm_Direction_Rev + mov @Temp1, #DEFAULT_PGM_MAIN_DIRECTION_REV + mov Temp1, #Pgm_Input_Pol + mov @Temp1, #DEFAULT_PGM_MAIN_RCP_PWM_POL + mov Temp1, #Pgm_Motor_Idle + mov @Temp1, #0 +ENDIF +IF MODE == 1 ; Tail + mov Temp1, #Pgm_Motor_Gain + mov @Temp1, #DEFAULT_PGM_TAIL_GAIN + mov Temp1, #Pgm_Motor_Idle + mov @Temp1, #DEFAULT_PGM_TAIL_IDLE_SPEED + mov Temp1, #Pgm_Startup_Pwr + mov @Temp1, #DEFAULT_PGM_TAIL_STARTUP_PWR + mov Temp1, #Pgm_Startup_Rpm + mov @Temp1, #DEFAULT_PGM_TAIL_STARTUP_RPM + mov Temp1, #Pgm_Startup_Accel + mov @Temp1, #DEFAULT_PGM_TAIL_STARTUP_ACCEL + mov Temp1, #Pgm_Startup_Method + mov @Temp1, #DEFAULT_PGM_TAIL_STARTUP_METHOD + mov Temp1, #Pgm_Comm_Timing + mov @Temp1, #DEFAULT_PGM_TAIL_COMM_TIMING + mov Temp1, #Pgm_Throttle_Rate + mov @Temp1, #DEFAULT_PGM_TAIL_THROTTLE_RATE + mov Temp1, #Pgm_Damping_Force + mov @Temp1, #DEFAULT_PGM_TAIL_DAMPING_FORCE + mov Temp1, #Pgm_Pwm_Freq + mov @Temp1, #DEFAULT_PGM_TAIL_PWM_FREQ + mov Temp1, #Pgm_Demag_Comp + mov @Temp1, #DEFAULT_PGM_TAIL_DEMAG_COMP + mov Temp1, #Pgm_Direction_Rev + mov @Temp1, #DEFAULT_PGM_TAIL_DIRECTION_REV + mov Temp1, #Pgm_Input_Pol + mov @Temp1, #DEFAULT_PGM_TAIL_RCP_PWM_POL + mov Temp1, #Pgm_Gov_Mode + mov @Temp1, #4 +ENDIF +IF MODE == 2 ; Multi + mov Temp1, #Pgm_Fir_Key ;2013.8.27 增加关键字默认值 + mov @Temp1, #DEFAULT_PGM_MULTI_FIRST_KEYWORD + mov Temp1, #Pgm_Gov_P_Gain + mov @Temp1, #DEFAULT_PGM_MAIN_P_GAIN ;Pgm_Gov_P_Gain=9 P增益 + mov Temp1, #Pgm_Gov_I_Gain + mov @Temp1, #DEFAULT_PGM_MAIN_I_GAIN ;Pgm_Gov_I_Gain=9 I增益 + mov Temp1, #Pgm_Gov_Mode + mov @Temp1, #DEFAULT_PGM_MAIN_GOVERNOR_MODE ;Pgm_Gov_Mode=4 + mov Temp1, #Pgm_Motor_Gain + mov @Temp1, #DEFAULT_PGM_MULTI_GAIN ;gm_Motor_Gain=3 + mov Temp1, #Pgm_Low_Voltage_Lim + mov @Temp1, #DEFAULT_PGM_MULTI_LOW_VOLTAGE_LIM ;Pgm_Low_Voltage_Lim=1 低压保护：1=Off 2=3.0V/c 3=3.1V/c 4=3.2V/c 5=3.3V/c 6=3.4V/c + mov Temp1, #Pgm_Low_Voltage_Ctl + mov @Temp1, #DEFAULT_PGM_MULTI_LOW_VOLTAGE_CTL ;Pgm_Low_Voltage_Ctl=1 低压保护: 1=功率逐渐降低到31% 2=关闭输出 + mov Temp1, #Pgm_Startup_Pwr + mov @Temp1, #DEFAULT_PGM_MULTI_STARTUP_PWR ;Pgm_Startup_Pwr=10 + mov Temp1, #Pgm_Startup_Rpm + mov @Temp1, #DEFAULT_PGM_MULTI_STARTUP_RPM ;Pgm_Startup_Rpm=1 启动转速：1=0.67 2=0.8 3=1.00 4=1.25 5=1.5 + mov Temp1, #Pgm_Startup_Accel + mov @Temp1, #DEFAULT_PGM_MULTI_STARTUP_ACCEL ;Pgm_Startup_Accel=5 +; mov Temp1, #Pgm_Startup_Method +; mov @Temp1, #DEFAULT_PGM_MULTI_STARTUP_METHOD ;Pgm_Startup_Method=2 启动方式：1=步进启动 2=直接启动 + mov Temp1, #Pgm_Comm_Timing + mov @Temp1, #DEFAULT_PGM_MULTI_COMM_TIMING ;Pgm_Comm_Timing=3 进角时间：1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=High + mov Temp1, #Pgm_Throttle_Rate + mov @Temp1, #DEFAULT_PGM_MULTI_THROTTLE_RATE ;Pgm_Throttle_Rate=13 油门范围：1=2 2=3 3=4 4=6 5=8 6=12 7=16 8=24 9=32 10=48 11=64 12=128 13=255 + mov Temp1, #Pgm_Damping_Force + mov @Temp1, #DEFAULT_PGM_MULTI_DAMPING_FORCE ;Pgm_Damping_Force=6 刹车力度：1=VeryLow 2=Low 3=MediumLow 4=MediumHigh 5=High 6=Highest + mov Temp1, #Pgm_Pwm_Freq + mov @Temp1, #DEFAULT_PGM_MULTI_PWM_FREQ ;Pgm_Pwm_Freq=1 1=Low 2=High 3=DampedLight 4=Damped + mov Temp1, #Pgm_Demag_Comp + mov @Temp1, #DEFAULT_PGM_MULTI_DEMAG_COMP ;Pgm_Demag_Comp=2 1=Disabled 2=15/0 3=15/7.5 4=7.5/7.5 5=7.5/15 (degrees blind advance / power off) + mov Temp1, #Pgm_Direction_Rev + mov @Temp1, #DEFAULT_PGM_MULTI_DIRECTION_REV ;Pgm_Direction_Rev=1 方向设置：1=正转 2=反转 + mov Temp1, #Pgm_Input_Pol + mov @Temp1, #DEFAULT_PGM_MULTI_RCP_PWM_POL ;Pgm_Input_Pol=1 接收信号极性设置：1=Positive（正） 2=Negative（负） + mov Temp1, #Pgm_Motor_Idle + mov @Temp1, #0 ;Pgm_Motor_Idle=0 +ENDIF + mov Temp1, #Pgm_Enable_TX_Program + mov @Temp1, #DEFAULT_PGM_ENABLE_TX_PROGRAM ;Pgm_Enable_TX_Program=1 1=Enabled 0=Disabled + mov Temp1, #Pgm_Main_Rearm_Start + mov @Temp1, #DEFAULT_PGM_MAIN_REARM_START ;Pgm_Main_Rearm_Start=0 1=Enabled 0=Disabled + mov Temp1, #Pgm_Gov_Setup_Target + mov @Temp1, #DEFAULT_PGM_MAIN_GOV_SETUP_TARGET ;Pgm_Gov_Setup_Target=180 Target for governor in setup mode. Corresponds to 70% throttle + mov Temp1, #Pgm_Ppm_Min_Throttle + mov @Temp1, #DEFAULT_PGM_PPM_MIN_THROTTLE ;Pgm_Ppm_Min_Throttle=37 最小油门：4*37+1000=1148 + mov Temp1, #Pgm_Ppm_Max_Throttle + mov @Temp1, #DEFAULT_PGM_PPM_MAX_THROTTLE ;Pgm_Ppm_Max_Throttle=208 最大油门：4*208+1000=1832 + mov Temp1, #Pgm_Beep_Strength + mov @Temp1, #DEFAULT_PGM_MAIN_BEEP_STRENGTH ;Pgm_Beep_Strength=120 声强设置 + mov Temp1, #Pgm_Beacon_Strength + mov @Temp1, #DEFAULT_PGM_MAIN_BEACON_STRENGTH ;Pgm_Beacon_Strength=200 灯亮长Beacon strength？？？？ + mov Temp1, #Pgm_Beacon_Delay + mov @Temp1, #DEFAULT_PGM_MAIN_BEACON_DELAY ;Pgm_Beacon_Delay=4 灯亮时间？？？？1=30s 2=1m 3=2m 4=3m 5=Infinite + mov Temp1, #Pgm_BEC_Voltage_High + mov @Temp1, #DEFAULT_PGM_BEC_VOLTAGE_HIGH ;Pgm_BEC_Voltage_High=0 0=Low 1= High + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Decode parameters +; +; No assumptions +; +; Decodes programming parameters +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +decode_parameters: + ; Load programmed damping force +;读取Damping force + mov Temp1, #Pgm_Damping_Force; Load damping force + mov A, @Temp1 + mov Temp8, A ; Store in Temp8 + ; Decode damping +;Damping force = 第1项 + clr Flags2.PGM_PWMOFF_DAMPED + clr Flags2.PGM_PWMOFF_DAMPED_LIGHT + clr Flags2.PGM_PWMOFF_DAMPED_FULL + clr Flags2.CURR_PWMOFF_DAMPED + setb Flags2.CURR_PWMOFF_COMP_ABLE +; mov Damping_Period, #9 ; Set default +; mov Damping_On, #1 + mov Rcp_Stop_Limit,#40 + clr C + cjne Temp8, #1, decode_damping_2 ; Look for 2 + jmp decode_damping_done + +;Damping force = 第2项 +decode_damping_2: + setb Flags2.PGM_PWMOFF_DAMPED +; setb Flags2.PGM_PWMOFF_DAMPED_LIGHT +; clr Flags2.PGM_PWMOFF_DAMPED_FULL + setb Flags2.CURR_PWMOFF_DAMPED + clr Flags2.CURR_PWMOFF_COMP_ABLE + mov Damping_Period, #5 + mov Damping_On, #1 + mov Rcp_Stop_Limit,#60 + clr C + cjne Temp8, #2, decode_damping_3 ; Look for 3 + jmp decode_damping_done + +;Damping force = 第3项 +decode_damping_3: +; setb Flags2.PGM_PWMOFF_DAMPED + setb Flags2.PGM_PWMOFF_DAMPED_LIGHT +; clr Flags2.PGM_PWMOFF_DAMPED_FULL +; setb Flags2.CURR_PWMOFF_DAMPED +; clr Flags2.CURR_PWMOFF_COMP_ABLE + mov Damping_Period, #5 + mov Damping_On, #2 + mov Rcp_Stop_Limit,#30 + clr C + cjne Temp8, #3, decode_damping_4 ; Look for 4 + jmp decode_damping_done + +;Damping force = 第4项 +decode_damping_4: +; setb Flags2.PGM_PWMOFF_DAMPED +; setb Flags2.PGM_PWMOFF_DAMPED_LIGHT +; clr Flags2.PGM_PWMOFF_DAMPED_FULL +; setb Flags2.CURR_PWMOFF_DAMPED +; clr Flags2.CURR_PWMOFF_COMP_ABLE + mov Damping_Period, #5 + mov Damping_On, #3 + mov Rcp_Stop_Limit,#25 + clr C + cjne Temp8, #4, decode_damping_5 ; Look for 5 + jmp decode_damping_done + +;Damping force = 第5项 +decode_damping_5: +; setb Flags2.PGM_PWMOFF_DAMPED +; setb Flags2.PGM_PWMOFF_DAMPED_LIGHT + setb Flags2.PGM_PWMOFF_DAMPED_FULL +; setb Flags2.CURR_PWMOFF_DAMPED +; clr Flags2.CURR_PWMOFF_COMP_ABLE + mov Damping_Period, #5 + mov Damping_On, #4 + mov Rcp_Stop_Limit,#20 + clr C + cjne Temp8, #5, decode_damping_6; Look for 6 + jmp decode_damping_done + +;Damping force = 第6项 +decode_damping_6: +; setb Flags2.PGM_PWMOFF_DAMPED +; setb Flags2.PGM_PWMOFF_DAMPED_LIGHT +; setb Flags2.PGM_PWMOFF_DAMPED_FULL +; setb Flags2.CURR_PWMOFF_DAMPED +; clr Flags2.CURR_PWMOFF_COMP_ABLE + mov Damping_Period, #5 + mov Damping_On, #5 + mov Rcp_Stop_Limit,#10 + +;*********************************************** +; 读取编程PWM频率(1) +;带硬刹/软刹 +;*********************************************** +decode_damping_done: + ; Load programmed pwm frequency +;读取Pwm frequency + mov Temp1, #Pgm_Pwm_Freq ; Load pwm freq + mov A, @Temp1 + mov Temp8, A ; Store in Temp8 +IF MODE == 0 ; Main + clr Flags2.PGM_PWMOFF_DAMPED_LIGHT + clr C + cjne Temp8, #3, ($+5) + setb Flags2.PGM_PWMOFF_DAMPED_LIGHT + clr Flags2.PGM_PWMOFF_DAMPED_FULL +ENDIF +IF MODE >= 1 ; Tail or multi +;////// clr Flags2.PGM_PWMOFF_DAMPED_LIGHT +;////// clr C +;////// cjne Temp8, #3, ($+5) +;Pwm frequency = 第3项 +;////// setb Flags2.PGM_PWMOFF_DAMPED_LIGHT +;////// clr Flags2.PGM_PWMOFF_DAMPED_FULL +;////// clr C +;////// cjne Temp8, #4, ($+5) +;Pwm frequency = 第4项 +;////// setb Flags2.PGM_PWMOFF_DAMPED_FULL +ENDIF +;Pwm frequency = 第1项 +;////// clr Flags2.PGM_PWMOFF_DAMPED ; Set damped flag if fully damped or damped light is set +;////// mov A, #((1 SHL PGM_PWMOFF_DAMPED_FULL)+(1 SHL PGM_PWMOFF_DAMPED_LIGHT)) +;////// anl A, Flags2 ; Check if any damped mode is set +;////// jz ($+4) +;Pwm frequency = 第2项 +;////// setb Flags2.PGM_PWMOFF_DAMPED +;Pwm frequency = 第1项 +;////// clr Flags2.CURR_PWMOFF_DAMPED ; Set non damped status as start +;////// jz ($+4) +;Pwm frequency = 第2项 +;////// setb Flags2.CURR_PWMOFF_DAMPED ; Set non damped status as start if damped +;Pwm frequency = 第1项 +;////// setb Flags2.CURR_PWMOFF_COMP_ABLE ; Set comparator usable status +;////// jz ($+4) +;Pwm frequency = 第2项 +;////// clr Flags2.CURR_PWMOFF_COMP_ABLE ; Set comparator not usable status if damped +;*********************************************** +; 读取编程电机旋转方向 +;*********************************************** +;读取Rotation direction +;Rotation direction = 第1项 + clr Flags3.PGM_DIR_REV + mov Temp1, #Pgm_Direction_Rev + mov A, @Temp1 + jnb ACC.1, ($+5) +;Rotation direction = 第2项 + setb Flags3.PGM_DIR_REV +;*********************************************** +; 读取编程输入信号RC PWM脉冲有效极性 +;*********************************************** +;读取Input pwm polarity +;Input pwm polarity = 第1项 + clr Flags3.PGM_RCP_PWM_POL + mov Temp1, #Pgm_Input_Pol + mov A, @Temp1 + jnb ACC.1, ($+5) +;Input pwm polarity = 第2项 + setb Flags3.PGM_RCP_PWM_POL +;*********************************************** +; 读取编程PWM频率(2) +;不带硬刹/软刹 +;*********************************************** + clr C + mov A, Temp8 + subb A, #1 + jz decode_pwm_freq_low +;Pwm frequency = 第1项 + mov CKCON, #01h ; Timer0 set for clk/4 (22kHz pwm) + setb Flags2.PGM_PWM_HIGH_FREQ + jmp decode_pwm_freq_end +;Pwm frequency = 第2项 +decode_pwm_freq_low: + mov CKCON, #00h ; Timer0 set for clk/12 (8kHz pwm) + clr Flags2.PGM_PWM_HIGH_FREQ + +decode_pwm_freq_end: + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Decode governor gain +; +; No assumptions +; +; Decodes governor gains +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +decode_governor_gains: + ; Decode governor gains +;读取 Closed loop P gain + mov Temp1, #Pgm_Gov_P_Gain ; Decode governor P gain + mov A, @Temp1 + dec A + mov DPTR, #GOV_GAIN_TABLE + movc A, @A+DPTR + mov Temp1, #Pgm_Gov_P_Gain_Decoded + mov @Temp1, A +;读取 Closed loop I gain + mov Temp1, #Pgm_Gov_I_Gain ; Decode governor P gain + mov A, @Temp1 + dec A + mov DPTR, #GOV_GAIN_TABLE + movc A, @A+DPTR + mov Temp1, #Pgm_Gov_I_Gain_Decoded + mov @Temp1, A + call switch_power_off ; Reset DPTR + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Decode throttle rate +; +; No assumptions +; +; Decodes throttle rate +; +;读取中点油门范围 +;**** **** **** **** **** **** **** **** **** **** **** **** **** +decode_throttle_rate: + ; Decode throttle rate +;读取 Throttle change rate + mov Temp1, #Pgm_Throttle_Rate + mov A, @Temp1 + dec A + mov DPTR, #THROTTLE_RATE_TABLE + movc A, @A+DPTR + mov Temp1, #Pgm_Throttle_Rate_Decoded + mov @Temp1, A + call switch_power_off ; Reset DPTR + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Decode startup power +; +; No assumptions +; +; Decodes startup power +; +;编程启动力度 +;**** **** **** **** **** **** **** **** **** **** **** **** **** +decode_startup_power: + ; Decode startup power +;读取 Startup power + mov Temp1, #Pgm_Startup_Pwr + mov A, @Temp1 + dec A + mov DPTR, #STARTUP_POWER_TABLE + movc A, @A+DPTR + mov Temp1, #Pgm_Startup_Pwr_Decoded + mov @Temp1, A + call switch_power_off ; Reset DPTR + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Decode demag compensation +; +; No assumptions +; +; Decodes throttle rate +; +;编程demag compensation参数 +;**** **** **** **** **** **** **** **** **** **** **** **** **** +decode_demag_comp: + ; Decode demag compensation +;读取Demag compensation + mov Temp1, #Pgm_Demag_Comp + mov A, @Temp1 + dec A + mov DPTR, #DEMAG_WAIT_TABLE + movc A, @A+DPTR + mov Temp1, #Pgm_Demag_Comp_Wait_Decoded + mov @Temp1, A + mov Temp1, #Pgm_Demag_Comp + mov A, @Temp1 + dec A + mov DPTR, #DEMAG_POWER_TABLE + movc A, @A+DPTR + mov Temp1, #Pgm_Demag_Comp_Power_Decoded + mov @Temp1, A + call switch_power_off ; Reset DPTR + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Set BEC voltage +; +; No assumptions +; +; Sets the BEC output voltage low or high +; +;读取 BEC +;**** **** **** **** **** **** **** **** **** **** **** **** **** +set_bec_voltage: + ; Set bec voltage +IF DUAL_BEC_VOLTAGE == 1 + Set_BEC_Lo ; Set default to low +;读取 Pgm_BEC_Voltage_High + mov Temp1, #Pgm_BEC_Voltage_High + mov A, @Temp1 + jz set_bec_voltage_exit + + Set_BEC_Hi ; Set to high + +set_bec_voltage_exit: +ENDIF + ret + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Find throttle gain +; +; The difference between max and min throttle must be more than 520us (a Pgm_Ppm_xxx_Throttle difference of 130) +; +; Finds throttle gain from throttle calibration values +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +find_throttle_gain: + ; Load programmed minimum and maximum throttle +;读取Pgm_Ppm_Min_Throttle和Pgm_Ppm_Max_Throttle增量 + mov Temp1, #Pgm_Ppm_Min_Throttle + mov A, @Temp1 + mov Temp3, A + mov Temp1, #Pgm_Ppm_Max_Throttle + mov A, @Temp1 + mov Temp4, A + ; Check if full range is chosen + jnb Flags3.FULL_THROTTLE_RANGE, find_throttle_gain_calculate + + mov Temp3, #0 + mov Temp4, #255 + +find_throttle_gain_calculate: + ; Calculate difference + clr C + mov A, Temp4 + subb A, Temp3 + mov Temp5, A + ; Check that difference is minimum 130 + clr C + subb A, #130 + jnc ($+4) + + mov Temp5, #130 + + ; Find gain + mov Ppm_Throttle_Gain, #0 +test_gain: + inc Ppm_Throttle_Gain + mov A, Temp5 + mov B, Ppm_Throttle_Gain ; A has difference, B has gain + mul AB + clr C + mov A, B + subb A, #128 + jc test_gain + ret + + + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +;**** **** **** **** **** **** **** **** **** **** **** **** **** +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Main program start +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +;**** **** **** **** **** **** **** **** **** **** **** **** **** +;**** **** **** **** **** **** **** **** **** **** **** **** **** + +reset: + ; Select register bank 0 for main program routines + clr PSW.3 ; Select register bank 0 for main program routines + ; Disable the WDT. + anl PCA0MD, #NOT(40h) ; Clear watchdog enable bit + ; Initialize stack + mov SP, #0c0h ; Stack = 64 upper bytes of RAM + ; Initialize VDD monitor + orl VDM0CN, #080h ; Enable the VDD monitor + + mov Temp2, #3 +wait_100us: + djnz ACC, $ + djnz Temp2,wait_100us + +; call wait1ms ; Wait at least 100us + mov RSTSRC, #02h ; Set VDD monitor as a reset source (PORSF) + ; Set clock frequency + orl OSCICN, #03h ; Set clock divider to 1 + mov A, OSCICL + add A, #04h ; 24.5MHz to 24MHz (~0.5% per step) + jc reset_cal_done ; Is carry set? - skip next instruction + + mov OSCICL, A + +reset_cal_done: + ; Ports initialization + mov P0, #P0_INIT + mov P0MDOUT, #P0_PUSHPULL + mov P0MDIN, #P0_DIGITAL + mov P0SKIP, #P0_SKIP + mov P1, #P1_INIT + mov P1MDOUT, #P1_PUSHPULL + mov P1MDIN, #P1_DIGITAL + mov P1SKIP, #P1_SKIP +IF PORT3_EXIST == 1 + mov P2, #P2_INIT +ENDIF + mov P2MDOUT, #P2_PUSHPULL +IF PORT3_EXIST == 1 + mov P2MDIN, #P2_DIGITAL + mov P2SKIP, #P2_SKIP + mov P3, #P3_INIT + mov P3MDOUT, #P3_PUSHPULL + mov P3MDIN, #P3_DIGITAL +ENDIF + mov XBR1, #41h ; Xbar enabled, CEX0 routed to pin Rcp_In + ; Switch power off + call switch_power_off + ; Clear RAM + clr A ; Clear accumulator + mov Temp1, A ; Clear Temp1 +clear_ram: + mov @Temp1, A ; Clear RAM + djnz Temp1, clear_ram ; Is A not zero? - jump + ; Set default programmed parameters + call set_default_parameters + ; Read all programmed parameters + call read_all_eeprom_parameters + ; Decode parameters + call decode_parameters + ; Decode governor gains + call decode_governor_gains + ; Decode throttle rate + call decode_throttle_rate + ; Decode startup power + call decode_startup_power + ; Decode demag compensation + call decode_demag_comp + ; Set BEC voltage + call set_bec_voltage + ; Find throttle gain from stored min and max settings + call find_throttle_gain + ; Set beep strength +;读取超强设置 + mov Temp1, #Pgm_Beep_Strength + mov Beep_Strength, @Temp1 + ; Switch power off + call switch_power_off + ; Timer control + mov TCON, #50h ; Timer0 and timer1 enabled + ; Timer mode + mov TMOD, #02h ; Timer0 as 8bit + ; Timer2: clk/12 for 128us and 32ms interrupts + mov TMR2CN, #24h ; Timer2 enabled, low counter interrups enabled + ; Timer3: clk/12 for commutation timing + mov TMR3CN, #04h ; Timer3 enabled + ; PCA + mov PCA0CN, #40h ; PCA enabled + ; Initializing beep 上电叫3声 + call wait1s + call wait1s + + ; Enable interrupts + mov IE, #22h ; Enable timer0 and timer2 interrupts + mov IP, #02h ; High priority to timer0 interrupts + mov EIE1, #90h ; Enable timer3 and PCA0 interrupts + mov EIP1, #10h ; High priority to PCA interrupts + ; Initialize comparator + mov CPT0CN, #80h ; Comparator enabled, no hysteresis + mov CPT0MD, #03h ; Comparator response time 1us +IF COMP1_USED==1 + mov CPT1CN, #80h ; Comparator enabled, no hysteresis + mov CPT1MD, #03h ; Comparator response time 1us +ENDIF + ; Initialize ADC + Initialize_Adc ; Initialize ADC operation + call wait1ms + setb EA ; Enable all interrupts + ; Measure number of lipo cells + call Measure_Lipo_Cells ; Measure number of lipo cells + ; Initialize rc pulse + Rcp_Int_Enable ; Enable interrupt + Rcp_Int_First ; Enable interrupt and set to first edge + Rcp_Clear_Int_Flag ; Clear interrupt flag + clr Flags2.RCP_EDGE_NO ; Set first edge flag + call wait200ms + ; Set initial arm variable + mov Initial_Arm, #1 + mov OS_FLAG, #1 +; mov Pca_First_Int, #1 + + ; Measure PWM frequency +measure_pwm_freq_init: + setb Flags0.RCP_MEAS_PWM_FREQ ; Set measure pwm frequency flag +; clr C ;2014.08.08 信号滤波 +; mov A, Pgm_Pulse_Cnt ;2014.08.08 信号滤波 +; subb A, #5 ;2014.08.08 信号滤波 +; jc measure_pwm_freq_init ;2014.08.08 信号滤波 +; mov Pca_First_Int, #0 ;2014.08.08 信号滤波 +; call wait200ms +measure_pwm_freq_start: + mov Temp3, #5 ; Number of pulses to measure +; mov Temp3, #1 +measure_pwm_freq_loop: + ; Check if period diff was accepted + mov A, Rcp_Period_Diff_Accepted +; jnz ($+4) + + jnz measure_pwm + call wait3ms + mov A, New_RCP + clr C + subb A,#230 + jc ($+7) + mov Pgm_Pulse_Cnt, #0 +; mov Temp3, #5 + ajmp measure_pwm_freq_start + + mov A, New_RCP + clr C + subb A,#30 + jnc ($+7) + mov Pgm_Pulse_Cnt, #0 +; mov Temp3, #5 + ajmp measure_pwm_freq_start + + inc Pgm_Pulse_Cnt + mov A, Pgm_Pulse_Cnt + clr C + subb A, #5 + jnc ($+6) + mov Temp3, #5 ; Reset number of pulses to measure + ajmp measure_pwm_freq_loop +; mov Temp3, #1 +measure_pwm: + call wait3ms ; Wait for next pulse (NB: Uses Temp1/2!) + mov A, New_Rcp ; Load value + clr C + subb A, #RCP_VALIDATE ; Higher than validate level? + jc measure_pwm_freq_start ; No - start over + + mov A, Flags3 ; Check pwm frequency flags +; anl A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) + anl A, #(1 SHL RCP_PWM_FREQ_1KHZ) + mov Prev_Rcp_Pwm_Freq, Curr_Rcp_Pwm_Freq ; Store as previous flags for next pulse + mov Curr_Rcp_Pwm_Freq, A ; Store current flags for next pulse + cjne A, Prev_Rcp_Pwm_Freq, measure_pwm_freq_start ; Go back if new flags not same as previous + + djnz Temp3, measure_pwm_freq_loop ; Go back if not required number of pulses seen + + ; Clear measure pwm frequency flag + clr Flags0.RCP_MEAS_PWM_FREQ + ; Set up RC pulse interrupts after pwm frequency measurement + Rcp_Int_First ; Enable interrupt and set to first edge + Rcp_Clear_Int_Flag ; Clear interrupt flag + clr Flags2.RCP_EDGE_NO ; Set first edge flag + call wait100ms ; Wait for new RC pulse + + ; Validate RC pulse +;Pgm_Main_Rearm_Start = 第1项 enabled +validate_rcp_start: + call wait3ms ; Wait for next pulse (NB: Uses Temp1/2!) + mov Temp1, #RCP_VALIDATE ; Set validate level as default +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) + mov A, #(1 SHL RCP_PWM_FREQ_1KHZ) + anl A, Flags3 ; Check pwm frequency flags + jnz ($+4) ; If a flag is set (PWM) - branch + + mov Temp1, #0 ; Set level to zero for PPM (any level will be accepted) + + clr C + mov A, New_Rcp ; Load value + subb A, Temp1 ; Higher than validate level? + jc validate_rcp_start ; No - start over + + ; Beep arm sequence start signal +;上电检测到信号后叫第1声（信号准备就绪） +;////// clr EA ; Disable all interrupts +;////// call beep_f1 ; Signal that RC pulse is ready +;////// call beep_f1 +;////// call beep_f1 +;////// setb EA ; Enable all interrupts + call wait200ms + + ; Arming sequence start + mov Gov_Arm_Target, #0 ; Clear governor arm target +arming_start: + call wait3ms +;读取Pgm_Enable_TX_Program + mov Temp1, #Pgm_Enable_TX_Program; Yes - start programming mode entry if enabled + mov A, @Temp1 + clr C + subb A, #1 ; Is TX programming enabled? + jnc arming_initial_arm_check ; Yes - proceed + + jmp program_by_tx_checked ; No - branch +;Pgm_Enable_TX_Program = 1 enabled +arming_initial_arm_check: + mov A, Initial_Arm ; Yes - check if it is initial arm sequence + clr C + subb A, #1 ; Is it the initial arm sequence? + jnc arming_ppm_check ; Yes - proceed + + jmp wait_for_power_on ; No - branch + +arming_ppm_check: + mov Temp1, #Pgm_Ppm_Min_Throttle ; Store + mov A, @Temp1 + mov Min_Throttle, A +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) + mov A, #(1 SHL RCP_PWM_FREQ_1KHZ) + anl A, Flags3 ; Check pwm frequency flags + jz throttle_high_cal_start ; If no flag is set (PPM) - branch + + ; PWM tx program entry +; clr C +; mov A, New_Rcp ; Load new RC pulse value +; subb A, #200 ; Is RC pulse max? +; jc program_by_tx_entry_wait_pwm ; Yes - proceed +; subb A, #RCP_MAX ; Is RC pulse max? +; jnc program_by_tx_entry_pwm ; Yes - proceed + +; jmp program_by_tx_checked ; No - branch + +;program_by_tx_entry_pwm: +; clr EA ; Disable all interrupts +; call beep_f4 +; setb EA ; Enable all interrupts +; call wait100ms +; clr C +; mov A, New_Rcp ; Load new RC pulse value +; subb A, #RCP_STOP ; Below stop? +; subb A, #200 +; jnc program_by_tx_entry_pwm ; No - start over + +;program_by_tx_entry_wait_pwm: + + clr EA + call beep_f2 + call beep_f3 + call beep_f1 + + mov TMOD, #11h ;T0/T1 工作方式1 + mov CKCON, #02h ;T0/T1 48分频 + mov P0MDIN, #80h + mov P0SKIP, #0FFh + call program_by_card ; Yes - enter programming mode +; ajmp program_by_card_entry + +;program_by_tx_entry_wait_pwm: +; clr EA ; Disable all interrupts +; call beep_f1 +; call wait10ms +; call beep_f1 +; setb EA ; Enable all interrupts +; call wait100ms +; clr C +; mov A, New_Rcp ; Load new RC pulse value +; subb A, #RCP_MAX ; At or above max? +; jc program_by_tx_entry_wait_pwm ; No - start over + +; call program_by_tx ; Yes - enter programming mode +; jmp program_by_tx_checked + + ; PPM throttle calibration and tx program entry +;计算PPM最大，最小油门并存储 +throttle_high_cal_start: + mov Temp8, #5 ; Set 3 seconds wait time +throttle_high_cal: + setb Flags3.FULL_THROTTLE_RANGE ; Set range to 1000-2020us + call Find_Throttle_Gain ; Set throttle gain + call wait100ms ; Wait for new throttle value + clr EA ; Disable interrupts (freeze New_Rcp value) + clr Flags3.FULL_THROTTLE_RANGE ; Set programmed range + call Find_Throttle_Gain ; Set throttle gain + clr C + mov A, New_Rcp ; Load new RC pulse value + subb A, #220 ; Is RC pulse above midstick? 1840 + setb EA ; Enable interrupts + jc ($+6) + djnz Temp8, throttle_high_cal + ajmp throttle_high_cal_save + + clr C + mov A, New_Rcp ; Load new RC pulse value + subb A, Min_Throttle ; Is RC pulse above midstick? 1148 + jnc ($+4) + ajmp arm_target_updated ; No - branch 检测低油门 + + clr EA + call beep_f1; + call wait100ms + setb EA + ajmp throttle_high_cal_start + +throttle_high_cal_save: +; call wait1s + clr C + mov A, New_Rcp ; Limit to max 250 + subb A, #5 ; Subtract about 2% and ensure that it is 250 or lower + mov Temp1, #Pgm_Ppm_Max_Throttle ; Store + mov @Temp1, A + call set_commu_sum_to_zero ;////////////////////////////////////************************** + call erase_and_store_all_in_eeprom + clr EA + call beep_f1;/////////////////////////////////////////成功存储最大油门叫声 + call beep_f1;/////////////////////////////////////////成功存储最大油门叫声 + call wait30ms;/////////////////////////////////////////成功存储最大油门叫声 + call beep_f4;/////////////////////////////////////////成功存储最大油门叫声 + call beep_f4;/////////////////////////////////////////成功存储最大油门叫声 + setb EA + + call wait1s + call wait1s +wait_loop_1: + clr C + mov A, New_Rcp ; Load new RC pulse value + subb A, #215 ;Is RC pulse above midstick? + jnc program_by_tx_entry_wait_ppm + clr C + mov A, New_Rcp ; Load new RC pulse value + subb A, #60 ;Is RC pulse above midstick? + jnc wait_loop_1 + ajmp throttle_low_cal_start + + ;PPM编程模式 +program_by_tx_entry_wait_ppm: + call program_by_tx ; Yes - enter programming mode 成功进入编程模式 + +;最小油门设置及退出 +throttle_low_cal_start: + mov Temp8, #5 ; Set 3 seconds wait time +throttle_low_cal: +; call wait1s + setb Flags3.FULL_THROTTLE_RANGE ; Set range to 1000-2020us + call Find_Throttle_Gain ; Set throttle gain + call wait100ms + clr EA ; Disable interrupts (freeze New_Rcp value) + clr Flags3.FULL_THROTTLE_RANGE ; Set programmed range + call Find_Throttle_Gain ; Set throttle gain + clr C + mov A, New_Rcp ; Load new RC pulse value + subb A, #60 ; Below midstick? + setb EA ; Enable interrupts + jnc throttle_low_cal_start ; No - start over 在中点油门以上，则继续等待，直到检测到在中点油门以下 + djnz Temp8, throttle_low_cal ; Yes - continue to wait +; call wait3ms + mov A, New_Rcp + add A, #5 ; Add about 2% + mov Temp1, #Pgm_Ppm_Min_Throttle ; Store + mov @Temp1, A + call set_commu_sum_to_zero ;////////////////////////////////////************************** + call erase_and_store_all_in_eeprom + +; clr EA ; Disable all interrupts +; mov RSTSRC, #12h ; Generate hardware reset 强制复位系统 + + ; Set default programmed parameters +; call set_default_parameters + ; Read all programmed parameters + call read_all_eeprom_parameters + ; Decode parameters + call decode_parameters + ; Decode governor gains + call decode_governor_gains + ; Decode throttle rate + call decode_throttle_rate + ; Decode startup power + call decode_startup_power + ; Decode demag compensation + call decode_demag_comp + ; Set BEC voltage + call set_bec_voltage + ; Find throttle gain from stored min and max settings + call find_throttle_gain + +program_by_tx_checked: + clr C + mov A, New_Rcp ; Load new RC pulse value + subb A, Gov_Arm_Target ; Is RC pulse larger than arm target? + jc arm_target_updated ; No - do not update + + mov Gov_Arm_Target, New_Rcp ; Yes - update arm target + +arm_target_updated: + call wait100ms ; Wait for new throttle value + clr C + mov A, New_Rcp ; Load new RC pulse value + subb A, #RCP_STOP ; Below stop? + jc arm_end_beep ; Yes - proceed ;最小油门计算好 及 编程结束叫声 + jmp arm_target_updated ; No - start over + +;上电检测到信号后叫第2声（信号检测结束） +arm_end_beep: +;改变上电叫方式 + clr EA ; Disable all interrupts + call beep_f1 + call beep_f1 + call beep_f1 + call wait1s + ; Beep arm sequence end signal + +;************************************************* +; 增加Lipo节数叫声 2013.5.29 +;************************************************* + mov Temp8,Lipo_Cell_Count ; +lipo_cell_beep: + call beep_f1 + call beep_f1 + call wait30ms + djnz Temp8,lipo_cell_beep + call wait200ms + call wait200ms +;///// call beep_f4 ; Signal that rcpulse is ready +;///// call beep_f4 +;///// call beep_f4 +;改变信号检测OK叫声方式 2013.5.29 + call beep_f1 + call wait30ms + call wait30ms + call beep_f2 + call wait30ms + call wait30ms + call beep_f3 + call wait30ms + call wait30ms + setb EA ; Enable all interrupts + call wait100ms + + ; Measure number of lipo cells + call Measure_Lipo_Cells ; Measure number of lipo cells + + ; Clear initial arm variable + mov Initial_Arm, #0 + + ; Armed and waiting for power on +wait_for_power_on: + clr LOW_LIMIT_STOP ; 清低压保护控制方式2 停止标志 +;////// clr A +;////// mov Power_On_Wait_Cnt_L, A ; Clear wait counter +;////// mov Power_On_Wait_Cnt_H, A +wait_for_power_on_loop: + mov Temp8, #50 +;////// inc Power_On_Wait_Cnt_L ; Increment low wait counter +;////// mov A, Power_On_Wait_Cnt_L +;////// cpl A +;////// jnz wait_for_power_on_no_beep; Counter wrapping (about 1 sec)? + +;////// inc Power_On_Wait_Cnt_H ; Increment high wait counter +;******************************************************** +; 读取油门最低未操作等待叫声时间 +;******************************************************** +;读取Pgm_Beacon_Delay +;////// mov Temp1, #Pgm_Beacon_Delay +;////// mov A, @Temp1 +;////// mov Temp1, #40 ; Approximately 30 sec +;////// dec A +;////// jz beep_delay_set + +;////// mov Temp1, #80 ; Approximately 1 min +;////// dec A +;////// jz beep_delay_set + +;////// mov Temp1, #160 ; Approximately 2 min +;////// dec A +;////// jz beep_delay_set + +;////// mov Temp1, #240 ; Approximately 3 min +;////// dec A +;////// jz beep_delay_set + +;////// mov Power_On_Wait_Cnt_H, #0 ; Reset counter for infinite delay + +;//////beep_delay_set: +;////// clr C +;////// mov A, Power_On_Wait_Cnt_H +;////// subb A, Temp1 ; Check against chosen delay +;////// jc wait_for_power_on_no_beep; Has delay elapsed? +;******************************************************** +; 读取油门最低未操作等待叫声声强设置 +;******************************************************** +;////// dec Power_On_Wait_Cnt_H ; Decrement high wait counter +;读取Pgm_Beacon_Strength +;////// mov Temp1, #Pgm_Beacon_Strength +;////// mov Beep_Strength, @Temp1 +;////// clr EA ; Disable all interrupts +;////// All_pFETs_Off;///////////////////////////////////////***************************** +;////// call beep_f4 ; Signal that there is no signal +;////// setb EA ; Enable all interrupts +;读取Pgm_Beep_Strength +;////// mov Temp1, #Pgm_Beep_Strength +;////// mov Beep_Strength, @Temp1 +;////// call wait100ms ; Wait for new RC pulse to be measured + +wait_for_power_on_no_beep: + call wait3ms + mov A, Rcp_Timeout_Cnt ; Load RC pulse timeout counter value + jnz wait_for_power_on_ppm_not_missing ; If it is not zero - proceed + +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) +; anl A, Flags3 ; Check pwm frequency flags +; jnz wait_for_power_on_ppm_not_missing ; If a flag is set (PWM) - branch + + jmp measure_pwm_freq_init ; If ppm and pulses missing - go back to measure pwm frequency + +wait_for_power_on_ppm_not_missing: + clr C + mov A, New_Rcp ; Load new RC pulse value +; subb A, #RCP_STOP ; Higher than stop (plus some hysteresis)? + subb A, #(RCP_STOP+7) ; Higher than stop (plus some hysteresis)? + jc wait_for_power_on ; No - start over + djnz Temp8, wait_for_power_on_no_beep + jb LOW_LIMIT_STOP, wait_for_power_on_loop + call wait100ms ; Wait to see if start pulse was only a glitch + mov A, Rcp_Timeout_Cnt ; Load RC pulse timeout counter value + jnz ($+5) ; If it is not zero - proceed + + ljmp measure_pwm_freq_init ; If it is zero (pulses missing) - go back to measure pwm frequency + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Start entry point +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +init_start: + setb ET0 + call switch_power_off + clr A + mov Requested_Pwm, A ; Set requested pwm to zero + mov Governor_Req_Pwm, A ; Set governor requested pwm to zero + mov Current_Pwm, A ; Set current pwm to zero + mov Current_Pwm_Limited, A ; Set limited current pwm to zero + mov Pwm_Limit, #0FFh ; Set pwm limit to max + mov Temp1, #Pgm_Motor_Idle + mov Pwm_Motor_Idle, @Temp1 ; Set idle pwm to programmed value + mov Gov_Target_L, A ; Set target to zero + mov Gov_Target_H, A + mov Gov_Integral_L, A ; Set integral to zero + mov Gov_Integral_H, A + mov Gov_Integral_X, A + mov Adc_Conversion_Cnt, A + mov Gov_Active, A + mov Flags0, A ; Clear flags0 + mov Flags1, A ; Clear flags1 + mov Rcp_Stop_Cnt, A ; Set RC pulse stop count to zero + mov Rcp_Stop_Limit, A + mov Lipo_Cell_Count, A + mov Limit_Count, A + mov New_Rcp, #Rcp_MIN + call initialize_all_timings ; Initialize timing + ;**** **** **** **** **** + ; Settle phase beginning + ;**** **** **** **** **** + mov Adc_Conversion_Cnt, #TEMP_CHECK_RATE ; Make sure a temp reading is done + Set_Adc_Ip_Temp + call wait1ms + call start_adc_conversion +read_initial_temp: + Get_Adc_Status + jb AD0BUSY, read_initial_temp + Read_Adc_Result ; Read initial temperature + mov A, Temp2 + jnz ($+3) ; Is reading below 256? + + mov Temp1, A ; Yes - set average temperature value to zero + + mov Current_Average_Temp, Temp1 ; Set initial average temperature + call check_temp_voltage_and_limit_power + mov Adc_Conversion_Cnt, #TEMP_CHECK_RATE ; Make sure a temp reading is done next time + Set_Adc_Ip_Temp + + ; Go to the desired startup mode +; mov Temp1, #Pgm_Startup_Method +; mov A, @Temp1 +; jnb ACC.0, direct_method_start + +; jmp stepper_method_start + +direct_method_start: + ; Set up start operating conditions + mov Temp1, #Pgm_Pwm_Freq + mov A, @Temp1 + mov Temp7, A ; Store setting in Temp7 + mov @Temp1, #1 ; Set nondamped low frequency pwm mode + call decode_parameters ; (Decode_parameters uses Temp1 and Temp8) + mov Temp1, #Pgm_Pwm_Freq + mov A, Temp7 + mov @Temp1, A ; Restore settings + ; Set max allowed power + setb Flags1.SETTLE_PHASE ; Set settle phase power as max + clr EA ; Disable interrupts to avoid that Requested_Pwm is overwritten + call set_startup_pwm + mov Pwm_Limit, Requested_Pwm + mov Pwm_Limit_Spoolup, Requested_Pwm + setb EA + clr Flags1.SETTLE_PHASE + mov Current_Pwm_Limited, #0 ; Set low pwm again after calling set_startup_pwm + mov Spoolup_Limit_Cnt, #0 + mov Spoolup_Limit_Skip, #1 + ; Begin startup sequence + setb Flags1.MOTOR_SPINNING ; Set motor spinning flag + setb Flags1.DIRECT_STARTUP_PHASE ; Set direct startup phase flag + mov Direct_Startup_Ok_Cnt, #0 ; Reset ok counter + call comm5comm6 ; Initialize commutation + call comm6comm1 + call calc_next_comm_timing ; Set virtual commutation point + call initialize_all_timings ; Initialize timing + jmp run1 + + +;stepper_method_start: +; ; Set up start operating conditions +; mov Temp1, #Pgm_Pwm_Freq +; mov A, @Temp1 +; mov Temp7, A ; Store setting in Temp7 +; mov @Temp1, #3 ; Set damped light mode +; mov Temp1, #Pgm_Damping_Force +; mov A, @Temp1 +; mov Temp6, A ; Store setting in Temp6 +; mov @Temp1, #5 ; Set high damping force +; call decode_parameters ; (Decode_parameters uses Temp1 and Temp8) +; mov Temp1, #Pgm_Pwm_Freq +; mov A, Temp7 +; mov @Temp1, A ; Restore settings +; mov Temp1, #Pgm_Damping_Force +; mov A, Temp6 +; mov @Temp1, A + ; Begin startup sequence +; setb Flags1.MOTOR_SPINNING ; Set motor spinning flag +; setb Flags1.SETTLE_PHASE ; Set motor start settling phase flag +; setb Flags2.CURR_PWMOFF_DAMPED; Set damped status, in order to ensure that pfets will be turned off in an initial pwm on +; call comm5comm6 ; Initialize commutation +; call comm6comm1 +; call set_startup_pwm +; call wait1ms +; call comm1comm2 +; call wait1ms +; call wait1ms +; call comm2comm3 +; call wait3ms +; call comm3comm4 +; call wait3ms +; call wait3ms +; call comm4comm5 +; call wait10ms ; Settle rotor +; call comm5comm6 +; call wait3ms +; call wait1ms +; clr Flags1.SETTLE_PHASE ; Clear settling phase flag +; setb Flags1.STEPPER_PHASE ; Set motor start stepper phase flag + +; ;**** **** **** **** **** +; ; Stepper phase beginning +; ;**** **** **** **** **** +;stepper_rot_beg: +; call start_adc_conversion +; call check_temp_voltage_and_limit_power +; call set_startup_pwm +; mov Adc_Conversion_Cnt, #TEMP_CHECK_RATE ; Make sure a temp reading is done next time +; Set_Adc_Ip_Temp + +; call comm6comm1 ; Commutate +; call calc_next_comm_timing_start ; Update timing and set timer +; call calc_new_wait_times +; call decrement_stepper_step +; call stepper_timer_wait + +; call comm1comm2 +; call calc_next_comm_timing_start +; call calc_new_wait_times +; call decrement_stepper_step +; call stepper_timer_wait + +; call comm2comm3 +; call calc_next_comm_timing_start +; call calc_new_wait_times +; call decrement_stepper_step +; call stepper_timer_wait + +; call comm3comm4 +; call calc_next_comm_timing_start +; call calc_new_wait_times +; call decrement_stepper_step +; call stepper_timer_wait + +; call comm4comm5 +; call calc_next_comm_timing_start +; call calc_new_wait_times +; call decrement_stepper_step +; call stepper_timer_wait + +; call comm5comm6 +; call calc_next_comm_timing_start +; call calc_new_wait_times +; call decrement_stepper_step +; ; Check stepper step versus end criteria +; clr C +; mov A, Wt_Stepper_Step_L +; subb A, Stepper_Step_End_L ; Minimum Stepper_Step_End +; mov A, Wt_Stepper_Step_H +; subb A, Stepper_Step_End_H +; jc stepper_rot_exit ; Branch if lower than minimum + +; ; Wait for step +; call stepper_timer_wait +; clr C +; mov A, New_Rcp ; Load new pulse value +; subb A, #RCP_STOP ; Check if pulse is below stop value +; jnc stepper_rot_beg + +; jmp run_to_wait_for_power_on + +;stepper_rot_exit: +; ; Wait for step +; call stepper_timer_wait +; ; Clear stepper phase +; clr Flags1.STEPPER_PHASE ; Clear motor start stepper phase flag +; ; Set dondamped low pwm frequency +; mov Temp1, #Pgm_Pwm_Freq +; mov A, @Temp1 +; mov Temp7, A ; Store setting in Temp7 +; mov @Temp1, #2 ; Set nondamped low frequency pwm mode +; call decode_parameters ; (Decode_parameters uses Temp1 and Temp8) +; mov Temp1, #Pgm_Pwm_Freq +; mov A, Temp7 +; mov @Temp1, A ; Restore settings +; ; Set spoolup power variables (power is now controlled from RCP) +; mov Pwm_Limit, Requested_Pwm +; mov Pwm_Limit_Spoolup, Pwm_Spoolup_Beg +; mov Current_Pwm_Limited, Pwm_Spoolup_Beg +; mov Spoolup_Limit_Cnt, #0 +; mov Spoolup_Limit_Skip, #1 +; ; Set direct startup phase to aquire sync quickly +; setb Flags1.DIRECT_STARTUP_PHASE ; Set direct startup phase flag +; mov Direct_Startup_Ok_Cnt, #0 ; Reset ok counter +; clr EA ; Disable interrupts +; ApFET_off ; Ap off. Turn off unused pfets (to turn off damping fets that might be on) +; CpFET_off ; Cp off +; mov A, #45 ; 8us delay for pfets to go off +; djnz ACC, $ +; setb EA ; Enable interrupts +; call comm6comm1 +; call calc_next_comm_timing ; Calculate next timing and start advance timing wait +; call wait_advance_timing ; Wait advance timing and start zero cross wait +; call calc_new_wait_times +; call wait_before_zc_scan ; Wait zero cross wait and start zero cross timeout +; mov Adc_Conversion_Cnt, #0 ; Make sure a voltage reading is done next time +; Set_Adc_Ip_Volt ; Set adc measurement to voltage +; jmp run1 + + + +;**** **** **** **** **** **** **** **** **** **** **** **** **** +; +; Run entry point +; +;**** **** **** **** **** **** **** **** **** **** **** **** **** +damped_transition: + ; Transition from nondamped to damped if applicable + call decode_parameters ; Set programmed parameters + call comm6comm1 + mov Adc_Conversion_Cnt, #0 ; Make sure a voltage reading is done next time + Set_Adc_Ip_Volt ; Set adc measurement to voltage + +; Run 1 = B(p-on) + C(n-pwm) - comparator A evaluated +; Out_cA changes from high to low +run1: + call wait_for_comp_out_high ; Wait zero cross wait and wait for high + call evaluate_comparator_integrity ; Check whether comparator reading has been normal + call setup_comm_wait ; Setup wait time from zero cross to commutation + call calc_governor_target ; Calculate governor target + call wait_for_comm ; Wait from zero cross to commutation + call comm1comm2 ; Commutate + call calc_next_comm_timing ; Calculate next timing and start advance timing wait + call wait_advance_timing ; Wait advance timing and start zero cross wait + call calc_new_wait_times + call wait_before_zc_scan ; Wait zero cross wait and start zero cross timeout + +; Run 2 = A(p-on) + C(n-pwm) - comparator B evaluated +; Out_cB changes from low to high +run2: + call wait_for_comp_out_low + call evaluate_comparator_integrity + call setup_comm_wait + call calc_governor_prop_error + call wait_for_comm + call comm2comm3 + call calc_next_comm_timing + call wait_advance_timing + call calc_new_wait_times + call wait_before_zc_scan + +; Run 3 = A(p-on) + B(n-pwm) - comparator C evaluated +; Out_cC changes from high to low +run3: + call wait_for_comp_out_high + call evaluate_comparator_integrity + call setup_comm_wait + call calc_governor_int_error + call wait_for_comm + call comm3comm4 + call calc_next_comm_timing + call wait_advance_timing + call calc_new_wait_times + call wait_before_zc_scan + +; Run 4 = C(p-on) + B(n-pwm) - comparator A evaluated +; Out_cA changes from low to high +run4: + call wait_for_comp_out_low + call evaluate_comparator_integrity + call setup_comm_wait + call calc_governor_prop_correction + call wait_for_comm + call comm4comm5 + call calc_next_comm_timing + call wait_advance_timing + call calc_new_wait_times + call wait_before_zc_scan + +; Run 5 = C(p-on) + A(n-pwm) - comparator B evaluated +; Out_cB changes from high to low +run5: + call wait_for_comp_out_high + call evaluate_comparator_integrity + call setup_comm_wait + call calc_governor_int_correction + call wait_for_comm + call comm5comm6 + call calc_next_comm_timing + call wait_advance_timing + call calc_new_wait_times + call wait_before_zc_scan + +; Run 6 = B(p-on) + A(n-pwm) - comparator C evaluated +; Out_cC changes from low to high +run6: + call wait_for_comp_out_low + call start_adc_conversion + call evaluate_comparator_integrity + call setup_comm_wait + call check_temp_voltage_and_limit_power + call wait_for_comm + call comm6comm1 + call calc_next_comm_timing + call wait_advance_timing + call calc_new_wait_times + call wait_before_zc_scan + + ; Check if it is direct startup + jnb Flags1.DIRECT_STARTUP_PHASE, normal_run_checks + + ; Set spoolup power variables +IF MODE == 0 OR MODE == 2 + mov Pwm_Limit, Pwm_Spoolup_Beg ; Set initial max power + mov Pwm_Limit_Spoolup, Pwm_Spoolup_Beg ; Set initial slow spoolup power +ENDIF +IF MODE == 1 + mov Pwm_Limit, #0FFh ; Allow full power + mov Pwm_Limit_Spoolup, #0FFh +ENDIF + mov Spoolup_Limit_Cnt, #0 + mov Spoolup_Limit_Skip, #1 + ; Check startup ok counter + clr C + mov A, Direct_Startup_Ok_Cnt ; Load ok counter + subb A, #100 ; Is counter above requirement? + jc direct_start_check_rcp ; No - proceed + + clr Flags1.DIRECT_STARTUP_PHASE ; Clear direct startup phase flag + setb Flags1.INITIAL_RUN_PHASE ; Set initial run phase flag + mov Startup_Rot_Cnt, #20 ; Set startup rotation count + jmp normal_run_checks + +direct_start_check_rcp: + clr C + mov A, New_Rcp ; Load new pulse value + subb A, #RCP_STOP ; Check if pulse is below stop value + jc ($+5) + + ljmp run1 ; Continue to run + + jmp run_to_wait_for_power_on + + +normal_run_checks: + ; Check if it is initial run phase + jnb Flags1.INITIAL_RUN_PHASE, initial_run_phase_done; If not initial run phase - branch + + ; Decrement startup rotaton count + mov A, Startup_Rot_Cnt + dec A + ; Check number of nondamped rotations + jnz normal_run_check_startup_rot ; Branch if counter is not zero + + clr Flags1.INITIAL_RUN_PHASE ; Clear initial run phase flag + ljmp damped_transition ; Do damped transition if counter is zero + +normal_run_check_startup_rot: + mov Startup_Rot_Cnt, A ; Not zero - store counter + + clr C + mov A, New_Rcp ; Load new pulse value + subb A, #RCP_STOP ; Check if pulse is below stop value + jc ($+5) + + ljmp run1 ; Continue to run + + jmp run_to_wait_for_power_on + + +initial_run_phase_done: + ; Exit run loop after a given time + clr C + mov A, Rcp_Stop_Cnt ; Load stop RC pulse counter value +;///// subb A, #RCP_STOP_LIMIT ; Is number of stop RC pulses above limit? + subb A, Rcp_Stop_Limit ; Is number of stop RC pulses above limit? + jnc run_to_wait_for_power_on ; Yes, go back to wait for poweron + +run6_check_rcp_timeout: +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) +; anl A, Flags3 ; Check pwm frequency flags +; jnz run6_check_speed ; If a flag is set (PWM) - branch + + mov A, Rcp_Timeout_Cnt ; Load RC pulse timeout counter value + jz run_to_wait_for_power_on ; If it is zero - go back to wait for poweron + +run6_check_speed: + clr C + mov A, Comm_Period4x_H ; Is Comm_Period4x more than 32ms (~1220 eRPM)? + subb A, #0F0h + jnc run_to_wait_for_power_on ; Yes - go back to motor start + jmp run1 ; Go back to run 1 + + +run_to_wait_for_power_on: + + call switch_power_off + mov Temp1, #Pgm_Pwm_Freq + mov A, @Temp1 + mov Temp7, A ; Store setting in Temp7 + mov @Temp1, #1 ; Set low pwm mode (in order to turn off damping) + call decode_parameters ; (Decode_parameters uses Temp1 and Temp8) + mov Temp1, #Pgm_Pwm_Freq + mov A, Temp7 + mov @Temp1, A ; Restore settings + clr A + mov Requested_Pwm, A ; Set requested pwm to zero + mov Governor_Req_Pwm, A ; Set governor requested pwm to zero + mov Current_Pwm, A ; Set current pwm to zero + mov Current_Pwm_Limited, A ; Set limited current pwm to zero + mov Pwm_Motor_Idle, A ; Set motor idle to zero + clr Flags1.MOTOR_SPINNING ; Clear motor spinning flag + call wait1ms ; Wait for pwm to be stopped + call switch_power_off + call wait10ms + clr ET0 + jb Flags2.PGM_PWMOFF_DAMPED, light_damp + ajmp damp_exit + +light_damp: + jb Flags2.PGM_PWMOFF_DAMPED_LIGHT, ($+7) + setb P1.BnFET + ajmp damp_exit + jb Flags2.PGM_PWMOFF_DAMPED_FULL, high_damp + A_B_nFETs_On + ajmp damp_exit +high_damp: + All_nFETs_On +damp_exit: + +IF MODE == 0 ; Main + mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) + anl A, Flags3 ; Check pwm frequency flags + jnz run_to_next_state_main ; If a flag is set (PWM) - branch + + mov A, Rcp_Timeout_Cnt ; Load RC pulse timeout counter value + jnz run_to_next_state_main ; If it is not zero - branch + + jmp measure_pwm_freq_init ; If it is zero (pulses missing) - go back to measure pwm frequency + +run_to_next_state_main: + mov Temp1, #Pgm_Startup_Method ; Check if it is stepped startup + mov A, @Temp1 + jnb ACC.0, run_to_next_state_main_wait_done ; If direct startup - jump + + call wait1s ; 3 second delay before new startup (in stepped mode) + call wait1s + call wait1s +run_to_next_state_main_wait_done: +;读取Pgm_Main_Rearm_Start + mov Temp1, #Pgm_Main_Rearm_Start + mov A, @Temp1 + clr C + subb A, #1 ; Is re-armed start enabled? + jc jmp_wait_for_power_on ; No - do like tail and start immediately + + jmp validate_rcp_start ; Yes - go back to validate RC pulse +;Pgm_Main_Rearm_Start = 第2项 disabled +jmp_wait_for_power_on: + jmp wait_for_power_on ; Go back to wait for power on +ENDIF +IF MODE >= 1 ; Tail or multi +; mov A, #((1 SHL RCP_PWM_FREQ_1KHZ)+(1 SHL RCP_PWM_FREQ_2KHZ)+(1 SHL RCP_PWM_FREQ_4KHZ)+(1 SHL RCP_PWM_FREQ_8KHZ)) +; anl A, Flags3 ; Check pwm frequency flags +; jnz jmp_wait_for_power_on ; If a flag is set (PWM) - branch + + mov A, Rcp_Timeout_Cnt ; Load RC pulse timeout counter value + jnz jmp_wait_for_power_on ; If it is not zero - go back to wait for poweron + + jmp measure_pwm_freq_init ; If it is zero (pulses missing) - go back to measure pwm frequency + +jmp_wait_for_power_on: + jmp wait_for_power_on_loop ; Go back to wait for power on +ENDIF + +;**** **** **** **** **** **** **** **** **** **** **** **** **** + +$include (BLHeliTxPgm.inc) ; Include source code for programming the ESC with the TX + +;**** **** **** **** **** **** **** **** **** **** **** **** **** + + + + +END