' ----------------------------------------------------------------------------- ' ' File: GPS_v90.pbp ' Date: 14-Jan-2007 ' ' PICBASIC PRO program to read GPS on LAB-X1 using Interupts. ' ' Uses only timer-1 interupts, every 10ms. Start either Pulse or just cycle. ' Gyro AD conversion used to start every-other 10 ms frame. ' Servo Pulses are going out at the correct rate. ' ' Compass and FPU routines are installed and working. ' Assembles into xxx bytes of code. ' ' ----------------------------------------------------------------------------- @ Device HS_OSC ' High Speed Crystal @ Device WDT_ON ' Watch Dog timer ON @ Device PWRT_ON ' Power Reset timer ON @ Device BOD_ON ' Brown out Protection ON @ Device LVP_OFF ' Low voltage programming capability @ Device CPD_OFF ' Code Protection OFF ' Shiftin Modes Symbol MSBPRE = 0 ' MSB first, sample before clock Symbol LSBPRE = 1 ' LSB first, sample before clock Symbol MSBPOST = 2 ' MSB first, sample after clock Symbol LSBPOST = 3 ' LSB first, sample after clock ' Shiftout Modes Symbol LSBFIRST = 0 ' LSB first Symbol MSBFIRST = 1 ' MSB first define __16F877A 1 define OSC 20 OSC_SPEED CON 20 DEFINE INTHAND DO_ISR '--------------------- Define LCD registers and bits -------------------------- DEFINE LCD_DREG PORTD DEFINE LCD_DBIT 4 DEFINE LCD_RSREG PORTE DEFINE LCD_RSBIT 0 DEFINE LCD_EREG PORTE DEFINE LCD_EBIT 1 DEFINE HSER_BAUD 4800 'DEFINE HSER_RCSTA 90h DEFINE HSER_TXSTA 20h DEFINE HSER_SPBRG 64 '------- Compass, FPU & I/O Pins ------------------------------------------- GPS VAR PORTA.4 ' GPS Serial data input pin DinDout VAR PORTB.0 ' Compass Data I/O pin Clk VAR PORTB.1 ' Compass Clock pin LED VAR PORTB.2 ' LED signals valid GPS FpuIn var PORTB.5 ' Floating Point Processor FpuClk var PORTB.6 ' FPU clock FpuOut var PORTB.7 ' FPU data pin RudderIn VAR PORTC.0 ' Ch4 Rudder input from Rcvr RudderOUT VAR PORTC.1 ' Output pulses to Rudder Servo ' C2 is Buzzer on LAB-X board En VAR PORTC.3 ' Enable pin for Compass Ch5ApPIN VAR PORTC.4 ' Ch5 Gear = AutoPilot ON/OFF ' PortC.6 & 7 are Rs232 pins ' ------------------------------------------------------- GIE VAR INTCON.7 ' Global Interupt Enable PEIE VAR INTCON.6 ' Peripherial Equip Enable ADIE VAR PIE1.6 ' AD Done Interupt Enable RCIE VAR PIE1.5 TMR1IE VAR PIE1.0 ' PIE1 address is in Bank1 TMR2IE VAR PIE1.1 TMR1ON VAR T1CON.0 ' Timer-1 ON/OFF bit TMR2ON VAR T2CON.2 '-------------------- uM-FPU register definitions ----------------------------- xVal CON 1 yVal CON 2 Distance CON 1 ' Distance to Waypoint Bearing CON 3 ' Bearing to Waypoint p1 con 3 ' must be the same as Bearing p2 con 4 p3 con 5 p4 con 1 dLat CON 6 ' (Lat2-Lat1) ^ 2 (squared) dLon CON 7 ' (Lon2-Lon1) ^ 2 Lat1 CON 8 ' This is where we are Lon1 CON 9 Lat2 CON 10 ' This is waypoint we're going towards Lon2 CON 11 eRad CON 12 ' Radius of Earth = 6378 Km (*PI) F57_29 CON 13 ' 180 / pi = 57.2958 F360 CON 14 ' 360.0 F_PI CON 15 ' Constant pi = 3.1415926 include "umfpuV2-spi.inc" ' include the uM-FPU support routines ' Constants for Compass Reset CON %0000 Measure CON %1000 Report CON %1100 Ready CON %1100 NegMask CON %1111100000000000 ' --------------------------------------------------------------------------- wsave VAR BYTE $20 SYSTEM ; Save W reg location if in bank0 wsave1 VAR BYTE $A0 SYSTEM ; Save W reg location if in bank1 wsave2 VAR BYTE $120 SYSTEM ; Save W reg location if in bank2 wsave3 VAR BYTE $1A0 SYSTEM ; Save W reg location if in bank3 ssave VAR BYTE Bank0 SYSTEM ; Save location for STATUS reg psave VAR BYTE Bank0 SYSTEM ; Save location for PCLATH reg ' --------------------------------------------------------------------------- ' Allocate variables ' --------------------------------------------------------------------------- GPSMAXDATA CON 72 GPSData var Byte[GPSMAXDATA] Bank1 sLen VAR BYTE Bank1 inChr VAR BYTE Bank1 T1_10ms_VALUE VAR WORD Bank0 Pulse_ON_Time VAR WORD Bank0 Pulse_OFF_Time VAR WORD Bank0 SteeringTimer VAR BYTE Bank0 Bank1Flags VAR BYTE Bank1 Bank0Flags VAR BYTE Bank0 StatFlag1 VAR BYTE Bank0 New_GPS_Data VAR Bank1Flags.0 StrInProgress VAR Bank1Flags.1 Valid_GPS_Data VAR Bank1Flags.2 LCD_Timer VAR Bank1Flags.3 OverRunError VAR Bank0Flags.0 CkSumValid VAR Bank0Flags.1 Radio_ON_Flag VAR Bank0Flags.2 AD_CycleFlag VAR StatFlag1.1 PinStatus VAR StatFlag1.2 PulseEnable VAR StatFlag1.3 ' ------------------------------------------------------- cStats Var Byte ' Compass Status - tmp xAxis Var Word yAxis Var Word cHeading VAR WORD ' Current Heading - from Compass cCor VAR WORD ' Compass correction factor [-25..0..25] wpHeading VAR WORD ' Desired Heading to Way-Point wpDist VAR WORD ' Distance to Way-Point hError VAR WORD iError VAR WORD PrevError VAR WORD LastPosition Var WORD Position VAR WORD Center VAR WORD trvlMin var WORD trvlMax var WORD FPU_Reg VAR Byte Degs VAR Byte Minutes VAR Byte mFract VAR Word glat1 var word ' Latitude glat2 var word glat3 var word glon1 var word ' Longitude glon2 var word glon3 var word gAlt var word ' Altitude in meters gCMG VAR WORD ' Course Made Good from GPS wp1_Lat1 VAR WORD ; stored way point 1 locations wp1_Lat2 VAR WORD wp1_Lat3 VAR WORD wp1_Lon1 VAR WORD wp1_Lon2 VAR WORD wp1_Lon3 VAR WORD wp2_Lat1 VAR WORD ; stored way point 2 locations wp2_Lat2 VAR WORD wp2_Lat3 VAR WORD wp2_Lon1 VAR WORD wp2_Lon2 VAR WORD wp2_Lon3 VAR WORD Gain var BYTE ' used to control correction amt PicOSC VAR BYTE pwiFactor VAR WORD tmpWord VAR WORD fVal var WORD ' tmp vars used in string parsing f var BYTE fNum var BYTE Cksum VAR f GPScs VAR fNum ' Overlap data - not used at same time fStart var BYTE fEnd var BYTE dPlace var BYTE nChrs var BYTE cStart var BYTE wpNum VAR BYTE tmpByte var BYTE ' used in check sum calcs i VAR BYTE ' tmp loop variable w var WORD ApON var BIT ' AutoPilot On/Off SteerDir var BIT PrevDir var BIT SignBit Var Bit Pause 500 GOTO Init_Seq ' ================================================================================= ' Do_ISR ' Interupt Service Routine ' Handle all the Interups ' ================================================================================= Asm DO_ISR IF (CODE_SIZE <= 2) movwf wsave ; save W register swapf STATUS,W ; save STATUS reg clrf STATUS ; switch to bank 0 movwf ssave ; save STATUS reg to area in Bank 0 movf PCLATH,W ; save Program Counter movwf psave ; save in Bank 0 location EndIF ; clrf PCLATH ; Page 0 - regardless of crnt page ; ========================================================================== ; See who caused the Interupt. ; Vector to the correct routines ; BTFSC PIR1, RCIF ; Rx Serial Interupt? ; GOTO RC_INT ; Yes - go to receive chr routine BTFSC PIR1,TMR1IF ; Timer1 overflow? GOTO T1_INT ; Yes - go to Timer1 routine ; BTFSC PIR1, ADIF ; A/D done Interupt? ; GOTO AD_INT ; Yes - go to A/D done routine CLRF PIR1 GOTO ISR_Done ; Error - never handled interupt ; =========================================================================== ; UART Chr waiting in Rx Register - Read chr to clear flag. ; Save chr in GPSData string array. Inc array len. ; Check for over-run errors, and check for piggy-back chrs ; waiting to be read. (just read them now) RC_INT ; UART Serial Chr ready Movf RCREG,w ; Read chr from UART BSF STATUS, RP0 ; Switch to Bank1 Movwf _inChr ; save chr just read BTFSS _Bank1Flags,0 ; if New_GPS_Data Flag is set, Goto Rx0 ; don't over-write the string in GPSData BCF STATUS, RP0 ; Back to Bank0 Goto ISR_DONE Rx0 Movlw 72 ; check for max str len = 72. Subwf _sLen,w BTFSC Status,C ; if C is set, sLen is 72 or 73.. too long CLRF _sLen ; set string index to zero Rx1 Movlw 13 ; check for end of line. Subwf _inChr,w BTFSS STATUS,z ; if replace with zero Goto Rx2 ; jump if not CLRF _inChr ; replace w/null-zero BCF _Bank1Flags,1 ; clear string-in-progress flag Movlw 'R' ; We only want $GPRMC messages Subwf _GPSData+3,w BTFSS STATUS,Z Goto Rx3 ; not the GPS data we're looking for - jump Movlw 'M' ; We only want $GPRMC messages Subwf _GPSData+4,w BTFSC STATUS,Z BSF _Bank1Flags,0 ; set New_GPS_Data flag - Goto Rx3 ; it's the GPS message we're looking for. Rx2 Movlw 36 ; check for $ chr Subwf _inChr,w BTFSS STATUS,z ; if $ start new string Goto Rx3 ; jump if normal chr CLRF _sLen ; set string index to zero BSF _Bank1Flags,1 ; set string-in-progress flag Rx3 Movlw _GPSData ; point to string array Movwf FSR ; setup indirect addressing Movf _sLen,w ; get index into array Addwf FSR,f ; point to end of string Movf _inChr,w ; get chr just read Movwf INDF ; save chr in array INCF _sLen ; inc array length BCF STATUS, RP0 ; Back to Bank0 BTFSS RCSTA,OERR ; Check for Over-run Error GOTO Check_Again BCF RCSTA,CREN ; clear the error BSF RCSTA,CREN BSF _OverRunError ; signal outside world of error Check_Again BTFSC PIR1, RCIF ; see if another chr is waiting Goto RC_INT GOTO ISR_Done ; =========================================================================== ; Timer1 Interupts ; Controls Servo Pulse Widths and start of AD conversions. ; There are 3 different timer states to each 20ms frame: PulseON, PulseOFF, 10msWait T1_INT ; Timer1 Overflow BCF T1CON,TMR1ON ; Turn OFF timer1 BTFSC _PinStatus ; Is Pin currently High? Goto TurnOFF ; Yup - turn off Pulse - reload w/OFF timer BTFSS _AD_CycleFlag ; Is it time to do Gyro AD or turn on Servo Pulse? Goto TurnON ; jump to turn on servo pulse ; BTFSS ADCON0,2 ; Don't start new conversion if prev one hasn't finished. ; BSF ADCON0,2 ; Start AD conversion - Set GO/DONE bit BCF _AD_CycleFlag ; Next interupt is start of Servo Pulse MOVF _T1_10ms_VALUE,W ; Reload 10ms value into Timer-1 ADDWF TMR1L,F BTFSC STATUS,C INCF TMR1H,F MOVF _T1_10ms_VALUE+1,W ADDWF TMR1H,F GOTO Timer1Done ; -------------------------------------------- ; Start 1.5ms Servo Pulse TurnON BSF _RudderOUT ; Set Servo Pin High BSF _PinStatus ; Flag copies Pin status INCF _SteeringTimer,F ; Inc timer at 50 Hz MOVF _Pulse_ON_Time,W ; Load Pulse_ON value into Timer-1 ADDWF TMR1L,F BTFSC STATUS,C INCF TMR1H,F MOVF _Pulse_ON_Time+1,W ADDWF TMR1H,F GOTO Timer1Done ; -------------------------------------------- ; Servo Pulse is High - Turn it OFF ; Reload Timer-1 with OFF time to next 10ms frame ; next event is start of Gyro AD conversion TurnOFF BCF _RudderOUT ; Turn Pin OFF BCF _PinStatus ; Flag copies Pin status MOVF _Pulse_OFF_Time,W ; Load Pulse_OFF value into Timer-1 ADDWF TMR1L,F BTFSC STATUS,C INCF TMR1H,F MOVF _Pulse_OFF_Time+1,W ADDWF TMR1H,F BSF _AD_CycleFlag ; Next interupt triggers AD conversion Timer1Done BSF T1CON,TMR1ON ; Turn ON timer1 BCF PIR1, TMR1IF ; Clear the interupt flag ; GOTO ISR_Done ; =============================== ISR_Done Movf psave,w ; Restore the PCLATH reg Movwf PCLATH swapf ssave,w ; Restore the STATUS reg ;(sets bank to original state) Movwf STATUS ; move W into Status reg swapf wsave,f ; Swap w_save swapf wsave,w ; Swap w_save into W RETFIE EndASM '-------------------------------------------------------------------------- '-------------------------------------------------------------------------- '-------------------------------------------------------------------------- Init_Seq: TRISA = %00011011 ' Set PORTA to some input TRISB = %11011100 ' Set PORTB TRISC = %10110001 ' Set PORTC TRISD = %00000000 ' Set PORTD to all output ADCON1 = $82 ' Set PORTA analog and right justify result ADCON0 = $91 ' Set A/D to Fosc/32, Channel 2, On T1CON = %00010000 ; Timer1 1:2 pre-scaler, Timer1 OFF T2CON = %01111011 ; Timer2 Pre- and Post-Scalers = 1:16, Timer OFF TMR2 = 0 ; This also clears pre and post scalers PR2 = 195 ; Set Timer2 Match Register for 10 ms. @ 20MHz TMR1H = 255 TMR1L = 254 SPBRG = 64 ; 4800 Baud - RS232 Input from GPS TXSTA.4 = 0 ; Clear SYNC bit RCSTA.7 = 1 ; Set SPEN bit RCSTA.4 = 1 ; Set CREN bit sLen = 0 LOW PORTE.2 ; LCD R/W line Low RudderOut ; PortC Pin1 - Servo Output Pin Low LED Pause 500 ; Wait for LCD to startup LCDOUT $fe,1,"GPS-Test v9.1-01" HSEROUT ["xxxx xxxx xxxx",13,13,13] HSEROUT [13,"GPS-R/C Interupt Test v9.1-01", 13] Pause 2000 Gosub Init_FPU PicOsc = 20 pwiFactor = 40 / PicOsc T1_10ms_VALUE = 65535 - 25000 ' 10 ms at 20 mHz w/ 1:2 prescaler SteeringTimer = 0 Bank0Flags = 0 StatFlag1 = 0 cCor = 20 cHeading = 90 ApON = 0 ' AutoPilot OFF gCMG = 0 SteerDir = 0 PrevDir = 0 PrevError = 0 hError = 0 iError = 0 Valid_GPS_Data = 0 New_GPS_Data = 0 wpNum = 0 GOSUB Load_Next_Waypoint '----- Start Interupts ----- Center = 1500 Position = 1500 Gosub Setup_Timers GIE = 1 ' Global Interupt Enable PEIE = 1 ' Peripheral Interupt Enable ' ADIE = 1 ' AD Done Interupt Enable ' RCIE = 1 ' RS-232 Rx Interupt Enable ' Setup Timer1 to trigger interupts every 10 ms ' This starts chain of repeating events wpNum = 1 Gosub Store_Current_GPS GOSUB Load_Next_Waypoint ' Load wp1 into FPU ' wpNum = 0 TMR1IE = 1 ; Timer1 Interupt Enable bit TMR1ON = 1 ; Turn Timer1 ON - start generating interupts GOTO Main_Loop ' End of Init_Seq ' ===================================================================================== ' -+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+- ' ===================================================================================== '------------------------------------------------------------------------- ' Reset the uM-FPU Floating Point processor '------------------------------------------------------------------------- Init_FPU: GOSUB Fpu_Reset 'reset the FPU hardware IF fpu_status <> SyncChar THEN LCDout $fe, 1, "uM-FPU not detected." END ENDIF ' Load constants into FPU for later use. tmpWord = 360 SHIFTOUT FpuOut, FpuClk, MSBFIRST, [F_PI, XOP, LOADPI, FSET] SHIFTOUT FpuOut, FpuClk, MSBFIRST, [F360, LOADWORD, tmpWord\16, FSET] SHIFTOUT FpuOut, FpuClk, MSBFIRST, [WRITEA+eRad, $46, $9C, $8A, $28] SHIFTOUT FpuOut, FpuClk, MSBFIRST, [F57_29, LOADUBYTE, 180, FSET, FDIV+F_PI] RETURN '--------------------------------------------------------------------------- ' Convert Integer Degrees, Minutes/Fractions into floating point Degrees ' Enter with FPU_Reg = register to load (Lon1,Lat1, etc) ' Deg, Minutes, mFract = lat/lon from GPS ' Result = (((mFract / 10000) + Minutes) / 60) + Degs '--------------------------------------------------------------------------- Load_LatLon: SHIFTOUT FpuOut, FpuClk, MSBFIRST, [FPU_Reg,_ LOADWORD, mFract.HIGHBYTE, mFract.LOWBYTE, FSET,_ WRITEB, $46, $1C, $40, $00, FDIV, LOADUBYTE, Minutes, FADD,_ WRITEB, $42, $70, $00, $00, FDIV, LOADUBYTE, Degs, FADD] ' Longitude values are always negitive numbers in the United States... IF (FPU_Reg = Lon1) OR (FPU_Reg = Lon2) then SHIFTOUT FpuOut, FpuClk, MSBFIRST, [FPU_Reg,NEGATE] ENDIF RETURN '-------------------------------------------------------------------- ' Calc Initial bearing between 2 points using Great Circle formulas ' Bearing = atan2( sin(lon2-lon1)*cos(lat2), ' (cos(lat1)*sin(lat2)) - (sin(lat1)*cos(lat2)*cos(lon2-lon1)) ) ' Store result in wpHeading '-------------------------------------------------------------------- CalcBearing: '--- P1 = sin(lon2-lon1)*cos(lat2) SHIFTOUT FpuOut, FpuClk, MSBFIRST, [p1, XOP, LEFT, FSET+Lon2, FSUB+Lon1, FSIN,_ XOP, RIGHT, FSET, XOP, LEFT, FSET+Lat2, FCOS, XOP, RIGHT, FMUL] '--- P2 = cos(lat1)*sin(lat2) SHIFTOUT FpuOut, FpuClk, MSBFIRST, [p2, XOP, LEFT, FSET+Lat1, FCOS, XOP, RIGHT,_ FSET, XOP, LEFT, FSET+Lat2, FSIN, XOP, RIGHT, FMUL] '--- P3 = sin(lat1)*cos(lat2)*cos(lon2-lon1) GOSUB Fpu_Wait SHIFTOUT FpuOut, FpuClk, MSBFIRST, [p3, XOP, LEFT, FSET+Lat1, FSIN, XOP, RIGHT,_ FSET, XOP, LEFT, FSET+Lat2, FCOS, XOP, RIGHT, FMUL, XOP, LEFT,_ FSET+Lon2, FSUB+Lon1, FCOS, XOP, RIGHT, FMUL] GOSUB Fpu_Wait SHIFTOUT FpuOut, FpuClk, MSBFIRST, [p4, FSET+p2, FSUB+p3] ' NOTE: p1 and Bearing must be the same register! ' Bearing = atan2(p1, p4) !!! Puts answer into p1 !!! SHIFTOUT FpuOut, FpuClk, MSBFIRST, [SELECTA+p1, SELECTB+p4, XOP, ATAN2] '--------------------- SHIFTOUT FpuOut, FpuClk, MSBFIRST, [Bearing, DEGREES] ' MOD function... ATAN2 may come back with negitive value GOSUB Fpu_Wait shiftout FpuOut, FpuClk, MSBFIRST, [SELECTA+Bearing, FSTATUS] GOSUB Fpu_ReadStatus if status_Sign = 1 then 'result is negitive -- add 360 SHIFTOUT FpuOut, FpuClk, MSBFIRST, [SELECTA+Bearing, FADD+F360] ENDIF SHIFTOUT FpuOut, FpuClk, MSBFIRST, [SELECTA+Bearing, ROUND, FIX] ' FIX puts Int value of A-reg into register zero GOSUB Fpu_Wait SHIFTOUT FpuOut, FpuClk, MSBFIRST, [SELECTA, XOP, READWORD] Pauseus ReadDelay shiftin FpuIn, FpuClk, MSBPRE, [wpHeading.HIGHBYTE, wpHeading.LOWBYTE] ' check for wpHeading > 360... While wpHeading > 359 wpHeading = wpHeading - 360 WEND RETURN '-------------------------------------------------------------------- ' Calc distance to next waypoint, using great circle formulas ' Distance is returned in meters. '-------------------------------------------------------------------- CalcDistance: ' dLat = (Lat2-Lat1)^2 (squared) : dLon = (Lon2-Lon1)^2 SHIFTOUT FpuOut, FpuClk, MSBFIRST, [dLat, FSET+Lat2, FSUB+Lat1, FMUL+dLat,_ dLon, FSET+Lon2, FSUB+Lon1, FMUL+dLon] SHIFTOUT FpuOut, FpuClk, MSBFIRST, [p1, FSET+Lat2, FDIV+F57_29, FCOS,_ p2, FSET+Lat1, FDIV+F57_29, FCOS] GOSUB Fpu_Wait tmpWord = 1000 ' convert Km to meters SHIFTOUT FpuOut, FpuClk, MSBFIRST, [Distance, FSET+p1, FMUL+p2, FMUL+dLon,_ FADD+dLat, SQRT, FMUL+eRad, LOADUBYTE, 180, FDIV,_ LOADWORD, tmpWord\16, FMUL, ROUND, FIX] GOSUB Fpu_Wait SHIFTOUT FpuOut, FpuClk, MSBFIRST, [SELECTA, XOP, READWORD] Pauseus ReadDelay shiftin FpuIn, FpuClk, MSBPRE, [wpDist.HIGHBYTE, wpDist.LOWBYTE] ' debug 10,13," Distance (m): ", sdec wpDist RETURN '-------------------------------------------------------------------- ' hError = Heading Error = degrees off course = [0..180] ' SteerDir = Direction to turn... 0=Left 1=Right CalcHeadingError: PrevError = hError PrevDir = SteerDir If cHeading = wpHeading then hError = 0 ' PrevError = 0 RETURN ENDIF If (wpHeading < cHeading) Then If((cHeading-wpHeading) < 180) then SteerDir = 0 ' Turn Left hError = cHeading - wpHeading Else SteerDir = 1 ' Turn Right hError = (wpHeading + 360) - cHeading ENDIF else ' (wpHeading > cHeading) If((wpHeading-cHeading) < 180) then SteerDir = 1 hError = wpHeading - cHeading Else SteerDir = 0 hError = (cHeading + 360) - wpHeading ENDIF ENDIF RETURN '------------------------------------------------------------------- ' Using hError, Gain, TravelEnds and ServoDir, set the rudder position ' Position = [0..600..1200] ' hError = [0..180] Set_Auto_Rudder: if SteerDir != PrevDir then iError = 0 Else IF hError > 12 THEN IF (ABS(PrevError - hError)) < 5 THEN ' correct cumulative steering error iError = iError + 5 EndIF EndIF EndIF tmpWord = (hError + iError) * Gain Position = Center - 900 ' Center the Rudder (600) if on course If hError <> 0 then If SteerDir = 0 then ' turn left Position = Position + tmpWord Else Position = Position - tmpWord ENDIF ENDIF Position = Position MAX trvlMin ' Constrain range [trvlMin..600..TrvlMax] Position = Position MIN trvlMax Position = Position + 900 ' Position = [900..1500..2100] ' DEBUG " AP Rudder: ", DEC4 Position, " hError:", DEC3 hError,13 RETURN ' ===================================================================================== ' -+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+- ' ===================================================================================== '------------------------------------------------------------------------------ ' Get GPS transit and Fix data from NMEA GPRMC string. ' Contains Lat, Lon, Ground Track, Speed, and Date/time '------------------------------------------------------------------------------ Get_GPS_Data: Valid_GPS_Data = 0 HSerOut ["Waiting for GPS data...",13] ' HSerin 2000, noData, [wait("RMC,"), str gpsdata\gpsmaxdata\"*"] ' Serin2 gps, 16572, 2000, noData, [wait("RMC,"), str gpsdata\gpsmaxdata\"*"] Serin2 gps, 16468, 2000, noData, [str gpsdata\gpsmaxdata\"*"] HSerOut [13,str gpsData,13] ' Cksum = 103 ' Partial check sum for GPRMC, portion of string. ' GoSub ScanCS ' If CkSumValid = 1 Then NEMA String Valid ' HSerOut ["Got something...", STR gpsdata, 13] ' HSerOut ["Got GPS.", 13] fNum = 2: GOSUB ScanField if GPSData[fStart] = "A" then Valid_GPS_Data = 1 ' HIGH LED ' Turn ON LED else Valid_GPS_Data = 0 ' LOW LED ' Turn OFF LED ENDIF fNum = 3: GOSUB ScanField cStart = fStart: nChrs = 2: GOSUB GetFVal: glat1 = fVal cStart = fStart + 2: nChrs = 2: GOSUB GetFVal: glat2 = fVal cStart = dplace + 1: nChrs = 4: GOSUB GetFVal: glat3 = fVal fNum = 5: GOSUB ScanField cStart = fStart: nChrs = 3: GOSUB GetFVal: glon1 = fVal cStart = dplace - 2: nChrs = 2: GOSUB GetFVal: glon2 = fVal cStart = dplace + 1: nChrs = 4: GOSUB GetFVal: glon3 = fVal fNum = 8: GOSUB ScanField cStart = fStart: nChrs = dPlace - fStart GOSUB GetFVal: gCMG = fVal ' gpsData[6] = 0 ' DEBUG "--> GPS TimeStamp: ", STR gpsData,13 RETURN '------------------------------------------------------------------------------ ' Get GPS Fix data from GPGGA NMEA string. ' Contains Lat, Lon, ALTITUDE, and timestamp '------------------------------------------------------------------------------ Get_GPGGA_Data: Serin2 gps, 16572, 2000, noData, [wait("GGA,"), str gpsdata\gpsmaxdata\13] ' HSerin 2000, noData, [wait("GGA,"), str gpsdata\gpsmaxdata\13] Cksum = 122 ' Partial checksum for GPGGA, string. GoSub ScanCS fNum = 2: GOSUB ScanField cStart = fStart: nChrs = 2: GOSUB GetFVal: glat1 = fVal cStart = fStart + 2: nChrs = 2: GOSUB GetFVal: glat2 = fVal cStart = dplace + 1: nChrs = 4: GOSUB GetFVal: glat3 = fVal fNum = 4: GOSUB ScanField cStart = fStart: nChrs = 3: GOSUB GetFVal: glon1 = fVal cStart = dplace - 2: nChrs = 2: GOSUB GetFVal: glon2 = fVal cStart = dplace + 1: nChrs = 4: GOSUB GetFVal: glon3 = fVal Valid_GPS_Data = 1 fNum = 6: GOSUB ScanField if GPSData[fStart] = "0" then Valid_GPS_Data = 0 ' fNum = 9: GOSUB ScanField ' cStart = fStart: nChrs = dPlace - fStart ' GOSUB GetFVal: gAlt = fVal ' if GPSData[fStart] = "-" then gAlt = 0 RETURN '------------------------------------------------------------------- NoData: glat1 = 0: gLat2 = 0: gLat3 = 0 gLon1 = 0: gLon2 = 0: gLon3 = 0 Low LED ' Turn OFF LED Valid_GPS_Data = 0 RETURN '------------------------------------------------------------------- ' Scan GPS data string - calc CheckSum ' Compare to GPScs on end of string for Valid CheckSum ScanCS: CkSumValid = 0 i = 0 ' Initialize the position counter While GPSdata[i] != "*" ' Count everthing until the * Cksum = Cksum ^GPSdata[i] ' XOR with checksum i = i + 1 ' Increment position counter WEnd ' A quick and easy atoh function LookDown GPSdata[i+1],["0123456789ABCDEF"],tmpByte GPScs = tmpByte * 16 LookDown GPSdata[i+2],["0123456789ABCDEF"],tmpByte GPScs = GPScs + tmpByte IF (Cksum = GPScs) Then CkSumValid = 1 Return '------------------------------------------------------------------- ' Scan the GPS data to find a given field ' Enter with fNum. ' Exits with fStart, dPlace and fEnd ' $GPRMC,xxxx,xx ScanField: f=1:dPlace=0:fStart=0 for i = 7 to GPSMAXDATA if GPSData[i] = "," then f = f + 1 if f = fNum then fStart = i + 1 while i < GPSMAXDATA i = i + 1 if GPSData[i] = "." then dPlace = i if GPSData[i] = "," or GPSData[i] = "*" or i = GPSMAXDATA then fEnd = i return ENDIF wend fEnd = i return ENDIF next i RETURN '------------------------------------------------------------------- ' Get a WORD value from a given field ' Enter with cStart pointing to first chr, nChrs = number of chars GetFval: fVal = 0 for i = cStart to (cStart + nChrs - 1) fVal = (fVal * 10) + (GPSData[i] - 48) next i RETURN ' ===================================================================================== ' -+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+- ' ===================================================================================== '------------------------------------------------------------------------------ ' Get raw X-Y Axis data from Compass module '------------------------------------------------------------------------------ Get_Compass_Axes: High En:Low En ' Turn the enable pin high then low to prepare compass for message Shiftout DinDout,clk,MSBFIRST,[Reset\4] ' SHIFTOUT the first 4 bits of Reset (i.e. 0000) High En:Low En ' Again,Turn the enable pin high then low to prepare compass for message Shiftout DinDout,Clk,MSBFIRST,[Measure\4] ' SHIFTOUT the first 4 bits of Measure (i.e 1000) cStats = 0 'Loop until the SHIFTIN command receives stats AND stats = ready While cStats != Ready cStats = 0 high en:low en shiftout DinDout,Clk,MSBFIRST,[Report\4] ' command for compass reading shiftin DinDout,CLk,MSBPOST,[cStats\4] 'shiftin the status of the reading WEND 'when the status of the compass is ready, SHIFTIN the X and Y values Shiftin DinDout,clk,MSBPOST,[xAxis\11,yAxis\11] high en ' turn off compass 'deal with negative numbers if (yAxis.10 = 1) then yAxis=yAxis | negmask if (xAxis.10 = 1) then xAxis=xAxis | negmask ' xAxis = xAxis - 3 ' yAxis = yAxis + 2 RETURN '------------------------------------------------------------------------------ ' Convert the x-y axis data from Compass into Degrees [0..360] ' In Stamp Basic it's bRads = x ATN -y Degrees = bRads */ 360 ' ATAN2 may come back with neg number .. subtract from 360 for CCW rotation ' -90.0 degrees = 360 - 90 = 270 degrees ' These routines now return an Integer value [0..360] in Result. '------------------------------------------------------------------------------ Calc_Compass_Angle: yAxis = -yAxis SHIFTOUT FpuOut, FpuClk, MSBFIRST,_ [yVal, LOADWORD, yAxis\16, FSET,_ xVal, LOADWORD, xAxis\16, FSET,_ SELECTA+yVal, SELECTB+xVal, XOP, ATAN2, DEGREES] GOSUB Fpu_Wait shiftout FpuOut, FpuClk, MSBFIRST, [FSTATUS] GOSUB Fpu_ReadStatus if status_Sign = 1 then 'result is negitive -- add 360 SHIFTOUT FpuOut, FpuClk, MSBFIRST, [SELECTA+yVal, FADD+F360] ENDIF SHIFTOUT FpuOut, FpuClk, MSBFIRST, [SELECTA+yVal, ROUND, FIX] ' GOSUB Print_Float ' SHIFTOUT FpuOut, FpuClk, MSBFIRST, [SELECTA+yVal, FIX] ' FIX puts Int value of A-reg into register zero GOSUB Fpu_Wait SHIFTOUT FpuOut, FpuClk, MSBFIRST, [SELECTA, XOP, READWORD] Pauseus ReadDelay shiftin FpuIn, FpuClk, MSBPRE, [cHeading.HIGHBYTE, cHeading.LOWBYTE] cHeading = cHeading + cCor ' cHeading = cHeading + 20 ' error correction based on real life While cHeading.Bit15 = 1 cHeading = cHeading + 360 WEnd While cHeading > 359 cHeading = cHeading - 360 WEnd RETURN '----------------------------------------------------------------------------- ' ===================================================================================== ' -+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+- ' ===================================================================================== '----------------------------------------------------------------------------- ' Read XMiter center position for Rudder during Init seq. ' Loop thru 25 times and return average result GetServoCenter: center = 0 for i = 1 to 25 PulsIN RudderIn,1,w ' LCDout $fe, 1, "Pulse: ", #w if w = 0 then ' no signal w = 1500 else w = w * pwiFactor w = w max 1000 w = w min 2000 ENDIF center = w + center next i center = center / 25 ' average the result RETURN '------------------------------------------------------------------- ' Read Pot 1 on XLAB-1 board ' Output value [1..10] GetGain: GOSUB getPot1 gain = tmpByte ' lcdout $fe, 1, "GainPot: ", #gain, " " if gain = 255 then ' byte goes negitive at 255 gain = 10 else gain = gain / 25 ENDIF ' lcdout "G25: ", #gain, " " gain = gain max 1 ' Output value [1..10] ' lcdout $fe, $c0, "Gain Output: ", #gain RETURN '------------------------------------------------------------------- ' Read Pot 2 on XLAB-1 board to adjust servo travel limits ' used to Return values [900..2100] ' now Return Position Min Max values [0..1200] GetTravelEnds: GOSUB getPot2 ' lcdout $fe, 1, "TravelPot: ", #tmpByte, " " w = tmpByte * 2353 tmpWord = Div32 1000 ' Produce value between [0..600] ' lcdout $fe, 1, "Travel:", #tmpWord, " Cntr:", #center trvlMax = center - 900 + tmpWord trvlMax = trvlMax MIN 1200 trvlMin = center - 900 - tmpWord trvlMin = trvlMin MAX 0 ' lcdout $fe, $c0, "Min:", #trvlMin, " Max:", #trvlMax RETURN '------------------------------------------------------------------- GetCompassCorrection: GOSUB getPot3 w = (tmpByte * 2) / 10 ' range = 0..50 cCor = 25 - w ' cCor = [-25..0..25] ' cCor = 20 RETURN '------------------------------------------------------------------- ' Subroutine to read a/d convertor getad: Pauseus 50 ' Wait for channel to setup ADCON0.2 = 1 ' Start conversion Pauseus 50 ' Wait for conversion RETURN ' Subroutine to get pot 1 value getPot1: ADCON1 = $02 ADCON0 = $41 ' Set A/D to Fosc/8, Channel 0, On GOSUB getad tmpByte = ADRESH ADCON0 = 0 ' Turn A/D Module OFF RETURN ' Subroutine to get pot 2 value getPot2: ADCON1 = $02 ADCON0 = $49 ' Set A/D to Fosc/8, Channel 1, On GOSUB getad tmpByte = ADRESH ADCON0 = 0 ' Turn A/D Module OFF RETURN ' Subroutine to get pot 3 value getPot3: ADCON1 = $02 ADCON0 = $59 ' Set A/D to Fosc/8, Channel 3, On GOSUB getad tmpByte = ADRESH ADCON0 = 0 ' Turn A/D Module OFF RETURN '------------------------------------------------------------------------------ ' Use Ch5 Landing Gear Switch to turn AutoPilot ON/OFF ' with 20 mhz pic, PulsIn will read 480 when gear down, and 1036 up. ' pwiFactor sets up everything as if it was a 40mhz, so with 20mhz OSC, ' pwiFactor is 2 - pulse width value will be 960 or 2080. Check_AP_On_Off: PulsIn Ch5ApPIN,1,tmpWord ' lcdout $fe, 1, "Ch5-AutoPilot: ", dec tmpWord, " " ' HSEROUT ["Read Ch5: ", dec tmpWord, 13] tmpWord = tmpWord * pwiFactor if tmpWord > 0 Then Radio_ON_Flag = 1 Else Radio_ON_Flag = 0 EndIF ' lcdout $fe, $C0, "pwi: ", #tmpWord, " " IF tmpWord > 1500 THEN ' Ap should be ON. If not, turn it ON. if ApON = 0 THEN ' re-init vars HSEROUT [13, 13, "Turn ON Auto Pilot.", 13] ' GOSUB GetTravelEnds ' GOSUB GetGain ' GOSUB GetServoDir wpNum = 2 GOSUB Store_Current_GPS ' Get current position for wp2 wpNum = 1 GOSUB Load_Next_Waypoint ' Now head towards wp1 GOSUB CalcBearing ApON = 1 ' AutoPilot is now ON ' lcdout $fe,1,"Auto Pilot ON" ' Debug 13,13,"Auto Pilot ON", 13, 13 ' debug "Center:,", dec center, ",Gain:,", dec gain,",TrvlMin:,", dec trvlMin, ",TrvlMax,", dec trvlMax, ",cCor:,", dec cCor, 13 ENDIF ELSE ' Ap should be OFF. If not, turn it OFF. IF ApON = 1 THEN ApON = 0 ' Turn AutoPilot OFF HSEROUT [13, 13, "Turn OFF Auto Pilot.", 13] ' lcdout $fe,1,"Auto Pilot OFF" ' Debug 13,"Auto Pilot OFF", 13, 13 ENDIF ENDIF RETURN '------------------------------------------------------------------------------ ' Setup the Timer-1 Values to trigger 1.5ms Servo Pulses ' These are loaded into Timer-1 within the Interupt Asm routines. ' Pulse ON and OFF times must add up to 10ms '------------------------------------------------------------------------------ Setup_Timers: Position = Position MIN 2100 Position = Position MAX 900 tmpWord = (Position * 5) / 2 ' Position = [900..1500..2100] Pulse_ON_Time = 65535 - tmpWord ' 1500 = 1.5ms Pulse Width Pulse_OFF_Time = 65535 - (25000 - tmpWord) ' 25k = 10ms RETURN '------------------------------------------------------------------------------------ ' Load the next Way-Point data into the FPU for Distance and Bearing Calcs '------------------------------------------------------------------------------------ Load_Next_Waypoint: HSEROUT [13,"Load Way Point: ", DEC wpNum, 13] IF wpNum > 2 THEN wpNum = 1 Select Case wpNum Case 0 HSEROUT [13,"Load WP-0", 13] FPU_Reg = Lat2 : Degs = 36 : Minutes = 57 : mFract = 4818 GOSUB Load_LatLon FPU_Reg = Lon2 : Degs = 122 : Minutes = 1 : mFract = 7580 GOSUB Load_LatLon Case 1 HSEROUT [13,"Load WP-1", DEC wp1_Lat1, 13] FPU_Reg = Lat2 : Degs = wp1_Lat1 : Minutes = wp1_Lat2 : mFract = wp1_Lat3 GOSUB Load_LatLon FPU_Reg = Lon2 : Degs = wp1_Lon1 : Minutes = wp1_Lon2 : mFract = wp1_Lon3 GOSUB Load_LatLon Case 2 HSEROUT [13,"Load WP-2", DEC wp2_Lat1, 13] FPU_Reg = Lat2 : Degs = wp2_Lat1 : Minutes = wp2_Lat2 : mFract = wp2_Lat3 GOSUB Load_LatLon FPU_Reg = Lon2 : Degs = wp2_Lon1 : Minutes = wp2_Lon2 : mFract = wp2_Lon3 GOSUB Load_LatLon End Select RETURN '------------------------------------------------------------------------------ ' ===================================================================================== ' -+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+- ' ===================================================================================== ' Called to save the current GPS location into either wp1 or wp2 ' called at Init time, and when user switches into auto mode ' This is just a test routine, to set 2 waypoints to cycle between Store_Current_GPS: LCDout $fe, 1, "Wait for GPS..." Store_Current_GPS1: ' IF New_GPS_Data THEN GOSUB Get_GPS_Data ' Get current GPS data into gLat1,gLon1 New_GPS_Data = 0 IF Valid_GPS_Data THEN ' Save GPS data Select Case wpNum Case 1 wp1_Lat1 = gLat1 : wp1_Lat2 = gLat2 : wp1_Lat3 = gLat3 wp1_Lon1 = gLon1 : wp1_Lon2 = gLon2 : wp1_Lon3 = gLon3 LCDout $fe, 1, "Got WayPoint 1" Case 2 wp2_Lat1 = gLat1 : wp2_Lat2 = gLat2 : wp2_Lat3 = gLat3 wp2_Lon1 = gLon1 : wp2_Lon2 = gLon2 : wp2_Lon3 = gLon3 LCDout $fe, 1, "Got WayPoint 2" End Select Return EndIF ' ELSE ' Pause 200 ' Goto Store_Current_GPS1 ' ENDIF Goto Store_Current_GPS ' ======================================================================================== DoTest: ' HSerOut ["Do Test...",13] GOSUB Get_GPS_Data New_GPS_Data = Valid_GPS_Data IF New_GPS_Data THEN HSerOut [13,"** -New GPS Data..",13] Toggle LED New_GPS_Data = 0 IF Valid_GPS_Data THEN ' Load GPS data into FPU for bearing calcs FPU_Reg = Lat1 : Degs = gLat1 : Minutes = gLat2 : mFract = gLat3 GOSUB Load_LatLon FPU_Reg = Lon1 : Degs = gLon1 : Minutes = gLon2 : mFract = gLon3 GOSUB Load_LatLon GOSUB CalcDistance ' Calc distance to next way point -> wpDist HSerOut ["GPS,", DEC wpNum, ",", DEC wpDist, ",", DEC gCMG] GOSUB CalcBearing ' Calculate heading to next way point -> wpHeading [0..359] ELSE LCDout $fe, 1, "Invalid GPS" LCDout $fe, $C0, "cHead:", DEC3 cHeading HSerOut [13, "Invalid GPS", 13] ENDIF SteeringTimer = 99 ' trigger steering routines ENDIF ' New_GPS_Data GOSUB Get_Compass_Axes GOSUB Calc_Compass_Angle ' Store result in cHeading GOSUB CalcHeadingError ' GOSUB Set_Auto_Rudder ' HSerOut ["--Steering-> ,", DEC wpHeading, ",", DEC hError, ",", DEC SteerDir, ",", DEC iError, ",", DEC Position, 13] lcdout $fe,1, "wpHd: ", DEC3 wpHeading, " cHead: ", SDEC3 cHeading lcdout $fe,$C0, "hErr: ", DEC3 hError, " d: ", DEC wpDist GOTO Main_Loop ' End of DoTest routine '------------------------------------------------------------------------------ ' GPS antenna returns data once per second - call steering routines 3 times per second ' GPS data is read during interupts. ' New_GPS_Data is flag stating when full GPS string is available DoAuto: ' HSerOut ["Do Auto...",13] LCD_Timer = 0 GOSUB Get_GPS_Data New_GPS_Data = Valid_GPS_Data IF New_GPS_Data THEN HSerOut [13,"** -New GPS Data..",13] Toggle LED LCD_Timer = 1 ' GOSUB Get_GPS_Data ' Get current GPS data into gLat1,gLon1 New_GPS_Data = 0 IF Valid_GPS_Data THEN ' Load GPS data into FPU for bearing calcs FPU_Reg = Lat1 : Degs = gLat1 : Minutes = gLat2 : mFract = gLat3 GOSUB Load_LatLon FPU_Reg = Lon1 : Degs = gLon1 : Minutes = gLon2 : mFract = gLon3 GOSUB Load_LatLon GOSUB CalcDistance ' Calc distance to next way point -> wpDist HSerOut ["GPS,", DEC wpNum, ",", DEC wpDist, ",", DEC gCMG] IF wpDist < 3 THEN ' If within 2 Meters, go to next way point wpNum = wpNum + 1 GOSUB Load_Next_Waypoint wpDist = 99 HSerOut [13,"NWP,", Dec wpNum, ",", DEC wpDist, ", , "] ENDIF GOSUB CalcBearing ' Calculate heading to next way point -> wpHeading [0..359] ' HSerOut [",", DEC wpHeading] ELSE LCDout $fe, 1, "Invalid GPS" LCDout $fe, $C0, "cHead:", DEC3 cHeading HSerOut [13, "Invalid GPS", 13] ENDIF SteeringTimer = 99 ' trigger steering routines ENDIF ' New_GPS_Data ' IF SteeringTimer > 25 THEN ' this is inc'd during servo pulses - do 3 times per second SteeringTimer = 0 GOSUB Get_Compass_Axes GOSUB Calc_Compass_Angle ' Store result in cHeading GOSUB CalcHeadingError GOSUB Set_Auto_Rudder Gosub Setup_Timers ' HSerOut ["--Steering-> ,", DEC wpHeading, ",", DEC hError, ",", DEC SteerDir, ",", DEC iError, ",", DEC Position, 13] ' ENDIf ' IF LCD_Timer Then lcdout $fe,1, "Go to wp: ", DEC wpNum ' lcdout $fe,1, "wpHd: ", DEC3 wpHeading, " cHead: ", SDEC3 cHeading lcdout $fe,$C0, "hErr: ", DEC3 hError, " d: ", DEC wpDist ' EndIf GOTO Main_Loop ' End of DoAuto routine ' ======================================================================================== ' AutoPilot is OFF - Read Rudder position from Xmitter and ' pass thru to servo PassThru: ' HSerOut ["Pass Thru...",13] PulsIN RudderIn,1,tmpWord ' Pulse values = [95..154..220] at 150% ' lcdout $fe,1,"PassThru Pulse: ",#tmpWord ' Normal range = [110..150..190] IF tmpWord = 0 THEN Position = Center ' Center Rudder if no signal ELSE tmpWord = tmpWord * pwiFactor ' w*p ~ [1000..1500..2000] Position = tmpWord ' Position = [1000..1500..2000] ' Position = (Position + LastPosition) / 2 ENDIF ' calc values to load into Timer1 to create the required interupts Gosub Setup_Timers ' IF 1=2 THEN ' GOSUB GetServoCenter GOSUB GetGain GOSUB GetTravelEnds GOSUB GetCompassCorrection ' lcdout $fe,1,"PassThru Pulse:",#w , #gain, #Center lcdout $fe,$C0, "G:", DEC3 Gain, " C:", DEC3 cCor, " T:", DEC3 trvlMIN ' ENDIF ' GOTO Main_Loop ' ===================================================================================== ' -+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+-+- ' ===================================================================================== Main_Loop: GOSUB Check_AP_On_Off ' Check Ch5 Landing Gear Switch for AutoPilot ON/OFF ' ApON = 1 ' Pause 500 ' Toggle LED ' Goto Test_Loop IF Radio_ON_Flag Then IF ApON = 1 THEN Goto DoAuto ELSE Goto PassThru ENDIF Else Goto DoTest EndIf END ' ======================================================================================== ' End of File