; =============================================================================
;                 ICS-100Li Electronic Speed Controller
; =============================================================================
;                Bruce Abbott (bhabbott@paradise.net.nz)
;
; Why you might want this code:-
;
; 1. Replace blown PIC chip in your ICS/GS-100li (also works with ICS-50li).
; 2. Enhanced features (exponential throttle, arming beeps, glitch filter).
;
; NOTE: Cutoff voltage is set to 5.5V/8.3V (same as the original). For
;       6V/9V cutoff, change the 11k resistor (05C) to 10k.
;
; Summary of Changes:
;
; 2008-4-12  V1.0 first release
; 2008-11-29 V1.1 added diagnostic output (GP5 stays high if signal was lost)
; 2011-7-20  V1.2 integrated throttle curve table (was in file 'spd400.inc')

; enable the next line only if CPU is NOT specified in assembly options

;		PROCESSOR PIC12F675

		INCLUDE   <P12F675.inc>

                radix     dec		; default number format is decimal

		errorlevel 0,-305,-302	; disable unimportant warnings

version  = 1
revision = 2

; ==========================================================================
; Configuration is:
;   Master clear pin disabled (used as input)
;   Code protection OFF
;   Watchdog timer ON
;   Oscillator is Internal RC

	__config  _MCLRE_OFF&_CP_OFF&_WDT_ON &_BODEN_ON&_INTRC_OSC_NOCLKOUT

; Bit definitions for I/O registers

#DEFINE BattBit    0               ; pin 7  battery voltage sense
#DEFINE BrakeBit   1               ; pin 6  Brake FET on when high
#DEFINE RunBit     2               ; pin 5  Motor FET on when high
#DEFINE ServoBit   3               ; pin 4  servo pulse input
#DEFINE LedBit     4               ; pin 3  Status LED (optional)
#DEFINE DiagBit    5               ; pin 2  Diagnostic LED (optional)

#DEFINE TrisBits   (1<<BattBit)|(1<<ServoBit) ; bit = 1 if input

; Bits to be set with the OPTION instruction
;   No weak pullups
;   Interrupt on rising edge of GP2 (not used)
;   Timer 0 source internal
;   Prescaler to watchdog 1:64 (~270mS timeout).
;
;
#DEFINE OptionBits  B'11011100'

; ==========================================================================
; Algorithm constants
#DEFINE PWM_Period     47              ; Number of 7-instr code cycles
#DEFINE PWM_Start      4               ; Lowest cycle count
#DEFINE ArmPulsesReqd  25              ; delay before arming (*20mS)
#DEFINE BRAKEDELAY     25	       ; delay before braking

;===========================================================================
; Macro to create offsets for file registers in RAM
;
ByteAddr	SET 32

BYTE            MACRO     ByteName
ByteName        EQU       ByteAddr
ByteAddr        SET       ByteAddr+1
                ENDM

; ==========================================================================
; User RAM Variable  Definitions.
;

                BYTE       Flags           ; various boolean flags
                BYTE       PwmCount        ; counts down to zero during PWM output
                BYTE       PwmWidth        ; Initial value to be loaded into PwmCount
                BYTE       ServoCount      ; length of servo pulse 0-185 * 7uS (0.9-2.2mS)
                BYTE       HiCount         ; coarse servo pulse length (ServoCount/47)
                BYTE       Command         ; PWM width (0-43) + BrakeEnableBit
		BYTE	   PortBits	   ; Shadow Copy of GPIO register
                BYTE       ArmPulseCount   ; arming safety delay (counts down)
                BYTE       ServoPulse      ; current averaged servo pulse width
                BYTE       LastPulse       ; last averaged servo pulse width
		BYTE       BadPulses       ; # of consecutive bad servo pulses
		BYTE	   BrakeDelayCount ; delay before braking
                BYTE       StatCopy        ; copy of STATUS register at startup

#DEFINE  MAXBADPULSES 50

; Bits in Flags

#DEFINE BattLow		0		; detected low battery voltage
#DEFINE PriorServoBit	1		; servo pulse was high
#DEFINE ThreeCells	2		; 3S Lipo detected
#DEFINE PulseBad	7               ; servo pulse width out of range

; =========================================================================
; Macro for generating short time delays
;
NO_OP           MACRO   count
NO_OP_COUNT     SET     count
                WHILE   NO_OP_COUNT>1
		goto	$+1		; 2 clocks
NO_OP_COUNT     SET     NO_OP_COUNT-2
                ENDW
		IF	NO_OP_COUNT
		nop			; 1 clock
		ENDIF
                ENDM

; =========================================================================
; Macro to start a PWM cycle
;
DoRunBit        MACRO
		movf	  PortBits,w
		movwf	  GPIO
                movf      PwmWidth,w
		movwf	  PwmCount
                ENDM

; =========================================================================
; Macro to keep up with the servo pulse duration
; The code in this Macro uses 2 clocks.
;
DoServoCount    MACRO
                btfsc     GPIO, ServoBit
                incf      ServoCount, F
                ENDM


; =============================== CODE ====================================
;
; NOTE: Critical instruction timings are shown in brackets
;       eg. ;(2) = 2 microseconds
;
;

Coldstart:      ORG     0

; jump to high ROM area (low area needed for subroutines)

		goto	Start


; version string

		ORG	8
                dt      "ICS100Li>>V"
                dt      version+48
                dt      "."
                dt      revision+48
		dt	"<<"

; ========================================================================
; The main PWM loop
;
; Two timing loops have to be maintained simultaneously:-
;
; 1/ Increment servo pulse width counter every 7 clocks (if servo pulse is high).
;    DoServoCount takes 2 clocks, leaving 5 clocks available for other processing.
;
; 2/ DoRunBit + 17 clocks + DoPwmPulse + 2 clocks etc. This creates the PWM
;    cycle time of 329 clocks, corresponding to a PWM frequency of 3Khz.
;
; NOTE: DoServoCount occurs 43 times inside DoPwmPulse.
;
;
ServoIsHigh:    bsf       Flags, PriorServoBit ;(1) servo pulse has started!
                DoServoCount                   ;(2) measure servo pulse width
                NO_OP     2                    ;(2)
                incf      HiCount,F            ;(1) HiCount = ServoCount/47
                skpnz                          ;(1) Limit HiCount to 255 max
                decf      HiCount,F            ;(1)
MainPwmLoop:    DoServoCount                   ;(2) measure servo pulse width
                NO_OP     1                    ;(1)
                call      DoPwmPulse           ;(4,2,5) complete PWM cycle
                DoServoCount                   ;(2) measure servo pulse width
MainPwmEntry:   DoRunBit                       ;(4) start PWM cycle
		nop			       ;(1)
                DoServoCount                   ;(2) measure servo pulse width
                movwf     PwmCount             ;(1)
                btfsc     GPIO, ServoBit       ;(2) is servo pulse high ?
                goto      ServoIsHigh          ;(+1)
                btfsc     Flags, PriorServoBit ;(2)  Servo pulse now low, but was it high?
                goto      EndOfPulse           ;(+1) yes, then must be end of pulse
                clrf      ServoCount           ;(1) no, so reset servo pulse count
                DoServoCount                   ;(2) measure servo pulse width
                NO_OP     2                    ;(2)
                goto      MainPwmLoop          ;(2)



; ============================================================================
; End of Servo Pulse. We don't have to measure the servo pulse now, but we
; still have to maintain the PWM cycle timing!
;
;  DoRunBit, +18 cycles, DoPwmCycle, +2 cycles, DoRunBit etc.
;
; Starting 9 clocks after the last DoRunBit
;
EndOfPulse:     movfw     ServoCount            ;(1)
                movwf     ServoPulse            ;(1) save servo pulse count
                movlw     3                     ;(1)
                subwf     HiCount,W             ;(1) HiCount < 3 ?
                skpc                            ;(1)
                bsf       Flags,PulseBad        ;(1) servo pulse may be too short!
                skpc                            ;(1)
                btfsc     ServoPulse,7          ;(1) servo count >= 128 ?
                bcf       Flags,PulseBad        ;(1) yes, pulse OK
                call      DoPwmPulse            ;<<<
                NO_OP     2                     ;(2)
                DoRunBit                        ;>>>
		nop				;(1)
                movwf     PwmCount              ;(1)
                movlw     8                     ;(1)
                subwf     HiCount,W             ;(1) HiCount >= 8 PWM cycles ?
                skpnc                           ;(1)
                bsf       Flags,PulseBad        ;(1) servo pulse is too long!
                clrf      HiCount               ;(1) reset HiCount
                movlw     128                   ;(1) min servo pulse width = 0.9mS
                subwf     ServoPulse            ;(1)
                movlw     186                   ;(1) max servo pulse width = 2.2mS
                subwf     ServoPulse,W          ;(1)
                skpnc                           ;(1) pulse width in range ?
                bsf       Flags,PulseBad        ;(1)
                btfsc     Flags,PulseBad        ;(2)  ignore bad servo pulse!
                goto      Bad_Pulse             ;(+1)
                movfw     LastPulse             ;(1)  calculate average of count and...
                addwf     ServoPulse            ;(1)  ...last count, to reduce jitter.
                rrf       ServoPulse            ;(1)  average = (count + last) / 2
                call      DoPwmPulse            ;<<<
                NO_OP     2                     ;(2)
                DoRunBit                        ;>>>  still using old PWM
                movf      ServoPulse, W         ;(1)
                call      ServoLookup           ;(7)  throttle curve lookup table
                movwf     Command               ;(1)
		andlw     b'00111111'           ;(1)  isolate PWM value
                movwf     PwmWidth              ;(1)  set new PWM width
		skpnz				;(1)
		bcf	  PortBits,RunBit	;(1)  if PWM = 0 then Motor off
                btfsc     Command,6	        ;(2)  Braking?
                goto      Braking               ;(+1)
No_Brake:       bcf       PortBits,BrakeBit     ;(1)  Turn the brake OFF
		NO_OP	  2			;(2)
                call      DoPwmPulse            ;<<<  still using old PWM
                movlw     BRAKEDELAY            ;(1)
                movwf     BrakeDelayCount       ;(1)  set brake delay
                DoRunBit                        ;>>>
		skpz				;(1)
		bsf	  PortBits,RunBit	;(1) if PWM > 0 then Motor on
                goto      Good_Pulse            ;(2)
;
; Here the brake is to be turned on. Let the motor slow down first. PWM
; timing isn't critical while braking, because the motor is always off.
;
Braking:        bcf       PortBits,RunBit       ; will stop the motor
		movf	  PortBits,w
		movwf	  GPIO
		decfsz	  BrakeDelayCount       ; Time to turn on the Brake?
                goto      Good_Pulse            ; No, wait for another servo pulse
Brake_On:       incf      BrakeDelayCount       ; Yes, keep brake on
                bsf       PortBits, BrakeBit    ; will turn the brake FET on
		movf	  PortBits,w
		movwf	  GPIO			; Motor FET off, Brake FET on/off

Good_Pulse:     movf      ServoPulse,W          ;(1)  remember good servo pulse
                movwf     LastPulse             ;(1)
                tstf	  BadPulses             ;(1)  another good pulse :)
		skpz				;(1)
		decf	  BadPulses		;(1)
Next_Pulse:	NO_OP     6                     ;(6)
		clrwdt				;(1)  reset watchdog timer
                bcf       Flags, PriorServoBit  ;(1)  servo pulse is finished
                clrf      ServoCount            ;(1)  reset servo pulse count
                call      DoPwmPulse            ;<<<
                goto      MainPwmEntry          ;(2)  DoRunBit is next

;
; Servo pulse bad, so just keep the existing PWM going.
; Don't remember the bad pulse.
;
Bad_Pulse:      NO_OP     2                     ;(2)
                call      DoPwmPulse            ;<<< still using old PWM
                NO_OP     1                     ;(1)
                bcf       Flags,PulseBad        ;(1)
                DoRunBit                        ;>>> still using old PWM
		NO_OP	  2			;(2)
                movwf     PwmCount              ;(1)
		incf	  BadPulses		;(1) another bad pulse :(
		movlw	  MAXBADPULSES		;(1)
		subwf	  BadPulses,W		;(1) too many bad pulses ?
		skpc				;(2) if so then shut down
                goto      Next_Pulse            ;(+1) else continue
;
; No Signal - shutdown with brake OFF and wait for arming
;
No_Signal:      bcf       PortBits, RunBit      ; will stop the Motor
		bcf	  PortBits, BrakeBit	; will turn the Brake off
		bcf	  PortBits, LedBit	; will turn status LED off
		bsf	  PortBits, DiagBit	; will turn diagnostic LED on
		movf	  PortBits,w
		movwf	  GPIO
                goto      Rearm

;
; Low Battery - shutdown with brake ON and wait for arming.
;
Low_Batt:       bcf       PortBits, RunBit      ; will stop the Motor
		bcf	  PortBits, LedBit	; will turn status LED off
		movf	  PortBits,w
		movwf	  GPIO
		bsf	  PortBits, BrakeBit	; will turn the Brake on.
		movf	  PortBits,w
		movwf	  GPIO
                goto      Rearm

;---------------------------------------------------------------------------
;                   Play 'Beep' sound through the motor
;---------------------------------------------------------------------------

Beep:           clrf      PwmCount
beep1:          bsf       GPIO,RunBit          ; motor on
                NO_OP     13
                bcf       GPIO,RunBit          ; motor off
                movlw     60
                movwf     PwmWidth
beep2:          clrwdt
                decfsz    PwmWidth,F           ; wait 0.24mS
                goto      beep2
                movlw     60
                movwf     PwmWidth
beep3:          clrwdt
                decfsz    PwmWidth,F           ; wait 0.24mS
                goto      beep3
                decfsz    PwmCount,F
                goto      beep1                ; do 256 cycles (128mS)
		retlw	  0


;---------------------------------------------------------------------------
;                              Pause W x 1mS
;---------------------------------------------------------------------------
;
Pause128:	movlw	  128
;
; Input: W = milliseconds
;
Pause:		movwf	  PwmCount
pause_1		movlw	  249
		movwf	  HiCount
pause_2		clrwdt			; clear the watchdog timer!!!
		decfsz	  HiCount
		goto	  pause_2
		decfsz	  PwmCount
		goto      pause_1
		retlw	  0


; ==========================================================================
; Servo pulse to command converter.  Returns with command in W.
; See speed400.inc for the table at 0x200 which is being referenced
; This routine takes 7 clocks to execute.
;
ServoLookup:
		bsf	  PCLATH,1	; = 0x02 (code page 2)
                movwf     PCL           ; goto 0x200+W


; ==========================================================================
; The main PWM pulse routine.  On entry, the motor may be on or off.
; If the motor is off, it will never be turned on.  However, it will be
; turned off when (if) the PWM_Count register reaches zero.
;
DoPwmPulse:	movf	  PortBits,w
		andlw	  ~(1<<RunBit)		; will turn motor off
PWM_LOOP_COUNT  SET       PWM_Period - PWM_Start
                WHILE     PWM_LOOP_COUNT	; following code repeated 43 times

                btfss     CMCON,COUT	        ;(2) check for low battery
                goto      Low_Batt
                DoServoCount                    ;(2) measure servo pulse length
                decf      PwmCount,F            ;(1) decrement PWM count
                skpnz                           ;(1) end of PWM high pulse?
                movwf     GPIO	                ;(1) yes, turn off motor

PWM_LOOP_COUNT  SET       PWM_LOOP_COUNT - 1
                ENDW

                retlw     0                     ;(2)


; ==========================================================================
; Servo command lookup table.  The index is 0-255, which is the lower 8 bits
; of the servo pulse width counter.  The output is:
;
;    bit 0:5 = PWM_width 0-63
;          6 = Brake ON
;          7 = invalid pulse
;
; If PWM_width = 0 then the motor will be turned OFF.
;
; NOTE: table MUST start at code memory location 0x200.
;
            ORG         0x200

            retlw       H'40'    ; (0) Brake on at 0.9mS
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'
            retlw       H'40'  
            retlw       H'00'  ; (43) Brake off at 1.2mS
            retlw       H'00'
            retlw       H'00'
            retlw       H'03'
            retlw       H'05'
            retlw       H'06'
            retlw       H'07'
            retlw       H'09'
            retlw       H'0A'
            retlw       H'0A'
            retlw       H'0B'
            retlw       H'0C'
            retlw       H'0D'
            retlw       H'0E'
            retlw       H'0E'
            retlw       H'0F'
            retlw       H'10'
            retlw       H'10'
            retlw       H'11'
            retlw       H'12'
            retlw       H'12'
            retlw       H'13'
            retlw       H'13'
            retlw       H'14'
            retlw       H'15'
            retlw       H'15'
            retlw       H'16'
            retlw       H'16'
            retlw       H'17'
            retlw       H'17'
            retlw       H'18'
            retlw       H'18'
            retlw       H'19'
            retlw       H'19'
            retlw       H'19'
            retlw       H'1A'
            retlw       H'1A'
            retlw       H'1B'
            retlw       H'1B'
            retlw       H'1C'
            retlw       H'1C'
            retlw       H'1C'
            retlw       H'1D'
            retlw       H'1D'
            retlw       H'1E'
            retlw       H'1E'
            retlw       H'1E'
            retlw       H'1F'
            retlw       H'1F'
            retlw       H'20'
            retlw       H'20'
            retlw       H'20'
            retlw       H'21'
            retlw       H'21'
            retlw       H'21'
            retlw       H'22'
            retlw       H'22'
            retlw       H'22'
            retlw       H'23'
            retlw       H'23'
            retlw       H'23'
            retlw       H'24'
            retlw       H'24'
            retlw       H'24'
            retlw       H'25'
            retlw       H'25'
            retlw       H'25'
            retlw       H'26'
            retlw       H'26'
            retlw       H'26'
            retlw       H'27'
            retlw       H'27'
            retlw       H'27'
            retlw       H'28'
            retlw       H'28'
            retlw       H'28'
            retlw       H'28'
            retlw       H'29'
            retlw       H'29'
            retlw       H'29'
            retlw       H'2A'
            retlw       H'2A'
            retlw       H'2A'
            retlw       H'2B'
            retlw       H'2B'   
            retlw       H'2B'   
            retlw       H'3F'  ; (128) full throttle at 1.8mS
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'   
            retlw       H'3F'
            retlw       H'3F'                                
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'   ; (186) greater than 2.2mS (or 0.4mS if servocount<128)
            retlw       H'3F'   ; Should NEVER get to here!
            retlw       H'3F'   
            retlw       H'3F'   
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'   
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'   
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'   
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'   
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'   
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'   
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'   
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F'
            retlw       H'3F' 


;-----------------------------------------------------------------------------

Start:          clrf    GPIO		; all outputs Low
		bsf     STATUS,RP0	; register bank 1
                movlw   TrisBits	; I/O direction bits
		movwf	TRISIO		; set I/O directions
		movlw	b'00000001'	; GPO is analog input
		movwf	ANSEL		; set Analog inputs
		call	0x3ff		; get OSCCAL value
		movwf	OSCCAL		; Calibrate internal oscilator
		movlw	b'10000010'	; Voltage reference = 1.56V (9V cutoff)
		movwf	VRCON		; configure Voltage Reference
		bcf	STATUS, RP0     ; register bank 0
		movlw	b'00011110'     ; COUT inverted, -in to pin 7, +in to VRef
        	movwf	CMCON		; configure comparator

; Move prescaler from tmr0 to the watchdog without causing accidental reset.

                clrwdt
                clrf      TMR0
                movlw     OptionBits | 7
		bsf	  STATUS,RP0
		movwf	  OPTION_REG
		bcf	  STATUS,RP0
                clrwdt
                movlw     OptionBits
		bsf	  STATUS,RP0
		movwf	  OPTION_REG
		bcf	  STATUS,RP0
                clrwdt


; wait 1 second for peripheral circuits to stabilize

		clrf	  PwmCount
delay1		clrf	  HiCount
delay2		clrwdt			; clear the watchdog timer!!!
		NO_OP     14
		decfsz	  HiCount
		goto	  delay2
		decfsz	  PwmCount
		goto      delay1

; detect number of cells in battery (2S or 3S)

		bsf	Flags,ThreeCells
		btfsc	CMCON,COUT	; Battery Voltage > 9V ?
		goto	Rearm

twocells:	bcf	Flags,ThreeCells
		bsf	STATUS,RP0	; register bank 1
		movlw	b'10100101'  	; Voltage Reference = 1.04V (6V cutoff)
		movwf	VRCON
		bcf	STATUS,RP0	; register bank 0

Rearm:          clrwdt
                btfss     CMCON,COUT    ; Don't leave if low voltage
                goto      Rearm
;
; Wait for a whole servo pulse before looking for arming pulses
;
RaLoop1:        clrwdt
		btfss     CMCON,COUT		; restart if battery low
                goto      Rearm
                btfss     GPIO, ServoBit	; wait for High
                goto      RaLoop1
RaLoop2:        btfss     CMCON,COUT		; restart if battery low
                goto      Rearm
                btfsc     GPIO, ServoBit	; wait for Low
                goto      RaLoop2

                movlw     ArmPulsesReqd
                movwf     ArmPulseCount

RaLoop3:        clrf      ServoCount
                clrwdt
;
; Measure length of the next servo pulse
;
RaLoop4:        btfss     CMCON,COUT	       ; restart if low battery voltage
                goto      Rearm
                btfss     GPIO, ServoBit       ; wait until servo pulse is high
                goto      RaLoop4
RaLoop5:        incf      ServoCount           ;(1) measure servo pulse width
                skpnz                          ;(2)
                goto      Rearm                ;    restart if > 255
                NO_OP     1                    ;(1) 7uS per count
                btfsc     GPIO,ServoBit        ;(1)
                goto      RaLoop5              ;(2) wait until pulse is low

; accept low throttle only

                clrwdt
                movlw     128                  ; min count = 0.9mS
                subwf     ServoCount,F
                movlw     43                   ; max count = 1.2mS
                subwf     ServoCount,W         ; is this a legal arming pulse?
                skpnc
                goto      RaLoop1              ; No - start over
                decfsz    ArmPulseCount,F      ; Do we have enough arm pulses?
                goto      RaLoop3              ; Not yet.
;
; Play 'beep' sounds through the motor, indicating successful arming.
;
		btfss	  Flags,ThreeCells
		goto	  twobeeps
		call	  Beep			; 3 beeps for 3S
		call	  Pause128
twobeeps:	call	  Beep
		call	  Pause128		; 2 beeps for 2S
		call	  Beep
;
; wait for end of the next servo pulse.
;
servo_hi:       btfss     GPIO,ServoBit        ; wait for start of pulse
                goto      servo_hi
servo_lo:       btfsc     GPIO,ServoBit        ; wait for end of pulse
                goto      servo_lo

Armed:          clrf      PwmWidth             ; PWM = 0 (motor off)
                clrf      ServoCount
                clrf      HiCount
                clrf      LastPulse
		clrf	  BadPulses
		bcf	  Flags,PulseBad
                bcf       Flags,PriorServoBit
		bcf	  Flags,BattLow

		clrf	  PortBits
		bsf	  PortBits,LedBit	; will turn status LED on

; Arming complete, jump to main operating loop. The motor is off, and
; will stay off until the next servo pulse has been processed.

                goto      MainPwmLoop


;
; This subroutine loads the OSCCAL value into W. It is normally
; pre-programmed by Microchip.
;
                org       0x3ff
		retlw	  0x48                 ; OSCCAL value

                END
