; BRUSHLESS ESC (flea101_dt1a.asm)

; This program is intended for small 3-phase permanent magnet brushless motors
; It is based on Bernhard Konze's 'Flea101' software with the following changes:
; (i) Modified to run on Turnigy 6A commercial ESC
; (ii) Commented to aid interpretation/understanding
; (iii) Modified to be a free-flight timer (FF)
; (iv) Implemented a simple stalled motor check
; (v) Added an LED to aid programming run time and flying at night

; See www.flyelectric.ukgateway.net for more info
; Copyright David Theunissen May 2008 and...

;-------------------------------------------------------------------------

;Die Benutzung der Software ist mit folgenden Bedingungen verbunden:
;
;1. Da ich alles kostenlos zur Verfügung stelle, gebe ich keinerlei Garantie
;   und übernehme auch keinerlei Haftung für die Folgen der Benutzung.
;
;2. Die Software ist ausschließlich zur privaten Nutzung bestimmt. Ich
;   habe nicht geprüft, ob bei gewerblicher Nutzung irgendwelche Patentrechte
;   verletzt werden oder sonstige rechtliche Einschränkungen vorliegen.
;
;3. Jeder darf Änderungen vornehmen, z.B. um die Funktion seinen Bedürfnissen
;   anzupassen oder zu erweitern. Ich würde mich freuen, wenn ich weiterhin als
;   Co-Autor in den Unterlagen erscheine und mir ein Link zur entprechenden Seite
;   (falls vorhanden) mitgeteilt wird.
;
;4. Auch nach den Änderungen sollen die Software weiterhin frei sein, d.h. kostenlos bleiben.
;
;!! Wer mit den Nutzungbedingungen nicht einverstanden ist, darf die Software nicht nutzen !!
;
; October 2004
; autor: Bernhard Konze
; email: bernhard.konze(at)versanet.de
; web:	 http://home.versanet.de/~b-konze/blmc_flea/blmc_en.htm

;---------------------------------------------

; Changes made to fblmc101.asm

; 1. changed portc OUT instructions in 'reset' (portd was being set twice)
; 2. deleted dedundant steps from 'switch_power_off' (already done in calling routine)
; 		ldi	temp1, INIT_PB			; all off
; 		out	PORTB, temp1
;		ldi	temp1, CHANGE_TIMEOUT	; reset change-timeout
;		mov	tcnt0_change_tot, temp1
; 3. PORTB changed to PORTD where pFets are on a different port
; 4. changed sbi to cbi and visa versa for all pFETs because driven via transistors
; 5. merged .inc file with this one for convenience
; 6. changed state0 to flags0 and state1 to flags1 register names (leaving state1 as sub-routine)
; 7. changed 'subi temp2, 0xca' (in calculate_next_timing_values) to 0xcc to match quoted max rpm
; 8. i_temp1 changed to temp1 in evaluate_sys_state
; 9. added new FF code/variables and changed sram addressing to dseg/.byte directive
; 10. load .eep file at least once to get default FF run time into eeprom
; 11. added LED to beeps
; 12. deleted 'evaluate_rc_puls' from state2 (triggered with another flag for wait_OCT1_before_switch)
; 13. deleted 'calculate_next_timing_values' from state6 (triggered with flag as well)
; 14. changed 'wait_if_spike' to 4us as intended according to comment (was ~14us)
; 15. added free flight code
; 16. added stalled motor check

; Queries

; should change 'ff_program' from sram to flag2
; does comparator need to be configured every time in wait_OCT1_tot
; add beeps after stopping? (lost model)
; lost signal counter rcpuls_timeout not working (when radio switched off) - reduce RCP_TOT?
; External Interrupt config would need changing if 1 instead of 0 was used (ISC00 ISC01)
; could change GIMSK to GICR as new name but both work
; voltage and current monitoring not enabled
; should cut throttle if over-current in evaluate_sys_state
; occassional hiccup while running
; FF setup program hangs or corrupts eeprom run time sometimes (setting added to restore default)

;====================================================================

; P R O G R A M    D E S C R I P T I O N


; FREE FLIGHT

; The push-button sets the throttle to max and existing acceleration dampner increases power smoothly
; An existing timer function has been modified to time run time and then cut the throttle
; The duration of the orginal decelleration dampner has been doubled to provide a 6s ramp down

; A stalled motor function has been written to cut the motor
; This should work in normal 'RC' mode as well as 'FF'

; CORE ESC OPERATION

; 3 phases are activated in 6 states considered to be 1 electrical revolution, ie: 360'
; 1 electrical revolution assumes 2 magnet poles; more physical poles = slower rpm
; Commutation occurs every 60' in a 2pole motor (360/6 states)
; BEMF is used to determine mid-point between states (30') called 'zero-crossing' (ZC)
; The next commutation occurs 30' later, earlier if using advance timing
; (this program uses 7.5' advance timing so commutation occurs at 30 - 7.5 = 22.5')

; The core program operation is summarised below
; Functions (:) are described in a tiered structure as they are called (ie: text-based flow chart)
; This sequence, along with clock speed and pwm frequency, limits max possible rpm

;--------------------------------------------------------------------

; reset:
; 	(main configuration function)

;	switch_power_off:
;		(setup configuration)

;	beep328ms:
;		(single beep)

;	control_start:
;		(setup configuration)

;		waits for button press to program run time or start motor
;		eeprom_read:
;			(reads FF run time from eeprom)
;		eeprom_write:
;			(saves new run times to eeprom
;		beep250:
;			(lower pitch beep)

;		beep328ms:
;			(double beep after radio checks)
;		calculate_next_timing_values:
;			(see below)
;		set_default_timeout:
;			(see below)
;		wait_OCT1_tot:
;			(sets startup timing (part of wait_OCT1_before_comp_scan)

;---------------------------------------------

; state#:
; 	(6 states, each starting shortly before ZC)
; 	(works closely with various interrupts, particularly t0ovfl_int which switches fets)
;	(timer1 times speed of operation (=rpm) and is used for timing (adjusted once per revolution)

;	waits for ZC (or startup timeout if shorter)

;	evaluate_ff_stop:
;		check for button press to stop motor manually (FF, state1 only)

;	evaluate_sys_state:
;		performed in state 5 only
;		check over-current/under-voltage every 65ms (not enabled)

;		set_new_duty:
;			controls throttle behaviour, essential for responding to throttle changes/enabling fets
;			sets pwm duty cycle and creates a throttle curve for low rpm
;			reduces throttle if under-voltage detected (not enabled)


;	wait_if_spike:
;		waits 14us after ZC to confirm ZC (bypassed during startup)

;	wait_OCT1_before_switch:
;		(imposes delay after ZC for next commutation to achieve desired advance timing)

;		tcnt1_to_temp:
;			gets current commutation time-stamp

;		add advance timing delay (22.5' if using 7.5' advance timing)
;		wait greater of that and performing following checks...

;			evaluate_rc_puls:
;				performed in state 2 only
;				converts throttle setting to fet duty cycle

;			calculate_next_timing_values:
;				performed in state 6 only
;				determines rpm, sets rpm limits, sets commutation timing

;				tcnt1_to_temp:
;					get current commutation time-stamp

;				calculate rpm/time taken to perform 6 state changes (360')
;				calculate 22.5' duration for delays/rpm comparisons
;				convert throttle position to an rpm scale to compare against this

;				set_default_timeout:
;					resets timing if rpm too slow or too fast

;				calculate advance timing delay (also 22.5' but different examples are provided)
;				set advance timing delay for next 6 states (22.5 if 7.5')

;	perform commutation (ie: enable/disable fets for next state)
;	configure comparator for use during next state

;	evaluate_stall:
;		stalled motor check (state3 only)

;		soft_reset:
;			resets program

;	wait_OCT1_before_comp_scan:
;		imposes delay after commutation before scanning for ZC (to avoid inductive spike)

;		tcnt1_to_temp:
;			get current commutation time-stamp

;		wait 22.5' to avoid inductive spike

;		wait_OCT1_tot:
;			configure startup timing delay (but don't wait)

;	move on to next state (endless loop unless errors detected)

;---------------------------------------------

; ext_int0:
;	interrupt to time rc pulses from receiver
;	performs some glitch validation
;	pulses typically go high every ~20ms and low after 1-2ms (= 2 interrupts)

;---------------------------------------------

; t1oca_int:
;	interrupt to signal end of commutation timing delays
;	occurs twice in every state
;	clears OCT1_PENDING flag

;---------------------------------------------

; t1ovfl_int:
;	interrupt to create a 24bit commutation timer
;	also acts as a lost signal counter
;	times FF run time and cuts throttle
;	occurs every 65ms

;---------------------------------------------

; t0ovfl_int:
;	interrupt to create pwm mechanism for fets
;	works in pairs (ON and OFF cycles) - 2 interrupts per period
;	one pair intended to sum to 100us (10kHz pwm period)
;	toggles nFETs only, working closely with the 6 'state' functions
;	imposes acceleration and deceleration dampening

;---------------------------------------------

; read_uart:
;	various uart control functions



;====================================================================

; P R O G R A M    S T A R T S    H E R E

;====================================================================

; set fuses to 8MHz with programmer

; device file
.include "m8def.inc"


.equ UART_CONTROL = 0	; UART (1) RC (0) FF (0)
.equ UART_FULL	  = 0	; UART (1) RC (0) FF (0)
.equ RC_PULS 	  = 1	; UART (0) RC (1) FF (1)
.equ FREE_FLIGHT  = 1	; UART (0) RC (0) FF (1)
; 1=enabled, 0=disabled
; for RC use, set only RC_PULS to 1
; for FF use, set RC_PULS and FREE_FLIGHT to 1
; UART control has not beet tested in this version


;--------------------------------------------------------------------
; port definitions

; (Comments: port#, pin#, direction, initial state/pullup enabled, purpose)
; (DDRx:  1=output, 0=input)
; (PORTx: 1=inputs:pullup enabled/outputs:high, 0=inputs:pu disabled/outputs:low)
; (to minimise current, unused ports made inputs with internal pullup (pu) enabled)

;.equ	spare	= 7				; PB7 pin8 	input 	high/pu	-
;.equ	spare	= 6				; PB6 pin7 	input 	high/pu	-
.equ	led		= 5				; PB5 pin17 output 	low		- LED (also SCK for programming)
;.equ	miso	= 4				; PB4 pin16 output 	low		-
;.equ	mosi	= 3				; PB3 pin15 input 	low		-
.equ	CnFET	= 2				; PB2 pin14 output 	low		- N-fet (low=off)
.equ	AnFET	= 1				; PB1 pin13 output 	low		-
.equ	BnFET	= 0				; PB0 pin12 output 	low		-
.equ	DIR_PB	= 0b00110111	; Configures Data Direction register 'DDRB'
.equ	INIT_PB	= 0b11000000	; Configures Data Register 'PORTB' state/pu's

;.equ	spare	= 7				; ADC7 pin22 input 	high/pu	-
;.equ	spare	= 6				; ADC6 pin19 input 	high/pu	- 
.equ	mux_c	= 5				; PC5 pin28 input 	low		- comparators (pu off)
.equ	mux_a	= 4				; PC4 pin27 input 	low		- 
;.equ	spare	= 3				; PC3 pin26 input 	high/pu	- 
.equ	mux_b	= 2				; PC2 pin25 input 	low		- 
;.equ	spare	= 1				; PC1 pin24 input 	high/pu	- 
.equ	voltage	= 0				; PC0 pin23 input 	low		- voltage measurement
.equ	DIR_PC	= 0b00000000	; Configures Data Direction register 'DDRC'
.equ	INIT_PC	= 0b11001010	; Configures Data Register 'PORTC' state/pu's

;.equ	spare	= 7				; PD7 pin11 input 	high/pu	-
.equ	c_comp	= 6				; PD6 pin10	input	low		- comparator common (pu off)
.equ	CpFET	= 5				; PD5 pin9 	output 	low		- P-fet (via tranny low=off)
.equ	ApFET	= 4				; PD4 pin2	output 	low		-
.equ	BpFET	= 3				; PD3 pin1	output	low		- 
.equ	rcp_in	= 2				; PD2 pin32 input 	low		- Rx / FF push-button
;.equ	spare	= 1				; PD1 pin31 input 	high/pu	-
;.equ	spare	= 0				; PD0 pin30 input 	high/pu	-
.equ	DIR_PD	= 0b00111000	; Configures Data Direction register 'DDRD'

.if FREE_FLIGHT == 1
.equ	INIT_PD	= 0b10000111	; enables pullup for FF push-button (press goes low)
.else
.equ	INIT_PD	= 0b10000011	; Configures Data Register 'PORTD' state/pu's
.endif


;--------------------------------------------------------------------
; variables and other configuration constants

.if FREE_FLIGHT == 1
.equ CHANGE_TIMEOUT	= 150	; ramp up time (150 ~1s motor start)
.equ CHANGE_TOT_LOW	= 255	; ramp down time (255 ~6s motor fade)
							; 255 is max possible; works with FF_STRETCH flag to double effect

.else
.equ CHANGE_TIMEOUT	= 50	; acceleration dampner (50 lowest recommended = responsive)
							; forces 'n' PWM periods before allowing duty cycle to be increased
.equ CHANGE_TOT_LOW	= 40	; deceleration dampner (40 lowest recommended)
							; forces 'n' PWM periods before allowing duty cycle to be reduced
.endif

.equ POWER_RANGE = 100			; max PWM duty cycle/period (100us = 10kHz)
.equ MIN_DUTY	 = 10			; min PWM duty cycle (=minimum startup energy/current)
.equ NO_POWER	 = 256-MIN_DUTY	; timer0 interrupts at 256 so start at 256-duration (eg: 246 for 10us)
								; used to switch FETs off (eg: when a smaller duty cycle exists)
.equ MAX_POWER = 256-POWER_RANGE ; as above but generates full duration (eg: 256-100=156)
								 ; used to hold FETs on when running at max duty cycle

.equ defaultTIMEOUT	 = 32000	; default time for 1 electrical revolution to calculate advance timing
.equ compScanTIMEOUT = 24000	; time between states on startup (before ZC is detected)
								; smaller = more eager to start

.equ T1STOP	= 0					; Disables timer 1
.equ T1CK8	= 0b010				; Enables timer1 with clock/8 prescaler (=1us/count)
								; leaves defaults eg: negative/falling edge

.equ EXT0_DIS	= 0				; disables ext0int in GIMSK settings

.if RC_PULS == 1
.equ	EXT0_EN		= 0b01000000 ; enables ext0int (int0)
.else
.equ	EXT0_EN		= 0			 ; disables ext0int
.endif

.equ	MIN_RC_PULS	= 1100		; threshold for zero throttle
								; program expects above number + 800 to be high throttle

.equ	FF_DEF_TIME	= 1	; default FF run time (x 5s) + ramp up and down time


;--------------------------------------------------------------------
; Register Definitions

.def	i_sreg			 = r1	; status register saved in interrupts
.def	tcnt0_power_on	 = r2	; timer0 counts duration nFETs are switched on
.def	tcnt0_change_tot = r3	; counter used to slow down rate at which changes are made
								; value set to either CHANGE_TIMEOUT or CHANGE_TOT_LOW
.def	TCNT1X	 		 = r4	; extra 8bit timer1 register (to make 24bit)

.def	uart_cnt		 = r5	; 

.def	tcnt0_pwron_next = r6	; duty cycle required for next PWM cycle

.def	start_rcpuls_l	 = r7	; previous timer value at start of a new pulse measurement
.def	start_rcpuls_h	 = r8
.def	new_rcpuls_l	 = r9	; latest pulse length
.def	new_rcpuls_h	 = r10

.def	rcpuls_timeout	 = r11	; lost signal detection (counts down from 100)
.equ	RCP_TOT			 = 100	; Timer1 overflows with no rc pulse being received before resetting

.def	current_err		 = r12	; counts over-current errors
.equ	CURRENT_ERR_MAX  = 3	; performs a reset after MAX errors

.def	sys_control		 = r13	; counts under-voltage errors (100 max) which is used to reduce duty cycle

.def	ee_h	= r14			; eeprom addresses (0-511)
.def	ee_l	= r15			

.def	temp1	= r16			; main temporary
.def	temp2	= r17			; main temporary
.def	temp3	= r18			; main temporary
.def	temp4	= r19			; main temporary

.def	i_temp1	= r20			; interrupt temporary
.def	i_temp2	= r21			; interrupt temporary
.def	i_temp3	= r22			; interrupt temporary


.def	flags0	= r23			; state flags (all cleared on reset) - numbers are bit numbers
	.equ	OCT1_PENDING	= 0	; if set, Output Compare/Timer 1 interrupt (OCT1) is pending (0=delay complete)
								; cleared after comparator interrupt, set during delays before and after commutation
								; and set on startup
	.equ	UB_LOW 			= 1	; set if accu voltage low
	.equ	I_pFET_HIGH		= 2	; set if over-current detect

	.equ	GET_STATE		= 3	; set if state is to be send

	.equ	C_FET			= 4	; if set, C-FET state is to be changed
	.equ	A_FET			= 5	; if set, A-FET state is to be changed
								; if neither A nor C are set, B-FET state is to be changed
	.equ	I_OFF_CYCLE		= 6	; if set, PWM ON cycle; 0 = OFF cycle
	.equ	EVAL_UB			= 7	; if set, next PWM on should check voltage


.def	flags1	= r24			; state flags (all cleared on reset) - numbers are bit numbers
	.equ	POWER_OFF		= 0	; if set, don't switch FETs on
	.equ	FULL_POWER		= 1	; if set, don't switch FETs off (but still do other steps in OFF cycles)
	.equ	CALC_NEXT_OCT1	= 2	; if set, calculate OCT1 offset (state6 only)
	.equ	RC_PULS_UPDATED	= 3	; new rc-puls value available (set when new rc pulse received)
	.equ	EVAL_RC_PULS	= 4	; if set, new rc puls is evaluated while waiting for OCT1 (state2 only)
	.equ	EVAL_SYS_STATE	= 5	; if set, overcurrent and undervoltage are checked (state5-not used)
	.equ	EVAL_I_pFET		= 6	; if set, next PWM on should look for current
	.equ	T1OVFL_FLAG		= 7	; each timer1 overflow sets this flag - used for voltage + current checks every 65ms


.def	flags2	= r25			; state flags
	.equ	FF_STRETCH		= 0	; toggles between 0 and 1 to double ramp down time in lower_pwm:


; XYZ registers (r26-r31) - no alias defined


;--------------------------------------------------------------------
; RAM Definitions (addresses for saving values in SRAM)

.dseg			; SRAM definitions
.org 0x0060		; start address

; 24-bit timer1 value (commutation timing etc)
tcnt1_sav_l:	.byte	1	; latest
tcnt1_sav_h:	.byte	1
tcnt1_sav_x:	.byte	1
last_tcnt1_l:	.byte	1	; previous
last_tcnt1_h:	.byte	1
last_tcnt1_x:	.byte	1

; commutation timing delays
wt_comp_scan_l:	 .byte	1	; wait time from switching FET off to comparator scan
wt_comp_scan_h:	 .byte	1   ; (22.5' delay before expected 30' zero-crossing)
wt_FET_switch_l: .byte	1	; time from zero-crossing to next commute
wt_FET_switch_h: .byte	1	; (22.5' delay if using 7.5' advance timing)
							; (timing is hard-coded in calculate_next_timing_values)
OCT1_high:	.byte	1		; indicates rpm for part of range (>14.7k 2pole if 0, 1831 2pole if 8, etc)

; FF variables
ff_runtime_x5:	.byte	1	; run time (multiples of 5sec) - saved to eeprom
ff_runtime_x65:	.byte	1	; run time (multiples of 65ms) - timer1 overflow interrupt
ff_program:		.byte	1	; 1=Program mode, 0=normal run mode
stall_buffer:	.byte	1	; counter to buffer stalled motor checks

uart_data:	.byte	100		; only for debug requirements


;-------------------------------------------------------------------------
; Reset and Interrupt jump table

; this section configures what happens at startup (vector 1)
; and if there are interrupts (vectors 2:19)
; each vector either has a 'jump to' instruction or does nothing (nop)

.eseg		; define eeprom segment
.org 16		; start address
.db	FF_DEF_TIME	; default run time (x 5s)
;.db	1		; default run time (x 5s)


.cseg		; start of program code
.org 0		; originating at address 0 (vector 1)

		rjmp	reset		;vector 1 - jump to label for start of real program
.if RC_PULS == 1
.if FREE_FLIGHT == 0
		rjmp	ext_int0	;2 rc pulse interrupt (Rx pulses in normal use)
.else
		nop					;2 int0 (not used when RC_PULS==0)
.endif
.endif
		nop					;3 ext_int1 (not used)
		nop					;4 t2oc_int (not used)
		nop					;5 t2ovfl_int (not used)
		nop					;6 icp1 (not used)
		rjmp	t1oca_int	;7 timer1/compA interrupt (commutation and delays)
		nop					;8 t1ocb_int (not used)
		rjmp	t1ovfl_int	;9 timer1 overflow (every 65536µs)
		rjmp	t0ovfl_int	;10 timer0 overflow (every 256µs)
		nop					;11 spi_int (not used)
		nop					;12 urxc (not used)
		nop					;13 udre (not used)
.if UART_CONTROL == 1
  .if UART_FULL == 1
		rjmp	utxc		;14 uart control (when enabled for testing)
  .else
		nop					;14 utxc
  .endif
.else
		nop					;14 utxc
.endif
; not used	nop	;15 adc_int (not used)
; not used	nop	;16 eep_int (not used)
; not used	nop	;17 aci_int (not used)
; not used	nop	;18 wire2_int (not used)
; not used	nop	;19 spmc_int (not used)


version:	.db	"bkf101"	;stores the version number in ram



;-------------------------------------------------------------------------
; Initialisation on reset
; run-once normally (startup) but also over-current abort and if rc signal is lost

reset:
	; define SRAM for use as Stack
		ldi	temp1, high(RAMEND)	;loads highest available SRAM address
		out	SPH, temp1			;into the stack pointer
		ldi	temp1, low(RAMEND)
		out SPL, temp1

	; portB config
		ldi	temp1, INIT_PB		;defines initial state (high/low)
		out	PORTB, temp1		;configures state
		ldi	temp1, DIR_PB		;defines input/output direction
		out	DDRB, temp1			;configures data direction

	; portC config
		ldi	temp1, INIT_PC
		out	PORTC, temp1		;configures state
		ldi	temp1, DIR_PC
		out	DDRC, temp1			;configures data direction

	; portD config
		ldi	temp1, INIT_PD
		out	PORTD, temp1		;configures state
		ldi	temp1, DIR_PD
		out	DDRD, temp1			;configures data direction

	; timer0 (8bit) config: PWM + beep control
		ldi	temp1, 0b010		;clock/8 prescaler (=1us/count)
		out	TCCR0, temp1		;configures CS bits

	; timer1 (16bit) config: commutation control + rc pulse length
		ldi	temp1, T1CK8		;clock/8 prescaler (=1us/count)
		out	TCCR1B, temp1		;configures CS bits; rest left at default

	; reset state flags
		clr	flags0
		clr	flags1
		clr	flags2
		clr temp1
		sts ff_program, temp1

	; set stalled motor timer
		ldi temp1, 8
		sts stall_buffer, temp1

	; power off
		rcall	switch_power_off	;sets some registers and flags1 to 1

	; reset rc puls timeout
		ldi	temp1, RCP_TOT			;100 (used to detect no rc pulse and reset)
		mov	rcpuls_timeout, temp1
		
.if UART_CONTROL == 1
		ldi	ZH,high(version*2)
		ldi	ZL,low(version*2)
		ldi	temp1, 0	; set to 38400baud (8MHz)
		out	UBRRH, temp1
		ldi	temp1, 12
		out	UBRRL, temp1
		ldi	temp1, 0x18
		out	UCSRB, temp1	; enable rx+tx - no interrupts !
		in 	temp1, UDR	; clear possibly pending rxc
		lpm
		adiw	ZL,1		;increment Z-pointer
		mov	temp1, r0	; b
		rcall	send_byte
.endif


; disable watchdog timer
; to stop motor once running, WDT can be enabled to reset program
; enabled in evaluate_ff_stop (FF only) and evaluate_stall
		WDR
	; Write logical one to WDCE and WDE
		in temp1, WDTCR
		ori temp1, (1<<WDCE)|(1<<WDE)
		out WDTCR, temp1
	; Turn off WDT
		ldi temp1, (0<<WDE)
		out WDTCR, temp1

		
		rcall	wait260ms	; 260ms delays
		rcall	wait260ms


.if UART_CONTROL == 1
		lpm
		adiw	ZL,1		;increment Z-pointer
		mov	temp1, r0	; k
		rcall	send_byte
.endif


;.if FREE_FLIGHT == 0
; beep ready
		rcall	beep250	; pre-signal check
;.endif


.if UART_CONTROL == 1
		lpm
		adiw	ZL,1		;increment Z-pointer
		mov	temp1, r0		; o
		rcall	send_byte
		lpm
		adiw	ZL,1		;increment Z-pointer
		mov	temp1, r0		; (1st number)
		rcall	send_byte
.endif


;-------------------------------------------------------------------------
; prepare to start motor

; follows reset
; checks that throttle is low and beeps twice when ready
; sets commutation timing and configures comparator

control_start:
	; init variables
		ldi	temp1, CHANGE_TIMEOUT		; starts at 50
		mov	tcnt0_change_tot, temp1
		ldi	temp1, NO_POWER				; minimum pwm on duration (246 to achieve 10us)
		mov	tcnt0_power_on, temp1

	; tcnt0_pwron_next was set to NO_POWER in reset:

		ldi	temp1, 0					; reset error counters
		mov	current_err, temp1			; over-current
		mov	sys_control, temp1			; under-voltage 

;==============================================================================

.if FREE_FLIGHT == 1
	
; Initialise eeprom and read free flight run time
		clr ee_h
		ldi temp1, 16
		mov ee_l, temp1			; use address 16
		rcall eeprom_read		; read eeprom
		in temp3, EEDR			; read current FF runtime
		sts ff_runtime_x5, temp3; save

ff_button_check:
	; wait for push button to be released (typically after stopping motor)
		sbis PIND, rcp_in		; skip next if 1 (1=button NOT pressed)
		rjmp ff_button_check	; loop if 1 (button still pressed)

ff_hold:
	; wait for button to be pressed
		sbic PIND, rcp_in		; skip next if 0 (0=button pressed)
		rjmp ff_hold			; loop if 1 (button not pressed)

;------------------------
; time button press

	; increment counter while push-button remains pressed
		clr temp4			; press duration
pb_time:
		rcall wait260ms
		inc temp4			; increment counter
		sbic PIND, rcp_in	; skip next if 0 (0=button still pressed)
		rjmp debounce		; jump if 1 (button released)
		rjmp pb_time		; keep looping while pressed

	; debounce if too short
debounce_reset:
		rjmp reset
debounce:
		tst temp4			; check if 0 (press too short)
		breq debounce_reset	; restart (no beeps)
		

	; determine whether in programming mode
pgm_mode_check:
		lds temp1, ff_program
		tst temp1				; check if 0
		brne pgm_mode_eval		; branch if >0 (1=already in program mode)

;------------------------
; normal run mode

	; check whether now enabling program mode (press >3s)
		cpi temp4, 8			; compare duration with 8 (3s)
		brcc pgm_mode_enable	; branch if carry 0 (0 when duration >= 8)

	; for shorter presses, check that run time >0
		lds temp3, ff_runtime_x5	; load run time (already retrieved from eeprom)
		tst temp3					; check if already 0
		brne run_config				; configure motor if >0

		rcall beep250				; error beep if 0
		rcall beep250
		rcall beep250
		rcall beep250
		rjmp ff_hold				; 0 so wait for another instruction

run_config:
	; if run time >0, prepare to start motor
		ldi temp1, 0b11010000		; prepare to configure HIGH throttle setting (2000us)
		ldi temp2, 0b111			; high byte
		mov	new_rcpuls_l, temp1		; save new pulse duration
		mov	new_rcpuls_h, temp2

		sbr	flags1, (1<<RC_PULS_UPDATED)	; a new 'rc pulse' has been set
		ldi	i_temp1, RCP_TOT				; 100
		mov	rcpuls_timeout, i_temp1			; reset lost signal counter

		ldi temp1, 76
		sts ff_runtime_x65, temp1	; reset 65ms timer1 overflow counter

		rjmp motor_start

	
;------------------------
; program mode

pgm_mode_enable:
	; triggered by >3s press in normal run mode
		ldi temp1, 1
		sts	ff_program, temp1	; set flag for program mode
		
		rcall beep328ms			; start/end tone
		rcall beep328ms
		rcall beep328ms
		rcall beep328ms
		rcall beep328ms
		rjmp ff_hold			; wait for next button press

pgm_mode_eval:
	; evaluates config options in program mode

	; check whether selecting Default time (used to replace corrupted values)
		cpi temp4, 8		; compare duration with 8 (>3s)
		brcc pgm_mode_def	; branch if carry 0 (0 when duration >= 8)

	; check whether Incrementing run time (press >1s but <3s)
		cpi temp4, 2		; compare duration with 2 (1s)
		brcc pgm_mode_inc	; branch if carry 0 (0 when duration >= 2)

	; shorter presses Decrement but check if run time is >0 (to avoid making it negative)
		lds temp3, ff_runtime_x5	; load run time (already retrieved from eeprom)
		tst temp3					; check if already 0
		brne pgm_mode_dec			; branch if >0 (proceed)

		rcall beep250				; error beep if 0
		rcall beep250
		rcall beep250
		rcall beep250
		rjmp ff_hold				; jump if 0 (do nothing)


; decrement run time if >0 (press <1s)
pgm_mode_dec:
		lds temp3, ff_runtime_x5	; load run time
		dec temp3					; reduce run time by '1' (5s)
		sts ff_runtime_x5, temp3	; store new run time in SRAM

	; save to eeprom
		clr ee_h
		ldi temp1, 16
		mov ee_l, temp1			; use address 16
		out EEDR, temp3			; save run time to eeprom
		rcall eeprom_write

	; beep and exit
		clr temp1
		sts	ff_program, temp1	; clear program mode flag
		rjmp beep_ff_run_time	; beeps current run time
		rcall wait260ms
		rjmp ff_hold			; await next instruction


; increment run time (press >1s <3s)
pgm_mode_inc:
		lds temp3, ff_runtime_x5	; load run time
		inc temp3					; increase run time by '1' (5s)
		sts ff_runtime_x5, temp3	; store new run time in SRAM

	; save to eeprom
		clr ee_h
		ldi temp1, 16
		mov ee_l, temp1			; use address 16
		out EEDR, temp3			; save run time to eeprom
		rcall eeprom_write

	; beep and exit
		clr temp1
		sts	ff_program, temp1	; clear program mode flag
		rjmp beep_ff_run_time	; beeps current run time
		rcall wait260ms
		rjmp ff_hold			; await next instruction

; default run time (press >3s)
pgm_mode_def:
		ldi temp3, FF_DEF_TIME			; load default run time
		sts ff_runtime_x5, temp3	; store new run time in SRAM

	; save to eeprom
		clr ee_h
		ldi temp1, 16
		mov ee_l, temp1			; use address 16
		out EEDR, temp3			; save run time to eeprom
		rcall eeprom_write

	; beep and exit
		clr temp1
		sts	ff_program, temp1	; clear program mode flag
		rjmp beep_ff_run_time	; beeps current run time
		rcall wait260ms
		rjmp ff_hold			; await next instruction


;------------------------
motor_start:

.endif

;==============================================================================


	; enable interrupt registers
		ldi	temp1, (1<<TOIE1)+(1<<OCIE1A)+(1<<TOIE0)	; mask for bits 0, 2 and 4
														; timer0, timer1 and timer1/compA match
		out	TIFR, temp1		; bits 0,2,4 set TOV0, TOV1 and OCF1A flags (cleared in interrupts)
		out	TIMSK, temp1	; enable timer0, timer1, timer1/compA interrupts

.if UART_CONTROL == 1
		lpm
		adiw	ZL,1
		mov	temp1, r0
		rcall	send_byte
.endif

		sei				; enable interrupts


.if RC_PULS == 1
	.if FREE_FLIGHT == 0

	; enable rc pulse interrupt
		ldi	temp1, (1<<ISC01)+(1<<ISC00)	; external interrupt 0 rising edge
		out	MCUCR, temp1
		ldi	temp1, EXT0_EN					; enable ext0int
		out	GIMSK, temp1

	; ensure throttle is off before starting
i_rc_puls1:	
		ldi	temp3, 10						; used to perform test 10 times to be sure (~200ms)
i_rc_puls2:	
		sbrs	flags1, RC_PULS_UPDATED		; loop (if 0) until a new pulse has been measured (skip next if 1)
		rjmp	i_rc_puls2
		
	; check if throttle is low
		mov	temp1, new_rcpuls_l				; read new pulse length set in ext_int0
		mov	temp2, new_rcpuls_h
		cbr	flags1, (1<<RC_PULS_UPDATED) 	; clears flag for reuse (identifies when a new pulse length exists)
		subi	temp1, low  (MIN_RC_PULS)	; deduct 1100 to check for low throttle
		sbci	temp2, high (MIN_RC_PULS)
		brcc	i_rc_puls1					; no (>min) - loop forever until yes (branch if carry 0)
											; carry flag is 1 when new_rcpuls < minimum (OK)
		dec	temp3							; yes (<min) - decrement counter
		brne	i_rc_puls2					; repeat until zero
		

	;beep twice when done
		cli									; disable all interrupts
		rcall	beep328ms					; post-signal check
		rcall	beep328ms
		sei									; enable all interrupts

	.endif
.endif


.if UART_CONTROL == 1
		lpm
		adiw	ZL,1
		mov	temp1, r0
		rcall	send_byte
.endif

; set starting PWM duty cycle and FETs
		ldi	ZH, MIN_DUTY-1		; starting duty cycle set to 9 (10-1)
		cbr	flags0, (1<<A_FET)	; clears A fet flag
		sbr	flags0, (1<<C_FET)	; sets C fet flag (ie: start with Phase C)
		
; configure motor control
		rcall	calculate_next_timing_values	; set commutation timing
		rcall	set_default_timeout				; more default commutation settings
		rcall	wait_OCT1_tot	; enables comparator, sets timeout delay and OCT1_PENDING flag
		ldi	temp1, mux_a		; set comparator multiplexer to phase A
		out	ADMUX, temp1

		rjmp	state1			; start your engines!

		
;-------------------------------------------------------------------------
; external interrupt0 = times rc pulse duration from Rx (ie: throttle position)

; triggered by both rising and falling edges (start/end of 1-2ms pulses)
; result stored in new_rcpuls_l/h and sets flag RC_PULS_UPDATED
; performs some glitch rejection and pulse length validation
; resets lost signal counter (rcpuls_timeout) if valid signal detected
; sees Futaba FF9 as 1033-1973us

.if RC_PULS == 1	; interrupt relevant to RC mode only
.if FREE_FLIGHT == 0

ext_int0:
		in	i_sreg, SREG		; saves status register
		clr	i_temp1				; initialise
		out	GIMSK, i_temp1		; disable this interrupt

	; evaluate edge of this interrupt (start or end)
		in	i_temp1, MCUCR			; read current edge detect settings for what triggered the interrupt
		sbrs	i_temp1, ISC00		; skip next test if expecting rising edge (start of a new pulse) (skip next if 1)
		rjmp	falling_edge		; determine pulse duration if this was a falling edge (end of a pulse)

	; if it is a rising edge check for jitter (will catch some but not all jitter)
		sbis	PIND, rcp_in		; skip next test if Rx input is still high as expected
		rjmp	extint1_exit		; exit if low state (jitter)

	; if rc pulse is still in high state (a valid new pulse)
		ldi	i_temp1, (1<<ISC01)
		out	MCUCR, i_temp1			; set to falling edge to detect the end of the high pulse

	; store current timer1 values to time high pulse
		in	i_temp1, TCNT1L				; read timer values
		in	i_temp2, TCNT1H
		mov	start_rcpuls_l, i_temp1		; save timer values in these 'start' registers
		mov	start_rcpuls_h, i_temp2

		rjmp	extint1_exit			; exit to re-enable interrupts etc

		
; rc pulse is at low state (end of a new pulse)
falling_edge:
	; check for glitch
		sbic	PIND, rcp_in		; check if Rx pin is still low (expected) (skip next if 0)
		rjmp	extint1_exit		; exit if pin is high (=glitch)

	; prepare for next pulse
		ldi	i_temp1, (1<<ISC01)+(1<<ISC00)
		out	MCUCR, i_temp1			; set interrupts for rising edges
		
	; check that we are meant to be setting a new pulse width (ie: flag is clear)
		sbrc	flags1, RC_PULS_UPDATED	; (skip next if 0)
		rjmp	extint1_exit			; exit if flag is already set

	; get timer1 values (if this is a valid new pulse)
		in	i_temp1, TCNT1L				; read timer values
		in	i_temp2, TCNT1H
		sub	i_temp1, start_rcpuls_l		; deduct starting values
		sbc	i_temp2, start_rcpuls_h

	; save new pulse length
		mov	new_rcpuls_l, i_temp1		; save new pulse duration
		mov	new_rcpuls_h, i_temp2		; i_temp2=0x04


	; check that length is not obviously wrong
		cpi	i_temp1, low (2200)		; (compare with)
		ldi	i_temp3, high(2200)		; test range high (i_temp3=0x08)
		cpc	i_temp2, i_temp3
		brsh	extint1_exit		; equal or greater - throw away and exit

		cpi	i_temp1, low (800)		; (compare with)
		ldi	i_temp3, high(800)		; test range low (i_temp3=0x03)
		cpc	i_temp2, i_temp3
		brlo	extint1_exit		; smaller - throw away and exit (branch if smaller)
		
	; update flags if OK
		sbr	flags1, (1<<RC_PULS_UPDATED)	; a new valid pulse has been determined
		ldi	i_temp1, RCP_TOT				; 100
		mov	rcpuls_timeout, i_temp1			; reset lost signal counter


; enable ext0int again -  also entry for glitch detect
extint1_exit:
		ldi	i_temp2, EXT0_EN	; read value to enable interrupt
		out	GIMSK, i_temp2		; enable ext0int
		out	SREG, i_sreg		; restore status register
		reti					; return to where main program was before interrupt

.endif
.endif


;-------------------------------------------------------------------------
; output compare timer1 interrupt (OCT1) used for commutation delays

; triggered when timer1 reaches values set in OCR1AH / OCR1AL pair
; timer1 is free-running and these registers are set to larger values to created required delays
; this ensures that commutation timing is largely independent of time taken to run program code
; used for commutation and timing delays
; clears OCT1_PENDING flag

t1oca_int:
		in	i_sreg, SREG				; save status register
		cbr	flags0, (1<<OCT1_PENDING)	; clear flag - signal OCT1 passed
		out	SREG, i_sreg
		reti

				
;-------------------------------------------------------------------------
; timer1 overflow interrupt (every 65536µs)

; creates 24bit timer for commutation timing
; generates delays for lost rc signal
; sets T1OVFL_FLAG to trigger system checks (voltage + current checks if enabled)

; times FF motor run and cuts motor when done

t1ovfl_int:
		in	i_sreg, SREG				; save status register
		inc	TCNT1X						; increment 24-bit counter
		sbr	flags1, (1<<T1OVFL_FLAG)	; set overflow flag

; lost signal counter
		tst	rcpuls_timeout		; check if zero
		breq	t1ovfl_99		; branch (exit) if it is zero (do not decrement further)
	.if FREE_FLIGHT == 0
		dec	rcpuls_timeout		; decrement if not zero yet (RC not FF mode)
	.endif


.if FREE_FLIGHT == 1

; 5s intervals (timer1 o/flow = 65ms x 76 = 5s)
		lds i_temp3, ff_runtime_x65	; load 65ms counter (counts to 5s)
		dec i_temp3					; reduce 5s timer
		breq ff_timer_5s			; branch to next level if 0 (5s complete)
		sts ff_runtime_x65, i_temp3	; save count
		rjmp t1ovfl_99				; exit (run time not complete yet)

; multiples of 5s
ff_timer_5s:
	; check if run time is complete
		lds i_temp3, ff_runtime_x5	; load main run time counter
		dec i_temp3					; reduce main timer
		breq ff_motor_stop			; branch to next level if 0 (full run complete)
		sts ff_runtime_x5, i_temp3	; save count

	; reset 5s counter (76 x 65ms)
		ldi i_temp3, 76
		sts ff_runtime_x65, i_temp3	; reset 5s timer
		rjmp t1ovfl_99				; exit (run time not complete yet)

; stop motor - simulate a short rc pulse (1000us)
ff_motor_stop:
		ldi i_temp1, 0b11101000		; prepare to configure LOW throttle setting (1000us)
		ldi i_temp2, 0b11			; high byte
		mov	new_rcpuls_l, i_temp1	; save new pulse duration
		mov	new_rcpuls_h, i_temp2

		sbr	flags1, (1<<RC_PULS_UPDATED)	; a new valid pulse has been determined
.endif

t1ovfl_99:
		out	SREG, i_sreg		; restore status register
		reti

				
;-------------------------------------------------------------------------
; timer0 overflow interrupt - generates a software-based PWM to switch FETs

; it toggles between ON and OFF cycles using the I_OFF_CYCLE flag
; two interrupts should always sum to the chosen PWM period (POWER_RANGE=100us)
; 'tcnt0_power_on' represents the current PWM duty cycle (156-246)
; 8bit counter so is pre-loaded with (256 - duration) to trigger interrupt on overflow
; a large number (eg: 246) represents a short ON period (256-246=10us)
; a small number (eg: 156) represents a long ON period (256-156=100us)

t0ovfl_int:
		in	i_sreg, SREG	; save status

.if RC_PULS == 0
		sbis	ACSR, ACO	; mirror ACO to rc_puls, if not used
		cbi	PORTD, rcp_in
		sbic	ACSR, ACO	; (skip next if 0)
		sbi	PORTD, rcp_in
.endif

; determine whether an ON or OFF cycle
		sbrc	flags0, I_OFF_CYCLE		; execute OFF cycle if 0 (by skipping the next jump)
		rjmp	t0_on_cycle				; jump to ON cycle if flag is 1


; OFF CYCLE (I_OFF_CYCLE==0)------------------------

t0_off_cycle:
	; determine whether to change ON duration for next interrupt
	; the 'power_on' setting is the current requirement
	; the 'next' setting is what is desired for the next loop
		mov	i_temp1, tcnt0_power_on		; current ON requirement (156-246)
		mov	i_temp2, tcnt0_pwron_next	; next ON requirement
		cp	i_temp2, i_temp1			; compare 'next' with 'power_on'
		brsh	lower_pwm				; branch (reduce pwm duration) if next >= power_on
										; eg1:	power_on = 246 (10us) at startup (set in control_start after reset)		
										; 		next   	 = 246 (10us) at startup (set in switch_power_off from reset)
										; 		next == power_on so jump to lower_pwm (although no change required)
										; eg2: 	power_on = 206 (50us)
										; 		next	 = 226 (30us)
										; 		226 > 206 so jump to lower_pwm (want shorter ON duration)
higher_pwm:
	; longer ON duration (accelerate/more power)
	; 'next' < 'power_on' (longer duration required)
	; 'tcnt0_change_tot' is a counter which slows down rate at which changes are made to pwm duty cycle
	; eg: 5ms between changes if CHANGE_TIMEOUT is set to 50 (50*100us/cycle)
		dec	tcnt0_change_tot			; reduce counter by 1
		brne	nFET_off				; skip (no pwm change) if counter is not 0 yet
	; counter reached 0
		ldi	i_temp2, CHANGE_TIMEOUT
		mov	tcnt0_change_tot, i_temp2	; reset counter when it does reach 0
		dec	i_temp1						; used to reduce tcnt0_power_on which increases pwm ON time
		rjmp	set_next_pwm

lower_pwm:
	; shorter ON duration (decelerate/less power or no change)
	; 'tcnt0_change_tot' forces 4ms between changes if CHANGE_TOT_LOW is set to 40 (40*100us/cycle)
		breq	nFET_off				; branch if 0 (next=power_on) (no pwm or counter changes)

.if FREE_FLIGHT == 1
; decrement counter every second loop in FF mode
; FF_STRETCH flag is toggled to double decelleration time
		sbrs flags2, FF_STRETCH			; skip next if 1
		rjmp lower_pwm_0				; if 0, jump
lower_pwm_1:
		dec	tcnt0_change_tot			; if 1, reduce counter by 1
		cbr	flags2, (1<<FF_STRETCH) 	; clear flag
		rjmp lower_pwm_tst				; continue

lower_pwm_0:
		sbr	flags2, (1<<FF_STRETCH) 	; if 0, set flag (and don't decrement counter)

lower_pwm_tst:
		tst tcnt0_change_tot			; check if 0

.else
; decrement counter on every loop when not in FF mode
		dec	tcnt0_change_tot			; if 1, reduce counter by 1
.endif

		brne	nFET_off				; skip (no pwm change) if counter is not 0 yet
		ldi	i_temp2, CHANGE_TOT_LOW
		mov	tcnt0_change_tot, i_temp2 	; reset counter when it does reach 0
		inc	i_temp1						; used to increase tcnt0_power_on which reduces pwm ON time

set_next_pwm:
		mov	tcnt0_power_on, i_temp1		; save duty cycle for next loop

		
; pwm is applied to the nFETs only (the pFETs stay on; no need to pulse both)
; only one nFET is ever on at a time
; if full power is required, any active (on) nFET is left on even in the 'off' cycle
; if full power is not required, the the active nFET is switched off in the OFF cycle (normal pwm action)

nFET_off:
		sbr	flags0, (1<<I_OFF_CYCLE) ; set flag so next cycle is an ON cycle

; test C_FET requirements
		sbrs	flags0, C_FET		; if Cn is active (1), skip next branch (don't jump to An)
		rjmp	test_AnFET			; Cn is not active (0) so consider An next

; C_FET is active
		sbrs	flags1, FULL_POWER	; if full power is required (1) do nothing (skip next leaving Cn on)
		cbi	PORTB, CnFET			; does not require full power so switch Cn off (clear bit)
		rjmp	reload_t0_off_cycle	; exit

test_AnFET:
		sbrs	flags0, A_FET		; if An is active (1), skip next branch (don't jump to Bn)
		rjmp	switch_BnFET		; both An and Cn are not active so that means Bn is active

; A_FET is active
switch_AnFET:
		sbrs	flags1, FULL_POWER	; if full power is required (1) do nothing (skip next leaving An on)
		cbi	PORTB, AnFET			; does not require full power so switch An off (clear bit)
		rjmp	reload_t0_off_cycle	; exit

; B_FET is active
switch_BnFET:
		sbrs	flags1, FULL_POWER	; if full power is required (1) do nothing (skip next leaving Bn on)
		cbi	PORTB, BnFET			; does not require full power so switch Bn off (clear bit)


; initialise settings to trigger next cycle, then exit
reload_t0_off_cycle:
		mov	i_temp1, tcnt0_power_on		; pick up duty cycle for next loop (eg: 236 if duty cycle is 20)
		subi	i_temp1, -POWER_RANGE	; convert to reciprocal of desired duration
										; this interrupt is used in pairs (ON and OFF) and sums to 100us (POWER_RANGE)
										; eg: if OFF just completed and duty cycle is 20us, set timer0 to yield 80us
										; subi = 236-(-100)=80 (wraps after 255 to 0)
		com	i_temp1			; invert bits eg: 80(0b01010000) = 175(0b10101111) ie: 80us before timer0 wraps again
		out	TCNT0, i_temp1	; reset timer0 counter (eg: with 175 for 80us ON cycle)

		rjmp	t0_int_exit


; ON CYCLE (I_OFF_CYCLE==1)------------------------

t0_on_cycle:
	; prepare for next cycle
		mov	i_temp1, tcnt0_power_on		; current ON requirement  (eg: 236 if duty cycle is 20)
		out	TCNT0, i_temp1				; reload timer0 (eg: 236/20us for next OFF cycle)
		cbr	flags0, (1<<I_OFF_CYCLE) 	; clear flag so next cycle is OFF

nFET_on:
; test C_FET requirements
		sbrs	flags0, C_FET		; if Cn is active (1), skip next branch (don't jump to An)
		rjmp	test_AnFET_on		; Cn is not active (0) so consider An next
		
; C_FET is active
		sbrs	flags1, POWER_OFF	; if switching of FETs is disabled (1) do nothing (skip next)
		sbi	PORTB, CnFET			; if OK to switch FETs on, activate Cn
		rjmp	eval_power_state
		
test_AnFET_on:
		sbrs	flags0, A_FET		; if An is active (1), skip next branch (don't jump to Bn)
		rjmp	sw_BnFET_on			; both An and Cn are not active so that means Bn is active
		
; A_FET is active
		sbrs	flags1, POWER_OFF	; if switching of FETs is disabled (1) do nothing (skip next)
		sbi	PORTB, AnFET			; if OK to switch FETs on, activate An
		rjmp	eval_power_state

; B_FET is active
sw_BnFET_on:
		sbrs	flags1, POWER_OFF	; if switching of FETs is disabled (1) do nothing (skip next)
		sbi	PORTB, BnFET			; if OK to switch FETs on, activate Bn

		
; sets FULL_POWER and POWER_OFF flags to control FET switching in next two cycles
eval_power_state:
		cpi	i_temp1, MAX_POWER+1	; compare current ON requirement with max possible+1 (256-100+1=157)
		brsh	not_full_power 		; branch if current duty cycle <= max+1 (not at full power)
									; eg1: branch if current duty cycle is 236 (20us)
									; eg2: no branch if currently at full power (256-100=156)

; FULL POWER
		sbr	flags1, (1<<FULL_POWER)	; set the full power flag
		cbr	flags1, (1<<POWER_OFF)	; allow FETs to be activated as required (clear flag)
		rjmp	t0_int_exit			; exit
		
not_full_power:
		cpi	i_temp1, NO_POWER			; compare current ON requirement with min possible (256-10=246)
		brlo	neither_full_nor_off	; branch if current duty cycle < minimum (ie: want off)

; ZERO POWER
		cbr	flags1, (1<<FULL_POWER)		; definitely don't want full power! (clear flag)
		sbr	flags1, (1<<POWER_OFF) 		; set flag to disable FETs
		rjmp	t0_int_exit
		
; IN BETWEEN POWER
neither_full_nor_off:
		cbr	flags1, (1<<FULL_POWER)		; don't want full power (clear flag)
		cbr	flags1, (1<<POWER_OFF)		; but don't have to disable FETs (clear flag too)


t0_int_exit:
		out	SREG, i_sreg				; restore status register
		reti

		
;--------------------------------------------------------------------
; log FF run time and test data to eeprom
; start address set elsewhere; incremented automatically in this function
; data to be saved also set elsewhere

eeprom_write:
; Wait for completion of previous write
	sbic EECR,EEWE
	rjmp eeprom_write    

; Set up address in address register
	out  EEARH, ee_h
	out  EEARL, ee_l

	cli			;disable global interrupts	

; Enable writes
	sbi  EECR,EEMWE
; Start eeprom write by setting EEWE
	sbi  EECR,EEWE

; Increment address for next write (0-511 then wraps)
	inc ee_l			; increment low address
	brne eeprom_w_end	; branch if low address does not wrap (zero flag is 0)
	inc ee_h			; increment high address after low wraps (zero flag 1)

eeprom_w_end:
	sei			;enable global interrupts
	ret


;--------------------------------------------------------------------
; read FF run time and test data to eeprom
; start address set elsewhere; incremented automatically in this function

eeprom_read:
; Wait for completion of previous write
	sbic EECR,EEWE
	rjmp eeprom_read

; Set up address in address register
	out  EEARH, ee_h
	out  EEARL, ee_l

; Start eeprom read
	sbi  EECR,EERE

; Increment address for next write (0-511 then wraps)
	inc ee_l			; increment low address
	brne eeprom_r_end	; branch if low address does not wrap (zero flag is 0)
	inc ee_h			; increment high address after low wraps (zero flag 1)

eeprom_r_end:
	ret


;-------------------------------------------------------------------------
; 500hz beep motor

; pulses motor 128 times in 265ms (ie: 2072us/pulse = 500hz)
; followed by 261ms delay

beep328ms:
		sbi	PORTB, led			; led on

		ldi	i_temp2, 128			;Sets i_temp2 to 128

beep_ApBn:
	; motor 'on' for 32us
		clr	i_temp1
		out	TCNT0, i_temp1		; Clears timer0
		sbi	PORTD, ApFET		; ApFET on
		sbi	PORTB, BnFET		; BnFET on
beep_ApBn10:
		in	i_temp1, TCNT0		; Reads timer0 count
		cpi	i_temp1, 32			; Compare against 32 (compare with)
		brne	beep_ApBn10		; Loop until 32µs (branch if not equal)
		cbi	PORTD, ApFET		; ApFET off
		cbi	PORTB, BnFET		; BnFET off
		
	; motor 'off' for 2040us (255*8)
		ldi	i_temp3, 8			; Set i_temp3 for 8 loops (261ms off)
beep_ApBn12:
		clr	i_temp1
		out	TCNT0, i_temp1		; Clear timer0
beep_ApBn13:
		in	i_temp1, TCNT0
		cpi	i_temp1, 255			; (compare with)
		brne	beep_ApBn13		; loop 255 times
		dec	i_temp3
		brne	beep_ApBn12		; loop 8 times (=8*255)

	; pulse motor 128 times (over 128*(32+2040)=265216us = 265ms)
		dec	i_temp2
		brne	beep_ApBn		; do this 128 times
		
		cbi	PORTB, led			; led off
		ret						; return

;-------------------------------------------------------------------------
; 250hz beep

; pulses motor 64 times in 265ms (ie: 4144us/pulse = 241hz)
; followed by 261ms delay

beep250:
		sbi	PORTB, led			; led on

		ldi	i_temp2, 64			;Sets i_temp2 to 128

beep250_0:
	; motor 'on' for 32us
		clr	i_temp1
		out	TCNT0, i_temp1		; Clears timer0
		sbi	PORTD, ApFET		; ApFET on
		sbi	PORTB, BnFET		; BnFET on
beep250_10:
		in	i_temp1, TCNT0		; Reads timer0 count
		cpi	i_temp1, 32			; Compare against 64
		brne	beep250_10		; Loop until 64µs (branch if not equal)
		cbi	PORTD, ApFET		; ApFET off
		cbi	PORTB, BnFET		; BnFET off
		
	; motor 'off' for 4080us (255*16)
		ldi	i_temp3, 12			; Set i_temp3 for 'n' loops ( ms off)
beep250_12:
		clr	i_temp1
		out	TCNT0, i_temp1		; Clear timer0
beep250_13:
		in	i_temp1, TCNT0
		cpi	i_temp1, 255			; (compare with)
		brne	beep250_13		; loop 255 times
		dec	i_temp3
		brne	beep250_12		; loop 'n' times (=16*255)

	; pulse motor 64 times (over 64*(64+4080)=265216us = 265ms)
		dec	i_temp2
		brne	beep250_0		; do this 255 times
		
		cbi	PORTB, led			; led off
		ret						; return

;-------------------------------------------------------------------------
; 260ms delay

wait260ms:	
	; 261ms delay
		ldi	i_temp2, 128			; 128 periods = 261ms delay (255*8*128*1us)
beep_ApBn20:
		ldi	i_temp3, 8			; Sets i_temp3 to 8
beep_ApBn21:
		clr	i_temp1
		out	TCNT0, i_temp1		; Clears Timer0 counter
beep_ApBn22:
		in	i_temp1, TCNT0		; Reads timer0 count
		cpi	i_temp1, 255			; Compares timer0 count with 255
		brne	beep_ApBn22		; Branch if not equal (ie: loop 255 times)
		dec	i_temp3				;
		brne	beep_ApBn21		; loop 8 times (255x8)
		dec	i_temp2				;
		brne	beep_ApBn20		; loop 128 times (255x8x128 = 261120us = 261ms)

		ret						; return

;-------------------------------------------------------------------------
; beeps FF run time
; 1 beep per 5sec

beep_ff_run_time:
		lds temp3, ff_runtime_x5	; load run time (already retrieved from eeprom)
		tst temp3					; check if 0
		breq beep_run_time_zero		; error tone if 0

beep_run_time:
	; beep run time duration
		rcall beep328ms			; beep
		rcall wait260ms
		dec temp3				; decrement counter
		brne beep_run_time		; loop until 0
		rjmp beep_run_time_end

beep_run_time_zero:
		rjmp beep250
		rjmp beep250
		rjmp beep250
		rjmp beep250

beep_run_time_end:
		ret


;-------------------------------------------------------------------------
; reads timer1 and stores count in temp1,2,3
; also disables ext0int (rc pulse interrupt)

tcnt1_to_temp:
		ldi	temp4, EXT0_DIS		; disable ext0int
		out	GIMSK, temp4
		ldi	temp4, T1STOP		; stop timer1
		out	TCCR1B, temp4
		ldi	temp4, T1CK8		; prepare to restart timer1 with /8 (1us)
		in	temp1, TCNT1L		; read values  - the preload cycle is needed to complete stop operation
		in	temp2, TCNT1H
		mov	temp3, TCNT1X
		out	TCCR1B, temp4		; restart timer1
		ret				; !!! ext0int stays disabled - must be enabled again by caller
	; there is only one TEMP register in the AVR (used to merge H/L registers)
	; so interrupts must be disabled when reading timer counts to avoid risk of corruption
	; timer1 is also stopped to prevent changes while reading results
	
		
;-------------------------------------------------------------------------
; set commutation timing

; this function determines motor speed, sets over-rpm limits and commutation timing
; one electrical revolution is 360' so with 6 states commutation occurs every 60'
; zero-crossing (ZC) occurs between each at the 30' position
; it is common to advance timing (commute early), usually to increase power
; the program author found 7.5' (1/16th of 360') to be a good setting on his motors
; this is equivalent to 22.5' past ZC which is how this program works

calculate_next_timing_values:
	; determine duration of 360° / 6 commutations
		rcall	tcnt1_to_temp	; reads timer1 values into temp1,2,3 (and disables rc pulse interrupts)
		ldi	temp4, EXT0_EN
		out	GIMSK, temp4		; enable rc pulse interrupt again
		
		sts	tcnt1_sav_l, temp1	; saves timer1 count in temp variables
		sts	tcnt1_sav_h, temp2
		sts	tcnt1_sav_x, temp3
		lds	YL, last_tcnt1_l	; load the previous timer1 count
		lds	YH, last_tcnt1_h
		lds	ZL, last_tcnt1_x
		sub	temp1, YL			; calculate difference to determine duration of 6 commutations
		sbc	temp2, YH
		sbc	temp3, ZL
		
; divide by 16 for 7.5° timing (360/16=22.5)
		ldi	temp4, 4			; 4 loops
calc_OCT1_d16:
		lsr	temp3				; 3rd byte/2 (logical right shift)
		ror	temp2				; 2nd byte/2 (rotates LSB from 3rd byte to MSB of 2nd)
		ror	temp1				; 1st byte/2 (as above)
		dec	temp4				; deduct 1 per iteration
		brne	calc_OCT1_d16	; loop until temp4 is 0 = divide by 2 four times (branch if not equal)
		mov	YL, temp1			; results (temp1,2,3 and YL,YH,ZL are now 1/16 of a complete cycle time)
		mov	YH, temp2
		mov	ZL, temp3

; these '1/16th' values are used to check rpm using individual bytes interpreted as follows:
; for simplicity, one byte is considered in isolation in examples below
; examples assume other bytes are zero and duration is for 6 states/1 electrical revolution
; 1 electrical revolution is always for 2 magnet poles (eg: /6 for 12pole motor physical rpm)

; if temp3 is 1, rpm is 57 	(1*256*256 = 65,536us x16 = 1,048,576us per revolution)
; 							(1minute/1,048,576us = 57rpm 2pole) - 9 rpm 12 pole

; typical startup speeds & higher:
; if temp2 is 128 : 114 rpm	(128*256 = 32,768us x16 = 524,288us per rev) - 19 rpm 12pole
; if temp2 is 16 : 915 rpm	(16*256 = 4096us x16 = 65,536us)			- 152 rpm 12pole
; if temp2 is 8 : 1,831 rpm	(8*256 = 2048us x16 = 32,768us)			  	- 305 rpm 12pole
; if temp2 is 4 : 3,662 rpm	(4*256 = 1024us x16 = 16,384us)			 	- 610 rpm 12pole
; if temp2 is 2 : 7,324 rpm	(2*256 = 512us x16  = 8,192us) 			 	- 1221 rpm 12pole
; if temp2 is 1 : 14,648 rpm (1*256 = 256us x16  = 4,096us) 			- 2441 rpm 12pole

; max rpm allowed at low/high throttle (explained further below):
; if temp1 is 49 (low) :  76,531 rpm (49us x16 = 784us per revolution)  - 12755 rpm 12 pole
; if temp1 is 27 (high): 138,888 rpm (27us x16 = 432us)					- 23148 rpm 12 pole


; temp3 (ZL) evaluation:
	; < 57 rpm
		tst	temp3				; test if 3rd byte is zero (<57rpm)
		brne	calc_OCT1_res	; if >0, unrealistic so reset pwm etc to slowest settings (branch if not 0)
		sts	OCT1_high, temp2	; if 0, motor could be spinning so save 2nd byte
		

; temp2 (YH) evaluation:
	; > 14,648 rpm (2pole)
		tst	temp2							; test for zero
		breq	calc_OCT1_of10				; if 0, (> 14,648 rpm), branch to #10 test against throttle position
		subi	temp1, low (defaultTIMEOUT)	; if >0, (< 14,648 rpm) reduce by 32000 to test 114rpm threshold
		sbci	temp2, high(defaultTIMEOUT)
	; < 114 rpm
		brcc	calc_OCT1_res				; if 1 (a negative value ie: <114rpm) too slow so reset
	; 114 - 14,648 rpm
		rjmp	calc_OCT1_of80				; if 0, neither too fast nor too slow so just save timing


; > 14,648 rpm (2pole) / 2441rpm (12 pole) - fast idle to full power
calc_OCT1_of10:
	; convert throttle pwm range to a scale that can be used to compare against the '1/16th' rpm durations
	; the formula is somewhat obscure but has the effect of compressing the throttle range
	;  while also aligning it to the temp1 value that represents maximum rpm
	; throttle range (timer0 count) is normally 156-246 and is converted to 27-49
	; '27' therefore represents both max throttle and max rpm to prevent an over-rpm condition
	; '49' represents zero throttle and 76,531 rpm
	; these has the effect of checking for over-rpm between 76,531 and 138,888 over the throttle range

		mov	temp2, tcnt0_power_on	; load current throttle position (156-246)
		asr	temp2					; eg: 156/0b10011100 becomes 206/0b11001110
		asr	temp2					; eg: 206/0b11001110 becomes 231/0b11100111
		subi temp2, 0xcc			; eg: 231 becomes 27 (ie: 156 becomes 27)
		
		cp	temp1, temp2		; compare temp2 (converted throttle level) with temp1 (rpm)
								; carry flag=0 if temp1 >= temp2
		brcc	calc_OCT1_of80	; branch to #80 (normal action) if throttle >= rpm (branch if carry flag 0)
								; continue to reset if throttle < rpm (rpm too high/possible timing error)
	
				
; reset pwm if unrealistic values detected
calc_OCT1_res:
		rcall	set_default_timeout		; set default commutation settings
		rjmp	calc_OCT1_of90			; save timer values and exit
		

; normal operation
calc_OCT1_of80:
	; save 1st timing value
		sts	wt_comp_scan_l, YL			; save 22.5' offset used to bypass inductive spike
		sts	wt_comp_scan_h, YH


; if an other timing than 7.5° is wanted, change YL,YH here:

; example (13.1°):
;		lsr	YH
;		ror	YL
;		mov	temp1,YL 		; 1/32 + 1/64 = 1/21.33 (30°-16.9°=13.1°)
;		mov	temp2,YH
;		lsr	temp2
;		ror	temp1
;		add	YL, temp1
;		adc	YH, temp2

; example (10.3°):
;		lsr	YH
;		ror	YL
;		mov	temp1,YL 		; 1/32 + 1/64 + 1/128 = 1/18.3 (30°-19.7°=10.3°)
;		mov	temp2,YH
;		lsr	temp2
;		ror	temp1
;		add	YL, temp1
;		adc	YH, temp2
;		lsr	temp2
;		ror	temp1
;		add	YL, temp1
;		adc	YH, temp2

; example (8.9°):
;		lsr	YH
;		ror	YL
;		mov	temp1,YL 		; 1/32 + 1/64 + 1/128 + 1/256 = 1/17.1 (30°-21.1°=8.9°)
;		mov	temp2,YH
;		lsr	temp2
;		ror	temp1
;		add	YL, temp1
;		adc	YH, temp2
;		lsr	temp2
;		ror	temp1
;		add	YL, temp1
;		adc	YH, temp2
;		lsr	temp2
;		ror	temp1
;		add	YL, temp1
;		adc	YH, temp2

; example (7.5°):
; do nothing

; example (6.1°):
;		mov	temp1,YL 		; 1/16 + 1/256 = 1/15.06 (30°-23.9°=6.1°)
;		mov	temp2,YH
;		ldi	temp4, 4		; divide by 16
;calc_OCT1_dt:	lsr	temp2
;		ror	temp1
;		dec	temp4
;		brne	calc_OCT1_dt (branch if not equal)
;		add	YL, temp1
;		adc	YH, temp2

; example (4.7°):
;		mov	temp1,YL 		; 1/16 + 1/128 = 1/14.22 (30°-25.3°=4.7°)
;		mov	temp2,YH
;		ldi	temp4, 3		; divide by 8
;calc_OCT1_dt:	lsr	temp2
;		ror	temp1
;		dec	temp4
;		brne	calc_OCT1_dt (branch if not equal)
;		add	YL, temp1
;		adc	YH, temp2


	; save 2nd timing value
		sts	wt_FET_switch_l, YL		; store time from ZC to next commutation to achieve correct advance timing
		sts	wt_FET_switch_h, YH
		
calc_OCT1_of90:
		lds	temp1, tcnt1_sav_l		; reload current cycle duration
		lds	temp2, tcnt1_sav_h
		lds	temp3, tcnt1_sav_x
		sts	last_tcnt1_l, temp1		; save as 'previous' for next cycle
		sts	last_tcnt1_h, temp2
		sts	last_tcnt1_x, temp3
		ret
		

.if RC_PULS == 1
;-------------------------------------------------------------------------
; interprets desired throttle position and sets fet's pwm duty cycle

; assumes >800us throttle range (rc pulse length)
; throttle considered to be closed if below 1100us (MIN_RC_PULS==1100)
; throttle considered to be at max if above 1900us (1100 + assumed 800 range)
; by deducting the 1100 zero throttle 'offset', remaining value represents a throttle scale (0-800)
; by dividing that by 8 scale ranges from 0-100
; this is used as the fet pwm duty cycle (subject to other adjustments)
; function called in state2 via wait_OCT1_before_switch

evaluate_rc_puls:
		sbrs	flags1, RC_PULS_UPDATED		; if 1, skip next, a new rc pulse exists
		rjmp	eval_rc_p90					; if 0, exit - new rc pulse is not available

; new rc pulse exists
		mov	temp1, new_rcpuls_l				; read new pulse length (typically 1000 to 2000us)
		mov	temp2, new_rcpuls_h
		cbr	flags1, (1<<RC_PULS_UPDATED) 	; clear flag once rc pulse value has been read
		subi	temp1, low  (MIN_RC_PULS)	; deduct the 'low throttle' offset (1100us)
		sbci	temp2, high (MIN_RC_PULS)
		brcc	eval_rc_p00					; if 1, convert signal if still positive (branch if 0)
		clr	temp1							; if 0, clear results if became negative
		clr	temp2

; the rc pulse is greater than the minimum
eval_rc_p00:
	; scale result to roughly 0-100 (depends on Tx)
		lsr	temp2		; right shift once (/2)
		ror	temp1
		lsr	temp2		; right shift a second time (=/4)
		ror	temp1
		lsr	temp2		; right shift a third time (=/8)
		ror	temp1		; eg: 2000-1100=900 /8 = 112
		mov	temp3, temp1					; result should be 0-100, but could be slightly more

	; use 100 (full throttle) if 100 or more
		subi	temp1, low  (POWER_RANGE)	; reduce by 100 (max allowed)
		sbci	temp2, high (POWER_RANGE)
		brcs	eval_rc_p10					; jump if <100 (branch if carry flag is 1)
		ldi	temp3, low  (POWER_RANGE)		; use 100 if >100

eval_rc_p10:
	; use actual if <100
		mov	ZH, temp3			; ZH is new duty cycle (0 and 100)

eval_rc_p90:
		ret
.endif


.if FREE_FLIGHT == 1
;-------------------------------------------------------------------------
; checks if button has been pressed while motor is running (FF)
; stops motor by resetting program
; function called in state1

evaluate_ff_stop:
	; check if button is being pressed
		sbic PIND, rcp_in		; skip next if 0 (0=button pressed)
		rjmp ff_stop_exit		; exit if 1 (button not pressed)
		rcall soft_reset		; force program to reset when button pressed

ff_stop_exit:
		ret

.endif


;-------------------------------------------------------------------------
; checks for stalled motor (won't start or model crashed)

; limits time that controller will keep trying to run motor
; function called in state3

evaluate_stall:
	; check rpm
		lds	temp1, OCT1_high	; get current RPM
		cpi	temp1, 16			; carry flag=1 if 16>temp1 - occurs when rpm>915(2pole)/152(12pole)
		brcs stall_exit			; branch if carry 1 (rpm>1831) - exit if running (not stalled)

	; motor stopped/slow so check power setting
		mov	temp1, tcnt0_power_on	; get current FET duty cycle (156-246)
		cpi temp1, 246				; carry=1 if temp2>temp1 (min(246)>current(156-246) =power ON)
		brsh stall_exit				; branch if current>=246 (power OFF)

	; motor stopped, power on = stalled so decrement counter to bypass startup phase
		lds temp1, stall_buffer		; load counter
		dec temp1					; reduce counter
		breq stalled				; branch if 0 (delay complete; motor stalled)
		sts stall_buffer, temp1		; save count if not 0 yet
		rjmp stall_exit				; exit

stalled:
		rcall soft_reset			; force program to reset

stall_exit:
		ret


;-------------------------------------------------------------------------
; resets program on error conditions
; WDT timer used to ensure all registers are initialised properly

soft_reset:
	; clear ports to disable fets (brake effect if not done)
		clr temp1
		out PORTB, temp1		; n-fets
		out PORTD, temp1		; p-fets

	; enable watchdog timer
		ldi	temp1, (1<<WDE)		; enable WDT (defaults to quickest rate)
		out	WDTCR, temp1

run_wdt:
	; wait here until program resets
		rjmp run_wdt			; infinite loop


;-------------------------------------------------------------------------
; performs system checks

; intended to check over-current and under-voltage but these are not enabled
; (T1OVFL_FLAG and UB_LOW never get set)
; called in state5
; calls 'set_new_duty' which sets pwm duty cycle and feathers throttle if rpm is low

evaluate_sys_state:
	; perform checks if timer1 has overflowed
		sbrs	flags1, T1OVFL_FLAG		; if 1, proceed with checks (skip next if 1)
		rjmp	eval_sys_s99			; if 0, bypass checks (don't perform checks too often)
		cbr	flags1, (1<<T1OVFL_FLAG)	; clear flag for reuse

eval_sys_i:
	; check for over-current
		sbrs	flags0, I_pFET_HIGH		; if 1, over-current has been detected before (skip next if 1)
									; I_pFET_HIGH never gets set anywhere (so does nothing)
		rjmp	eval_sys_i_ok			; if 0, over-current not detected again so decrement count
		
	; over-current detected
		cbr	flags0, (1<<I_pFET_HIGH)	; clear flag for reuse
		mov	temp1, current_err			; read number of previous over-current conditions detected
;		mov	i_temp1, current_err		; read number of previous over-current conditions detected
		cpi	temp1, CURRENT_ERR_MAX		; compare with max allowed (eg: 3)
;		cpi	i_temp1, CURRENT_ERR_MAX	; compare with max allowed (eg: 3)
		brcc	panic_exit				; if 0, error count == max allowed so panic! (branch if 0)
		inc	current_err					; if 1, error count < max allowed so increment error count
		rjmp	eval_sys_ub				; move on to test battery voltage

eval_sys_i_ok:
	; over-current not detected
		tst	current_err					; test for number of previous errors
		breq	eval_sys_ub				; if 0, branch to voltage checks
		dec	current_err					; if >0, reduce count

eval_sys_ub:
	; check for under-voltage
		sbrs	flags0, UB_LOW		; if 1, voltage is low proceed with checks (skip next if 1)
								; UB_LOW never gets set
		rjmp	eval_sys_ub_ok		; if 0, under-voltage not detected
		
	; under-voltage detected
		cbr	flags0, (1<<UB_LOW)		; clear flag for reuse
		mov	temp1, sys_control		; read number of previous under-voltage conditions detected
;		mov	i_temp1, sys_control	; read number of previous under-voltage conditions detected
		cpi	temp1, POWER_RANGE		; compare with max allowed (eg: 100)
;		cpi	i_temp1, POWER_RANGE	; compare with max allowed (eg: 100)
		brcc	eval_sys_s99		; if 0, error count == max allowed so go straight to change duty cycle (branch if 0)
		inc	sys_control				; if 1, error count < max allowed so increment error count
		rjmp	eval_sys_s99		; now go set duty cycle

eval_sys_ub_ok:
	; under-voltage not detected
		tst	sys_control				; test for number of previous errors
		breq	eval_sys_s99 		; if 0, branch to set duty cycle
		dec	sys_control				; if >0, reduce count
		
eval_sys_s99:
	; sets throttle curve and pwm duty cycle
		rcall	set_new_duty		; reduces duty cycle by increasing amounts as low voltage count goes up
		ret

panic_exit:	; !!!!!! OVERCURRENT !!!!!!!!
		cli							; disable all interrupts
		rjmp	reset				; restart ESC program (why not reduce throttle?)
		

;-------------------------------------------------------------------------
; delay between last commute and looking for zero-crossing

; the motor's windings store energy when powered (act as an inductor)
; this is released when no longer driven (called the inductive spike)
; purpose of delay this is to avoid looking for zero-crossing (ZC) until after this spike
; function is called from all 6 states
; the last part (wait_OCT1_tot) is called on startup to initialise settings

; the delay comprises 2 parts:
; 1. 30 electrical degrees exist between switching fets off and ZC; this delay bypasses initial 22.5'
; 2. startup timing to force states to change on starup
; (starup timing and reduce hangups)

; the OCT1_PENDING flag is used to indicate when delay is complete (cleared in an interrupt)


; inductive spike delay (wait 22.5')
wait_OCT1_before_comp_scan:
	; configure delay
		sbr	flags0, (1<<OCT1_PENDING)	; set flag to allow timer1/compA interrupt to clear it

		lds	YL, wt_comp_scan_l	; read required delay (22.5')
		lds	YH, wt_comp_scan_h
		rcall	tcnt1_to_temp	; read timer1 values into temp1,2,3 (and disable rc pulse interrupts)
		add	YL, temp1			; add required delay to time elapsed in states commuted already
		adc	YH, temp2

		ldi	temp1, (1<<TOIE1)+(1<<TOIE0); select config bits/mask (bits not selected get cleared)
		out	TIMSK, temp1				; disable timer1/compA (only) to avoid conflicts
		out	OCR1AH, YH					; load output compare A registers with required timing
		out	OCR1AL, YL

		ldi	temp1, (1<<TOIE1)+(1<<OCIE1A)+(1<<TOIE0); set mask to enable all 3 interrupts
		out	TIMSK, temp1							; enable timer1/compA and timer interrupts
		ldi	temp4, EXT0_EN							; set mask
		out	GIMSK, temp4							; enable rc pulse (ext0int) interrupt

wait_OCT1_next:
	; wait until 1st delay is complete
		sbrc	flags0, OCT1_PENDING	; if 0, skip next (OCT1_PENDING cleared in interrupt=delay is complete)
		rjmp	wait_OCT1_next			; if 1, keep looping (delay not complete yet)


; startup delay to force states to change
wait_OCT1_tot:
	; enable comparator multiplexer
		cbi	ADCSRA, ADEN		; ADC disabled to allow multiplexed comparator (clear bit)
		in	temp1, SFIOR		; read special function register (defaults 0)
		ori	temp1, (1<<ACME)	; OR/invert (mask to set ACME)
		out	SFIOR, temp1		; set ACME bit to enable comparator multiplexer

	; configure 2nd delay to force states to change
		sbr	flags0, (1<<OCT1_PENDING)	; set flag to allow timer1/compA interrupt to clear it
		rcall	tcnt1_to_temp			; refresh cycle time duration into temp 1,2,3 (& disable rc interrupt)
		ldi	YL, low (compScanTIMEOUT)	; load fixed delay (48000=48ms)
		ldi	YH, high(compScanTIMEOUT)
		add	temp1, YL		; add to time elapsed so far
		adc	temp2, YH

		ldi	temp3, (1<<TOIE1)+(1<<TOIE0)	; set mask
		out	TIMSK, temp3			; disable timer1/compA interrupt
		out	OCR1AH, temp2			; load output compare A registers with required timing
		out	OCR1AL, temp1
		ldi	temp3, (1<<TOIE1)+(1<<OCIE1A)+(1<<TOIE0)	; set mask
		out	TIMSK, temp3					; enable timer1/compA & timer interrupts
		ldi	temp4, EXT0_EN		; set mask
		out	GIMSK, temp4		; enable rc pulse (ext0int) interrupt

		ret


;-------------------------------------------------------------------------
; delay between detecting zero-crossing and performing next commutation

; states change every 60' and zero-crossings (ZC) occur in the middle at 30'
; it is normal to use advance timing (eg: by 7.5')
; so the next commutation needs to be at 22.5' after zero-crossing (30-7.5) = this delay
; YL and YH hold the time elapsed since the end of state1 (free-running, never reset)
; so correct timing is achieved by adding this advance timing offset (22.5') to time elapsed so far
; uses OCT1_PENDING flag to indicate when delay is complete (cleared in interrupt)
; monitoring tasks are performed during the delay

wait_OCT1_before_switch:
	; configure delay
		sbr	flags0, (1<<OCT1_PENDING)	; set flag to allow timer1/compA interrupt to clear it

		rcall	tcnt1_to_temp			; reads timer1 values into temp1,2,3
						; (and disables rc pulse interrupts to avoid corrupting TEMP)
		lds	YL, wt_FET_switch_l		; load advance timing delay (22.5' portion)
		lds	YH, wt_FET_switch_h		; (set in calculate_next_timing_values)
		add	temp1, YL				; add to time elapsed in states commuted already
		adc	temp2, YH

		ldi	temp3, (1<<TOIE1)+(1<<TOIE0); select config bits/mask (bits not selected get cleared)
		out	TIMSK, temp3				; disable timer1/compA (only) to avoid conflicts
		out	OCR1AH, temp2				; load output compare A registers with required timing
		out	OCR1AL, temp1

		ldi	temp3, (1<<TOIE1)+(1<<OCIE1A)+(1<<TOIE0); set mask to enable all 3 interrupts
		out	TIMSK, temp3							; enable timer1/compA and timer interrupts
		ldi	temp4, EXT0_EN							; set mask
		out	GIMSK, temp4							; enable rc pulse (ext0int) interrupt

OCT1_ready:
	; perform monitoring tasks while waiting
		sbrc	flags1, CALC_NEXT_OCT1			; if 0, skip next (only set in state6)
		rcall	calculate_next_timing_values 	; if 1, calculate values while compare timer is active

.if RC_PULS == 1
		sbrc	flags1, EVAL_RC_PULS	; if 0, skip next (only set in state2)
		rcall	evaluate_rc_puls		; if 1, adjust pwm duty cycle
.endif

		sbrc	flags1, EVAL_SYS_STATE	; if 0, skip next (only set in state5)
		rcall	evaluate_sys_state		; if 1, check over-current/under-voltage

OCT1_wait:
	; wait until delay complete
		sbrc	flags0, OCT1_PENDING	; if 0, skip next and exit function (delay is complete)
		rjmp	OCT1_wait				; if 1, keep looping (delay not complete yet)
		ret
	
	
;-------------------------------------------------------------------------
; set throttle behaviour

; sets tcnt0_pwron_next which is used in t0ovfl_int to enable fets and change pwm duty cycle
; creates throttle curve by controlling pwm duty cycle at low rpm
; reduces power if voltage is low (not enabled)
; called in state 5 only

set_new_duty:
		mov	temp1, ZH				; read current duty cycle (0-100)
		sub	temp1, sys_control		; reduce power by increasing amounts (when under-voltage checks are active)
		brcc	set_new_duty10		; if 0, set to minimum in #10 (branch if 0)
		ldi	temp1, MIN_DUTY-1		; else, cut throttle (set to 9 (10-1))
		
set_new_duty10:
	; rpm > 1831 (2pole)
		lds	temp2, OCT1_high		; get actual RPM
		cpi	temp2, 8				; carry flag=1 if 8>temp2 (occurs when rpm>1831)
		brcs	set_new_duty20		; if 1 (rpm>1831), proceed

	; rpm < 1831 ensure throttle < 25% at this low rpm
		ldi	temp2, POWER_RANGE		; if 0 (rpm <1831) check throttle position
		lsr	temp2					; /2
		lsr	temp2					; /2
		cp	temp1, temp2		; carry flag=1 if temp2>temp1 (occurs when 25% > throttle now)
		brcs	set_new_duty30		; if 1 (throttle is not > 25%), no restriction (use actual)
		mov	temp1, temp2			; if 0, throttle too high for actual rpm so set to 25%
		rjmp	set_new_duty30			; exit

set_new_duty20:
	; 1831 < rpm < 3662
		cpi	temp2, 4		; carry flag=1 if 4>temp2 (occurs when rpm>3662)
		brcs	set_new_duty30		; if 1 (rpm>3662), exit no change
		ldi	temp2, POWER_RANGE	; if 0 (rpm<3662) check throttle position
		lsr	temp2			; /2
		cp	temp1, temp2		; carry flag=1 if temp2>temp1 (occurs when 50% > throttle now)
		brcs	set_new_duty30		; if 1 (throttle is not > 50%), no restriction (use actual)
		mov	temp1, temp2		; if 0, throttle too high for actual rpm so set to 50%

set_new_duty30:
		com	temp1			; invert bits to load into timer0 counter
						; eg: 64 (0b01000000) becomes 191 (0b10111111) ie: 64 from 255
		mov	tcnt0_pwron_next, temp1	; save as 'next' pwm duty cycle
		ret
	
	
;-------------------------------------------------------------------------
; restart/unrealistic commutation/rpm values detected

set_default_timeout:
		ldi	temp1, NO_POWER				; 246 / low throttle
		mov	tcnt0_power_on, temp1		; set FET duty cycle to minimum (eg: 246=10us)
		ldi	temp1, 8				; 8 represents 1831 rpm
		sts	OCT1_high, temp1			; set flag which forces throttle to 25% or less
		ldi	YL, low (defaultTIMEOUT)	; set 6state duration to 1831 equiv (overrides timer1 value)
		ldi	YH, high(defaultTIMEOUT)
		sts	wt_comp_scan_l, YL			; set 'commutation to scan' delay
		sts	wt_comp_scan_h, YH
		ret

				
;-------------------------------------------------------------------------
; uart function

.if UART_CONTROL == 1
read_uart:	in	temp1,UDR	; read data - clear rxc-flag
		cpi	temp1,'+'
		brne	read_uart_10
	; do (+) job
		cpi	ZH, POWER_RANGE
		brsh	read_uart_90
		rcall	send_byte
		inc	ZH
		rcall	set_new_duty
		rjmp	read_uart_90
read_uart_10:	cpi	temp1,'-'
		brne	read_uart_20
	; do (-) job
		cpi	ZH, MIN_DUTY-1
		breq	read_uart_90 (branch if equal)
		rcall	send_byte
		dec	ZH
		rcall	set_new_duty
		rjmp	read_uart_90
read_uart_20:
.if UART_FULL == 1
		cpi	temp1,'#'
		brne	read_uart_30
	; do (#) job
		clr	XH
		ldi	XL, uart_data
		sbr	flags0, (1<<GET_STATE)	; (set bit)
		ldi	temp1,0x0d
		rcall	send_byte
		ldi	temp1,'#'
		st	X+,temp1
		mov	temp1,flags0
		rcall	hex2buf
		mov	temp1,flags1
		rcall	hex2buf
		mov	temp1,tcnt0_power_on
		rcall	hex2buf
		lds	temp1,wt_comp_scan_h
		rcall	hex2buf
		lds	temp1,wt_comp_scan_l
		rcall	hex2buf
		ldi	temp1,'-'
		st	X+,temp1
		rjmp	read_uart_90
.endif
read_uart_30:	cpi	temp1,'1'
		brne	read_uart_40
	; do (1) job
		ldi	ZH, MIN_DUTY+0x30
		rcall	send_byte
		rcall	set_new_duty
		rjmp	read_uart_90
read_uart_40:	cpi	temp1,'2'
		brne	read_uart_89
	; do (2) job
		ldi	ZH, MIN_DUTY-1
		rcall	send_byte
		rcall	set_new_duty
		rjmp	read_uart_90
read_uart_89:	ldi	temp1,'?'
		rcall	send_byte
read_uart_90:	ret

hex_out:
		mov	temp2,temp1
		swap	temp1
		rcall	nibble_out
		mov	temp1,temp2
		rcall	nibble_out
		ret
	
nibble_out:
		andi	temp1,0x0f
		ori	temp1,0x30
		cpi	temp1,0x3a
		brlo	nibble_out_10
		subi	temp1,-7		; (reduce by)
nibble_out_10:
send_byte:	sbi	UCSRA,TXC	; clear flag (set bit)
		out	UDR,temp1
nibble_out_20:	sbis	UCSRA,TXC (set bit)
		rjmp	nibble_out_20
		ret
		
  .if UART_FULL == 1
hex2buf:	mov	temp2,temp1
		swap	temp1
		rcall	nibble2buf
		mov	temp1,temp2
		rcall	nibble2buf
		ret
	
nibble2buf:	andi	temp1,0x0f
		ori	temp1,0x30
		cpi	temp1,0x3a
		brlo	nibble2buf_10
		subi	temp1,-7
nibble2buf_10:	st	X+,temp1
		ret
		
save_state:
		mov	temp1,flags0
		rcall	hex2buf
		mov	temp1, current_err
		rcall	hex2buf
		mov	temp1, sys_control
		rcall	hex2buf
		ldi	temp1,'-'
		st	X+,temp1
		ret		
		
send_state:
		ldi	temp1,0x0d
		st	X+,temp1
		ldi	temp1,0x0a
		st	X+,temp1
		subi	XL,uart_data
		mov	uart_cnt,XL
		ldi	XL,uart_data
		sbi	UCSRA, TXC		; clear flag (set bit)
		sbi	UCSRB, TXCIE		; enable irq (set bit)
		ldi	temp1,0x0a		; start transmission
		out	UDR,temp1
		cbr	flags0, (1<<GET_STATE)	; (clear bits)
		ret		

utxc:
		ld	i_temp1,X+
		out	UDR, i_temp1
		dec	uart_cnt
		brne	utxc_90
		cbi	UCSRB, TXCIE		; disable irq (clear bit)
utxc_90:	reti

  .endif		; UART_FULL == 1
.endif		; UART_CONTROL == 1


;-------------------------------------------------------------------------
; startup function

switch_power_off:
		ldi	temp1, NO_POWER			; lowest throttle/pwm value (246 / 10us)
		mov	tcnt0_pwron_next, temp1	; start low
;		ldi	temp1, CHANGE_TIMEOUT	; reset change-timeout (dt-also appears in control_start so may be redundant
;		mov	tcnt0_change_tot, temp1
		sbr	flags1, (1<<POWER_OFF)	; disable power on (set bit)
		ret

		
;-------------------------------------------------------------------------
; delay to test for false comparator readings

wait_if_spike:
	nop
	nop
	nop
	nop

	ret


;-------------------------------------------------------------------------
; 'State Machine'

; this function scrolls through the 6 states and determines which fets/comparators to use
; fets stay active for two successive states so each state only changes two fets
; eg: Cn is activated at the end of state6, is ON for states 1 and 2, and deactivated at the end of state2

; this function works closely with the timer0 interrupt which is the 'throttle' (pwm) mechanism
; only nFETs are pulsed with the pFETs just staying on while active (no need to pulse both)
; the A_FET and C_FET flags are set below to tell the pwm function which fets to pulse (B when neither A nor C)
; these flags only need to be set every two states, again because fets stay active for two consecutive states
; interrupts are disabled while changing fets to give higher priority to this function

; to have effective control over rpm, the state of fets cannot be changed quicker than the pwm period (100us)
; since they stay active for two states, the shortest duration each state can be is 50us (=300us for 6 states=1 rev)
; so, with chosen pwm period/clock speed, rpm can never exceed 200k for 2 pole motors (1minute / 300us = rpm)
; in reality it has to be much slower than this due to processing (code) overhead
; eg: ~32 timer0 config instructions before setting the timer0 counter between every 2 pwm cycles = 96us
; program author indicates 138,888 rpm is max possible implying 432us per electrical (2pole) revolution
; (1 minute / 138,888 = 432us per revolution - 300us min = 132us code overhead)

; each state has two other important differences:
; 1. BEMF always opposes the change producing it (if a phase was high, BEMF goes low and visa versa)
;    So each state senses BEMF in the opposite direction to the previous one
; 2. Most states do 'extra' things to spread other workload:
;    evaluate rc pulse, battery voltage, current, system state or cycle time/timing
;	 these are not all fully functional in this program

; the AIN0 (+) input to the comparator is always connected to all 3 phases via a virtual neutral point
; this averages the BEMF of all 3 phases and serves as a reference to compare against any one phase
; the un-driven phase is connected to the AIN1 (-) input of the comparator
; 'zero-crossing' (ZC) occurs when its BEMF crosses the virtual neutral
; the ACO output of the comparator reveals BEMF state (high/low) relative to neutral ie: ZC
; ACO is high when AIN0 (neutral) is higher than ANI1 (un-driven phase)
; for phases that were low (n-fet on) before becoming un-driven, the ACO goes high to lowafter ZC
; for phases that were high (p-fet on) before becoming un-driven, the ACO goes low to high after ZC

; each state starts with its fets and comparator having been configured in the previous state
; its first task is to look for ZC
; it then branches to 'wait_OCT1_before_switch' where stays until ready to commute with chosen timing
; while waiting it performs various tests described as '2' above

; at the end of each electrical revolution, rpm is determined from the time taken to perform 6 state changes
; this duration sets advanced timing duration for the next revolution (calculate_next_timing_values)

; each state then configures the fets and comparator for the next one
; the 'wait_OCT1_before_comp_scan' is called to wait for the inductive spike to pass before we can test for ZC
; it then sets a precautionary 48ms timeout / startup timing but returns immediately

; this cycle will only break on signal loss (or over-current/under-voltage if enabled)



; =========================

; state1 = B(p-on) + C(n-choppered) - comparator A evaluated
;  (these are set at the end of the previous state 6)
; out_cA changes from low to high
;  (ie: A-Fet was low in preceding state so BEMF goes low to high)
;  (the comparator's + input is common to all phases)
;  (the - input is connected to the measured phase)
;  (the comparator's output starts high because - input < + input)
;  (and the output goes low when ZC occurs with the - input becoming > + input)

state1:

; OCT1_PENDING = 1 = Delay pending (set on startup and during delays before and after commutation)
; OCT1_PENDING = 0 = Delay complete (after 'manual' startup timing delays)

		sbrc	flags0, OCT1_PENDING	; if 0, skip next to move to next state (delay is complete)
		rjmp	state1_2				; if 1, check for ZC (startup & normal running)

.if FREE_FLIGHT == 1
		rcall	evaluate_ff_stop		; check if button pressed to stop FF motor
.endif
		rjmp	s1_close				; move to next state to force states to change 'manually' (startup)
		
state1_2:
		sbic	ACSR, ACO				; if 0 (after ZC), skip next and move on
		rjmp	state1					; if 1 (before ZC), keep looping

		rcall	wait_if_spike			; 4us delay
		sbic	ACSR, ACO				; if 0 (still low after ZC) skip next and move on
		rjmp	state1					; if 1, is high again so was just a spike (loop again)

		rcall	wait_OCT1_before_switch	; ZC has occurred so wait for advance timing before commuting


; switch fets and configure comparator for next state
s1_close:

.if UART_CONTROL == 1
  .if UART_FULL == 1
		sbrc	flags0, GET_STATE
		rcall	save_state
  .endif
.endif

		cbi	PORTD, BpFET			; switch Bp off
	
		sbrs	flags1, POWER_OFF	; if 1 (throttle is off), skip next (avoid enabling fet unnecessarily)
		sbi	PORTD, ApFET			; if 0 (power is required), switch Ap on
	
		ldi	temp1, mux_b
		out	ADMUX, temp1			; set comparator multiplexer to phase B
		
		rcall	wait_OCT1_before_comp_scan	; delay to skip inductive spike (waits until complete)
											; configure startup timing delay (but don't wait)

; =========================

; state2 = A(p-on) + C(n-choppered) - comparator B evaluated
; out_cB changes from high to low

state2:
		sbrc	flags0, OCT1_PENDING	; if 0, skip next ZC not detected/starting motor so move on to next phase
		rjmp	state2_2				; if 1, check if zero-crossing has occurred
		rjmp	s2_close				; move to next state
		
state2_2:
		sbis	ACSR, ACO				; if 1 (after ZC), skip next to check if just a spike
		rjmp	state2					; if 0 (before ZC), loop again
		rcall	wait_if_spike			; 4us delay
		sbis	ACSR, ACO				; if 1 (confirms ZC), skip next
		rjmp	state2					; if 0 (just a glitch), keep looping

		sbr	flags1, (1<<EVAL_RC_PULS)	; set flag to trigger system monitoring checks
		rcall	wait_OCT1_before_switch	; perform checks while waiting to commute
		cbr	flags1, (1<<EVAL_RC_PULS)	; clear flag once done
;		cbr	flags0, (1<<EVAL_UB)		; voltage evaluation request stop redundant (never set; 0 on startup)
		
; switch fets and configure comparator for next state
s2_close:

.if UART_CONTROL == 1
  .if UART_FULL == 1
		sbrc	flags0, GET_STATE	;
		rcall	save_state
  .endif
.endif

	; disable interrupts to give this function priority
		ldi	temp1, (1<<OCIE1A)+(1<<TOIE0)	; stop timer1/compA and timer0 interrupts (keeps timer1 going)
		out	TIMSK, temp1					; avoids the risk of 't0ovfl_int' (pwm function) changing same fets
		nop

	; set An and Cn flags to 0 to activate Bn fet for pwm pulsing (in t0ovfl_int)
		cbr	flags0, (1<<A_FET)		; clear An flag
		cbr	flags0, (1<<C_FET)		; clear Cn flag

	; set phases for next state
		cbi	PORTB, CnFET			; Cn off
		
		sbrc	flags1, FULL_POWER	; if 0 (throttle is not at max), skip next (don't enable Bn yet)
		rjmp	s2_switch			; if 1 (power is required), proceed to enable Bn
		sbrc	flags0, I_OFF_CYCLE	; if 0 (in a pwm OFF cycle), skip next and change Bn as normal
		rjmp	s2_done				; if 1 (in a pwm ON cycle), don't disturb now (complete current pwm ON cycle)
									; (pwm function will enable correct nFET on next pwm cycle using flags set above)
		
s2_switch:
	; OK to enable fet
		sbrs	flags1, POWER_OFF	; if 1 (throttle is off),skip next (don't enable fet unnecessarily)
		sbi	PORTB, BnFET			; if 0 (power is required), switch Bn on as normal for next phase
		
s2_done:
		ldi	temp1, (1<<TOIE1)+(1<<OCIE1A)+(1<<TOIE0) 	; enable interrupts for normal operation again
		out	TIMSK, temp1
		ldi	temp1, mux_c								; set comparator multiplexer to phase C
		out	ADMUX, temp1
		rcall	wait_OCT1_before_comp_scan				; pause for correct timing

; =========================

; state3 = A(p-on) + B(n-choppered) - comparator C evaluated
; out_cC changes from low to high

state3:
		sbrc	flags0, OCT1_PENDING	; startup delay
		rjmp	state3_2				; wait for ZC loop
		rjmp	s3_close
		
state3_2:
		sbic	ACSR, ACO
		rjmp	state3
		rcall	wait_if_spike
		sbic	ACSR, ACO
		rjmp	state3

		rcall	wait_OCT1_before_switch	; timing delay and perform system checks
		
s3_close:

.if UART_CONTROL == 1
  .if UART_FULL == 1
		sbrc	flags0, GET_STATE
		rcall	save_state
  .endif
.endif

		cbi	PORTD, ApFET			; Ap off
		sbrs	flags1, POWER_OFF
		sbi	PORTD, CpFET			; Cp on if power is required

.if UART_CONTROL == 1
  .if UART_FULL == 1
		sbrc	flags0, GET_STATE
		rcall	send_state
  .endif
  		sbic	UCSRA,RXC
		rcall	read_uart
.endif

		ldi	temp1, mux_a					; set comparator multiplexer to phase A
		out	ADMUX, temp1

	; check if motor stalled
		rcall	evaluate_stall

		rcall	wait_OCT1_before_comp_scan	; avoid inductive spike

; =========================

; state4 = C(p-on) + B(n-choppered) - comparator A evaluated
; out_cA changes from high to low

state4:
		sbrc	flags0, OCT1_PENDING	; startup delay
		rjmp	state4_2				; ZC loop
		rjmp	s4_close
		
state4_2:
		sbis	ACSR, ACO
		rjmp	state4
		rcall	wait_if_spike
		sbis	ACSR, ACO
		rjmp	state4

		rcall	wait_OCT1_before_switch	; timing delay
;		cbr	flags1, (1<<EVAL_I_pFET) ; current evaluation request stop (flag is never set; 0 on startup)
		
s4_close:

.if UART_CONTROL == 1
  .if UART_FULL == 1
		sbrc	flags0, GET_STATE
		rcall	save_state
  .endif
.endif

		ldi	temp1, (1<<OCIE1A)+(1<<TOIE0) ; disable interrupts
		out	TIMSK, temp1
		nop
		
		sbr	flags0, (1<<A_FET)		; allow An to be activated in pwm function
		cbr	flags0, (1<<C_FET)
		
		cbi	PORTB, BnFET			; Bn off
		sbrc	flags1, FULL_POWER	
		rjmp	s4_switch
		sbrc	flags0, I_OFF_CYCLE
		rjmp	s4_done
		
s4_switch:
		sbrs	flags1, POWER_OFF
		sbi	PORTB, AnFET			; An on if power required
		
s4_done:
		ldi	temp1, (1<<TOIE1)+(1<<OCIE1A)+(1<<TOIE0); enable interrupts
		out	TIMSK, temp1
		ldi	temp1, mux_b							; set comparator multiplexer to phase B
		out	ADMUX, temp1
		
		rcall	wait_OCT1_before_comp_scan			; bypass inductive spike

; =========================

; state5 = C(p-on) + A(n-choppered) - comparator B evaluated
; out_cB changes from low to high

state5:
		sbrc	flags0, OCT1_PENDING
		rjmp	state5_2

		rcall	evaluate_sys_state	; sets pwm duty cycle and other checks

		rjmp	s5_close
		
state5_2:
		sbic	ACSR, ACO
		rjmp	state5
		rcall	wait_if_spike
		sbic	ACSR, ACO
		rjmp	state5

		sbr	flags1, (1<<EVAL_SYS_STATE)	; set flag for system checks
		rcall	wait_OCT1_before_switch	; timing delay and perform system checks
		cbr	flags1, (1<<EVAL_SYS_STATE)	; clear flag
		
s5_close:

.if UART_CONTROL == 1
  .if UART_FULL == 1
		sbrc	flags0, GET_STATE
		rcall	save_state
  .endif
.endif

		cbi	PORTD, CpFET			; Cp off
		sbrs	flags1, POWER_OFF
		sbi	PORTD, BpFET			; Bp on if power required
		ldi	temp1, mux_c			; set comparator multiplexer to phase C
		out	ADMUX, temp1
		
		rcall	wait_OCT1_before_comp_scan

; =========================

; state6 = B(p-on) + A(n-choppered) - comparator C evaluated
; out_cC changes from high to low

state6:
		sbrc	flags0, OCT1_PENDING
		rjmp	state6_2
		rjmp	s6_close
		
state6_2:
		sbis	ACSR, ACO
		rjmp	state6
		rcall	wait_if_spike
		sbis	ACSR, ACO
		rjmp	state6

		sbr	flags1, (1<<CALC_NEXT_OCT1)	; sets timing and rpm limits
		rcall	wait_OCT1_before_switch	; timing delay and perform above calculations
		cbr	flags1, (1<<CALC_NEXT_OCT1)	; clear flag
		
s6_close:

.if UART_CONTROL == 1
  .if UART_FULL == 1
		sbrc	flags0, GET_STATE
		rcall	save_state
  .endif
.endif

		ldi	temp1, (1<<OCIE1A)+(1<<TOIE0) ; disable interrupts
		out	TIMSK, temp1
		nop
		
		cbr	flags0, (1<<A_FET)
		sbr	flags0, (1<<C_FET)		; allow Cn to be enabled in pwm function
		
		cbi	PORTB, AnFET			; switch An off
		
		sbrc	flags1, FULL_POWER
		rjmp	s6_switch
		sbrc	flags0, I_OFF_CYCLE
		rjmp	s6_done
		
s6_switch:
		sbrs	flags1, POWER_OFF
		sbi	PORTB, CnFET			; Cn on if power required
		
s6_done:
		ldi	temp1, (1<<TOIE1)+(1<<OCIE1A)+(1<<TOIE0) ; emable interrupts
		out	TIMSK, temp1
		ldi	temp1, mux_a					; set comparator multiplexer to phase A
		out	ADMUX, temp1
		rcall	wait_OCT1_before_comp_scan	; use newly calculated timing values

; =========================

; lost signal check
		tst	rcpuls_timeout		; check if zero (no rc pulse for 100 x timer1 overflows = 6.5s)
		breq restart_control	; restart ESC if not detecting signals (kills motor) (branch if equal)

		rjmp	state1			; go back to state1 if signal OK

restart_control:
		cli				; disable all interrupts
		rjmp	reset

.exit		; end of code