; Copyright 1996-2001 Andy Shaw ; ; This file is part of Neptune. ; ; Neptune is free software; you can redistribute it and/or modify ; it under the terms of the GNU General Public License as published by ; the Free Software Foundation; either version 2 of the License, or ; (at your option) any later version. ; ; Neptune is distributed in the hope that it will be useful, ; but WITHOUT ANY WARRANTY; without even the implied warranty of ; MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the ; GNU General Public License for more details. ; ; You should have received a copy of the GNU General Public License ; along with Foobar; if not, write to the Free Software ; Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA ; ; Contact ; Details of the Neptune sub control system can be found at: ; www.gloomy-place.com ; for further information to report fixes etc. please contact Andy Shaw ; andy@gloomy-place.com ; ; General Purpose Macros CBLOCK ; Start of general macro RAM allocation WSav ; W reg stack save location for ints StatSav ; Staus reg stack save location for ints Temp ; General Temp var Temp2 ; 2nd gen purpose reg ENDC page ; Push and pop macros, save state during interrupts etc. Note these can ; only be used once they may not be nested due to an effective stack depth ; of only 1. ; push macro movwf WSav ; Save W reg swapf WSav, F ; swap to make pop easy swapf STATUS, W ; same for STATUS movwf StatSav ; and save it endm pop macro swapf StatSav, W ; get saved status movwf STATUS ; put it back swapf WSav, W ; and same for W endm page ; ; Macro to limit the value in W to fall in the range -Lim .. Lim ; LimitN Macro Lim Local LimitNPos addlw 0x80 ; test to see if -ve btfss STATUS, C ; C set if -ve goto LimitNPos ; no go do +ve limit addlw 0x80 ; get orig value back addlw Lim ; add limit btfss STATUS, C ; C is set if > -Lim retlw -Lim ; return -ve limit addlw -Lim ; put back old value return ; and return it LimitNPos addlw 0x80 ; get orig value back addlw -Lim ; add - limit btfsc STATUS, C ; C is set if > Lim retlw Lim ; return limit addlw Lim ; get orig value back return endm ; ; Take a zero based value and create a dead band of +/- DEADB ; DeadN Macro Lim Local DeadNPos addlw 0x80 ; test to see if -ve btfss STATUS, C ; C set if -ve goto DeadNPos ; no go do +ve limit addlw 0x80 ; get orig value back addlw (Lim-1) ; add limit btfsc STATUS, C ; C is set if > -Lim movlw (Lim-1) ; Load value to return 0 (and set flags) addlw -(Lim-1) ; put back old value return ; and return it DeadNPos addlw 0x80 ; get orig value back addlw -Lim ; add - limit btfss STATUS, C ; C is set if > Lim movlw -(Lim) ; Load value to return 0 (and set flags) addlw Lim ; get orig value back return endm ; Helper macros for input smoothing. BAsically we hold the last n input values ; and perform a simple average on these. ; ; Helper routine for angle smoothing performs addition of 8 bit value ; to 16 bit accumulation ; Add816 Macro VarLo, VarHi addwf VarLo, F ; Add in value to low byte addcf VarHi, F ; take care of carry andlw 0x80 ; was the value -ve btfsc STATUS, Z return ; no so all done movlw 0xff ; yes so add in upper byte addwf VarHi, F ; to total return ; all done endm Clear16 Macro VarLo, VarHi clrf VarLo clrf VarHi return endm Div4 Macro VarLo, VarHi movlw 2 ; Round up btfsc VarHi, 7 ; is it -ve movlw 1 ; yes so round for -ve addwf VarLo, F ; Add in the low byte addcf VarHi, F ; take care of carry rrf VarHi, F ; divide hi by 2 rrf VarLo, F ; and Lo rrf VarHi, F ; and again rrf VarLo, F ; all done movf VarLo, W ; and return result return endm Div8 Macro VarLo, VarHi movlw 4 ; Round up btfsc VarHi, 7 ; is it -ve movlw 3 ; yes so round for -ve addwf VarLo, F ; Add in the low byte addcf VarHi, F ; take care of carry rrf VarHi, F ; divide hi by 2 rrf VarLo, F ; and Lo rrf VarHi, F ; divide hi by 2 rrf VarLo, F ; and Lo rrf VarHi, F ; and again rrf VarLo, F ; all done movf VarLo, W ; and return result return endm Div32 Macro VarLo, VarHi movlw .16 ; Round up btfsc VarHi, 7 ; is it -ve movlw .15 ; yes so round for -ve addwf VarLo, F ; Add in the low byte addcf VarHi, F ; take care of carry rrf VarHi, F ; divide hi by 2 rrf VarLo, F ; and Lo rrf VarHi, F ; divide hi by 2 rrf VarLo, F ; and Lo rrf VarHi, F ; divide hi by 2 rrf VarLo, F ; and Lo rrf VarHi, F ; divide hi by 2 rrf VarLo, F ; and Lo rrf VarHi, F ; and again rrf VarLo, F ; all done movf VarLo, W ; and return result return endm ; Helper macro to shuffle bytes in an array. Given a pointer in the FSR reg ; to byte n in an array copy yhe value from byte n-1 into it. Leave FSR pointing ; at byte n-1 ready for next shuffle Shuffle Macro decf FSR, F ; point at previous byte movf INDF, W ; get value from previous byte incf FSR, F ; point back to current byte movwf INDF ; and complete shuffle decf FSR, F ; and point at next byte to move endm ; Macro to take an array that is 4 bytes long and shuffle the values in it ; down. The current value in the W reg becomes the new first element Shuffle4 Macro Start movwf Temp ; Save new element zero for later movlw Start+3 ; get address of last element of the array movwf FSR ; and store it in pointer reg Shuffle ; Shuffle down 1 Shuffle ; 2 Shuffle ; 3 movf Temp, W ; get new value movwf Start ; and store it return ; all done endm ; Macro to take an array that is 2 bytes long and shuffle the values in it ; down. The current value in the W reg becomes the new first element Shuffle2 Macro Start movwf Temp ; Save new element zero for later movlw Start+1 ; get address of last element of the array movwf FSR ; and store it in pointer reg Shuffle ; Shuffle down 1 movf Temp, W ; get new value movwf Start ; and store it return ; all done endm ; ; Macros to provide a simple average of 16 8 bit values. ; Ave8 Macro VarLo, VarHi, VarCnt, VarCur Local A8Add816, A8Clear16, A8Div8 call A8Add816 ; Add in most recent element into total movf VarCur, W ; Assume current value decfsz VarCnt, F ; Decrement item count return ; cycle not yet complete return current movlw .16 ; reset counter ready movwf VarCnt ; for next cycle call A8Div8 ; get new average movwf VarCur ; and save it away call A8Clear16 ; start new cycle return ; and return new value A8Add816 addwf VarLo, F ; Add in value to low byte addcf VarHi, F ; take care of carry andlw 0x80 ; was the value -ve btfsc STATUS, Z return ; no so all done movlw 0xff ; yes so add in upper byte addwf VarHi, F ; to total return ; all done A8Clear16 clrf VarLo clrf VarHi return A8Div8 movlw 4 ; Round up btfsc VarHi, 7 ; is it -ve movlw 3 ; yes so round for -ve addwf VarLo, F ; Add in the low byte addcf VarHi, F ; take care of carry rrf VarHi, F ; divide hi by 2 rrf VarLo, F ; and Lo rrf VarHi, F ; divide hi by 2 rrf VarLo, F ; and Lo rrf VarHi, F ; divide hi by 2 rrf VarLo, F ; and Lo ; rrf VarHi, F ; and again ; rrf VarLo, F ; all done movf VarLo, W ; and return result return endm Ave8Init Macro VarLo, VarHi, VarCnt, VarCur clrf VarLo clrf VarHi clrf VarCur movlw 8 movwf VarCnt return endm IIR8Init Macro VarLo, VarHi, VarCur clrf VarLo clrf VarHi clrf VarCur return endm IIR8 Macro VarLo, VarHi, Temp, VarCur Local IIR8Add816, IIR8Div32, IIR8Nz subwf VarCur, W ; Get delta sublw 0 ; Change sign btfss STATUS, Z ; any diff? goto IIR8Nz ; Yes so go handle it movf VarLo, W ; If current value is same as new andlw 0x1f ; make sure we actually tend btfsc STATUS, Z ; towards zero rather than goto IIR8Nz ; sticking at edge cases andlw 0x10 movlw 0xff btfss STATUS, Z movlw 0x1 IIR8Nz call IIR8Add816 ; Add delta into average movf VarLo, W ; Get new low movwf VarCur ; and prepare for div movf VarHi, W ; do same with movwf Temp ; High byte call IIR8Div32 ; do the divide movwf VarCur ; and save the new current val. return IIR8Add816 Add816 VarLo, VarHi IIR8Div32 Div32 VarCur, Temp endm IIR8Get Macro VarLo, VarHi, Temp, Temp2 Local IIR8Div8 movf VarLo, W movwf Temp2 movf VarHi, W movwf Temp ifdef ACCEL ; Accel code uses unscaled values IIR8Div8 Div32 Temp2, Temp else ; normal code muliplies result by four IIR8Div8 Div8 Temp2, Temp endif endm ifndef TASKS #define TASKS 0 endif if TASKS > 0 ; Task definition macros ; The following macros are used to define time scheduled tasks. The events ; that can be used to trigger a task are given below and are scheduled by ; the TMR0 interrupt handler. ; NOTES ; Calls to these macros must appear before the Schedule function handler ; Only 7 tasks may be defined ; The priority of the tasks is defined by the order in which they appear. ; Task0 is a special task and runs when no other tasks are running. It should ; always be the first task defined. It is possible to have more than one ; set of tasks. ; TiState bits CBLOCK TiTask ; Bitmask of runnable tasks TiCount ; Timer counter ms in low nib 10 ms in upper TkState ; Dive control switch settings TkTick ; Tank control clock ENDC SCALE equ 0x2 ; TMR0 clock scale to get approx time of 2ms TICK equ .125 ; TMR0 count for 1ms TETick equ 7 variable TaskNo=0 variable TaskSet=0 variable TaskMask=0 variable TE1MS=0 variable TE10MS=0 variable TE20MS=0 variable TE50MS=0 variable TE100MS=0 ; Task Set definitions ; Start of Task set Tasks Macro TaskMask = 0 ; Reset mask for this set of tasks TaskLoop#v(TaskSet) ; And define task loop label endm ; End of Task Set EndTs Macro ScheduleTasks#v(TaskSet) btfss TiTask, TETick ; Should we schedule new tasks goto TaskLoop#v(TaskSet) ; No so go try and run any currently sceduled tasks call Schedule ; Re-evaluate run list movlw TaskMask ; Clear down any tasks not andwf TiTask, F ; in the current task set goto TaskLoop#v(TaskSet) ; and go try and run them EndTasks#v(TaskSet) ; Label for jump to end of current taks set movlw 0x7f ; Clear down schedule flag andwf TiTask, F ; All done in this set TaskSet += 1 ; Move on to next set endm ; Exit current task set ExitTs Macro goto EndTasks#v(TaskSet) ; Jump to end of current task set endm ; Define a new task (TEvent specifies the event that will make this task runnable) Task Macro TEvent TEvent = TEvent | (1< 0 ; ; Variables and definitions for processing standard R/C pulse inputs. ; CBLOCK RxTime ; RX time value RxStat ; RX Status bits RxPort ; State of RX port at int. time RxTOut ; Rx Timeout used to detect loss of signal ENDC RXCYCLE equ .20 ; Cycle time for Rx 20ms MIDPNT equ (TICK/2) ; Offset of midpoint from 1ms RXBASE equ (TICK+MIDPNT) ; Standard offset for midpoint in terms of pulse len MAXTHW equ MIDPNT ; Max stick throw value MINTHW equ -(MIDPNT) ; Min stick throw value MAXVAL equ (MAXTHW+.16) ; max valid value BADVAL equ (MAXVAL+1) ; invalid value WINSTRT equ (TICK-(MIDPNT/2)); Min time interval that will be accepted WINEND equ (MIDPNT*3) ; Max time interval that will be accepted LOWVAL equ -(MIDPNT/4) ; Stick on/off low value HIGHVAL equ MIDPNT/4 ; Stick on/off high value RXCHNS equ RXINPUT ; Number of RX chans in use RXGOOD equ (RXCHNS*4) ; Timeout inc for good reception RXBAD equ -1 ; Timeout inc for bad reception RXTO equ .50 ; Range for timeout RXTOIN equ -RXTO ; Initial value for RxTOut DEADB equ (MIDPNT/5) ; dead band size ; RX State values RXINT equ 4 RXT1 equ 5 RXT2 equ 6 RXSMASK equ 0x0f endif page