list E=0,C=120 __config _XT_OSC & _WDT_ON & _CP_OFF subtitl "Neptune - Submarine Controller" page ; 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 ; page ; Module: neptune.asm ; Author: Andy Shaw ; Date: 7/4/96 - 6/9/98 ; ; Copyright (C) Andy Shaw 1996, 1997, 1998 All Rights Reserved. ; ; Description ; ; This module provides control over a model submarine ; it takes input from 3 radio control signals - Planes, tank blow/flood, ; and depth control. It also has water detection, angle of sub, battery state and ; a proportional depth sensor inputs. The outputs are on off ; signals for the pump and valve and a standard radio control ; pulse output for control of the rear vanes servo. Thus this code ; provides control over the tanks and rear vanes. It provides manual ; control of the tanks, manual and automatic level control and automatic ; depth control. The rear vanes are adjusted to provide automatic ; level control. The unit detects loss of signal, low voltage and water ; ingress and will automatically surface in these cases. ; ; This code now supports two revisions of hardware (Neptune I & II) ; the tables below show the port usage for the two versions. Note ; that Neptune I uses a different type of level sensor and that it ; does not currently support voltage detection ; page ; Neptune I ; ; Input assignment ; Port_B bit 7 Radio control signal RX0 (Flood/Blow) ; Port_B bit 6 Radio control signal RX1 (depth) ; Port_B bit 5 Radio control signal RX2 (rear vanes) ; Port_A bit 0 Attitude D/A input (level control) ; Port_A bit 1 Presure sensor D/A (depth control) ; Port_A bit 2 Not Used ; Port_A bit 3 Not Used ; ; Port_B bit 2 Leak detect ; Port_B bit 3 Tank full sensor ; ; Output assignment ; Port_A bit 4 Status indicator ; Port_B bit 4 Rear vane control ; Port_B bit 0 Blow valve ; Port_B bit 1 Tank control ; page ; ; Neptune II ; ; Input assignment ; PORTB bit 7 Radio control signal RX0 (Flood/Blow) ; PORTB bit 6 Radio control signal RX1 (Rear vanes) ; PORTB bit 5 Radio control signal RX2 (Depth) ; PORTA bit 0 Current Battery State ; PORTA bit 2 Attitude D/A input (level control) ; PORTA bit 3 Presure sensor D/A (depth control) ; ; PORTB bit 0 Leak detect ; PORTB bit 1 Tank full sensor ; ; Output assignment ; PORTA bit 1 Level Control Pulse ; PORTA bit 4 Status indicator ; PORTB bit 4 Rear vane control ; PORTB bit 3 Blow valve ; PORTB bit 2 Tank control ; ; Things still to do....... ; 1) Proper timeout stuff for loss of signal. - Complete 3 June 97 ; 2) Automatic tank control ; 3) Battery monitoring - Complete 19 Oct 97 page ; ; Depth Control Calculations ; ; The Neptune hardware makes use of a SenSym SPX200AN pressure sensor. ; This is capable of measuring 0 - 200Kpa. The output voltage is ; approx 300uV/kPa when used with a 3V supply. In this application ; it is used at 5V giving 500uV/kPa. The output from the sensor is ; processed via a differential amp with a gain of 1000. Thus a 0-5V ; swing represents a pressure range of 10kPa which is approx 40" (1010mm) of ; water this is sampled by the ADC into a range of 0 - 256 and then ; smoothed to a range -64..64 thus a value of 1 in the depth calculations ; is approx 0.3125" (8mm). ; page include "common\as16c7x.inc" ; ; Configuration ; This file contains a number of configurable options. Some of these are provided ; to allow the operation of experimental sensor systems. Others are there for ; testing purposes. The following is the standard configuration for use with ; standard neptune hardware. The comments indicate what the DEFINED version of ; of the value enables/disables. a double ; ; indicates that by default this ; entry is NOT defined. ; #define TASKS 1 ; enable use of standard task switching system ; #define RXINPUT 3 ; number of active RC input chans ; #define SMOOTHPR ; enable use of smoothing for pressure sensor ; ; #define NORXTO ; Turn off RX timeout detection ; ; #define NODEPTH ; Turn off depth control ; ; #define ANGRA ; Use running average for attitude smoothing ; ; #define NODIFF ; Disable differential part of attitude control ; ; #define MEDIAN ; Use median filter for attitude smoothing ; ; #define IIR ; Use IIR filter for attitude smoothing ; ; #define ACCEL ; Enable use of accelerometer for angle sensing #define TASKS 1 #define RXINPUT 3 #define SMOOTHPR ;#define NORXTO ;#define NODEPTH ;#define ANGRA ;#define NODIFF ;#define MEDIAN #define IIR ;#define ACCEL page ;Port Usage ifdef N1 PACON1 equ B'00000000' ; RA0, RA1, RA2, RA3 analog PADIR equ B'11101111' ; RA4 output others input PACON0 equ B'01001001' ; Set internal osc, ch1, enable ADC PBDIR equ B'11101100' ; RB0, RB1, RB4 output others input VANES equ 4 LEAK equ 2 TANK equ 3 RX2 equ 5 RX1 equ 6 RX0 equ 7 RXCMASK equ 0xe0 DISP equ 4 ANGLE equ 0 DEPTH equ 1 CENTRE equ 2 ANGADJ equ 3 PUMP equ 1 VALVE equ 0 else PACON1 equ B'00000000' ; RA0, RA1, RA2, RA3 analog PADIR equ B'11101101' ; RA1 RA4 output others input PACON0 equ B'01001001' ; Set internal osc, ch1, enable ADC PBDIR equ B'11100011' ; RB2, RB3, RB4 output others input VANES equ 4 LEAK equ 0 TANK equ 1 RX2 equ 5 RX1 equ 6 RX0 equ 7 RXCMASK equ 0xe0 DISP equ 4 ANGLE equ 2 DEPTH equ 3 VOLTS equ 0 PULSE equ 1 PUMP equ 2 VALVE equ 3 endif page include "common\asgen.inc" ; ; ; Variables CBLOCK ; Start of Ram variable allocation Chan0 ; Chan 0 value Chan1 ; Chan 1 value Chan2 ; Chan 2 value TxVal ; Value to be transmitted DspTime ; Display code timer DspVal ; Display value GenStat ; Direction and general status bits Volts ; Scaled version of current battery level PDOld ; Previous angle for PD calc PDTick ; Timer for PD calc PDDTerm ; Diff term for PD calc Ang0 ; 4 byte sensed angle array Ang1 ; Ang2 ; Ang3 ; CurAng Prs0 ; Two byte sensed pressure array Prs1 ; Surface ; Pressure reading at surface DITerm ; Depth Integral term ENDC ifdef ACCEL CBLOCK Test ; Special test var for accel harware ENDC endif ; End of RAM variables page ; Constants DSPTICK equ ((.1000/4)/.20) TKON equ (.50/.10) ; Tank Pulse on time (ms) TKOFF equ (.1000/.100) ; Tank off time (ms) TKTOUT equ (.10000/.100) ; Tank timeout time (ms) ADTIME equ .3 ; Number of ticks to allow a/d inputs to settle SURFACE equ (MAXTHW-.16) ; value of pressure at surface SCOPE equ (SURFACE-.10) ; Pressure at Periscope depth DEPTHDB equ .1 ; dead band size for depth integral MAXDP equ .24 ; Max plane throw for depth errors VGOOD equ 2 ; increment for a good voltage VBAD equ -1 ; increment for a bad voltage VLOW equ .50 ; Limit at which to fail ifdef IIR MAXANG equ .31 else MAXANG equ .127 endif ; Masks MODEMSK equ 0x0f ; General switch bits SWPOS0 equ 0 SWPOS1 equ 1 SWPOS2 equ 2 ; Mode Switch setting bits all stored in low nibble of TkState NOTANK equ SWPOS0 BLOW equ SWPOS2 FLOOD equ SWPOS1 ; General status bits GSLEVEL equ 0 ; Submarine Level GSSETUP equ 1 ; Submarine in precise level setup zone GSLEAK equ 2 ; Leak detected GSNOSIG equ 3 ; Signal lost GSPANIC equ 4 ; General panic! GSDEPTH equ 5 ; At requested depth GSONTOP equ 6 ; Sub is surfaced GSVLOW equ 7 ; Low voltage detected ; Display values (i.e. number of times to flash the LED) DSNORM equ 1 ; normal DSLEVEL equ 2 ; Sub level DSSETUP equ 3 ; Setup point for level control DSDEPTH equ 4 ; at depth DSONTOP equ 5 ; on surface DSNOSIG equ 6 ; Signal Lost DSLEAK equ 7 ; leak detected DSVLOW equ 8 ; Low voltage page ; ; Normal Entry point ; org 0 goto Start ; ; Interrupt Entry point ; org 4 InterruptVector push ; Always save registers etc btfss INTCON, RBIE ; Are PORTB ints enabled? goto EndInterrupt ; No so all done btfsc INTCON, RBIF ; Is it a PORTB int ? goto ServicePORTB ; yes so go do it goto EndInterrupt ; No so all done page ; ; Interrupt service routines ; Note we avoid use of function calls to ensure minimal stack usage ; EndInterrupt pop ; restore things retfie ; return and re-enable ints ; ; Task definitions ; These are the tasks that make up the controller. They are all scheduled ; by timer events. They are defined in priority order MainTasks Tasks ; Begin set of tasks Task0 ; Define default task clrwdt ; make sure watch dog timer is clear call RxProcess ; try and turn input processing back on call TaskTick ; Check the system timer EndT0 Task TE10MS ; High priority 10ms tasks call TkFastTimeout ; do tank timeout stuff ifndef ANGRA call RxDisable ; Make sure we don't get interrupted call SenseAngle ; Read the angle sensor endif EndT Task TE100MS ; High priority 100ms tasks call DoPlanesPID ; Run the PID algorithm call CheckVoltage ; Check system voltage level EndT Task TE50MS ; High Priority 50ms tasks ifdef ANGRA call RxDisable ; Make sure we don't get interrupted call SenseAngle ; Read the angle sensor endif call DoPlanes ; Compute new planes position EndT Task TE20MS ; High priority 20ms tasks call RxDisable ; disable Rx Processing call RxTimeout ; Check for Rx timeout call SetTxOutput ; Do output stuff EndT Task TE100MS ; Low priority 100ms tasks call TkSlowTimeout EndT Task TE20MS ; Low priority 20ms tasks call TkProcess ; Work out what to do with tanks ifndef ACCEL call CheckLeak ; and check for leaks NB Leak input used for accel hardware endif call CheckPanic ; Look for any kind of problem call DisplayOutput ; update status display EndT EndTs ; End of task definitions include "common\asgen.asm" ; ; Main entry point chack to see if we have had a WDT reset if we have enter ; a special routine to display trace information on the LED. Note in release ; builds of the firmware we do not do this and treat WDT reset as a normal start. ; Start goto NormalStart ; btfss STATUS, NOT_PD ; goto NormalStart ; btfss STATUS, NOT_TO ; goto TimeOutReset ; goto NormalStart NormalStart clrwdt ; make sure watch dog timer is clear call SystemInit ; Initialise system. call TasksInit ; Initialise tasking system call TMR0Init ; Initialise the TMR0 call SetPORTA ; Setup PORTA call SetPORTB ; setup the I/O ports on B and allow ints. call DisplayInit ; Initialise the display system. call TkInit ; Setup the tank control system. call PlanesInit ; Initialise planes control call SetDefaults ; Get zero position for RX inputs call RxInit ; Initialise Rx system. goto MainTasks ; Go Start things going ; ; Entry Point for Watch Dog Timer Reset ; ifdef TIMEOUT TimeOutReset clrf TiTask ; No tasks runnable clrf GenStat ; make sure general state is clear call TMR0Init ; Initialise the TMR0 call SetPORTA ; Setup PORTA call SetPORTB ; setup the I/O ports on B and allow ints. movlw 1 ; set the movwf DspTime ; Set display timer movwf DspVal ; and value movf ToStat, W ; and display the reason why movwf GenStat ; have had a reset TimeOutLoop1 clrwdt ; make sure watch dog timer is clear btfss TiCount, 5 goto TimeOutLoop1 call DoDisplay ; update status display TimeOutLoop2 clrwdt btfsc TiCount, 5 goto TimeOutLoop2 goto TimeOutLoop1 endif ; ; Actual limit routines ; LimitVolts LimitN VLOW LimitDepthThrow LimitN MAXDP LimitAngleThrow LimitN MAXANG DepthDeadBand DeadN DEPTHDB ifdef ACCEL AccDB DeadN 3 endif ; Helper macros for input smoothing. BAsically we hold the last n input values ; and perform a simple average on these. ; ; Actual routines we make use of Temp and Temp2 as the 16 bit working registers Add16Temp Add816 Temp, Temp2 Smooth16Temp Div4 Temp, Temp2 Clear16Temp Clear16 Temp, Temp2 ; Actual Shuffle code for the Angle and Pressure arrays. Note that we have 4 Angle samples ; and only two pressure samples. ShuffleAngle Shuffle4 Ang0 ShufflePrs Shuffle2 Prs0 ifdef IIR AveAngle IIR8 Ang0, Ang1, Ang2, Ang3 AngleInit IIR8Init Ang0, Ang1, Ang3 AngleGet IIR8Get Ang0, Ang1, Temp, Temp2 else AveAngle Ave8 Ang0, Ang1, Ang2, Ang3 AngleInit Ave8Init Ang0, Ang1, Ang2, Ang3 endif ; ; ; Functions ; ; Setup PORTA for two analog input chans and one digital output ; SetPORTA bsf STATUS, RP0 ; select bank 1 movlw PACON1 ; get type of pins movwf ADCON1 & 0x7f ; set them movlw PADIR ; get direction movwf TRISA & 0x7f ; Set state bcf STATUS, RP0 ; select bank 0 movlw PACON0 ; get A/D control movwf ADCON0 ; set bits retlw 0 ; all done ; ; Setup PORTB for various input/output settings ; SetPORTB bsf STATUS, RP0 ; select bank 1 movlw PBDIR ; get direction movwf TRISB & 0x7f bcf STATUS, RP0 ; select bank 0 clrf PORTB movf PORTB, W bcf INTCON, RBIF ; Clear port B interrupt flag retfie ; return and enable interrupts ; Initialise the basic system. Clear watchdog timer. Clear status and get ready to run. ; SystemInit clrf GenStat ; Set intial state clrf Volts ; and volts return ; ; Setup default values for stick cntre positions etc. We set things up ; such that internally a value of 128 represents centre stick and full ; throw is +32 to -32 ; SetDefaults return ; ; Rx input event handlers called when we obtain new R/C input for each chan ; Rx0NewValue movwf Chan0 return Rx1NewValue movwf Chan1 return Rx2NewValue movwf Chan2 return RxSignalOK bcf GenStat, GSNOSIG; clear no signal bit return RxSignalLost bsf GenStat, GSNOSIG; Set no signal bit return ; ; Helper function to sample the A/D converter. Assumes that the desired ; channel has already been setup on entry. Waits for ADTIME TICKS for the ; input value to be captured then performs the conversion. Returns the value ; in W SampleAD clrwdt ; make sure we do not cause a timeout movf TMR0, W ; get current time movwf Temp ; and save ADCapture movf Temp, W ; get start time subwf TMR0, W ; subtract from current time sublw ADTIME ; compare against capture period btfsc STATUS, C ; Is ElapsedTime < ADTIME goto ADCapture ; Yes So continue to wait bsf ADCON0, GO ; Start the conversion bcf PORTA, PULSE AdConvert btfsc ADCON0, GO ; Complete yet ? goto AdConvert ; no movf ADRES, W ; complete, now get result btfsc STATUS, Z ; is it zero movlw 1 ; yes make it 1 addlw -.128 ; make zero based return ; and return the result ; ; Synchronize with TMR0 ; SyncTMR0 WaitZero btfss TMR0, 0 ; We need to sync with timer 0 goto WaitZero ; so wait for a zero value WaitOne btfsc TMR0, 0 ; Then sync on rising edge goto WaitOne ; then rest of code will be in sync return ; ; Check the current voltage of the system. The Neptune II hardware has ; a potential divider network that presents 1/3 of the main supply voltage ; to a multi-turn pot. This should be set such that if the supply voltage ; falls below 10 volts then a value of less than 2.5 volts is read on pin 0 ; the following code means that if over 100 successive readings are less ; than 2.5 volts then the systems will declare a low voltage situation ; CheckVoltage ifdef N2 bcf ADCON0, CHS1 ; Select Channel 0 bcf ADCON0, CHS0 call SampleAD ; and get the value andlw 0x80 ; is the sign bit set? btfss STATUS, Z ; if it is Z will be clear goto VoltsLow ; we have a low voltage VoltsOK movf Volts, W ; get current voltage state addlw VGOOD ; inc with good value call LimitVolts ; Limit range movwf Volts ; and save addlw -VLOW ; are we at the good value threshold btfsc STATUS, Z ; if we are Z will be set bcf GenStat, GSVLOW ; clear low volts bit return ; all done VoltsLow movf Volts, W ; get current voltage state addlw VBAD ; inc with bad value call LimitVolts ; Limit range movwf Volts ; and save addlw VLOW ; Low voltage threshold? btfsc STATUS, Z ; if so Z will be set bsf GenStat, GSVLOW ; Set no signal bit endif return ; ; Sort out what to do with the rear planes. ; We have three possible sources of rear plane control. ; 0) Lost signal ; 1) Depth control from the presure sensor. ; 2) Attitude control from the angle sensor. ; We treat these in the priority order given above. ; ; First we have the helper routines ; ; ; ; Planes control system initialisation. ; PlanesInit clrf Ang0 ; Setup two byte smoothed angle clrf Ang1 clrf Ang2 clrf Ang3 clrf Prs0 ; and same for pressure clrf Prs1 clrf DITerm clrf Surface ; Setup initial value for Surface offset clrf PDOld ; set up the PD controller clrf PDDTerm ; values movlw .10 ; Set up differential timer movwf PDTick ; to be 1000ms clrf Chan1 ; set zero for Chan1 clrf Chan2 ; ditto for Chan2 clrf TxVal ; and output value return ; ; Pressure sensing code. basically read the current value from the pressure ; sensor via the AD convertor. To improve performance when in a slightly ; noisy system we use a simple smoothing system. ; SensePressure ifdef N1 bcf ADCON0, CHS1 ; Select Channel 1 bsf ADCON0, CHS0 else bsf ADCON0, CHS1 ; Select Channel 3 bsf ADCON0, CHS0 endif call SampleAD ; and get the value ifdef SMOOTHPR call ShufflePrs ; and add it to the array else call Div2 ; get into range and lose 1 bit of noise movwf Prs0 endif return ; ; Angle sensing code. We make use of a liquid based sensor. To sense it we ; first have to generate a pulse and then sample via the AD. This device tends ; to generate noise so we employ a smoothing scheme. ; SenseAngle btfsc DspVal, 0 ; are we doing a gap ? bsf PORTA, DISP ; no turn LED off ifdef N1 bcf ADCON0, CHS1 ; Select Channel 0 bcf ADCON0, CHS0 call SampleAD ; and get the value call LimitAngleThrow ; Limit it else ifdef ACCEL ; Special case code for experimental acceleromter based tilt sensor btfsc PORTB, LEAK ; Are we in low part of cycle? return ; no so wait for it AccWait btfss PORTB, LEAK ; Wait for rising edge goto AccWait ; go and check again bsf PORTB, PUMP ; indicate edge seen movlw .25 ; New delay to get to close to the movwf Temp ' end of the data value AccDelay nop nop nop nop incfsz Temp, F ; Have we completed the delay yet? goto AccDelay bcf PORTB, PUMP ; turn of delay indicator ; unrolled loop looking for end of pulse (unrolling gives more accuracy) AccTime btfsc PORTB, LEAK incf Temp, F btfsc PORTB, LEAK incf Temp, F btfsc PORTB, LEAK incf Temp, F btfsc PORTB, LEAK incf Temp, F btfsc PORTB, LEAK incf Temp, F btfsc PORTB, LEAK incf Temp, F btfsc PORTB, LEAK incf Temp, F btfss PORTB, LEAK goto AccComplete incfsz Temp, F goto AccTime AccComplete movf Temp, W AccTest2 subwf Test, W ; Compare to previous ; call AccDB btfss STATUS, Z ; Use trace hardware to capture any jitter AccTest nop movf Temp, W ; get result movwf Test ; save for next time btfsc STATUS, Z ; is it zero movlw 1 ; yes make it 1 addlw -.128 ; make zero based call LimitThrow ; Limit it and call AveAngle ; do average return else bsf ADCON0, CHS1 ; Select Channel 2 bcf ADCON0, CHS0 call SyncTMR0 ; Ensure consistant pulse len bsf PORTA, PULSE ; turn on the angle sensor call SampleAD ; Go get the value bcf PORTA, PULSE ; turn it off call LimitAngleThrow ifndef IIR call AveAngle call SyncTMR0 bsf PORTA, PULSE call SampleAD bcf PORTA, PULSE call LimitAngleThrow call AveAngle call SyncTMR0 bsf PORTA, PULSE call SampleAD bcf PORTA, PULSE call LimitAngleThrow endif endif ifdef ANGRA call ShuffleAngle ; Store it in buffer else call AveAngle ifdef MEDIAN movf Ang3, W subwf CurAng, W btfsc STATUS, Z goto MNoChange andlw 0x80 movlw -1 btfss STATUS, Z movlw 1 MNoChange addwf CurAng, W call LimitThrow movwf CurAng endif endif endif btfsc DspVal, 0 ; are we doing a gap ? bcf PORTA, DISP ; no turn LED on retlw 0 ; ; Function to return zero based angle ; GetAttitude ifdef ANGRA call Clear16Temp ; Clear things ready to go movf Ang0, W ; get first element call Add16Temp ; add it in movf Ang1, W ; 2nd call Add16Temp movf Ang2, W ; 3rd call Add16Temp movf Ang3, W ; 4th call Add16Temp call Smooth16Temp ; and get average else ifdef MEDIAN movf CurAng, W else ifdef IIR call AngleGet else movf Ang3, W endif endif endif sublw 0 ; get direction correct return ; all done ; ; Same for presure. Note that we also scale the pressure range by 2. We do this ; by using only two samples as input to the divide by 4 averaging code. ; GetDepth ifdef SMOOTHPR call Clear16Temp ; Clear things down movf Prs0, W ; get first reading call Add16Temp ; add it in movf Prs1, W ; get 2nd reading call Add16Temp ; add it in call Smooth16Temp ; get average else movf Prs0, W endif return ; ; GetPanicPlanes ; return a value for dealing with loss of signal, basically try and get to ; the surface asap ; GetPanicPlanes btfss GenStat, GSPANIC; Have we lost control ? retlw 0 ; no so do normal stuff retlw MINTHW ; set to force nose up ; ; GetLevelPlanes ; ; For the main plane control we run a PD (proportional plus differential) ; system. This proportional term ensures reasonably rapid response to ; major changes the differential term ensures that we do not have to big ; an overshoot. ; ; We have the following helper functions ; ; CalcDiffTerm ; Calculate the Differential term of the PD calculation we have a sample ; period of 100ms. ; CalcDiffTerm ifdef NODIFF clrf PDDTerm return endif call GetPropTerm ; get current prop term movwf Temp2 ; save it movf PDOld, W ; get previous value subwf Temp2, W ; and do subtraction ; btfsc STATUS, Z ; Test code to allow trace hardware ; goto DiffTest2 ; to be used to log ; nop ; changes in diff term ; nop ; ;DiffTest ; ; nop ; ;DiffTest2 ; call LimitThrow ; limit it call Mult2 ; double it call LimitThrow ; limit result movwf PDDTerm ; PDD = 2*(P - OldP) movf Temp2, W ; get current position movwf PDOld ; set up for next time return ; all done ; ; Following is test code for detrmining correct Diff term movf Chan2, W ; get control chan call Div2 call Div2 call Div2 ; in range -8..+8 call Div2 ; in range -4..+4 addlw 4 ; movwf Temp2 movlw 0 btfsc Temp2, 7 goto Complete movf PDDTerm, W DecLoop movf Temp2, F btfsc STATUS, Z goto Complete call Mult2 call LimitThrow decf Temp2, F goto DecLoop Complete call LimitThrow movwf PDDTerm return ; ; GetPropTerm ; return the proportional term of the PD calculation ; basically return a value that is proportional to the current angle of ; the sub. ; GetPropTerm call GetAttitude call LimitThrow return ; ; ; GetLevelPlanes ; Return a value that will try and level the sub. Basically calculate ; and combine the proportional and differential terms and combine these ; with a manual offset to allow over-riding of the system by user control. ; GetLevelPlanes call GetPropTerm ; get the proportional term addwf Chan1, W ; add in manual offset value call LimitThrow ; and make sure in range movwf Temp2 ; save for return call Abs ; get abs version movwf Temp ; save for later sublw .5 ; are we in level zone ? btfss STATUS, C ; if (abs(Angle) < 5) C is set goto EndLevelPlanes ; no so all done bsf GenStat, GSLEVEL; say we are level movf Temp, W ; get abs value sublw .2 ; are we in the setup zone? btfsc STATUS, C ; if (abs(Angle) < 2) C is set bsf GenStat, GSSETUP; say we are in the zone EndLevelPlanes movf PDDTerm, W ; get diff term addwf Temp2, W ; merge it in call LimitThrow ; limit the whole thing return ; and return it ; ; GetDepthPlanes ; We make use of a P (proportional) PID calculation to provide ; drive to the planes for automatic depth control. ; ; Helper routines ; ; GetDepthError ; ; Return a value that is proprtional to the error term between the manually ; selected depth (chan2) and the current pressure sensor depth reading. Setting ; the depth to one extream of the range disables automatic depth control, ; the other extream automatically selects periscope depth ; GetDepthError call GetDepth ; Get current depth movwf Temp ; save it for later btfsc GenStat, GSONTOP; are we on the surface? goto NoDepthControl ; yes so do nothing movf Chan2, W ; Get RX value call LimitThrow ; limit it in size movwf Temp2 ; save for later addlw -MAXTHW ; are we at upper limit ? btfsc STATUS, Z ; if so we will have a value of 0 goto NoDepthControl ; at upper limit do nothing movf Temp2, W ; get again addlw MAXTHW ; lower limit movlw SCOPE ; goto periscope depth btfsc STATUS, Z ; are we at limit movwf Temp2 ; yes so set target to be periscope depth movf Temp, W ; get current depth addwf Surface, W ; adjust to be relative to surface subwf Temp2, W ; get error term call LimitThrow ; limit it movwf Temp2 ; save for later call Abs ; get abs value sublw .1 ; are we in the setup zone? btfsc STATUS, C ; if (abs(error) < 2) C is set bsf GenStat, GSDEPTH; say we are in the zone movf Temp2, W ; get value back return NoDepthControl retlw 0x80 ; return special value to indicate no control ; ; CalcDepthInt ; Calculate the integral error term for the current depth setting. Basically we ; set MAXDP or -MAXDP if we are currently outside of the target zone. ; CalcDepthInt call GetDepthError ; get current error call DepthDeadBand ; allow a dead band andlw 0xff ; Make sure bits are set btfsc STATUS, Z ; if in dead band we are OK return andlw 0x80 movlw MAXDP ; assume +ve btfss STATUS, Z ; check -ve movlw -MAXDP movwf DITerm ; set value return ; ; Return the proportial term of the depth input. Basically we return a value based ; upon the current depth error. ; GetDepthPlanes ifdef NODEPTH retlw 0 endif call GetDepthError ; get proportional error term addlw 0x80 ; Are we disabled val == 0x80 if we are btfsc STATUS, Z ; Z will be set if we are disabled retlw 0 ; disabled addlw -0x80 ; get back orig value addwf DITerm, W ; Add in integral term call LimitThrow ; limit it ifndef N1 sublw 0 ; Invert direction on Neptune 2 endif return ; all done ; ; Routine called to perform PID differential and integral calcs ; DoPlanesPID call SensePressure incf PDTick, F ; Increment counter btfss PDTick, 0 ; Is this an even tick? call CalcDiffTerm ; Yes so do differential part movf PDTick, W ; get current value addlw -.4 ; 0.5 seconds? btfsc STATUS, Z ; if it is Z will be set clrf DITerm ; clear Integral value down addlw -.26 ; 3 seconds btfss STATUS, Z ; if at 3 seconds point Z is set return ; not there yet! clrf PDTick ; reset counter btfss GenStat, GSONTOP; are we on the surface ? call CalcDepthInt ; no so calc new int term btfss GenStat, GSONTOP; are we on the surface? return ; no so all done call GetDepth ; get current depth sublw SURFACE ; and calculate offset movwf Surface ; and store away return ; ; ClearPlaneStatus ; Clear down all the status bits relationg to planes ; ClearPlaneStatus bcf GenStat, GSLEVEL; say not bcf GenStat, GSSETUP bcf GenStat, GSDEPTH return ; ; Calculate the actual value for the rear planes. Take into account, manual, depth, ; and attitude inputs. ; DoPlanes call ClearPlaneStatus; Clear down status info clrf TxVal ; start off level call GetPanicPlanes ; Get loss signal value if any addwf TxVal, F ; and set it btfss STATUS, Z ; did we have a value return ; yes so all done call GetDepthPlanes ; Get depth control planes input addwf TxVal, F ; merge it in call GetLevelPlanes ; Get Attitude control addwf TxVal, F ; and add it in EndPlanes movf TxVal, W ; get plane control call LimitThrow ; make sure it is valid movwf TxVal ; and save it away return ; all done ; ; Send the current value from TxVal to the servo by generating a ; 1-2mS pulse. This routine should be called once every 20mS or so ; to ensure correct timing note TxVal is used as a working variable by ; this code and so should not be changed while it is executing. ; ; Note This routine is implemented as two loops to avoid overflow problems ; when creating pulses which are >= 2ms in length. The first loop waits ; for approx 0.5ms with ints enabled followed by the second loop which waits ; 0.4 - 1.1 ms with ints disabled. This provides an accurate timed period ; with minimal problems from ints etc. ; SetTxOutput movlw RXBASE ; Get current mid point base addlw -.64 ; Take off fixed period portion addwf TxVal, F ; use to convert TxVal to time units movf TMR0, W ; get current time movwf Temp ; save for later bsf PORTB, VANES ; turn on output movwf TMR0 ; start timer (clears pre-scaler) TxLoop1 ; fixed period loop for .64 units movf Temp, W ; get start time subwf TMR0, W ; take from current time sublw .64 ; have we done enough btfsc STATUS, C ; is W > 64 goto TxLoop1 ; No movlw .64 ; add on the fixed time addwf Temp, F ; to our start time TxLoop2 movf Temp, W ; get start time subwf TMR0, W ; take off from current time subwf TxVal, W ; have we done enough btfsc STATUS, C ; is W > TxVal goto TxLoop2 ; No bcf PORTB, VANES ; yes all done clear output movlw RXBASE ; restore the value addlw -.64 ; add in the fixed part subwf TxVal, F ; of TxVal retlw 0 ; and return ; ; Turn the value in the W register into a three position switch ; value. The result is returned in the W register. ; GetSwitchVal call DeadBand ; allow for a zero area andlw 0xff ; make sure status bits are set btfsc STATUS,Z ; is it 0 retlw 1<