' =============================================================================== ' ' File....... 21LSD_Cannon.BS2 ' Purpose.... Prototype This: 21st Century Lifesaving, Cannon Targeting System ' Author..... J. Grand ' ' {$STAMP BS2sx} ' {$PBASIC 2.5} ' ' =============================================================================== ' -----[ Program Description ]--------------------------------------------- ' ' This code is for the cannon targeting system portion of the 21st Century ' Lifesaving Project. The system receives GPS coordinates from the person ' in distress via the Aerocomm AC4490 transceiver. An external anemometer ' is used to calculate wind speed and direction. The cannon and target ' GPS coordinates, wind speed, and wind direction are transmitted via ' the Lantronix XPORT in a UDP broadcast packet over the network to Zoz, ' who calculates the targeting solution (direction of cannon and pressure) ' and sends the result to Terry's cannon motor control system. ' ' A small OLED panel is used to display GPS coordinate information and other ' system status. ' ' -----[ I/O Definitions ]------------------------------------------------- GPS_SIO PIN 14 ' connects to GPS Module SIO pin RF_RX PIN 15 ' input from AC4490 radio modem WindSpeedIn PIN 4 ' input from R.M. Young 03002 anemometer through LM393 comparator XPORT_TX PIN 0 ' Serial data input to Xport FROM BS2 XPORT_RX PIN 1 ' Serial data output from XPort TO BS2 ' uOLED-128-GMD1 OLED_TX PIN 10 ' BS2 Transmit --> uOLED RX via 1K OLED_RX PIN 11 ' BS2 Receieve <-- uOLED TX 'uM-FPU V3 SPI FpuClk PIN 3 ' SPI clock (connects to SCLK/SCL) FpuOut PIN 2 ' SPI data out (connects to SIN/SDA) FpuIn PIN 2 ' SPI data in (connects to SOUT) ' -----[ Constants ]------------------------------------------------------- #SELECT $STAMP #CASE BS2, BS2E, BS2PE T1200 CON 813 T2400 CON 396 T4800 CON 188 T9600 CON 84 T19K2 CON 32 T38K4 CON 6 #CASE BS2SX, BS2P T1200 CON 2063 T2400 CON 1021 T4800 CON 500 T9600 CON 240 T19K2 CON 110 T38K4 CON 45 #ENDSELECT ' OLED colors blk1 CON $00 ' Black blk2 CON $00 wh1 CON $FF ' White wh2 CON $FF red1 CON $F8 ' Red red2 CON $00 grn1 CON $07 ' Green grn2 CON $E0 blu1 CON $00 ' Blue blu2 CON $1F cy1 CON $07 ' Cyan cy2 CON $FF yel1 CON $FF ' Yellow yel2 CON $E0 mag1 CON $F8 ' Magenta mag2 CON $1F OLED_Baud CON T9600 GPS_Baud CON T4800 RF_Baud CON T9600 XPORT_Baud CON T9600 '---- [GPS Constants] ---- DegSym CON 176 ' degrees symbol for report MinSym CON 39 ' minutes symbol SecSym CON 34 ' seconds symbol ' GPS Module Commands GetInfo CON $00 GetValid CON $01 GetSats CON $02 GetTime CON $03 GetDate CON $04 GetLat CON $05 GetLong CON $06 GetAlt CON $07 GetSpeed CON $08 GetHead CON $09 '-------------------- uM-FPU V3 opcodes --------------------------------------- SELECTA CON $01 ' Select register A SELECTX CON $02 ' Select register X CLR CON $03 ' reg[nn] = 0 CLRA CON $04 ' reg[A] = 0 CLRX CON $05 ' reg[X] = 0, X = X + 1 CLR0 CON $06 ' reg[0] = 0 COPY CON $07 ' reg[nn] = reg[mm] COPYA CON $08 ' reg[nn] = reg[A] COPYX CON $09 ' reg[nn] = reg[X], X = X + 1 LOAD CON $0A ' reg[0] = reg[nn] LOADA CON $0B ' reg[0] = reg[A] LOADX CON $0C ' reg[0] = reg[X], X = X + 1 ALOADX CON $0D ' reg[A] = reg[X], X = X + 1 XSAVE CON $0E ' reg[X] = reg[nn], X = X + 1 XSAVEA CON $0F ' reg[X] = reg[A], X = X + 1 COPY0 CON $10 ' reg[nn] = reg[0] COPYI CON $11 ' reg[nn] = long(unsigned bb) SWAP CON $12 ' Swap reg[nn] and reg[mm] SWAPA CON $13 ' Swap reg[A] and reg[nn] LEFT CON $14 ' Left parenthesis RIGHT CON $15 ' Right parenthesis FWRITE CON $16 ' Write 32-bit float to reg[nn] FWRITEA CON $17 ' Write 32-bit float to reg[A] FWRITEX CON $18 ' Write 32-bit float to reg[X], X = X + 1 FWRITE0 CON $19 ' Write 32-bit float to reg[0] FREAD CON $1A ' Read 32-bit float from reg[nn] FREADA CON $1B ' Read 32-bit float from reg[A] FREADX CON $1C ' Read 32-bit float from reg[X], X = X + 1 FREAD0 CON $1D ' Read 32-bit float from reg[0] ATOF CON $1E ' Convert ASCII to float, store in reg[0] FTOA CON $1F ' Convert float to ASCII FSET CON $20 ' reg[A] = reg[nn] FADD CON $21 ' reg[A] = reg[A] + reg[nn] FSUB CON $22 ' reg[A] = reg[A] - reg[nn] FSUBR CON $23 ' reg[A] = reg[nn] - reg[A] FMUL CON $24 ' reg[A] = reg[A] * reg[nn] FDIV CON $25 ' reg[A] = reg[A] / reg[nn] FDIVR CON $26 ' reg[A] = reg[nn] / reg[A] FPOW CON $27 ' reg[A] = reg[A] ** reg[nn] FCMP CON $28 ' Float compare reg[A] - reg[nn] FSET0 CON $29 ' reg[A] = reg[0] FADD0 CON $2A ' reg[A] = reg[A] + reg[0] FSUB0 CON $2B ' reg[A] = reg[A] - reg[0] FSUBR0 CON $2C ' reg[A] = reg[0] - reg[A] FMUL0 CON $2D ' reg[A] = reg[A] * reg[0] FDIV0 CON $2E ' reg[A] = reg[A] / reg[0] FDIVR0 CON $2F ' reg[A] = reg[0] / reg[A] FPOW0 CON $30 ' reg[A] = reg[A] ** reg[0] FCMP0 CON $31 ' Float compare reg[A] - reg[0] FSETI CON $32 ' reg[A] = float(bb) FADDI CON $33 ' reg[A] = reg[A] + float(bb) FSUBI CON $34 ' reg[A] = reg[A] - float(bb) FSUBRI CON $35 ' reg[A] = float(bb) - reg[A] FMULI CON $36 ' reg[A] = reg[A] * float(bb) FDIVI CON $37 ' reg[A] = reg[A] / float(bb) FDIVRI CON $38 ' reg[A] = float(bb) / reg[A] FPOWI CON $39 ' reg[A] = reg[A] ** bb FCMPI CON $3A ' Float compare reg[A] - float(bb) FSTATUS CON $3B ' Float status of reg[nn] FSTATUSA CON $3C ' Float status of reg[A] FCMP2 CON $3D ' Float compare reg[nn] - reg[mm] FNEG CON $3E ' reg[A] = -reg[A] FABS CON $3F ' reg[A] = | reg[A] | FINV CON $40 ' reg[A] = 1 / reg[A] SQRT CON $41 ' reg[A] = sqrt(reg[A]) ROOT CON $42 ' reg[A] = root(reg[A], reg[nn]) LOG CON $43 ' reg[A] = log(reg[A]) LOG10 CON $44 ' reg[A] = log10(reg[A]) EXP CON $45 ' reg[A] = exp(reg[A]) EXP10 CON $46 ' reg[A] = exp10(reg[A]) FSIN CON $47 ' reg[A] = sin(reg[A]) FCOS CON $48 ' reg[A] = cos(reg[A]) FTAN CON $49 ' reg[A] = tan(reg[A]) ASIN CON $4A ' reg[A] = asin(reg[A]) ACOS CON $4B ' reg[A] = acos(reg[A]) ATAN CON $4C ' reg[A] = atan(reg[A]) ATAN2 CON $4D ' reg[A] = atan2(reg[A], reg[nn]) DEGREES CON $4E ' reg[A] = degrees(reg[A]) RADIANS CON $4F ' reg[A] = radians(reg[A]) FMOD CON $50 ' reg[A] = reg[A] MOD reg[nn] FLOOR CON $51 ' reg[A] = floor(reg[A]) CEIL CON $52 ' reg[A] = ceil(reg[A]) ROUND CON $53 ' reg[A] = round(reg[A]) FMIN CON $54 ' reg[A] = min(reg[A], reg[nn]) FMAX CON $55 ' reg[A] = max(reg[A], reg[nn]) FCNV CON $56 ' reg[A] = conversion(nn, reg[A]) FMAC CON $57 ' reg[A] = reg[A] + (reg[nn] * reg[mm]) FMSC CON $58 ' reg[A] = reg[A] - (reg[nn] * reg[mm]) LOADBYTE CON $59 ' reg[0] = float(signed bb) LOADUBYTE CON $5A ' reg[0] = float(unsigned byte) LOADWORD CON $5B ' reg[0] = float(signed word) LOADUWORD CON $5C ' reg[0] = float(unsigned word) LOADE CON $5D ' reg[0] = 2.7182818 LOADPI CON $5E ' reg[0] = 3.1415927 LOADCON CON $5F ' reg[0] = float constant(nn) FLOAT CON $60 ' reg[A] = float(reg[A]) FIX CON $61 ' reg[A] = fix(reg[A]) FIXR CON $62 ' reg[A] = fix(round(reg[A])) FRAC CON $63 ' reg[A] = fraction(reg[A]) FSPLIT CON $64 ' reg[A] = int(reg[A]), reg[0] = frac(reg[A]) SELECTMA CON $65 ' Select matrix A SELECTMB CON $66 ' Select matrix B SELECTMC CON $67 ' Select matrix C LOADMA CON $68 ' reg[0] = matrix A[bb, bb] LOADMB CON $69 ' reg[0] = matrix B[bb, bb] LOADMC CON $6A ' reg[0] = matrix C[bb, bb] SAVEMA CON $6B ' matrix A[bb, bb] = reg[A] SAVEMB CON $6C ' matrix B[bb, bb] = reg[A] SAVEMC CON $6D ' matrix C[bb, bb] = reg[A] MOP CON $6E ' matrix operation FFT CON $6F ' FFT operation LOADIND CON $7A ' reg[0] = reg[reg[nn]] SAVEIND CON $7B ' reg[reg[nn]] = reg[A] INDA CON $7C ' Select A using reg[nn] INDX CON $7D ' Select X using reg[nn] FCALL CON $7E ' Call user-defined function in Flash memory EECALL CON $7F ' Call user-defined function in EEPROM memory RET CON $80 ' Return from user-defined function BRA CON $81 ' Unconditional branch BRACC CON $82 ' Conditional branch JMP CON $83 ' Unconditional jump JMPCC CON $84 ' Conditional jump TABLE CON $85 ' Table lookup FTABLE CON $86 ' Floating point reverse table lookup LTABLE CON $87 ' Long integer reverse table lookup POLY CON $88 ' reg[A] = nth order polynomial FGOTO CON $89 ' Computed goto LWRITE CON $90 ' Write 32-bit long integer to reg[nn] LWRITEA CON $91 ' Write 32-bit long integer to reg[A] LWRITEX CON $92 ' Write 32-bit long integer to reg[X], X = X + 1 LWRITE0 CON $93 ' Write 32-bit long integer to reg[0] LREAD CON $94 ' Read 32-bit long integer from reg[nn] LREADA CON $95 ' Read 32-bit long integer from reg[A] LREADX CON $96 ' Read 32-bit long integer from reg[X], X = X + 1 LREAD0 CON $97 ' Read 32-bit long integer from reg[0] LREADBYTE CON $98 ' Read lower 8 bits of reg[A] LREADWORD CON $99 ' Read lower 16 bits reg[A] ATOL CON $9A ' Convert ASCII to long integer LTOA CON $9B ' Convert long integer to ASCII LSET CON $9C ' reg[A] = reg[nn] LADD CON $9D ' reg[A] = reg[A] + reg[nn] LSUB CON $9E ' reg[A] = reg[A] - reg[nn] LMUL CON $9F ' reg[A] = reg[A] * reg[nn] LDIV CON $A0 ' reg[A] = reg[A] / reg[nn] LCMP CON $A1 ' Signed long compare reg[A] - reg[nn] LUDIV CON $A2 ' reg[A] = reg[A] / reg[nn] LUCMP CON $A3 ' Unsigned long compare reg[A] - reg[nn] LTST CON $A4 ' Long integer status of reg[A] AND reg[nn] LSET0 CON $A5 ' reg[A] = reg[0] LADD0 CON $A6 ' reg[A] = reg[A] + reg[0] LSUB0 CON $A7 ' reg[A] = reg[A] - reg[0] LMUL0 CON $A8 ' reg[A] = reg[A] * reg[0] LDIV0 CON $A9 ' reg[A] = reg[A] / reg[0] LCMP0 CON $AA ' Signed long compare reg[A] - reg[0] LUDIV0 CON $AB ' reg[A] = reg[A] / reg[0] LUCMP0 CON $AC ' Unsigned long compare reg[A] - reg[0] LTST0 CON $AD ' Long integer status of reg[A] AND reg[0] LSETI CON $AE ' reg[A] = long(bb) LADDI CON $AF ' reg[A] = reg[A] + long(bb) LSUBI CON $B0 ' reg[A] = reg[A] - long(bb) LMULI CON $B1 ' reg[A] = reg[A] * long(bb) LDIVI CON $B2 ' reg[A] = reg[A] / long(bb) LCMPI CON $B3 ' Signed long compare reg[A] - long(bb) LUDIVI CON $B4 ' reg[A] = reg[A] / unsigned long(bb) LUCMPI CON $B5 ' Unsigned long compare reg[A] - ulong(bb) LTSTI CON $B6 ' Long integer status of reg[A] AND ulong(bb) LSTATUS CON $B7 ' Long integer status of reg[nn] LSTATUSA CON $B8 ' Long integer status of reg[A] LCMP2 CON $B9 ' Signed long compare reg[nn] - reg[mm] LUCMP2 CON $BA ' Unsigned long compare reg[nn] - reg[mm] LNEG CON $BB ' reg[A] = -reg[A] LABS CON $BC ' reg[A] = | reg[A] | LINC CON $BD ' reg[nn] = reg[nn] + 1 LDEC CON $BE ' reg[nn] = reg[nn] - 1 LNOT CON $BF ' reg[A] = NOT reg[A] LAND CON $C0 ' reg[A] = reg[A] AND reg[nn] LOR CON $C1 ' reg[A] = reg[A] OR reg[nn] LXOR CON $C2 ' reg[A] = reg[A] XOR reg[nn] LSHIFT CON $C3 ' reg[A] = reg[A] shift reg[nn] LMIN CON $C4 ' reg[A] = min(reg[A], reg[nn]) LMAX CON $C5 ' reg[A] = max(reg[A], reg[nn]) LONGBYTE CON $C6 ' reg[0] = long(signed byte bb) LONGUBYTE CON $C7 ' reg[0] = long(unsigned byte bb) LONGWORD CON $C8 ' reg[0] = long(signed word wwww) LONGUWORD CON $C9 ' reg[0] = long(unsigned word wwww) LONGCON CON $CA ' reg[0] = long constant(nn) SETOUT CON $D0 ' Set OUT1 and OUT2 output pins ADCMODE CON $D1 ' Set A/D trigger mode ADCTRIG CON $D2 ' A/D manual trigger ADCSCALE CON $D3 ' ADCscale[ch] = B ADCLONG CON $D4 ' reg[0] = ADCvalue[ch] ADCLOAD CON $D5 ' reg[0] = float(ADCvalue[ch]) * ADCscale[ch] ADCWAIT CON $D6 ' wait for next A/D sample TIMESET CON $D7 ' time = reg[0] TIMELONG CON $D8 ' reg[0] = time (long) TICKLONG CON $D9 ' reg[0] = ticks (long) EESAVE CON $DA ' EEPROM[nn] = reg[mm] EESAVEA CON $DB ' EEPROM[nn] = reg[A] EELOAD CON $DC ' reg[mm] = EEPROM[nn] EELOADA CON $DD ' reg[A] = EEPROM[nn] EEWRITE CON $DE ' Store bytes in EEPROM EXTSET CON $E0 ' external input count = reg[0] EXTLONG CON $E1 ' reg[0] = external input counter (long) EXTWAIT CON $E2 ' wait for next external input STRSET CON $E3 ' Copy string to string buffer STRSEL CON $E4 ' Set selection point STRINS CON $E5 ' Insert string at selection point STRCMP CON $E6 ' Compare string with string buffer STRFIND CON $E7 ' Find string and set selection point STRFCHR CON $E8 ' Set field separators STRFIELD CON $E9 ' Find field and set selection point STRTOF CON $EA ' Convert string selection to float STRTOL CON $EB ' Convert string selection to long READSEL CON $EC ' Read string selection SYNC CON $F0 ' Get synchronization byte READSTATUS CON $F1 ' Read status byte READSTR CON $F2 ' Read string from string buffer VERSION CON $F3 ' Copy version string to string buffer IEEEMODE CON $F4 ' Set IEEE mode (default) PICMODE CON $F5 ' Set PIC mode CHECKSUM CON $F6 ' Calculate checksum for uM-FPU code BREAK CON $F7 ' Debug breakpoint TRACEOFF CON $F8 ' Turn debug trace off TRACEON CON $F9 ' Turn debug trace on TRACESTR CON $FA ' Send string to debug trace buffer TRACEREG CON $FB ' Send register value to trace buffer READVAR CON $FC ' Read internal variable, store in reg[0] SyncChar CON $5C ' sync character OneSec CON 2500 ' BS2sx WindSpeedTime CON 4 '-------------------- uM-FPU register definitions ----------------------------- Wind_Direction CON 1 Wind_Speed CON 2 '-------------------- BS2 EEPROM memory locations ----------------------------- c_lat_deg CON 0 ' cannon latitude c_lat_min CON 1 c_lat_minD_H CON 2 c_lat_minD_L CON 3 c_lat_dir CON 4 c_long_deg CON 5 ' cannon longitude c_long_min CON 6 c_long_minD_H CON 7 c_long_minD_L CON 8 c_long_dir CON 9 tmp_storage CON 10 ' -----[ Variables ]------------------------------------------------------- tmp VAR BYTE ' loop counter speed_str VAR BYTE(6) wind_dir VAR WORD ' --- GPS --- deg VAR BYTE ' degrees minutes VAR BYTE ' minutes minutesD VAR WORD ' decimal minutes dir VAR BIT ' direction (latitude: 0 = N, 1 = S, longitude: 0 = E, 1 = W) target_deg VAR BYTE ' degrees target_minutes VAR BYTE ' minutes target_minutesD VAR WORD ' decimal minutes target_dir VAR BIT ' direction (latitude: 0 = N, 1 = S, longitude: 0 = E, 1 = W) target_long_deg VAR BYTE ' degrees target_long_minutes VAR BYTE ' minutes target_long_minutesD VAR WORD ' decimal minutes target_long_dir VAR BIT ' direction (latitude: 0 = N, 1 = S, longitude: 0 = E, 1 = W) '-------------------- uM-FPU variables ---------------------------------------- dataWord VAR WORD ' data word dataHigh VAR dataWord.HIGHBYTE ' high byte of dataWord dataLow VAR dataword.LOWBYTE ' low byte of dataLow dataByte VAR dataLow ' (alternate name) opcode VAR dataHigh ' opcode (same as dataHigh) format VAR dataLow ' format (same as dataLow) status VAR dataLow ' status (same as dataLow) status_Zero VAR status.BIT0 ' Zero status bit (0-not zero, 1-zero) status_Sign VAR status.BIT1 ' Sign status bit (0-positive, 1-negative) status_NaN VAR status.BIT2 ' Not a Number status bit (0-valid number, 1-NaN) status_Inf VAR status.BIT3 ' Infinity status bit (0-not infinite, 1-infinite) ' -----[ Initialization ]-------------------------------------------------- Init: PAUSE 2000 DEBUG CLS SEROUT OLED_TX, OLED_Baud, ["U"] ' Establish Auto-Baud Detection GOSUB GetACK PAUSE 200 SEROUT OLED_TX, OLED_Baud, ["E", "O", 1] ' Clear Screen, set text mode to Opaque GOSUB GetACK ' DEBUG "Cannon Targeting System Ready!", CR ' write initial start-up string to the display 'SEROUT OLED_TX, OLED_Baud, ["s", 0, 0, 0, red1, red2, "PT Cannon Targeting", 0] 'GOSUB GetAck 'DEBUG CR, "umfpuV3-spi", CR GOSUB Fpu_Reset ' reset the FPU hardware IF status <> SyncChar THEN ' check for synchronization 'DEBUG "uM-FPU not detected" SEROUT OLED_TX, OLED_Baud, ["s", 0, 1, 0, grn1, grn2, "Error: NO uM-FPU ", 0] GOSUB GetAck PAUSE 10000 GOTO Init ENDIF ' -----[ Program Code ]---------------------------------------------------- Main: ' get GPS coordinates from cannon's on-board GPS, so we know its current position ' check for valid GPS signal ' DEBUG "Receiving Cannon GPS..." SEROUT OLED_TX, OLED_Baud, ["s", 0, 1, 0, grn1, grn2, "Receiving Cannon GPS ", 0] GOSUB GetAck Wait_Valid: ' is the GPS signal valid? 0 = NOT valid, 1 = valid SEROUT GPS_Sio, GPS_Baud, ["!GPS", GetValid] SERIN GPS_Sio, GPS_Baud, 3000, No_GPS, [tmp] ' wait up to 3 seconds for a response IF (tmp = 0) THEN ' DEBUG "." PAUSE 500 ' wait a bit and try again GOTO Wait_Valid ENDIF ' DEBUG CR ' get coordinates SEROUT OLED_TX, OLED_Baud, ["s", 0, 3, 0, yel1, yel2, "Cannon Coordinates:", 0] GOSUB GetAck SEROUT GPS_Sio, GPS_Baud, ["!GPS", GetLat] ' get cannon's latitude SERIN GPS_Sio, GPS_Baud, 3000, No_GPS, [deg, minutes, minutesD.HIGHBYTE, minutesD.LOWBYTE, dir] 'DEBUG "Can Lat: ", " " + (dir * 13), DEC deg, ":", DEC minutes, ".", DEC4 minutesD, CR WRITE c_lat_deg, deg ' store coordinates in EEPROM WRITE c_lat_min, minutes WRITE c_lat_minD_H, minutesD.HIGHBYTE WRITE c_lat_minD_L, minutesD.LOWBYTE WRITE c_lat_dir, dir SEROUT OLED_TX, OLED_Baud, ["s", 0, 4, 0, yel1, yel2, "Lat: ", 43 + (dir * 2), DEC deg, ":", DEC minutes, ".", DEC4 minutesD, 0] GOSUB GetAck SEROUT GPS_Sio, GPS_Baud, ["!GPS", GetLong] ' get cannon's longitude SERIN GPS_Sio, GPS_Baud, 3000, No_GPS, [deg, minutes, minutesD.HIGHBYTE, minutesD.LOWBYTE, dir] 'DEBUG "Can Long: ", " " + (dir * 13), DEC deg, ":", DEC minutes, ".", DEC4 minutesD, CR WRITE c_long_deg, deg ' store coordinates in EEPROM WRITE c_long_min, minutes WRITE c_long_minD_H, minutesD.HIGHBYTE WRITE c_long_minD_L, minutesD.LOWBYTE WRITE c_long_dir, dir SEROUT OLED_TX, OLED_Baud, ["s", 0, 5, 0, yel1, yel2, "Long: ", 43 + (dir * 2), DEC deg, ":", DEC minutes, ".", DEC4 minutesD, 0] GOSUB GetAck ' now we sit and wait until we receive the coordinates from wristband... ' DEBUG "Waiting for Wristband..." SEROUT OLED_TX, OLED_Baud, ["s", 0, 1, 0, grn1, grn2, "Waiting for Wristband", 0] GOSUB GetAck ' wait for the header character from the wristband SERIN RF_RX, RF_Baud, [WAIT("A"), target_deg, target_minutes, target_minutesD.HIGHBYTE, target_minutesD.LOWBYTE, target_dir, target_long_deg, target_long_minutes, target_long_minutesD.HIGHBYTE, target_long_minutesD.LOWBYTE, target_long_dir, tmp] IF (tmp <> "B") THEN GOTO No_RF ' if we don't receive the footer, we might not have received the proper data stream ' DEBUG CR, "Wristband signal received! Person in distress!", CR GOSUB Print_Target_GPS ' display target's GPS coordinates on DEBUG and LCD ' DEBUG "Getting Wind Speed/Direction..." SEROUT OLED_TX, OLED_Baud, ["s", 0, 1, 0, grn1, grn2, "Getting Wind Spd/Dir ", 0] GOSUB GetAck SEROUT OLED_TX, OLED_Baud, ["s", 0, 11, 0, mag1, mag2, "Wind Measurements:", 0] GOSUB GetAck COUNT WindSpeedIn, WindSpeedTime * OneSec, tmp ' Count pulses tmp = tmp / WindSpeedTime ' load pulse count into FPU & calculate wind speed SHIFTOUT FpuOut, FpuClk, MSBFIRST, [SELECTA, Wind_Speed, LONGUBYTE, tmp, LSET0, FLOAT, ATOF, "0.75", 0, FMUL0]', ATOF, "0.2", 0, FADD0] DEBUG "Spd: " GOSUB Print_Float FOR tmp = tmp_storage TO tmp_storage+5 ' move EEPROM contents to local variable READ tmp, speed_str(tmp-tmp_storage) NEXT SEROUT OLED_TX, OLED_Baud, ["s", 0, 12, 0, mag1, mag2, "Speed: ", STR speed_str, " m/s", 0] GOSUB GetAck ' set up ADC SHIFTOUT FpuOut, FpuClk, MSBFIRST, [ADCMODE, $10+15, ATOF, "0.08789", 0, ADCSCALE] ' set up scaling factor to bring 12-bit (0-4096) down to 0-360 degrees for direction FOR tmp = 0 TO 2 ' this needs to be run more than one time in order to get a reading for some reason... SHIFTOUT FpuOut, FpuClk, MSBFIRST, [ADCTRIG, SELECTA, Wind_Direction, ADCLOAD, 0, LSET0, FIX] NEXT DEBUG "Dir: " GOSUB Print_Long ' read wind direction from EEPROM and turn it into a decimal value READ tmp_storage, tmp wind_dir = (tmp - $30) * 100 READ tmp_storage+1, tmp IF (tmp = 0) THEN wind_dir = wind_dir / 100 ' single digit direction GOTO Dir_Done ENDIF wind_dir = wind_dir + ((tmp - $30) * 10) READ tmp_storage+2, tmp IF (tmp = 0) THEN wind_dir = wind_dir / 10 ' two digits GOTO Dir_Done ENDIF wind_dir = wind_dir + (tmp - $30) ' three digits Dir_Done: SEROUT OLED_TX, OLED_Baud, ["s", 0, 13, 0, mag1, mag2, "Direction: ", DEC wind_dir, 0] GOSUB GetAck ' DEBUG "Sending Measurements..." SEROUT OLED_TX, OLED_Baud, ["s", 0, 1, 0, grn1, grn2, "Sending Measurements ", 0] GOSUB GetAck READ c_lat_deg, deg ' get coordinates from EEPROM READ c_lat_min, minutes READ c_lat_minD_H, minutesD.HIGHBYTE READ c_lat_minD_L, minutesD.LOWBYTE READ c_lat_dir, dir SEROUT XPORT_TX, XPORT_Baud, [43 + (dir * 2), DEC deg, ":", DEC minutes, ".", DEC4 minutesD, " "] READ c_long_deg, deg ' get coordinates from EEPROM READ c_long_min, minutes READ c_long_minD_H, minutesD.HIGHBYTE READ c_long_minD_L, minutesD.LOWBYTE READ c_long_dir, dir SEROUT XPORT_TX, XPORT_Baud, [43 + (dir * 2), DEC deg, ":", DEC minutes, ".", DEC4 minutesD, " "] SEROUT XPORT_TX, XPORT_Baud, [43 + (target_dir * 2), DEC target_deg, ":", DEC target_minutes, ".", DEC4 target_minutesD, " ", 43 + (target_long_dir * 2), DEC target_long_deg, ":", DEC target_long_minutes, ".", DEC4 target_long_minutesD, " ", STR speed_str, " ", DEC wind_dir] ' DEBUG "Targeting Done!" SEROUT OLED_TX, OLED_Baud, ["s", 0, 1, 0, red1, red2, "System Done! ", 0] GOSUB GetAck END ' -----[ Subroutines ]----------------------------------------------------- Print_Target_GPS: SEROUT OLED_TX, OLED_Baud, ["s", 0, 7, 0, cy1, cy2, "Target Coordinates:", 0] GOSUB GetAck ' latitude 'DEBUG "Tar Lat: ", " " + (target_dir * 13), DEC target_deg, ":", DEC target_minutes, ".", DEC4 target_minutesD, CR SEROUT OLED_TX, OLED_Baud, ["s", 0, 8, 0, cy1, cy2, "Lat: ", 43 + (target_dir * 2), DEC target_deg, ":", DEC target_minutes, ".", DEC4 target_minutesD, 0] GOSUB GetAck ' longitude 'DEBUG "Tar Long: ", " " + (target_long_dir * 13), DEC target_long_deg, ":", DEC target_long_minutes, ".", DEC4 target_long_minutesD, CR SEROUT OLED_TX, OLED_Baud, ["s", 0, 9, 0, cy1, cy2, "Long: ", 43 + (target_long_dir * 2), DEC target_long_deg, ":", DEC target_long_minutes, ".", DEC4 target_long_minutesD, 0] GOSUB GetAck RETURN ' ---------------------------------------------------- No_RF: 'DEBUG "Error: Incomplete response from wristband!", CR ' ruh-roh, Shaggy SEROUT OLED_TX, OLED_Baud, ["s", 0, 1, 0, grn1, grn2, "Error: NO WRISTBAND ", 0] GOSUB GetAck PAUSE 10000 GOTO Init RETURN ' ---------------------------------------------------- No_GPS: 'DEBUG "Error: No response from GPS!", CR SEROUT OLED_TX, OLED_Baud, ["s", 0, 1, 0, grn1, grn2, "Error: NO GPS ", 0] GOSUB GetAck PAUSE 10000 GOTO Init RETURN ' -----[ OLED Subroutines ]----------------------------------------------------- GetACK: SERIN OLED_RX, OLED_Baud, 300, Missed, [tmp] IF (tmp = $06) THEN RETURN ELSE Missed: ENDIF RETURN PowerUp: SEROUT OLED_TX, OLED_Baud, [$59, $03, $01] 'Power Up The Screen PAUSE 500 RETURN '============================================================================== '-------------------- uM-FPU V3 SPI support routines --------------- 2006-10-27 '============================================================================== Fpu_Reset: SHIFTOUT FpuOut, FpuClk, MSBFIRST, [$FF, $FF, $FF, $FF, $FF, $FF, $FF, $FF, $FF, $FF] PAUSE 10 ' check for synchronization SHIFTOUT FpuOut, FpuClk, MSBFIRST, [SYNC] GOTO Fpu_Status2 Fpu_Wait: INPUT FpuIn ' (required for 2-wire interface) Fpu_Wait2: IF (FpuIn = 1) THEN GOTO Fpu_Wait2 ' wait until uM-FPU is ready RETURN Fpu_ReadStatus: GOSUB Fpu_Wait ' read status byte SHIFTOUT FpuOut, FpuClk, MSBFIRST, [READSTATUS] Fpu_Status2: SHIFTIN FpuIn, FpuClk, MSBPRE, [status] RETURN 'Print_Version: ' print the uM-FPU version string ' SHIFTOUT FpuOut, FpuClk, MSBFIRST, [VERSION] ' GOTO Print_FpuString Print_Float: format = 0 ' set format to zero (free format) ' (fall through to Print_FloatFormat) Print_FloatFormat: opcode = FTOA ' convert floating point to formatted ASCII GOTO Print_Format Print_Long: format = 0 ' set format to 0 (free format) ' (fall through to Print_LongFormat) Print_LongFormat: opcode = LTOA ' convert long integer to formatted ASCII ' (fall through to Print_Format) Print_Format: ' send conversion command SHIFTOUT FpuOut, FpuClk, MSBFIRST, [opcode, format] Print_FpuString: tmp = tmp_storage GOSUB Fpu_Wait ' wait until uM-FPU is ready SHIFTOUT FpuOut, FpuClk, MSBFIRST, [READSTR] DO SHIFTIN FpuIn, FpuClk, MSBPRE, [dataByte] IF (dataByte = 0 OR dataByte >= 127) THEN EXIT DEBUG dataByte WRITE tmp, dataByte ' store retrieved data in EEPROM in case we need it for later tmp = tmp + 1 LOOP WRITE tmp, 0 ' add null for footer WRITE tmp+1, 0 WRITE tmp+2, 0 WRITE tmp+3, 0 DEBUG CR RETURN '==================== end of uM-FPU SPI support routines ======================