|
|
$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 <http://www.gnu.org/licenses/>.;;**** **** **** **** ****;; 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 1XP_3A_Tail EQU 2XP_3A_Multi EQU 3XP_7A_Main EQU 4XP_7A_Tail EQU 5XP_7A_Multi EQU 6XP_7A_Fast_Main EQU 7XP_7A_Fast_Tail EQU 8XP_7A_Fast_Multi EQU 9XP_12A_Main EQU 10XP_12A_Tail EQU 11XP_12A_Multi EQU 12XP_18A_Main EQU 13XP_18A_Tail EQU 14XP_18A_Multi EQU 15XP_25A_Main EQU 16XP_25A_Tail EQU 17XP_25A_Multi EQU 18DP_3A_Main EQU 19DP_3A_Tail EQU 20DP_3A_Multi EQU 21Supermicro_3p5A_Main EQU 22Supermicro_3p5A_Tail EQU 23 Supermicro_3p5A_Multi EQU 24 Turnigy_Plush_6A_Main EQU 25Turnigy_Plush_6A_Tail EQU 26 Turnigy_Plush_6A_Multi EQU 27 Turnigy_Plush_10A_Main EQU 28Turnigy_Plush_10A_Tail EQU 29 Turnigy_Plush_10A_Multi EQU 30 Turnigy_Plush_12A_Main EQU 31Turnigy_Plush_12A_Tail EQU 32 Turnigy_Plush_12A_Multi EQU 33 Turnigy_Plush_18A_Main EQU 34Turnigy_Plush_18A_Tail EQU 35 Turnigy_Plush_18A_Multi EQU 36 Turnigy_Plush_25A_Main EQU 37Turnigy_Plush_25A_Tail EQU 38 Turnigy_Plush_25A_Multi EQU 39 Turnigy_Plush_30A_Main EQU 40Turnigy_Plush_30A_Tail EQU 41 Turnigy_Plush_30A_Multi EQU 42 Turnigy_Plush_40A_Main EQU 43Turnigy_Plush_40A_Tail EQU 44 Turnigy_Plush_40A_Multi EQU 45 Turnigy_Plush_60A_Main EQU 46Turnigy_Plush_60A_Tail EQU 47 Turnigy_Plush_60A_Multi EQU 48 Turnigy_Plush_80A_Main EQU 49Turnigy_Plush_80A_Tail EQU 50 Turnigy_Plush_80A_Multi EQU 51 Turnigy_AE_20A_Main EQU 52Turnigy_AE_20A_Tail EQU 53 Turnigy_AE_20A_Multi EQU 54 Turnigy_AE_25A_Main EQU 55Turnigy_AE_25A_Tail EQU 56 Turnigy_AE_25A_Multi EQU 57 Turnigy_AE_30A_Main EQU 58Turnigy_AE_30A_Tail EQU 59 Turnigy_AE_30A_Multi EQU 60 Turnigy_AE_45A_Main EQU 61Turnigy_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 70Skywalker_20A_Tail EQU 71 Skywalker_20A_Multi EQU 72 Skywalker_40A_Main EQU 73Skywalker_40A_Tail EQU 74 Skywalker_40A_Multi EQU 75 HiModel_Cool_22A_Main EQU 76HiModel_Cool_22A_Tail EQU 77 HiModel_Cool_22A_Multi EQU 78 HiModel_Cool_33A_Main EQU 79HiModel_Cool_33A_Tail EQU 80 HiModel_Cool_33A_Multi EQU 81 HiModel_Cool_41A_Main EQU 82HiModel_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 statementsIF BESC == XP_3A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (XP_3A.inc) ; Select XP 3A pinoutENDIF
IF BESC == XP_3A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (XP_3A.inc) ; Select XP 3A pinoutENDIF
IF BESC == XP_3A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (XP_3A.inc) ; Select XP 3A pinoutENDIF
IF BESC == XP_7A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (XP_7A.inc) ; Select XP 7A pinoutENDIF
IF BESC == XP_7A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (XP_7A.inc) ; Select XP 7A pinoutENDIF
IF BESC == XP_7A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (XP_7A.inc) ; Select XP 7A pinoutENDIF
IF BESC == XP_7A_Fast_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (XP_7A_Fast.inc) ; Select XP 7A Fast pinoutENDIF
IF BESC == XP_7A_Fast_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (XP_7A_Fast.inc) ; Select XP 7A Fast pinoutENDIF
IF BESC == XP_7A_Fast_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (XP_7A_Fast.inc) ; Select XP 7A Fast pinoutENDIF
IF BESC == XP_12A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (XP_12A.inc) ; Select XP 12A pinoutENDIF
IF BESC == XP_12A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (XP_12A.inc) ; Select XP 12A pinoutENDIF
IF BESC == XP_12A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (XP_12A.inc) ; Select XP 12A pinoutENDIF
IF BESC == XP_18A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (XP_18A.inc) ; Select XP 18A pinoutENDIF
IF BESC == XP_18A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (XP_18A.inc) ; Select XP 18A pinoutENDIF
IF BESC == XP_18A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (XP_18A.inc) ; Select XP 18A pinoutENDIF
IF BESC == XP_25A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (XP_25A.inc) ; Select XP 25A pinoutENDIF
IF BESC == XP_25A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (XP_25A.inc) ; Select XP 25A pinoutENDIF
IF BESC == XP_25A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (XP_25A.inc) ; Select XP 25A pinoutENDIF
IF BESC == DP_3A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (DP_3A.inc) ; Select DP 3A pinoutENDIF
IF BESC == DP_3A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (DP_3A.inc) ; Select DP 3A pinoutENDIF
IF BESC == DP_3A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (DP_3A.inc) ; Select DP 3A pinoutENDIF
IF BESC == Supermicro_3p5A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Supermicro_3p5A.inc) ; Select Supermicro 3.5A pinoutENDIF
IF BESC == Supermicro_3p5A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Supermicro_3p5A.inc) ; Select Supermicro 3.5A pinoutENDIF
IF BESC == Supermicro_3p5A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Supermicro_3p5A.inc) ; Select Supermicro 3.5A pinoutENDIF
IF BESC == Turnigy_Plush_6A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Turnigy_Plush_6A.inc) ; Select Turnigy Plush 6A pinoutENDIF
IF BESC == Turnigy_Plush_6A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Turnigy_Plush_6A.inc) ; Select Turnigy Plush 6A pinoutENDIF
IF BESC == Turnigy_Plush_6A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Turnigy_Plush_6A.inc) ; Select Turnigy Plush 6A pinoutENDIF
IF BESC == Turnigy_Plush_10A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Turnigy_Plush_10A.inc) ; Select Turnigy Plush 10A pinoutENDIF
IF BESC == Turnigy_Plush_10A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Turnigy_Plush_10A.inc) ; Select Turnigy Plush 10A pinoutENDIF
IF BESC == Turnigy_Plush_10A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Turnigy_Plush_10A.inc) ; Select Turnigy Plush 10A pinoutENDIF
IF BESC == Turnigy_Plush_12A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Turnigy_Plush_12A.inc) ; Select Turnigy Plush 12A pinoutENDIF
IF BESC == Turnigy_Plush_12A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Turnigy_Plush_12A.inc) ; Select Turnigy Plush 12A pinoutENDIF
IF BESC == Turnigy_Plush_12A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Turnigy_Plush_12A.inc) ; Select Turnigy Plush 12A pinoutENDIF
IF BESC == Turnigy_Plush_18A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Turnigy_Plush_18A.inc) ; Select Turnigy Plush 18A pinoutENDIF
IF BESC == Turnigy_Plush_18A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Turnigy_Plush_18A.inc) ; Select Turnigy Plush 18A pinoutENDIF
IF BESC == Turnigy_Plush_18A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Turnigy_Plush_18A.inc) ; Select Turnigy Plush 18A pinoutENDIF
IF BESC == Turnigy_Plush_25A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Turnigy_Plush_25A.inc) ; Select Turnigy Plush 25A pinoutENDIF
IF BESC == Turnigy_Plush_25A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Turnigy_Plush_25A.inc) ; Select Turnigy Plush 25A pinoutENDIF
IF BESC == Turnigy_Plush_25A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Turnigy_Plush_25A.inc) ; Select Turnigy Plush 25A pinoutENDIF
IF BESC == Turnigy_Plush_30A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Turnigy_Plush_30A.inc) ; Select Turnigy Plush 30A pinoutENDIF
IF BESC == Turnigy_Plush_30A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Turnigy_Plush_30A.inc) ; Select Turnigy Plush 30A pinoutENDIF
IF BESC == Turnigy_Plush_30A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Turnigy_Plush_30A.inc) ; Select Turnigy Plush 30A pinoutENDIF
IF BESC == Turnigy_Plush_40A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Turnigy_Plush_40A.inc) ; Select Turnigy Plush 40A pinoutENDIF
IF BESC == Turnigy_Plush_40A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Turnigy_Plush_40A.inc) ; Select Turnigy Plush 40A pinoutENDIF
IF BESC == Turnigy_Plush_40A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Turnigy_Plush_40A.inc) ; Select Turnigy Plush 40A pinoutENDIF
IF BESC == Turnigy_Plush_60A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Turnigy_Plush_60A.inc) ; Select Turnigy Plush 60A pinoutENDIF
IF BESC == Turnigy_Plush_60A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Turnigy_Plush_60A.inc) ; Select Turnigy Plush 60A pinoutENDIF
IF BESC == Turnigy_Plush_60A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Turnigy_Plush_60A.inc) ; Select Turnigy Plush 60A pinoutENDIF
IF BESC == Turnigy_Plush_80A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Turnigy_Plush_80A.inc) ; Select Turnigy Plush 80A pinoutENDIF
IF BESC == Turnigy_Plush_80A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Turnigy_Plush_80A.inc) ; Select Turnigy Plush 80A pinoutENDIF
IF BESC == Turnigy_Plush_80A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Turnigy_Plush_80A.inc) ; Select Turnigy Plush 80A pinoutENDIF
IF BESC == Turnigy_AE_20A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Turnigy_AE_20A.inc) ; Select Turnigy AE-20A pinoutENDIF
IF BESC == Turnigy_AE_20A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Turnigy_AE_20A.inc) ; Select Turnigy AE-20A pinoutENDIF
IF BESC == Turnigy_AE_20A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Turnigy_AE_20A.inc) ; Select Turnigy AE-20A pinoutENDIF
IF BESC == Turnigy_AE_25A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Turnigy_AE_25A.inc) ; Select Turnigy AE-25A pinoutENDIF
IF BESC == Turnigy_AE_25A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Turnigy_AE_25A.inc) ; Select Turnigy AE-25A pinoutENDIF
IF BESC == Turnigy_AE_25A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Turnigy_AE_25A.inc) ; Select Turnigy AE-25A pinoutENDIF
IF BESC == Turnigy_AE_30A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Turnigy_AE_30A.inc) ; Select Turnigy AE-30A pinoutENDIF
IF BESC == Turnigy_AE_30A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Turnigy_AE_30A.inc) ; Select Turnigy AE-30A pinoutENDIF
IF BESC == Turnigy_AE_30A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Turnigy_AE_30A.inc) ; Select Turnigy AE-30A pinoutENDIF
IF BESC == Turnigy_AE_45A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Turnigy_AE_45A.inc) ; Select Turnigy AE-45A pinoutENDIF
IF BESC == Turnigy_AE_45A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Turnigy_AE_45A.inc) ; Select Turnigy AE-45A pinoutENDIF
IF BESC == Turnigy_AE_45A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Turnigy_AE_45A.inc) ; Select Turnigy AE-45A pinoutENDIF
IF BESC == Turnigy_KForce_40A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Turnigy_KForce_40A.inc) ; Select Turnigy KForce 40A pinoutENDIF
IF BESC == Turnigy_KForce_40A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Turnigy_KForce_40A.inc) ; Select Turnigy KForce 40A pinoutENDIF
IF BESC == Turnigy_KForce_40A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Turnigy_KForce_40A.inc) ; Select Turnigy KForce 40A pinoutENDIF
IF BESC == Turnigy_KForce_120A_HV_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Turnigy_KForce_120A_HV.inc) ; Select Turnigy KForce 120A HV pinoutENDIF
IF BESC == Turnigy_KForce_120A_HV_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Turnigy_KForce_120A_HV.inc) ; Select Turnigy KForce 120A HV pinoutENDIF
IF BESC == Turnigy_KForce_120A_HV_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Turnigy_KForce_120A_HV.inc) ; Select Turnigy KForce 120A HV pinoutENDIF
IF BESC == Skywalker_20A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Skywalker_20A.inc) ; Select Skywalker 20A pinoutENDIF
IF BESC == Skywalker_20A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Skywalker_20A.inc) ; Select Skywalker 20A pinoutENDIF
IF BESC == Skywalker_20A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Skywalker_20A.inc) ; Select Skywalker 20A pinoutENDIF
IF BESC == Skywalker_40A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Skywalker_40A.inc) ; Select Skywalker 40A pinoutENDIF
IF BESC == Skywalker_40A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Skywalker_40A.inc) ; Select Skywalker 40A pinoutENDIF
IF BESC == Skywalker_40A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Skywalker_40A.inc) ; Select Skywalker 40A pinoutENDIF
IF BESC == HiModel_Cool_22A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (HiModel_Cool_22A.inc) ; Select HiModel Cool 22A pinoutENDIF
IF BESC == HiModel_Cool_22A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (HiModel_Cool_22A.inc) ; Select HiModel Cool 22A pinoutENDIF
IF BESC == HiModel_Cool_22A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (HiModel_Cool_22A.inc) ; Select HiModel Cool 22A pinoutENDIF
IF BESC == HiModel_Cool_33A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (HiModel_Cool_33A.inc) ; Select HiModel Cool 33A pinoutENDIF
IF BESC == HiModel_Cool_33A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (HiModel_Cool_33A.inc) ; Select HiModel Cool 33A pinoutENDIF
IF BESC == HiModel_Cool_33A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (HiModel_Cool_33A.inc) ; Select HiModel Cool 33A pinoutENDIF
IF BESC == HiModel_Cool_41A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (HiModel_Cool_41A.inc) ; Select HiModel Cool 41A pinoutENDIF
IF BESC == HiModel_Cool_41A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (HiModel_Cool_41A.inc) ; Select HiModel Cool 41A pinoutENDIF
IF BESC == HiModel_Cool_41A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (HiModel_Cool_41A.inc) ; Select HiModel Cool 41A pinoutENDIF
IF BESC == RCTimer_6A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (RCTimer_6A.inc) ; Select RC Timer 6A pinoutENDIF
IF BESC == RCTimer_6A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (RCTimer_6A.inc) ; Select RC Timer 6A pinoutENDIF
IF BESC == RCTimer_6A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (RCTimer_6A.inc) ; Select RC Timer 6A pinoutENDIF
IF BESC == Align_RCE_BL15X_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Align_RCE_BL15X.inc) ; Select Align RCE-BL15X pinoutENDIF
IF BESC == Align_RCE_BL15X_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Align_RCE_BL15X.inc) ; Select Align RCE-BL15X pinoutENDIF
IF BESC == Align_RCE_BL15X_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Align_RCE_BL15X.inc) ; Select Align RCE-BL15X pinoutENDIF
IF BESC == Align_RCE_BL35X_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Align_RCE_BL35X.inc) ; Select Align RCE-BL35X pinoutENDIF
IF BESC == Align_RCE_BL35X_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Align_RCE_BL35X.inc) ; Select Align RCE-BL35X pinoutENDIF
IF BESC == Align_RCE_BL35X_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Align_RCE_BL35X.inc) ; Select Align RCE-BL35X pinoutENDIF
IF BESC == Align_RCE_BL35P_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (Align_RCE_BL35P.inc) ; Select Align RCE-BL35P pinoutENDIF
IF BESC == Align_RCE_BL35P_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (Align_RCE_BL35P.inc) ; Select Align RCE-BL35P pinoutENDIF
IF BESC == Align_RCE_BL35P_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (Align_RCE_BL35P.inc) ; Select Align RCE-BL35P pinoutENDIF
IF BESC == H_King_10A_MainMODE EQU 0 ; Choose mode. Set to 0 for main motor$include (H_King_10A.inc) ; Select H-King 10A pinoutENDIF
IF BESC == H_King_10A_TailMODE EQU 1 ; Choose mode. Set to 1 for tail motor$include (H_King_10A.inc) ; Select H-King 10A pinoutENDIF
IF BESC == H_King_10A_MultiMODE EQU 2 ; Choose mode. Set to 2 for multirotor$include (H_King_10A.inc) ; Select H-King 10A pinoutENDIF
;**** **** **** **** ****; 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;; MainDEFAULT_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.0DEFAULT_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.0DEFAULT_PGM_MAIN_GOVERNOR_MODE EQU 1 ; 1=Tx 2=Arm 3=Setup 4=OffDEFAULT_PGM_MAIN_GOVERNOR_RANGE EQU 1 ; 1=High 2=LowDEFAULT_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/cDEFAULT_PGM_MAIN_STARTUP_RPM EQU 3 ; 1=0.67 2=0.8 3=1.00 4=1.25 5=1.5DEFAULT_PGM_MAIN_STARTUP_ACCEL EQU 1 ; 1=0.4 2=0.7 3=1.0 4=1.5 5=2.3DEFAULT_PGM_MAIN_COMM_TIMING EQU 3 ; 1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=HighDEFAULT_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=255DEFAULT_PGM_MAIN_DAMPING_FORCE EQU 1 ; 1=VeryLow 2=Low 3=MediumLow 4=MediumHigh 5=High 6=HighestDEFAULT_PGM_MAIN_PWM_FREQ EQU 2 ; 1=High 2=Low 3=DampedLightDEFAULT_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=ReversedDEFAULT_PGM_MAIN_RCP_PWM_POL EQU 1 ; 1=Positive 2=NegativeDEFAULT_PGM_MAIN_GOV_SETUP_TARGET EQU 180 ; Target for governor in setup mode. Corresponds to 70% throttleDEFAULT_PGM_MAIN_REARM_START EQU 0 ; 1=Enabled 0=DisabledDEFAULT_PGM_MAIN_BEEP_STRENGTH EQU 80 ; Beep strengthDEFAULT_PGM_MAIN_BEACON_STRENGTH EQU 100 ; Beacon strengthDEFAULT_PGM_MAIN_BEACON_DELAY EQU 2 ; 1=30s 2=1m 3=2m 4=3m 5=Infinite; TailDEFAULT_PGM_TAIL_GAIN EQU 3 ; 1=0.75 2=0.88 3=1.00 4=1.12 5=1.25DEFAULT_PGM_TAIL_IDLE_SPEED EQU 4 ; 1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=HighDEFAULT_PGM_TAIL_STARTUP_RPM EQU 3 ; 1=0.67 2=0.8 3=1.00 4=1.25 5=1.5DEFAULT_PGM_TAIL_STARTUP_ACCEL EQU 5 ; 1=0.4 2=0.7 3=1.0 4=1.5 5=2.3DEFAULT_PGM_TAIL_COMM_TIMING EQU 3 ; 1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=HighDEFAULT_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=255DEFAULT_PGM_TAIL_DAMPING_FORCE EQU 5 ; 1=VeryLow 2=Low 3=MediumLow 4=MediumHigh 5=High 6=HighestIF DAMPED_MODE_ENABLE == 1DEFAULT_PGM_TAIL_PWM_FREQ EQU 1 ; 1=High 2=Low 3=DampedLight 4=Damped ELSEDEFAULT_PGM_TAIL_PWM_FREQ EQU 1 ; 1=High 2=Low 3=DampedLightENDIFDEFAULT_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=ReversedDEFAULT_PGM_TAIL_RCP_PWM_POL EQU 1 ; 1=Positive 2=NegativeDEFAULT_PGM_TAIL_BEEP_STRENGTH EQU 250 ; Beep strengthDEFAULT_PGM_TAIL_BEACON_STRENGTH EQU 250 ; Beacon strengthDEFAULT_PGM_TAIL_BEACON_DELAY EQU 4 ; 1=30s 2=1m 3=2m 4=3m 5=Infinite; MultiDEFAULT_PGM_MULTI_FIRST_KEYWORD EQU 66h ;增加首个关键字 2013.8.27DEFAULT_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.0DEFAULT_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.0DEFAULT_PGM_MULTI_GOVERNOR_MODE EQU 1 ; 1=Off 2=LoRange 3=MidRange 4=HiRangeDEFAULT_PGM_MULTI_GAIN EQU 3 ; 1=0.75 2=0.88 3=1.00 4=1.12 5=1.25DEFAULT_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/cDEFAULT_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.5DEFAULT_PGM_MULTI_STARTUP_ACCEL EQU 5 ; 1=0.4 2=0.7 3=1.0 4=1.5 5=2.3DEFAULT_PGM_MULTI_COMM_TIMING EQU 3 ; 1=Low 2=MediumLow 3=Medium 4=MediumHigh 5=HighDEFAULT_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=255DEFAULT_PGM_MULTI_DAMPING_FORCE EQU 1 ; 1=VeryLow 2=Low 3=MediumLow 4=MediumHigh 5=High 6=HighestIF DAMPED_MODE_ENABLE == 1DEFAULT_PGM_MULTI_PWM_FREQ EQU 1 ; 1=Low 2=High ELSEDEFAULT_PGM_MULTI_PWM_FREQ EQU 1 ; 1=Low 2=High ENDIFDEFAULT_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=ReversedDEFAULT_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=InfiniteDEFAULT_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 等待摇杆操作时间; CommonDEFAULT_PGM_ENABLE_TX_PROGRAM EQU 1 ; 1=Enabled 0=DisabledDEFAULT_PGM_PPM_MIN_THROTTLE EQU 37 ; 4*37+1000=1148DEFAULT_PGM_PPM_MAX_THROTTLE EQU 208 ; 4*208+1000=1832DEFAULT_PGM_BEC_VOLTAGE_HIGH EQU 0 ; 0=Low 1= High
;**** **** **** **** ****; Constant definitions for mainIF 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 lostRCP_TIMEOUT EQU 64 ; Number of timer2L overflows (about 128us) before considering rc pulse lostRCP_SKIP_RATE EQU 32 ; Number of timer2L overflows (about 128us) before reenabling rc pulse detectionRCP_MIN EQU 0 ; This is minimum RC pulse lengthRCP_MAX EQU 254 ; This is maximum RC pulse lengthRCP_VALIDATE EQU 2 ; Require minimum this pulse length to validate RC pulseRCP_STOP EQU 1 ; Stop motor at or below this pulse lengthRCP_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 tailIF MODE == 1
GOV_SPOOLRATE EQU 1 ; Number of steps for governor requested pwm per 32msRCP_TIMEOUT_PPM EQU 10 ; Number of timer2H overflows (about 32ms) before considering rc pulse lostRCP_TIMEOUT EQU 24 ; Number of timer2L overflows (about 128us) before considering rc pulse lostRCP_SKIP_RATE EQU 6 ; Number of timer2L overflows (about 128us) before reenabling rc pulse detectionRCP_MIN EQU 0 ; This is minimum RC pulse lengthRCP_MAX EQU 255 ; This is maximum RC pulse lengthRCP_VALIDATE EQU 2 ; Require minimum this pulse length to validate RC pulseRCP_STOP EQU 1 ; Stop motor at or below this pulse lengthRCP_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 multiIF 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 lostRCP_TIMEOUT EQU 40 ; Number of timer2L overflows (about 128us) before considering rc pulse lostRCP_SKIP_RATE EQU 6 ; Number of timer2L overflows (about 128us) before reenabling rc pulse detectionRCP_MIN EQU 0 ; This is minimum RC pulse lengthRCP_MAX EQU 255 ; This is maximum RC pulse lengthRCP_VALIDATE EQU 2 ; Require minimum this pulse length to validate RC pulseRCP_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 definitionsTemp1 EQU R0Temp2 EQU R1Temp3 EQU R2Temp4 EQU R3Temp5 EQU R4Temp6 EQU R5Temp7 EQU R6Temp8 EQU R7
;**** **** **** **** ****; Register definitionsDSEG 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 pwmCurrent_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_startT3_PENDING EQU 0 ; Timer3 pending flagRCP_MEAS_PWM_FREQ EQU 1 ; Measure RC pulse pwm frequencyPWM_ON EQU 2 ; Set in on part of pwm cycleDEMAG_DETECTED EQU 3 ; Set when excessive demag time is detectedDEMAG_CUT_POWER EQU 4 ; Set when demag compensation cuts power; EQU 5PROGRAM_FUNC_FLAG EQU 6SUCCESS_BEEP_FLAG EQU 7
Flags1: DS 1 ; State flags. Reset upon init_start MOTOR_SPINNING EQU 0 ; Set when in motor is spinningSETTLE_PHASE EQU 1 ; Set when in motor start settling phaseSTEPPER_PHASE EQU 2 ; Set when in motor start stepper motor phaseDIRECT_STARTUP_PHASE EQU 3 ; Set when in direct startup phaseINITIAL_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_startRCP_UPDATED EQU 0 ; New RC pulse length value availableRCP_EDGE_NO EQU 1 ; RC pulse edge no. 0=rising, 1=fallingPGM_PWMOFF_DAMPED EQU 2 ; Programmed pwm off damped mode. Set when fully damped or damped light mode is selectedPGM_PWMOFF_DAMPED_FULL EQU 3 ; Programmed pwm off fully damped mode. Set when all pfets shall be on in pwm_off periodPGM_PWMOFF_DAMPED_LIGHT EQU 4 ; Programmed pwm off damped light mode. Set when only 2 pfets shall be on in pwm_off periodPGM_PWM_HIGH_FREQ EQU 5 ; Progremmed pwm high frequencyCURR_PWMOFF_DAMPED EQU 6 ; Currently running pwm off cycle is dampedCURR_PWMOFF_COMP_ABLE EQU 7 ; Currently running pwm off cycle is usable for comparator
Flags3: DS 1 ; State flags. NOT reset upon init_startRCP_PWM_FREQ_1KHZ EQU 0 ; RC pulse pwm frequency is 1kHzRCP_PWM_FREQ_2KHZ EQU 1 ; RC pulse pwm frequency is 2kHzRCP_PWM_FREQ_4KHZ EQU 2 ; RC pulse pwm frequency is 4kHzRCP_PWM_FREQ_8KHZ EQU 3 ; RC pulse pwm frequency is 8kHzPGM_DIR_REV EQU 4 ; Programmed direction. 0=normal, 1=reversedPGM_RCP_PWM_POL EQU 5 ; Programmed RC pulse pwm polarity. 0=positive, 1=negativeFULL_THROTTLE_RANGE EQU 6 ; When set full throttle range is used (1000-2000us) and stored calibration values are ignoredERRO_DATA EQU 7 ;增加编程卡数据错误标志 2013.8.27;**** **** **** **** ****; RAM definitionsDSEG AT 30h ; Ram data segment, direct addressing
Initial_Arm: DS 1 ; Variable that is set during the first arm sequence after power on;Pca_First_Int: DS 1Power_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 counterDirect_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 phaseComp_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 errorGov_Arm_Target: DS 1 ; Governor arm target valueGov_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 acceptableNew_Rcp: DS 1 ; New RC pulse value in pca countsPrev_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 mainPwm_Spoolup_Beg: DS 1 ; Pwm to begin main spoolup withPwm_Motor_Idle: DS 1 ; Motor idle speed pwmPwm_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 limitSpoolup_Limit_Skip: DS 1 ; Interrupt skips for spoolup limit increment (0=no skips, 1=skip one etc)
Damping_Period: DS 1 ; Damping on/off periodDamping_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 counterLipo_Cell_Count: DS 1 ;增加电池节数累计变量 2013.5.29Limit_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 inputBeep_Strength: DS 1 ; Strength of beeps
Tx_Pgm_Func_No: DS 1 ; Function number when doing programming by txTx_Pgm_Paraval_No: DS 1 ; Parameter value number when doing programming by txTx_Pgm_Beep_No: DS 1 ; Beep number when doing programming by txCommu_Data_Buffer: DS 1 ;编程卡接收，发送数据暂存 2013.8.27Commu_Sum: DS 1 ;增加编程卡校验和 2013.8.27Min_Throttle: DS 1
; Indirect addressing data segmentISEG AT 080h Pgm_Fir_Key: DS 1 ;增加首个关键字 2013.8.27Pgm_Gov_P_Gain: DS 1 ; Programmed governor P gainPgm_Gov_I_Gain: DS 1 ; Programmed governor I gainPgm_Gov_Mode: DS 1 ; Programmed governor mode
Pgm_Low_Voltage_Lim: DS 1 ; Programmed low voltage limitPgm_Low_Voltage_Ctl: DS 1 ;增加低压保护控制方式编程项 2013.7.5Pgm_Motor_Gain: DS 1 ; Programmed motor gainPgm_Motor_Idle: DS 1 ; Programmed motor idle speedPgm_Startup_Pwr: DS 1 ; Programmed startup powerPgm_Pwm_Freq: DS 1 ; Programmed pwm frequencyPgm_Direction_Rev: DS 1 ; Programmed rotation directionPgm_Input_Pol: DS 1 ; Programmed input pwm polarityInitialized_L_Dummy: DS 1 ; Place holderInitialized_H_Dummy: DS 1 ; Place holderPgm_Enable_TX_Program: DS 1 ; Programmed enable/disable value for TX programmingPgm_Main_Rearm_Start: DS 1 ; Programmed enable/disable re-arming main every start Pgm_Gov_Setup_Target: DS 1 ; Programmed main governor setup targetPgm_Startup_Rpm: DS 1 ; Programmed startup rpmPgm_Startup_Accel: DS 1 ; Programmed startup accelerationPgm_Volt_Comp_Dummy: DS 1 ; Place holderPgm_Comm_Timing: DS 1 ; Programmed commutation timingPgm_Damping_Force: DS 1 ; Programmed damping forcePgm_Gov_Range: DS 1 ; Programmed governor range;Pgm_Startup_Method: DS 1 ; Programmed startup methodPgm_Ppm_Min_Throttle: DS 1 ; Programmed throttle minimumPgm_Ppm_Max_Throttle: DS 1 ; Programmed throttle maximumPgm_Beep_Strength: DS 1 ; Programmed beep strengthPgm_Beacon_Strength: DS 1 ; Programmed beacon strengthPgm_Beacon_Delay: DS 1 ; Programmed beacon delayPgm_Throttle_Rate: DS 1 ; Programmed throttle ratePgm_Demag_Comp: DS 1 ; Programmed demag compensationPgm_BEC_Voltage_High: DS 1 ; Programmed BEC voltage
Pgm_Gov_P_Gain_Decoded: DS 1 ; Programmed governor decoded P gainPgm_Gov_I_Gain_Decoded: DS 1 ; Programmed governor decoded I gainPgm_Throttle_Rate_Decoded: DS 1 ; Programmed throttle rate decodedPgm_Startup_Pwr_Decoded: DS 1 ; Programmed startup power decodedPgm_Demag_Comp_Wait_Decoded: DS 1 ; Programmed demag compensation wait decodedPgm_Demag_Comp_Power_Decoded: DS 1 ; Programmed demag compensation power cut decoded
; Indirect addressing data segmentISEG AT 0D0h Tag_Temporary_Storage: DS 48 ; Temporary storage for tags when updating "Eeprom"
;**** **** **** **** ****CSEG AT 1A00h ; "Eeprom" segmentEEPROM_FW_MAIN_REVISION EQU 10 ; Main revision of the firmwareEEPROM_FW_SUB_REVISION EQU 2 ; Sub revision of the firmwareEEPROM_LAYOUT_REVISION EQU 16 ; Revision of the EEPROM layout
Eep_FW_Main_Revision: DB EEPROM_FW_MAIN_REVISION ; EEPROM firmware main revision numberEep_FW_Sub_Revision: DB EEPROM_FW_SUB_REVISION ; EEPROM firmware sub revision numberEep_Layout_Revision: DB EEPROM_LAYOUT_REVISION ; EEPROM layout revision number
IF MODE == 0Eep_Pgm_Gov_P_Gain: DB DEFAULT_PGM_MAIN_P_GAIN ; EEPROM copy of programmed governor P gainEep_Pgm_Gov_I_Gain: DB DEFAULT_PGM_MAIN_I_GAIN ; EEPROM copy of programmed governor I gainEep_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 powerEep_Pgm_Pwm_Freq: DB DEFAULT_PGM_MAIN_PWM_FREQ ; EEPROM copy of programmed pwm frequencyEep_Pgm_Direction_Rev: DB DEFAULT_PGM_MAIN_DIRECTION_REV ; EEPROM copy of programmed rotation directionEep_Pgm_Input_Pol: DB DEFAULT_PGM_MAIN_RCP_PWM_POL ; EEPROM copy of programmed input polarityEep_Initialized_L: DB 0A5h ; EEPROM initialized signature low byteEep_Initialized_H: DB 05Ah ; EEPROM initialized signature high byteEep_Enable_TX_Program: DB DEFAULT_PGM_ENABLE_TX_PROGRAM ; EEPROM TX programming enableEep_Main_Rearm_Start: DB DEFAULT_PGM_MAIN_REARM_START ; EEPROM re-arming main enableEep_Pgm_Gov_Setup_Target: DB DEFAULT_PGM_MAIN_GOV_SETUP_TARGET ; EEPROM main governor setup targetEep_Pgm_Startup_Rpm: DB DEFAULT_PGM_MAIN_STARTUP_RPM ; EEPROM copy of programmed startup rpmEep_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 timingEep_Pgm_Damping_Force: DB DEFAULT_PGM_MAIN_DAMPING_FORCE ; EEPROM copy of programmed damping forceEep_Pgm_Gov_Range: DB DEFAULT_PGM_MAIN_GOVERNOR_RANGE ; EEPROM copy of programmed governor rangeEep_Pgm_Startup_Method: DB DEFAULT_PGM_MAIN_STARTUP_METHOD ; EEPROM copy of programmed startup methodEep_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 strengthEep_Pgm_Beacon_Strength: DB DEFAULT_PGM_MAIN_BEACON_STRENGTH ; EEPROM copy of programmed beacon strengthEep_Pgm_Beacon_Delay: DB DEFAULT_PGM_MAIN_BEACON_DELAY ; EEPROM copy of programmed beacon delayEep_Pgm_Throttle_Rate: DB DEFAULT_PGM_MAIN_THROTTLE_RATE ; EEPROM copy of programmed throttle rateEep_Pgm_Demag_Comp: DB DEFAULT_PGM_MAIN_DEMAG_COMP ; EEPROM copy of programmed demag compensationEep_Pgm_BEC_Voltage_High: DB DEFAULT_PGM_BEC_VOLTAGE_HIGH ; EEPROM copy of programmed BEC voltageENDIF
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 gainEep_Pgm_Motor_Idle: DB DEFAULT_PGM_TAIL_IDLE_SPEED ; EEPROM copy of programmed tail idle speedEep_Pgm_Startup_Pwr: DB DEFAULT_PGM_TAIL_STARTUP_PWR ; EEPROM copy of programmed startup powerEep_Pgm_Pwm_Freq: DB DEFAULT_PGM_TAIL_PWM_FREQ ; EEPROM copy of programmed pwm frequencyEep_Pgm_Direction_Rev: DB DEFAULT_PGM_TAIL_DIRECTION_REV ; EEPROM copy of programmed rotation directionEep_Pgm_Input_Pol: DB DEFAULT_PGM_TAIL_RCP_PWM_POL ; EEPROM copy of programmed input polarityEep_Initialized_L: DB 05Ah ; EEPROM initialized signature low byteEep_Initialized_H: DB 0A5h ; EEPROM initialized signature high byteEep_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 rpmEep_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 timingEep_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 methodEep_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 strengthEep_Pgm_Beacon_Strength: DB DEFAULT_PGM_TAIL_BEACON_STRENGTH ; EEPROM copy of programmed beacon strengthEep_Pgm_Beacon_Delay: DB DEFAULT_PGM_TAIL_BEACON_DELAY ; EEPROM copy of programmed beacon delayEep_Pgm_Throttle_Rate: DB DEFAULT_PGM_TAIL_THROTTLE_RATE ; EEPROM copy of programmed throttle rateEep_Pgm_Demag_Comp: DB DEFAULT_PGM_TAIL_DEMAG_COMP ; EEPROM copy of programmed demag compensationEep_Pgm_BEC_Voltage_High: DB DEFAULT_PGM_BEC_VOLTAGE_HIGH ; EEPROM copy of programmed BEC voltageENDIF
IF MODE == 2Eep_Pgm_Fir_Key: DB DEFAULT_PGM_MULTI_FIRST_KEYWORD ;增加首个关键字 2013.8.27Eep_Pgm_Gov_P_Gain: DB DEFAULT_PGM_MULTI_P_GAIN ; EEPROM copy of programmed closed loop P gainEep_Pgm_Gov_I_Gain: DB DEFAULT_PGM_MULTI_I_GAIN ; EEPROM copy of programmed closed loop I gainEep_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 limitEep_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 speedEep_Pgm_Startup_Pwr: DB DEFAULT_PGM_MULTI_STARTUP_PWR ; EEPROM copy of programmed startup powerEep_Pgm_Pwm_Freq: DB DEFAULT_PGM_MULTI_PWM_FREQ ; EEPROM copy of programmed pwm frequencyEep_Pgm_Direction_Rev: DB DEFAULT_PGM_MULTI_DIRECTION_REV ; EEPROM copy of programmed rotation directionEep_Pgm_Input_Pol: DB DEFAULT_PGM_MULTI_RCP_PWM_POL ; EEPROM copy of programmed input polarityEep_Initialized_L: DB 055h ; EEPROM initialized signature low byteEep_Initialized_H: DB 0AAh ; EEPROM initialized signature high byteEep_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 rpmEep_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 timingEep_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 methodEep_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 strengthEep_Pgm_Beacon_Strength: DB DEFAULT_PGM_MULTI_BEACON_STRENGTH ; EEPROM copy of programmed beacon strengthEep_Pgm_Beacon_Delay: DB DEFAULT_PGM_MULTI_BEACON_DELAY ; EEPROM copy of programmed beacon delayEep_Pgm_Throttle_Rate: DB DEFAULT_PGM_MULTI_THROTTLE_RATE ; EEPROM copy of programmed throttle rateEep_Pgm_Demag_Comp: DB DEFAULT_PGM_MULTI_DEMAG_COMP ; EEPROM copy of programmed demag compensationEep_Pgm_BEC_Voltage_High: DB DEFAULT_PGM_BEC_VOLTAGE_HIGH ; EEPROM copy of programmed BEC voltageENDIF
Eep_Dummy: DB 0FFh ; EEPROM address for safety reason
CSEG AT 1A60hEep_Name: DB " " ; Name tag (16 Bytes)
;**** **** **** **** **** Interrupt_Table_Definition ; SiLabs interruptsCSEG AT 80h ; Code segment after interrupt vectors
;**** **** **** **** ****
; Table definitionsGOV_GAIN_TABLE: DB 02h, 03h, 04h, 06h, 08h, 0Ch, 10h, 18h, 20h, 30h, 40h, 60h, 80hTHROTTLE_RATE_TABLE: DB 02h, 03h, 04h, 06h, 08h, 0Ch, 10h, 18h, 20h, 30h, 40h, 80h, 0FFhSTARTUP_POWER_TABLE: DB 04h, 06h, 08h, 0Ch, 10h, 18h, 20h, 30h, 40h, 60h, 80h, 0A0h, 0C0hDEMAG_WAIT_TABLE: DB 0, 0, 0, 1, 1DEMAG_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 == 0TX_PGM_PARAMS_MAIN: DB 13, 13, 4, 2, 6, 2, 13, 5, 5, 5, 13, 6, 3, 5, 2, 2ENDIFIF MODE == 1 IF DAMPED_MODE_ENABLE == 1TX_PGM_PARAMS_TAIL: DB 5, 5, 2, 13, 5, 5, 5, 13, 6, 4, 5, 2, 2 ENDIF IF DAMPED_MODE_ENABLE == 0TX_PGM_PARAMS_TAIL: DB 5, 5, 2, 13, 5, 5, 5, 13, 6, 3, 5, 2, 2 ENDIFENDIFIF 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 ENDIFENDIF
;**** **** **** **** **** **** **** **** **** **** **** **** ****;; 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_executeENDIFIF 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_updateENDIF
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 startt0_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 escsENDIF 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 AELSE mov A, #PFETON_DELAY djnz ACC, $ CnFET_On ; Comm phase 1 - turn on CENDIF jmp t0_int_pwm_off_exit
t0_int_pwm_off_comm_2_3: jb ACC.0, t0_int_pwm_off_comm_3IF DAMPED_MODE_ENABLE == 0 BnFET_On ; Comm phase 2 - turn on BELSE mov A, #PFETON_DELAY djnz ACC, $ CnFET_On ; Comm phase 2 - turn on CENDIF jmp t0_int_pwm_off_exit
t0_int_pwm_off_comm_3:IF DAMPED_MODE_ENABLE == 0 CnFET_On ; Comm phase 3 - turn on CELSE mov A, #PFETON_DELAY djnz ACC, $ BnFET_On ; Comm phase 3 - turn on BENDIF 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 AELSE mov A, #PFETON_DELAY djnz ACC, $ BnFET_On ; Comm phase 4 - turn on BENDIF jmp t0_int_pwm_off_exit
t0_int_pwm_off_comm_5:IF DAMPED_MODE_ENABLE == 0 BnFET_On ; Comm phase 5 - turn on BELSE mov A, #PFETON_DELAY djnz ACC, $ AnFET_On ; Comm phase 5 - turn on AENDIF jmp t0_int_pwm_off_exit
t0_int_pwm_off_comm_6:IF DAMPED_MODE_ENABLE == 0 CnFET_On ; Comm phase 6 - turn on CELSE mov A, #PFETON_DELAY djnz ACC, $ AnFET_On ; Comm phase 6 - turn on AENDIF
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_exitpwm_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_exitpwm_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_exitpwm_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_exitpwm_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_exitpwm_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_exitpwm_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 flagt0_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 - branchENDIFIF MODE == 2 ; Multi;读取Closed loop mode mov Temp1, #Pgm_Gov_Mode ; Closed loop mode? cjne @Temp1, #1, t2_int_pwm_exit ; Yes - branchENDIF
; 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 defaultt2_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 pwmENDIFIF 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 pwmENDIF
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, AENDIFIF 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, AENDIF
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 ; 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 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:;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, #23waitxms_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, #5wait1s_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, Afunction_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_belowfun_par_end: setb EA ; Enable all interrupts
ret
;**** **** **** **** **** **** **** **** **** **** **** **** ****;; 接收一个数据（一个字节）;;**** **** **** **** **** **** **** **** **** **** **** **** ****receive_a_byte: mov Temp8,#0 mov Commu_Data_Buffer, #0 mov Temp4, #8wait_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 ;启动T0wait_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, Temp8receive_exit: ret
;**** **** **** **** **** **** **** **** **** **** **** **** ****;; 接收一组数据;;**** **** **** **** **** **** **** **** **** **** **** **** ****receive_bytes: mov Temp5, #48 mov Temp2, #Tag_Temporary_Storage ; Set RAM address mov Commu_Data_Buffer, #0wait_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, #48store_tag: cjne Temp5, #45, store_two mov Temp1, #Pgm_Gov_Mode jmp store_eepstore_two: cjne Temp5, #44, store_three mov Temp1, #Pgm_Low_Voltage_Lim jmp store_eepstore_three: cjne Temp5, #43, store_four mov Temp1, #Pgm_Low_Voltage_Ctl jmp store_eepstore_four: cjne Temp5, #40, store_five mov Temp1, #Pgm_Startup_Pwr jmp store_eepstore_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_eepstore_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 addresswait_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 addresswait_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, #0div_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-bytediv_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, Amult_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 divisionsmult_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 targetIF MODE <= 1 ; Main or tailcalc_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, #1governor_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, Temp2calc_governor_target_exit: ret ENDIFIF MODE == 2 ; Multicalc_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项 MidRangegovernor_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项 LoRangegovernor_activate_50k: clr C mov A, Temp4 rrc A mov Temp4, A mov A, Temp3 rrc A mov Temp3, A ; 50k eRPM range heregovernor_activate_range_set: call div_u16_by_u16 ; Store governor target mov Gov_Target_L, Temp1 mov Gov_Target_H, Temp2calc_governor_target_exit: ret ENDIF
; Second governor routine - calculate governor proportional errorcalc_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, AENDIFIF 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, AENDIF ; 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, Temp2calc_governor_prop_error_exit: ret
; Third governor routine - calculate governor integral errorcalc_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, Temp3calc_governor_int_error_exit: ret
; Fourth governor routine - calculate governor proportional correctioncalc_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 pwmgovernor_store_prop_corr: ; Store proportional pwm mov Gov_Prop_Pwm, Temp1calc_governor_prop_corr_exit: ret
; Fifth governor routine - calculate governor integral correctioncalc_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 pwmgovernor_store_int_corr: ; Store current pwm mov Current_Pwm, Temp1calc_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_exitENDIFmeasure_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 ;3measure_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.0Vmeasure_lipo_update: ; Set ADC limit mov Lipo_Adc_Limit_L, Temp7 mov Lipo_Adc_Limit_H, Temp8measure_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 zerotemp_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_Temptemp_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 - branchcheck_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_Hread_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 timesdivide_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, #120pwm_wait: clr C mov A, TL1 subb A, Temp2IF DAMPED_MODE_ENABLE==0 jc comp_wait_on_comp_able ; Re-evaluate pwm cycle for slower escsENDIFIF 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 startENDIF
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 waitwait_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 waitwait_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 waitwait_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_nondampcomm12_damp:IF DAMPED_MODE_ENABLE == 0 mov DPTR, #pwm_cnfet_apfet_on_fast jb Flags2.PGM_PWMOFF_DAMPED_LIGHT, comm12_nondampENDIFIF 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_nondampcomm23_damp:IF DAMPED_MODE_ENABLE == 0 mov DPTR, #pwm_bnfet_apfet_on_fastENDIFIF 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_nfetcomm23_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 oncomm23_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_nondampcomm34_damp:IF DAMPED_MODE_ENABLE == 0 mov DPTR, #pwm_bnfet_cpfet_on_fast jb Flags2.PGM_PWMOFF_DAMPED_LIGHT, comm34_nondampENDIFIF DAMPED_MODE_ENABLE == 1 mov DPTR, #pwm_bnfet_cpfet_on_safeENDIF 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_nondampcomm45_damp:IF DAMPED_MODE_ENABLE == 0 mov DPTR, #pwm_anfet_cpfet_on_fastENDIFIF DAMPED_MODE_ENABLE == 1 mov DPTR, #pwm_anfet_cpfet_on_safeENDIF 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_nfetcomm45_nondamp: mov DPTR, #pwm_afet_oncomm45_nfet: BnFET_off ; Bn off jnb Flags0.PWM_ON, comm45_cp ; Is pwm on? AnFET_on ; Yes - An oncomm45_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_nondampcomm56_damp:IF DAMPED_MODE_ENABLE == 0 mov DPTR, #pwm_anfet_bpfet_on_fast jb Flags2.PGM_PWMOFF_DAMPED_LIGHT, comm56_nondampENDIFIF DAMPED_MODE_ENABLE == 1 mov DPTR, #pwm_anfet_bpfet_on_safeENDIF 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_nondampcomm61_damp:IF DAMPED_MODE_ENABLE == 0 mov DPTR, #pwm_cnfet_bpfet_on_fastENDIFIF DAMPED_MODE_ENABLE == 1 mov DPTR, #pwm_cnfet_bpfet_on_safeENDIF 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_nfetcomm61_nondamp: mov DPTR, #pwm_cfet_oncomm61_nfet: AnFET_off ; An off jnb Flags0.PWM_ON, comm61_cp ; Is pwm on? CnFET_on ; Yes - Cn oncomm61_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, #0ENDIFIF 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, #4ENDIFIF 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=0ENDIF 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,#250 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 Temp8IF 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_FULLENDIFIF 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_FULLENDIF;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 voltageIF 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, #0test_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, #3wait_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 Temp1clear_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 1usIF COMP1_USED==1 mov CPT1CN, #80h ; Comparator enabled, no hysteresis mov CPT1MD, #03h ; Comparator response time 1usENDIF ; 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 Pca_First_Int, #1
; Measure PWM frequencymeasure_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 wait200msmeasure_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, #1measure_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项 enabledvalidate_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 targetarming_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 enabledarming_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 timethrottle_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 wait1swait_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 timethrottle_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 onwait_for_power_on: mov Temp8, #50 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:;////// 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_conversionread_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 lowrun1: 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 highrun2: 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 lowrun3: 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 highrun4: 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 lowrun5: 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 highrun6: 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 variablesIF 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 powerENDIFIF 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_exithigh_damp: All_nFETs_Ondamp_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 wait1srun_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项 disabledjmp_wait_for_power_on: jmp wait_for_power_on ; Go back to wait for power onENDIFIF 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 onENDIF
;**** **** **** **** **** **** **** **** **** **** **** **** ****
$include (BLHeliTxPgm.inc) ; Include source code for programming the ESC with the TX
;**** **** **** **** **** **** **** **** **** **** **** **** ****
END
|
