/* DragonOSD Firmware version 4.8k Date: 22 January 2009 Copyright Daniel Wee 2007-2009 Intelligent Flight, AU This software is provided as is, and is made open sourced so that users can make their own modifications. However, the usage of this software is governed by the following conditions:- 1. You may not use this software to make your own boards for pecuniary interests without seeking permission from Intelligent Flight, AU first. 2. If you develop or modify this software, you should make it available for others. This would include all derivative code. In other words:- The software is released with the understanding that no part thereof or derivative may be used in whole or in part, for pecuniary interests without express permission from Intelligent Flight. And if you do make changes and improvements, do share it with the community so as to make it more fun for everyone. Code is compiled with MikroC for dsPIC - not fully independent of it's libraries yet but almost independent. */ //#define DEBUG #include #include #include #include "xprintf.c" #define BYTE unsigned char #define UCHAR unsigned char #define USHORT unsigned short #define UINT unsigned int #define ULONG unsigned long // Serial Port Input Buffer... // --------------------------- // Buffer Contention is handled by the fact that endbuf is only written // by the ISR, and startbuf is only written by the non-ISR routines. #define SBUFF_SIZE 160 // a power of 2 for fast index manipulation. #define BUFOVFL 1 #define CARRIAGE_RETURN 0x0D #define LINE_FEED 0x0A #define PACKETMAX 85 #define NAUTKM 1.8520000031807997 #define KMTOMILE 0.621369949495 #define MTOFT 3.28083989501 #define LOWV2S 7.0 // 2S cutoff #define LOWV3S 10.0 // 3S cutoff #define LOWV4S 13.0 // 4S cutoff #define LOWALT 5.0 #define LOWALTRATIO 0.07 // low cutoff as a percentage of range #define LOWRATE -3.0 #define MAXRANGE 1200 #define VALIDHDOP 1.3 // 1.3 #define VALIDVDOP 1.0 #define VALIDPDOP 3.5 #define MINVALIDREADINGS 75 // 75 #define SHOWHOMEDIST 5000 #define ROTLIMIT 15.0 #define ROCLIMIT 10.0 #define ROTSTEPGAIN 8 // 5 #define ROCSTEPGAIN 0 // 5 #define APROTGAIN 48 #define APROCGAIN 40.0 #define SPEEDLOWLIMIT 5.0 #define CRUISEALT 100.0 #ifndef VMULT1 #define VMULT1 0.02610 //0.01464 #define VMULT2 0.02585 //0.01464 #define IMULT2 0.081100 //0.10955 // 0.19584 // 0.244140625 // For +-120A full-scale 10-bits #define MAHMULT 0.0057642 // 0.00039 // 0.01695421007 // 25/36*IMULT2 #endif #define MAXHDGCHANGE 45 #define MAXALTCHANGE 10 #define MAXGAIN 50 #define MAXWAYPOINTS 2 #define HIGHMAH 1700 #define AN3OFFSET 120 // 44 #define CURSAMPS 2 #define CONFIGLIMIT 28 #define DEFAULTVIDEO 1 // PAL=1, NTSC=0 #define DEFLOGPERIOD 5 #define LOGSTART 0x7FFC5A // reserve 94 first bytes for other stuff #define LOGEND 0x7FFFF4 #define PI 3.141592653589793 #define SAFEZONE 0 #define DEFAHRS 0 #define CURTHRESH 5 #define PWMNEUTRAL 3750 #define DEADZONE 200 #define XCENTER 240 #define YCENTER 144 #define HORZL 80 #define VERTL 200 typedef struct { double lon; double lat; double alt; // char name[6]; } Waypoint; typedef struct { UINT prn; UINT snr; } SVStruct; BYTE ccbuf[SBUFF_SIZE]; UINT startbuf; UINT endbuf; UINT sError; /* typedef struct { BYTE ccbuf[SBUFF_SIZE]; UINT startbuf; UINT endbuf; UINT sError; // ULONG baud; } AsyncStruct; */ // void FlexUsartInit( ULONG baud , ULONG clk_khz ) ; int getccb(void); BYTE peekccb(void); void Setup_Interrupts(void); void Setup_GP_Ports(void); long double strtodbl(char *); void extractnumbers(char *); int traverse(int, char *); double altratio, vcutoff, distcutoff, vcutoff2, mahcutoff, mahmult, imult2; double aprotgain, aprocgain, cruisealt, showhome, validhdop; double rotlimit, roclimit, rotstepgain, rocstepgain, speedlowlimit, maxhdgchg, maxaltchg; int ruddir, eledir, minvalid, safezonex, safezoney; Waypoint wp[MAXWAYPOINTS]; long oldtimer7, ppm7, oldtimer8, ppm8, oldtimer2, ppm2, oldtimer1, ppm1; long ppm7neutral, ppm8neutral; double lon, lat, speed, hdg, alt, galt, hdop, homedist, homehdg, absdist, voltage2, current2, mAh, curmax; //altrate double speedmax, altmax, distmax, hdopvalid, voltage1, rateofturn, rateofclimb, palt; int stabilized, sv, elevator, rudder, starthour, startminute, startsecond, elevtemp, ruddtemp, metric, setupchanged; int hour, minute, setupselect, displayon, packeterror; // ds2450present, elhour, elminute, elsecond float second; int maxdataon, gpsdataon, GSVchanged, passthrough, autopilotinfo, homeset, setuppage, palmode, logperiod; int tempdataon, disabv1; //rudnorm2 rudnorm1 elenorm int pitch, roll; int x1, y1, x2, y2, x3, y3, drawahrs; unsigned long utcdate; unsigned long mAhRAW; int AN0RAW, AN1RAW, AN3RAW, an3offset, an3count, AN3AVE, AN4RAW, AN4AVE; unsigned long AN3SUM, AN4SUM; unsigned char lock, lonq, latq, quarter; //unsigned char outbuffer1[60]; unsigned char outbuffer2[60]; unsigned char packetstr[PACKETMAX]; //AsyncStruct as; SVStruct Sats[12]; // int xxx=0; typedef struct { unsigned int *ptr; unsigned int wordsize; } EEvar; EEvar eevars[CONFIGLIMIT-1] = { {(unsigned int *)&distcutoff, 2}, {(unsigned int *)&altratio, 2}, {(unsigned int *)&validhdop, 2}, {(unsigned int *)&minvalid, 1}, {(unsigned int *)&showhome, 2}, {(unsigned int *)&roclimit, 2}, {(unsigned int *)&rocstepgain, 2}, {(unsigned int *)&aprocgain, 2}, {(unsigned int *)&rotlimit, 2}, {(unsigned int *)&rotstepgain, 2}, {(unsigned int *)&aprotgain, 2}, {(unsigned int *)&speedlowlimit, 2}, {(unsigned int *)&cruisealt, 2}, {(unsigned int *)&maxhdgchg, 2}, {(unsigned int *)&maxaltchg, 2}, {(unsigned int *)&mahcutoff, 2}, {(unsigned int *)&eledir, 1}, {(unsigned int *)&ruddir, 1}, {(unsigned int *)&metric, 1}, {(unsigned int *)&an3offset, 1}, {(unsigned int *)&palmode, 1}, {(unsigned int *)&logperiod, 1}, {(unsigned int *)&mahmult, 2}, {(unsigned int *)&imult2, 2}, {(unsigned int *)&safezonex, 1}, {(unsigned int *)&drawahrs, 1}, {(unsigned int *)&disabv1, 1} }; void write_EEP(unsigned long *dest, unsigned int *sourced, int words) { int i; unsigned int *tmp; unsigned long desttmp; tmp = sourced; desttmp = *dest; for (i = 0; i < words; i++) { Eeprom_Write(desttmp, *tmp); tmp++; desttmp += 2; while(NVMCONbits.WR); } *dest += 2 * words; } void read_EEP(unsigned long *src, unsigned int *destd, int words) { int i; unsigned int *tmp; unsigned long srctmp; tmp = destd; srctmp = *src; for (i = 0; i < words; i++) { *tmp = Eeprom_Read(srctmp); tmp++; srctmp += 2; } *src += 2 * words; } long double strtodbl(char *string) { int i, len, hasdeci, neg = 0; long double dtemp = 0, place = 0.1; char temp[20] = ""; int toadd = 0; i = 0; while (i < 19 && string[i] >= '-' && string[i] <= '9') { temp[i] = string[i]; i++; } temp[i] = 0; len = i; hasdeci = len; if (temp[0] == '-'){ neg = 1; } for (i = neg; i < len; i++){ if (temp[i] == '.'){ hasdeci = i; if (i == neg){ hasdeci = -1; } break; }} if (hasdeci != -1){ for (i = hasdeci - 1; i >= neg; i--){ place = place * 10; dtemp = dtemp + ((temp[i] - 48) * place);}} place = 1; if (hasdeci != len - 1){ if (hasdeci == -1){ hasdeci = neg; } toadd = 0; if (neg == 0){ toadd = 1; } for (i = hasdeci + neg + toadd; i < len; i++){ place = place / 10; dtemp = dtemp + ((temp[i] - 48) * place);}} if (neg == 1){ dtemp = dtemp * -1; } return dtemp; } void extractnumbers(char *string) { char temp[20] = ""; int index = -1, len = strlen(string), i, gotno = 0, gotneg = 0, gotdeci = 0; for (i = 0; i < len; i++) { if (gotno == 0 && gotneg == 0 && string[i] == 45){ index++; temp[index] = 45; gotneg = 1; } if (gotdeci == 0 && string[i] == 46){ index++; temp[index] = 46; gotdeci = 1; } if (string[i] >= 48 && string[i] <= 57){ index++; temp[index] = string[i]; gotno = 1; } } strcpy(string, temp); } void Setup_GP_Ports(void) { // ADPCFGbits.PCFG2 = 1; // except _SS1/AN2 as output // ADPCFGbits.PCFG4 = 1; // and IC7 // ADPCFGbits.PCFG5 = 1; // and IC8 ADPCFG = 0x0034; // All ADC pins as analog inputs // Alternative configuration // ADCONbits.SIMSAM = 0; // No simultaneous sampling // ADCON1bits.SSRC = 2; // ADCON1bits.ASAM = 1; ADCON1 = 0x0044; // ADCON2bits.VCFG = 0; // ADCON2bits.SMPI = 5; // Interrupt on every 6th sample (due to 6 channels) // ADCON2bits.BUFM = 0; // ADCON2bits.ALTS = 0; // ADCON2bits.CSCNA = 1; // Scan channels ADCON2 = 0x0414; // ADCON3bits.ADCS = 3; // ADCON3bits.SAMC = 1; ADCON3 = 0x0103; // ADCHSbits.CH0SA = 0; ADCHS = 0; ADCSSL = 0b111001011; // AN0, AN1, AN3, AN6, AN7, AN8 - ADCBUF0, ADCBUF1, ADCBUF2, ADCBUF3 /* // ADCON1bits.SIMSAM = 1; // Simultaneous sampling seems faster // ADCON1bits.ASAM = 1; // Automatically re-start cycle once done // ADCON1bits.SSRC = 2; // ADC conversion triggered by TMR3 match with PR3 ADCON1 = 0x004C; // ADCON2bits.CHPS = 2; // Sample and convert CH0-3 - 01x // ADCON2bits.SMPI = 0; // Interrupt after 1 samples (simultaneous) and 4 conversions // ADCON2bits.VCFG = 0; // Use AVdd and AVss as the reference voltages // ADCON2bits.ALTS = 0; // Don't alternate multiplexers // ADCON2bits.CSCNA = 0; // No scanning // ADCON2bits.BUFM = 1; // Double buffering ADCON2 = 0x0202; // ADCON3bits.SAMC = 20; // Auto sample 4 Tad // ADCON3bits.ADCS = 7; // Conversion clock 8 Tcy ADCON3 = 0x0107; // ADCHSbits.CH0SA = 3; // AN3 points to ADCBUF0 // ADCHSbits.CH0SB = 3; // Same but on MUX B ADCHS = 0x0303; */ TRISB = 0xFFFB; // All PORTB/ADC pins configure as inputa for the moment // TRISBbits.TRISB2 = 0; // except for RB2 which is used to select BOB4 // TRISBbits.TRISB4 = 1; // and RB5 which is used to capture PPM channel PORTD = 0; TRISD = 0xFFFF; // Set as inputs (if LCD used, set as outputs) TRISE = 0; // All as outputs (mostly for LCD), RE8 as SCK TRISF = 0; } /* void FlexUsartInit( ULONG baud , ULONG clk_khz ) { // Take advantage of the 16-bit baud generator // built into the PIC18F4685... long lBrg ; lBrg = ( ((long)clk_khz) * 1000 / 64 / baud ); U1BRG = lBrg; } */ // Determines which interrupts should be enabled... void Setup_Interrupts() { U1STAbits.URXISEL = 0; // Interrupt at every character IPC2bits.U1RXIP = 2; // U1RXIP - Rx interrupt priority IFS0bits.U1RXIF = 0; // Clear U1RXIF - Rx flag IEC0bits.U1RXIE = 1; // U1RXIE - Enable Rx interrupt U2STAbits.URXISEL = 0; // Interrupt at every character IPC6bits.U2RXIP = 1; // U2RXIP - Rx interrupt priority IEC1bits.U2RXIE = 1; // U2RXIE - Enable Rx interrupt // Setup Timer 2 for use with PWM Capture TMR2 = 0; // T2CONbits.T32 = 0; // 32-bit timer (settings affect both TMR2 and TMR3) /* T2CONbits.TCKPS = 1; // Set prescaler to 1:8 (1, 8, 64, 256) T2CONbits.TCS = 0; // Use internal clock = 80MHz/4 = 20MHz T2CONbits.TON = 1; */ T2CON = 0x8010; PR3 = 78; // Sample at 32051 Hz TMR3 = 0; /* T3CONbits.TCKPS = 1; // Set prescaler to 1:64 (1, 8, 64, 256) (3 = 256, 2 = 64) T3CONbits.TCS = 0; // Use internal clock = 80MHz/4 = 20MHz T3CONbits.TON = 1; // Start timer */ T3CON = 0x8010; ADCON1bits.ADON = 1; // Turn on ADC module IFS0bits.ADIF = 0; // Clear ADIF flag IEC0bits.ADIE = 1; // Turn on ADC interrupts IC7CONbits.ICTMR = 1; // Select TMR2 IC7CONbits.ICM = 1; // Edge detection mode IPC4bits.IC7IP = 2; // Set interrupt priority 2 (higher than 1) IFS1bits.IC7IF = 0; // Clear IC7IF flag IEC1bits.IC7IE = 1; // IC7 - Enable IC7 capture interrupt IC8CONbits.ICTMR = 1; // Select TMR2 IC8CONbits.ICM = 1; // Edge detection mode IPC4bits.IC8IP = 2; // Set interrupt priority 2 (higher than 1) IFS1bits.IC8IF = 0; // Clear IC8IF flag IEC1bits.IC8IE = 1; // IC8 - Enable IC8 capture interrupt IC1CONbits.ICTMR = 1; // Select TMR2 IC1CONbits.ICM = 1; // Edge detection mode IPC0bits.IC1IP = 2; // Set interrupt priority 2 (higher than 1) IFS0bits.IC1IF = 0; // Clear IC1IF flag IEC0bits.IC1IE = 1; // IC1 - Enable IC1 capture interrupt IC2CONbits.ICTMR = 1; // Select TMR2 IC2CONbits.ICM = 1; // Edge detection mode IPC1bits.IC2IP = 2; // Set interrupt priority 2 (higher than 1) IFS0bits.IC2IF = 0; // Clear IC2IF flag IEC0bits.IC2IE = 1; // IC2 - Enable IC2 capture interrupt } void ADCinterrupt() org 0x2A { AN0RAW = ADCBUF0; AN1RAW = ADCBUF1; AN3RAW = ADCBUF2; AN4RAW = ADCBUF3; /* AN0RAW = ADCBUF1; AN1RAW = ADCBUF2; AN3OUT = ADCBUF0; AN3SUM += AN3OUT; */ AN3SUM += AN3RAW; AN4SUM += AN4RAW; an3count++; if (an3count >= 1000) { AN3AVE = AN3SUM/an3count - an3offset; AN4AVE = AN4SUM/an3count; an3count = AN3SUM = AN4SUM = 0; if (AN3AVE < 0) AN3AVE = 0; mAhRAW += AN3AVE; quarter++; } IFS0bits.ADIF = 0; // Clear interrupt } void IC1interrupt() org 0x16 { asm { IC1L0020: btss IC1CON, 3 // test IC1BNE, if set then do 0025 else 0030 goto IC1L0030 mov IC1BUF, W0 // scratch = IC1BUF mov #0, W0+2 goto IC1L0020 IC1L0030: mov #@_oldtimer1, W1 btss PORTD, 0 // test RD0, if set then do 0035 else 0040 goto IC1L0040 mov W0, [W1] // oldtimer = IC1BUF mov W0+2, [W1+2] goto IC1L0050 IC1L0040: mov W0, W2 sub W2, [W1], W0 mov W0, _ppm1 clr W1 mov W1, _ppm1+2 mov _setuppage, W1 cp0 W1 bra nz, IC1L0050 // if not zero - ie. setuppage == 1, don't move servos mov _passthrough, W1 cp0 W1 bra z, IC1L0050 // if not zero - ie. passthrough = 1 then skip IC1L0045: mov W0, PDC3 IC1L0050: bclr IFS0, #1 // clear IC1IF IC1L0055: } } void IC2interrupt() org 0x1C { asm { IC2L0020: btss IC2CON, 3 // test IC2BNE, if set then do 0025 else 0030 goto IC2L0030 mov IC2BUF, W0 // scratch = IC2BUF mov #0, W0+2 goto IC2L0020 IC2L0030: mov #@_oldtimer2, W1 btss PORTD, 1 // test RD1, if set then do 0035 else 0040 goto IC2L0040 mov W0, [W1] // oldtimer = IC2BUF mov W0+2, [W1+2] goto IC2L0050 IC2L0040: mov W0, W2 sub W2, [W1], W0 mov W0, _ppm2 clr W0 mov W0, _ppm2+2 IC2L0050: bclr IFS0, #4 // clear IC2IF IC2L0055: } } void IC7interrupt() org 0x36 { asm { IC7L0020: btss IC7CON, 3 // test IC7BNE, if set then do 0025 else 0030 goto IC7L0030 mov IC7BUF, W0 // scratch = IC7BUF mov #0, W0+2 goto IC7L0020 IC7L0030: mov #@_oldtimer7, W1 btss PORTB, 4 // test RB4, if set then do 0035 else 0040 goto IC7L0040 mov W0, [W1] // oldtimer = IC7BUF mov W0+2, [W1+2] goto IC7L0050 IC7L0040: mov W0, W2 sub W2, [W1], W0 mov W0, _ppm7 clr W1 mov W1, _ppm7+2 mov _setuppage, W1 cp0 W1 bra nz, IC7L0050 // if not zero - ie. setuppage == 1, don't move servos mov _passthrough, W1 cp0 W1 bra z, IC7L0050 // if not zero - ie. passthrough = 1 then skip IC7L0045: mov W0, PDC1 IC7L0050: bclr IFS1, #1 // clear IC7IF IC7L0055: } } void IC8interrupt() org 0x38 { asm { IC8L0020: btss IC8CON, 3 // test IC8BNE, if set then do 0025 else 0030 goto IC8L0030 mov IC8BUF, W0 // scratch = IC8BUF mov #0, W0+2 goto IC8L0020 IC8L0030: mov #@_oldtimer8, W1 btss PORTB, 5 // test RB5, if set then do 0035 else 0040 goto IC8L0040 mov W0, [W1] // oldtimer = IC8BUF mov W0+2, [W1+2] goto IC8L0050 // This is the up pulse so don't do anything yet IC8L0040: mov W0, W2 sub W2, [W1], W0 mov W0, _ppm8 clr W1 mov W1, _ppm8+2 mov _setuppage, W1 cp0 W1 bra nz, IC8L0050 // if not zero - ie. setuppage == 1, don't move servos mov _passthrough, W1 cp0 W1 bra z, IC8L0050 // if passthrough is NOT 0 (ie, NOT in autopilot) - then passthrough IC8L0045: mov W0, PDC2 IC8L0050: bclr IFS1, #2 // clear IC8IF IC8L0055: } } unsigned int buffspace(void) { asm { bclr PORTB, 1 BSL0: bclr SPI1STAT, #6 // SPIROV BSL1: btsc SPI1STAT, 1 goto BSL1 // wait till SPITBF is clear - ie. TX buffer clear for loading mov #0xFF, W0 mov W0, SPI1BUF // send the status request command BSL2: btss SPI1STAT, 0 // wait for data to be available, SPIRBF must be 1 if full goto BSL2 mov SPI1BUF, W0 // read the data, resets SPIRBF flag at the same time mov #0xFF, W1 cp.b W0, W1 bra nz, BSL0 // keep sending status request until status is received bclr SPI1STAT, #6 // SPIROV BSL3: btsc SPI1STAT, 1 goto BSL3 // wait till SPITBF is clear - ie. TX buffer clear for loading clr W0 mov W0, SPI1BUF // send a NULL to clock in next byte - the status byte BSL4: btss SPI1STAT, 0 // wait for data to be available goto BSL4 mov SPI1BUF, W0 // read the status byte and #0x0F, W0, W0 mov #1, W1 sl W1, W0, W0 bset PORTB, 1 } } void send_BOB(unsigned char *bobstr) { unsigned int slen; slen = strlen(bobstr); while (slen > buffspace()); // Delay_ms(2); asm { bclr PORTB, 1 sub W14, #8, W0 mov [W0], W1 // pointer to bobstr SPIL0: mov.b [W1++], W0 ze W0,W0 // zero extend, also tests for zero bra z, SPIL2 SPIL1: btsc SPI1STAT, 1 goto SPIL1 // wait till SPITBF is clear - ie. TX buffer clear for loading mov W0, SPI1BUF goto SPIL0 SPIL2: bset PORTB, 1 } } void U1RXinterrupt() org 0x26 { if (U1STAbits.OERR == 0) { // No errors... while (U1STAbits.URXDA) { ccbuf[endbuf++] = U1RXREG; endbuf %= SBUFF_SIZE; if ((endbuf+1)%SBUFF_SIZE == startbuf) { // Buffer Overflow!... sError = BUFOVFL; startbuf = 0; endbuf = 0; } } } else U1STAbits.OERR = 0; // clear error flag IFS0bits.U1RXIF = 0; } int attd[4] = { 0, 0, 0, 0 }; int *imudata; int imusign; void U2RXinterrupt() org 0x44 { unsigned int c, d; if (U2STAbits.OERR == 0) { // error-handling while (U2STAbits.URXDA) { /* asm { mov U2RXREG, W0 // W0 is character mov W0, W1 lsr W1, #4, W1 // check extension if it's bigger than 0x90 and #0x0F. W1 cp W1, #9 bra le, U2ISR0010 // if not skip to next section mov #1, W1 // get sign into imusign btsc W0, 5 neg W1, W1 mov W1, _imusign mov W0, W1 // get offset into W1 lsr W1, #5, W1 sub #5, W1 add W1, W1, W1 // double offset due to integer (word-sized) array add #@_attd, W1, W1 // W1 = &attd + offset*2 mov W1, _imudata and #0x0F, W0 // get hundreds value mov #100, W2 mul.uu W0, W2, W0 mov W0, [W1] // store into *imudata bra U2ISR0040 U2ISR0010: mov W0, W1 // get tens into W1 lsr W1, #4, W1 and #0x0F, W1 mul.uu W1, #10, W1 and #0x0F, W0 // get ones add W0, W1, W0 mov _imudata, W1 // add to whatever's in *imudata add W0, [W1], W0 mov _imusign, W1 // process the sign as well mul.ss W0, W1, W0 mov W0, _imudata // store it all away in imudata mov #@_attd, W1 mov [W1], W0 mov W0, _roll add W1, #2, W1 mov [W1], W0 mov W0, _pitch U2ISR0040: } */ c = U2RXREG; if ((c & 0xF0) > 0x90) { // if the high nibble is 0x0A or greater then this is the high byte imudata = &attd + ((c>>5) - 5); *imudata = (c & 0x0F) * 100; if (c & 0x10) imusign = -1; else imusign = 1; } else { d = c>>4; d &= 0x0F; d *= 10; d += (c & 0x0F); *imudata += d; *imudata *= imusign; roll = -attd[0]; pitch = attd[1]; } } } else U2STAbits.OERR = 0; // clear error flag IFS1bits.U2RXIF = 0; } // Retrieves a byte from the circular buffer... int getccb(void) { int ret_value; ret_value = -1; if (endbuf != startbuf) { ret_value = (int)ccbuf[startbuf++]; startbuf %= SBUFF_SIZE; } return(ret_value); } // Tells whether or not a byte is available in the circular buffer... BYTE peekccb(void) { return(endbuf != startbuf); } void Uart_write_str(char *outstr) { int j; j = 0; while (outstr[j] != 0) { while (U1STAbits.UTXBF); // wait for TX buffer to clear U1TXREG = outstr[j++]; } } void send_PMTK(unsigned char *Pkt) { unsigned int checksum; unsigned char Chk[4]; int i; checksum = 2; // Already computed TalkerID checksum "PMTK" i = 0; while (Pkt[i] != 0) checksum ^= Pkt[i++]; xprintf(Chk,"%02x", checksum); i = 0; while (Chk[i] != 0) { Chk[i] = toupper(Chk[i]); i++; } xprintf(outbuffer2, "$PMTK%s*%s\r\n", Pkt, Chk); Uart_write_str(outbuffer2); } void bprintf(const char *fmt, ...) { va_list args; int n; unsigned char outbuffer1[60]; va_start(args, fmt); n = vsprintf(outbuffer1, fmt, args); send_BOB(outbuffer1); } void pattrib(int attrib) { bprintf("\033[%dm",attrib); } void resetmode(void) { pattrib(69); } void cls(void) { send_BOB("\033[2J"); } void cursorH(int x, int y) { bprintf( "\033[0z\033[%d;%dH", y, x); } void cursor(int x, int y) { bprintf( "\033[0z\033[%d;%dx", x, y); } void cursor4(int x, int y) { bprintf( "\033[4z\033[%d;%dx", x, y); } void cursor1(int selection) { int x, y, z; x = 24; z = 16 + palmode*3; y = selection; if (y > z) { x = 220; y -= (z+1); } y *= 13; y += 26; bprintf("\033[1z\033[%d;%dx", x, y); } void centre_BOB(unsigned char *bobstr) { unsigned int i; if (displayon) { i = (40 - strlen(bobstr))/2; cursorH(i,8+palmode); send_BOB(bobstr); } } void setline(int xa, int ya, int xb, int yb) { bprintf("\033[%d;%d.r", xa+XCENTER, ya+YCENTER); bprintf("\033[%d;%d-r", xb+XCENTER, yb+YCENTER); } void drawline(int xa, int ya, int xb, int yb) { setline(xa, ya, xb, yb); send_BOB("\033[/r"); } void eraseline(int xa, int ya, int xb, int yb) { setline(xa, ya, xb, yb); send_BOB("\033[0/r"); } void drawhorizon(void) { int xx, yy; float vv; // send_BOB("\033[q"); // We may want to limit the length of vv to prevent draw errors vv = -VERTL * sin((double)pitch *PI/180.0); if (vv > 100) vv = 100; else if (vv < -100) vv = -100; // Draw the vertical line yy = vv * sin(((double)roll+90.0) *PI/180.0); xx = vv * cos(((double)roll+90.0) *PI/180.0) * 4/3; // * 3/2 if (xx != x3 || yy != y3) { eraseline(0, 0, x3, y3); x3 = xx; y3 = yy; drawline(0, 0, x3, y3); } // Draw the horizon yy = HORZL * sin((double)roll *PI/180.0); xx = HORZL * cos((double)roll *PI/180.0) * 4/3; // * 3/2 if (xx != x1 || yy != y1) { eraseline(x1, y1, x2, y2); y1 = yy + y3; x1 = xx + x3; y2 = -yy + y3; x2 = -xx + x3; drawline(x1, y1, x2, y2); } // Draw the little circle for the centre marker /* bprintf("\033[%d;%d.r", 4+XCENTER, YCENTER); send_BOB("\033[4;0;360)r\033[/r"); */ // send_BOB("\033[q"); } void DRAWOSD(void) { int i, j, lefthdg, dat, xref; double scratch; unsigned char altarrow, utfhigh; #ifdef DEBUG cursor(20, 61); if (sError == BUFOVFL) { sError = 0; send_BOB("OVERFLOW"); } else send_BOB(" "); #endif // This section handles the time display at the top left cursor(1+safezonex,11+safezoney); if (homeset == 0) bprintf("%02d:%02d\033[4z%02d UTC", hour, minute, (int)second); else { i = (int)second - startsecond; j = minute; lefthdg = hour; if (i < 0) { i += 60; j--; } j -= startminute; if (j < 0) { j += 60; lefthdg--; } lefthdg -= starthour; if (lefthdg < 0) lefthdg += 24; bprintf("%02d:%02d\033[4z%02d", lefthdg, j, i); } // This section handles the altitude and speed info at the top right group cursor(375-safezonex,11+safezoney); if (metric) bprintf("%#6.1f\033[1zkph", speed); else bprintf("%#6.1f\033[1zmph", speed * KMTOMILE); if (rateofclimb > -0.2 && rateofclimb < 0.2) { altarrow = 0x0A0; utfhigh = 0x0C0; } else { utfhigh = 0x0C4; if (rateofclimb > 0) altarrow = 0x082; else altarrow = 0x081; } if (alt < altratio*homedist) pattrib(65); else resetmode(); cursor(375-safezonex,24+safezoney); if (metric) bprintf("%#6.1f\033[1zm", alt); else bprintf("%#6.1f\033[1zft", alt * MTOFT); bprintf("\033[0z%c%c", utfhigh, altarrow); resetmode(); // This section handles the vertical recticle - incomplete /* cursor4(375-safezonex, 80+safezoney); send_BOB(" "); for (j=1; j<11; j++) { cursor(375-safezonex, 80+safezoney+j*10); send_BOB("- "); } cursor(375-safezonex, 80+safezoney+j*10); send_BOB(" "); */ // This section handles the voltage display at the top left if (!disabv1 && voltage1 < vcutoff) pattrib(65); else resetmode(); cursor(1+safezonex,24+safezoney); bprintf("%#5.2f\033[1zV", voltage1); resetmode(); // This is for the temperature display at the top left, below the voltage cursor(1+safezonex,37+safezoney); if (AN4AVE > 83) { scratch = ((AN4AVE * 0.0032226525)-0.401)/0.00625+0.5; if (metric) bprintf("\033[0z%5.0f\033[1zC", scratch); else bprintf("\033[0z%5.0f\033[1zF",32.0+scratch*9.0/5.0); tempdataon = 1; } else if (tempdataon) { bprintf("\033[0z\033[7X"); tempdataon = 0; } resetmode(); // This section handles the voltage and current group at the bottom left dat = 208 + 48*palmode - safezoney; xref = 1 + safezonex; if (voltage2 > CURTHRESH) { cursor(xref,dat); if (voltage2 < vcutoff2) pattrib(65); else resetmode(); bprintf("%#5.2f\033[1zV\033[69m\033[4z EXT", voltage2); cursor(xref,dat+13); bprintf("%#5.1f\033[1zA", current2); cursor(xref,dat+26); if (mAh > mahcutoff) pattrib(65); else resetmode(); bprintf("%5.0f\033[1zmAh", mAh); resetmode(); } else { cursor(xref,dat); send_BOB("\033[8X"); cursor(xref,dat+13); send_BOB("\033[6X"); cursor(xref,dat+26); send_BOB("\033[7X"); } if (homeset != 0) { // This section handles the distance group at bottom right dat = 221 + 48*palmode - safezoney; cursor(387 - safezonex,dat); if (homedist < showhome) { if (metric) bprintf("%5.0f\033[1zm", homedist); else { if (homedist * KMTOMILE / 10000 > 1.0) bprintf("%#5.2f\033[1zml", homedist * KMTOMILE / 1000); else bprintf("%5.0f\033[1zft", homedist * MTOFT); } } else send_BOB("\033[7X"); cursor(363 - safezonex,dat+13); send_BOB("\033[4zLOS \033[0z"); if (absdist > distcutoff) pattrib(65); else resetmode(); if (metric) bprintf("%5.0f\033[1zm", absdist); else { if (absdist * KMTOMILE / 10000 > 1.0) bprintf("%#5.2f\033[1zml", absdist * KMTOMILE / 1000); else bprintf("%5.0f\033[1zft", absdist * MTOFT); } resetmode(); // This section handles the peak values displayed when stopped dat = 143 + 48*palmode - safezoney; xref = 339 - safezonex; if (speed < 0.5) { cursor4(xref+40,dat); send_BOB("PEAK VALUES"); if (metric) { cursor(xref,dat+13); bprintf("\033[4zSPEED \033[0z%#6.1f\033[1zkph", speedmax); cursor(xref,dat+26); bprintf("\033[4z ALT \033[0z%#6.1f\033[1zm", altmax); cursor(xref,dat+39); bprintf("\033[4zRANGE \033[0z%6.0f\033[1zm", distmax); } else { cursor(xref,dat+13); bprintf("\033[4zSPEED \033[0z%#6.1f\033[1zmph", speedmax * KMTOMILE); cursor(xref,dat+26); bprintf("\033[4z ALT \033[0z%#6.1f\033[1zft", altmax * MTOFT); cursor(xref,dat+39); bprintf("\033[4zRANGE \033[0z%#6.2f\033[1zml", distmax * KMTOMILE / 1000); } if (voltage2 > 1) { cursor(xref,dat+52); bprintf("\033[4z CURR \033[0z%#6.1f\033[1zA", curmax); } maxdataon = 1; } else if (maxdataon == 1) { // erase readings cursor4(xref+40,dat); send_BOB("\033[11X"); // 11X cursor(xref,dat+13); send_BOB("\033[12X"); // 12X cursor(xref,dat+26); send_BOB("\033[12X"); // 12X cursor(xref,dat+39); send_BOB("\033[12X"); // 12X cursor(xref,dat+52); send_BOB("\033[12X"); // 12X maxdataon = 0; } } // This section handles the display of GPS info at the top centre group if ((homeset == 0 || alt < altratio*homedist || passthrough == 0) && setuppage == 0) { cursor(120,11+safezoney); if (sv < 3) bprintf("\033[65m%2d\033[69m\033[4z SATS", sv); else bprintf("%2d\033[4z SATS", sv); cursor(230,11+safezoney); bprintf("%#10.6f\033[4z%c", lat, latq); cursor(230,24+safezoney); bprintf("%#10.6f\033[4z%c", lon, lonq); cursor(110,24+safezoney); if (hdop > 0) bprintf("\033[4zHDOP \033[0z%#-6.1f", hdop); else send_BOB("\033[9X"); gpsdataon = 1; } else if (gpsdataon == 1) { // erase GPS data cursor(120,11+safezoney); send_BOB("\033[5X"); cursor(230,11+safezoney); send_BOB("\033[11X"); cursor(230,24+safezoney); send_BOB("\033[11X"); cursor(110,24+safezoney); send_BOB("\033[9X"); gpsdataon = 0; } /* if (test++ >= 360) test = 0; hdg = test; homehdg = 0; homeset = 1; */ // This section handles the compass reticule i = 0; dat = 217 + 48*palmode - safezoney; xref = 140; lefthdg = hdg; lefthdg -= 108; if (lefthdg < 0) lefthdg += 360; lefthdg /= 6; lefthdg *= 6; while (i < 39) { lefthdg += 6; if (lefthdg >= 360) lefthdg -= 360; if ((lefthdg % 30) != 0) outbuffer2[i++] = '-'; else { if ((lefthdg % 90) != 0) { xprintf(outbuffer2+i,"%03d", lefthdg); i += 3; } else { outbuffer2[i++] = '-'; switch (lefthdg) { case 0: altarrow = 'N'; break; case 90: altarrow = 'E'; break; case 180: altarrow = 'S'; break; case 270: altarrow = 'W'; break; default: altarrow = '?'; break; } outbuffer2[i++] = altarrow; outbuffer2[i++] = '-'; } lefthdg += 12; } } outbuffer2[i] = 0; i = hdg; i %= 6; if (i == 0) outbuffer2[34] = 0; else outbuffer2[35] = 0; i = xref - i; // determines horizontal start position outbuffer2[35] = 0; cursor4(i,dat+10); bprintf(" %s ", outbuffer2+3); // This section handles the center heading readout above the compass lefthdg = (double)(homehdg - hdg); cursor4(xref+90,dat); // * if (lefthdg == 0 && homeset) bprintf("\033[74m%03.0f", hdg); else bprintf("%03.0f", hdg); resetmode(); #ifdef DEBUG drawhorizon(); #endif if (homeset) { if (drawahrs) drawhorizon(); // This section handles the "H" home indicator if (lefthdg < -180) lefthdg += 360; else if (lefthdg > 180) lefthdg -= 360; // lefthdg is now -180 to 180 for (j = 0; j<31; j++) outbuffer2[j] = ' '; outbuffer2[j] = 0; if (lefthdg > -96 && lefthdg < 96) { j = lefthdg + 90; // j = lefthdg + 84 - 0.5; i = j % 6; j /= 6; if (j >= 0 && j < 31) outbuffer2[j] = 'H'; // "\033[65mH\033[69m" } else i = 0; // i = hdg - homehdg; // i %= 6; // i = xref - i; // * // cursor4(i+6,dat+20); // * i += xref; cursor4(i, dat+20); if (lefthdg == 0) bprintf(" \033[71m%s\033[69m ", outbuffer2); else if (lefthdg < 7 || lefthdg > -7) bprintf(" %s ", outbuffer2); else bprintf(" %s ", outbuffer2); // This section handles the indicator arrows at either ends of the compass if (lefthdg == 0) { cursor(xref-7,dat+10); // * send_BOB("\xC0\xA0"); cursor(xref+191,dat+10); // * send_BOB("\xC0\xA0"); } else { if (lefthdg < 0) { cursor(xref-7,dat+10); // * if (lefthdg < -90) send_BOB("\xC3\xBF"); else send_BOB("\xC4\x80"); cursor(xref+191,dat+10); // * send_BOB("\xC0\xA0"); } if (lefthdg > 0) { cursor(xref+191,dat+10); // * if (lefthdg > 90) send_BOB("\xC4\x80"); else send_BOB("\xC3\xBF"); cursor(xref-7,dat+10); // * send_BOB("\xC0\xA0"); } } } else if (GSVchanged) { // This section handles the GPS satellites display during home-setting dat = 11 + palmode; GSVchanged = 0; // shell_sort(); cursorH(11,dat); for (i = 0; i < 12; i++) { if (Sats[i].snr < 50) utfhigh = 0x090; else { utfhigh = 0x090 + (Sats[i].snr - 49) / 7; if (utfhigh > 0x096) utfhigh = 0x096; } bprintf("\xC4%c\033[4z \033[0z", utfhigh); } cursorH(11,dat+1); for (i = 0; i < 12; i++) { if (Sats[i].snr >= 49) utfhigh = 0x096; else utfhigh = 0x090 + (Sats[i].snr) / 7; bprintf("\xC4%c\033[4z \033[0z", utfhigh); } cursorH(11,dat+2); send_BOB("\033[4z"); for (i = 0; i < 12; i++) { if (Sats[i].prn != 0) { bprintf("%02d ", Sats[i].prn); } else send_BOB("-- "); } } // error occured in packet - either unrecognized packet or corrupt /* if (packeterror) { switch (packeterror) { case 1: send_BOB("\033[0z\033[1;256xCHKSUM "); break; case 2: send_BOB("\033[0z\033[1;256xUNKNOWN"); break; default: break; } } else send_BOB("\033[0z\033[1;256x\033[7X"); */ } unsigned int traverse(int idx, char *haystack) { unsigned int i, j; j = idx; i = 0; while (j > 0) { j--; while (haystack[i++] != ','); } return i; } int process_NMEA(char *nmeastr) { int i, j, k; unsigned long logdate; int checksum; double min, templon, templat; unsigned long logsrc; checksum = 0; i = strlen(nmeastr) - 2; if (nmeastr[i-1] != '*') return 0; nmeastr[i-1] = 0; // null terminate at the '*' j = nmeastr[i] - '0'; if (j > 9) j -= 7; checksum = j * 16; j = nmeastr[i+1] - '0'; if (j > 9) j -= 7; checksum += j; // got the whole checksum now i = j = packeterror = 0; while (nmeastr[i] != 0) j ^= nmeastr[i++]; if (checksum != j) { packeterror = 1; return 0; } if (strncmp("GPRMC", nmeastr, 5) == 0) { lock = nmeastr[17]; i = traverse(1, nmeastr); nmeastr[i+6] = 0; // second = atoi(nmeastr+i+4); second = strtodbl(nmeastr+i+4); nmeastr[i+4] = 0; minute = atoi(nmeastr+i+2); nmeastr[i+2] = 0; hour = atoi(nmeastr+i); i = traverse(3, nmeastr); min = strtodbl(nmeastr+i+2); nmeastr[i+2] = 0; lat = strtodbl(nmeastr+i) + min/60; latq = nmeastr[traverse(4, nmeastr)]; i = traverse(5, nmeastr); min = strtodbl(nmeastr+i+3); nmeastr[i+3] = 0; lon = strtodbl(nmeastr+i) + min/60; lonq = nmeastr[traverse(6, nmeastr)]; speed = strtodbl(nmeastr+traverse(7, nmeastr)) * NAUTKM; hdg = strtodbl(nmeastr+traverse(8, nmeastr)); utcdate = (unsigned long)atol(nmeastr+traverse(9, nmeastr)); return 1; } if (strncmp("GPGGA", nmeastr, 5) == 0) { /* i = traverse(1, nmeastr); nmeastr[i+6] = 0; second = atoi(nmeastr+i+4); nmeastr[i+4] = 0; minute = atoi(nmeastr+i+2); nmeastr[i+2] = 0; hour = atoi(nmeastr+i); i = traverse(2, nmeastr); min = strtodbl(nmeastr+i+2); nmeastr[i+2] = 0; lat = strtodbl(nmeastr+i) + min/60; latq = nmeastr[traverse(3, nmeastr)]; i = traverse(4, nmeastr); min = strtodbl(nmeastr+i+3); nmeastr[i+3] = 0; lon = strtodbl(nmeastr+i) + min/60; lonq = nmeastr[traverse(5, nmeastr)]; */ sv = atoi(nmeastr+traverse(7, nmeastr)); hdop = strtodbl(nmeastr+traverse(8, nmeastr)); // if (hdop < 0) hdop = 0 - hdop; galt = strtodbl(nmeastr+traverse(9, nmeastr)); return 1; } /* if (strncmp("GPVTG", nmeastr, 5) == 0) { hdg = strtodbl(nmeastr+traverse(1, nmeastr)); if (hdg < 0) hdg = 0; speed = strtodbl(nmeastr+traverse(5, nmeastr)); // speed = strtodbl(nmeastr+traverse(7, nmeastr)); // Speed in km/h if (speed < 0) speed = 0; return 1; } if (strncmp("GPGSA", nmeastr, 5) == 0) { i = traverse(2, nmeastr); fixmode = nmeastr[i]; i = traverse(15, nmeastr); if (nmeastr[i] != ',') { pdop = strtodbl(nmeastr+i); // if (pdop < 0) pdop = 0 - pdop; } i = traverse(16, nmeastr); if (nmeastr[i] != ',') { hdop = strtodbl(nmeastr+i); // if (hdop < 0) hdop = 0 - hdop; } i = traverse(17, nmeastr); if (nmeastr[i] != 0) { vdop = strtodbl(nmeastr+i); // if (vdop < 0) vdop = 0 - vdop; } return 1; } */ if (strncmp("GPGSV", nmeastr, 5) == 0) { GSVchanged = 1; j = atoi(nmeastr+traverse(3, nmeastr)); if (j < 0 ) j = 0; else if (j > 12) j = 12; i = j; for (;j < 12; j++) { Sats[j].prn = 0; Sats[j].snr = 0; } k = atoi(nmeastr+traverse(2, nmeastr)) - 1; if (k < 3) { i -= k * 4; if (i > 4) i = 4; for (j = 0; j < i; j++) { checksum = traverse(4+(j*4), nmeastr); if ((nmeastr[checksum] != 0) && (nmeastr[checksum] != ',')) { Sats[j+(k*4)].prn = atoi(nmeastr+checksum); checksum = traverse(4+(j*4)+3, nmeastr); Sats[j+(k*4)].snr = atoi(nmeastr+checksum) * 2; if (Sats[j+(k*4)].snr > 99) Sats[j+(k*4)].snr = 99; } } } return 1; } if (strncmp("PING", nmeastr, 5) == 0) { Uart_write_str("PONG\n"); return 1; } if (strncmp("GETLOG", nmeastr, 5) == 0) { logsrc = LOGSTART-2; read_EEP(&logsrc, (unsigned int *)&j, 1); read_EEP(&logsrc, (unsigned int *)&logdate, 2); // Uart_write_str("\nabsolute\n"); xprintf(outbuffer2, "<\n%010lu\n", logdate); Uart_write_str(outbuffer2); while (j-- > 0 && logsrc <= LOGEND) { read_EEP(&logsrc, (unsigned int *)&templon, 2); read_EEP(&logsrc, (unsigned int *)&templat, 2); read_EEP(&logsrc, (unsigned int *)&i, 1); xprintf(outbuffer2,"%8.6f,%8.6f,%d\n", templon, templat, i); Uart_write_str(outbuffer2); asm { clrwdt } } Uart_write_str(">\n"); // Uart_write_str("\n"); return 1; } packeterror = 2; return 0; } void initgps(int state) { send_PMTK("314,0,1,0,1,0,"); if (state) send_PMTK("1"); else send_PMTK("0"); send_PMTK(",0,0,0,0,0,0,0,0,0,0,0"); } void sethome(void) { int i; if (hdop > 0 && hdop <= hdopvalid) { hdopvalid = hdop; if (stabilized <= 0) { if (displayon) cls(); wp[0].lon = lon; wp[0].lat = lat; wp[0].alt = galt; if (latq == 'S') wp[0].lat = -wp[0].lat; if (lonq == 'W') wp[0].lon = -wp[0].lon; starthour = hour; startminute = minute; startsecond = (int)second; homeset = 1; initgps(0); resetmode(); } else { stabilized--; xprintf(outbuffer2, "Setting HOME %-3d", stabilized); centre_BOB(outbuffer2); } } else { if (stabilized != minvalid) { for (i = 0; i < 22; i++) outbuffer2[i] = ' '; outbuffer2[i] = 0; centre_BOB(outbuffer2); stabilized = minvalid; centre_BOB("HOME not set"); } hdopvalid = validhdop; } } void logposition(unsigned long *dest, int count) { double templon, templat; unsigned long countkeep = LOGSTART-2; int intalt; templon = lon; templat = lat; if (latq == 'S') templat = -templat; if (lonq == 'W') templon = -templon; intalt = (int)(alt + wp[0].alt); write_EEP(dest, (unsigned int *)&templon, 2); write_EEP(dest, (unsigned int *)&templat, 2); write_EEP(dest, (unsigned int *)&intalt, 1); intalt = count; write_EEP(&countkeep, (unsigned int *)&intalt, 1); } /* double myatan2(double y, double x) { return atan2(x, y); } */ double myatan2(double y, double x) { if (x > 0) return atan(y/x); if (y >= 0 && x < 0) return PI + atan(y/x); if (y < 0) { if (x < 0) return -PI + atan(y/x); if (x == 0) return -PI/2; } if (x == 0) if (y > 0) return PI/2; return 0; } void wayhome(int wpx) { double rlona, rlonb, rlata, rlatb; rlona = lon * PI/180; rlata = lat * PI/180; if (lonq == 'W') rlona = -rlona; if (latq == 'S') rlata = -rlata; rlonb = wp[wpx].lon * PI/180; rlatb = wp[wpx].lat * PI/180; homedist = 2*asin(sqrt(pow(sin((rlatb-rlata)/2),2)+cos(rlata)*cos(rlatb)*pow(sin((rlonb-rlona)/2),2))) * 180/PI * 60000 * NAUTKM; absdist = sqrt(pow(homedist,2) + pow(alt,2)); homehdg = myatan2(sin(rlonb-rlona)*cos(rlatb),cos(rlata)*sin(rlatb)-sin(rlata)*cos(rlatb)*cos(rlonb-rlona)) * 180/PI; if (homehdg < 0) homehdg += 360; // 0 to 359 degrees } void autopilot(int flag, int tick) { int dat, elenorm, rudnorm1; double alterror, hdgerror, desiredroc, desiredrot; dat = 50 + safezoney; if (flag == 0) { if (autopilotinfo) { autopilotinfo = 0; cursor(183,dat); send_BOB("\033[9X"); } } else { if (autopilotinfo == 0) { autopilotinfo = 1; elevtemp = ruddtemp = 0; cursor(183,dat); send_BOB("\033[79mAUTOPILOT"); } if (tick) { alterror = cruisealt-alt; if (alterror > maxaltchg) alterror = maxaltchg; else if (alterror < 0.0) alterror = 0.0; // else if (alterror < -maxaltchg) alterror = -maxaltchg; desiredroc = alterror * aprocgain; if (desiredroc > roclimit) desiredroc = roclimit; else if (desiredroc < -roclimit) desiredroc = -roclimit; if (speed < speedlowlimit) desiredroc *= speed/speedlowlimit; hdgerror = homehdg - hdg; // range from 359 to -359 if (hdgerror > 180) hdgerror -= 360; else if (hdgerror < -180) hdgerror += 360; if (hdgerror > maxhdgchg) hdgerror = maxhdgchg; // limit the size of the error else if (hdgerror < -maxhdgchg) hdgerror = -maxhdgchg; desiredrot = hdgerror/180 * aprotgain; if (desiredrot > rotlimit) desiredrot = rotlimit; else if (desiredrot < -rotlimit) desiredrot = -rotlimit; elevator = (desiredroc - rateofclimb) * rocstepgain; if (elevator > 300) elevator = 300; else if (elevator < -300) elevator = -300; rudder = (desiredrot - rateofturn) * rotstepgain; if (rudder > 300) rudder = 300; else if (rudder < -300) rudder = -300; } elevtemp += (elevator - elevtemp) / 3; ruddtemp += (rudder - ruddtemp) / 3; elenorm = elevtemp * eledir; rudnorm1 = ruddtemp * ruddir; PDC1 = ppm7 - elenorm; PDC2 = ppm8 + rudnorm1; PDC3 = ppm1 + rudnorm1; // For dual servo driven ailerons } } void enableUART2int(int onoff) { IFS1bits.U2RXIF = 0; #ifdef DEBUG IEC1bits.U2RXIE = 1; onoff; // U2RXIE - Enable Rx interrupt #else // IEC1bits.U2RXIE = onoff; if (onoff) IEC1bits.U2RXIE = 1; // U2RXIE - Enable Rx interrupt else IEC1bits.U2RXIE = 0; #endif U2STAbits.OERR = 0; } void menulabel(int sel) { cursor1(sel); if (sel == setupselect) pattrib(74); else resetmode(); switch(sel) { case 0: xprintf(outbuffer2," MAXRANGE \033[0z%6.0f ", distcutoff); break; case 1: xprintf(outbuffer2," LOWALTRATIO \033[0z%6.2f ", altratio); break; case 2: xprintf(outbuffer2," VALIDHDOP \033[0z%6.1f ", validhdop); break; case 3: xprintf(outbuffer2," MINVALID \033[0z%6d ", minvalid); break; case 4: xprintf(outbuffer2," SHOWHOMELIM \033[0z%6.0f ", showhome); break; case 5: xprintf(outbuffer2," ROCLIMIT \033[0z%6.1f ", roclimit); break; case 6: xprintf(outbuffer2," ROCSTEPGAIN \033[0z%6.0f ", rocstepgain); break; case 7: xprintf(outbuffer2," APROCGAIN \033[0z%6.1f ", aprocgain); break; case 8: xprintf(outbuffer2," ROTLIMIT \033[0z%6.1f ", rotlimit); break; case 9: xprintf(outbuffer2," ROTSTEPGAIN \033[0z%6.0f ", rotstepgain); break; case 10: xprintf(outbuffer2," APROTGAIN \033[0z%6.1f ", aprotgain); break; case 11: xprintf(outbuffer2," SPEEDLOWLIM \033[0z%6.1f ", speedlowlimit); break; case 12: xprintf(outbuffer2," CRUISEALT \033[0z%6.0f ", cruisealt); break; case 13: xprintf(outbuffer2," MAXALTCHG \033[0z%6.0f ", maxaltchg); break; case 14: xprintf(outbuffer2," MAXHDGCHG \033[0z%6.0f ", maxhdgchg); break; case 15: xprintf(outbuffer2," MAHWARNING \033[0z%6.0f ", mahcutoff); break; case 16: xprintf(outbuffer2," ELESERVODIR \033[0z%6s ", (eledir==1?"NOR":"REV")); break; case 17: xprintf(outbuffer2," RDRSERVODIR \033[0z%6s ", (ruddir==1?"NOR":"REV")); break; case 18: xprintf(outbuffer2," UNITS %8s ", (metric?"METRIC":"IMPERIAL")); break; case 19: xprintf(outbuffer2," AMPSZERO \033[0z%6d ", an3offset); break; case 20: xprintf(outbuffer2," VIDEOMODE %8s ", (palmode?"PAL":"NTSC")); break; case 21: xprintf(outbuffer2," LOGPERIOD \033[0z%6d ", logperiod); break; case 22: xprintf(outbuffer2," HOMELOC %8s ", (homeset?"SET":"UNSET")); break; case 23: xprintf(outbuffer2," MAHMULT \033[0z %7.5f ", mahmult); break; case 24: xprintf(outbuffer2," AMPMULT \033[0z%6.3f ", imult2); break; case 25: xprintf(outbuffer2," SAFEZONE \033[0z%6d ", safezonex); break; case 26: xprintf(outbuffer2," VOLT1LOWARN \033[0z%6s ", (disabv1?"OFF":" ON")); break; case 27: xprintf(outbuffer2," DRAWHORIZON \033[0z%6s ", (drawahrs?"ON":" OFF")); break; case CONFIGLIMIT: xprintf(outbuffer2," SAVE CONFIG \033[0z%6s ", (setupchanged?"*":"DONE")); break; default: break; } send_BOB(outbuffer2); } void drawmenu(void) { int k; for (k = 0; k <= CONFIGLIMIT; k++) menulabel(k); } void dosetup(void) { int adj, oldselection, i; unsigned long dest; oldselection = setupselect; if (ppm7 < ppm7neutral-DEADZONE) { setupselect += eledir; if (setupselect < 0) setupselect = 0; if (setupselect > CONFIGLIMIT) setupselect = CONFIGLIMIT; } else if (ppm7 > ppm7neutral+DEADZONE) { setupselect -= eledir; if (setupselect < 0) setupselect = 0; if (setupselect > CONFIGLIMIT) setupselect = CONFIGLIMIT; } else { adj = 0; if (ppm8 > ppm8neutral+DEADZONE) adj = 1; else if (ppm8 < ppm8neutral-DEADZONE) adj = -1; adj *= ruddir; if (adj != 0) { if(setupselect != CONFIGLIMIT) { setupchanged = 1; // indicate that some setting has been changed menulabel(CONFIGLIMIT); } switch (setupselect) { case 0: // distcutoff if ((distcutoff >= 1 && adj < 0) || (distcutoff < 99999 && adj > 0)) distcutoff += adj; break; case 1: // lowaltratio if ((altratio >= 0.01 && adj < 0) || (altratio < 1.00 && adj > 0)) altratio += 0.01 * adj; break; case 2: // validhdop if ((validhdop >= 0.2 && adj < 0) || (validhdop < 19.9 && adj > 0)) validhdop += 0.1 * adj; break; case 3: // minvalid if ((minvalid > 1 && adj < 0) || (minvalid < 149 && adj > 0)) minvalid += adj; break; case 4: // showhome if ((showhome > 1 && adj < 0) || (showhome < 99999 && adj > 0)) showhome += adj; break; case 5: // roclimit if ((roclimit > 0.1 && adj < 0) || (roclimit < 99.9 && adj > 0)) roclimit += 0.1 * adj; break; case 6: // rocstepgain if ((rocstepgain > 0 && adj < 0) || (rocstepgain < 49 && adj > 0)) rocstepgain += adj; break; case 7: // aprocgain if ((aprocgain > 0.1 && adj < 0) || (aprocgain < 99.9 && adj > 0)) aprocgain += 0.1 * adj; break; case 8: // rotlimit if ((rotlimit > 0.1 && adj < 0) || (rotlimit < 99.9 && adj > 0)) rotlimit += 0.1 * adj; break; case 9: // rotstepgain if ((rotstepgain > 0 && adj < 0) || (rotstepgain < 49 && adj > 0)) rotstepgain += adj; break; case 10: // aprotgain if ((aprotgain > 1 && adj < 0) || (aprotgain < 99 && adj > 0)) aprotgain += adj; break; case 11: // speedlowlimit if ((speedlowlimit > 0.1 && adj < 0) || (speedlowlimit < 99.9 && adj > 0)) speedlowlimit += 0.1 * adj; break; case 12: // cruisealt if ((cruisealt > 1 && adj < 0) || (cruisealt < 999 && adj > 0)) cruisealt += adj; break; case 13: // maxaltchg if ((maxaltchg > 1 && adj < 0) || (maxaltchg < 99 && adj > 0)) maxaltchg += adj; break; case 14: // maxhdgchg if ((maxhdgchg > 1 && adj < 0) || (maxhdgchg < 179 && adj > 0)) maxhdgchg += adj; break; case 15: // mahcutoff if ((mahcutoff > 10 && adj < 0) || (mahcutoff < 9999 && adj > 0)) mahcutoff += 10 * adj; break; case 16: // eledir if ((eledir > 0 && adj < 0) || (eledir < 0 && adj > 0)) eledir = adj; break; case 17: // ruddir if ((ruddir > 0 && adj < 0) || (ruddir < 0 && adj > 0)) ruddir = adj; break; case 18: // metric if ((metric > 0 && adj < 0) || (metric <= 0 && adj > 0)) metric += adj; break; case 19: // calibrate ammeter if ((an3offset > 0 && adj < 0) || (an3offset < 1000 && adj > 0)) an3offset += adj; break; case 20: // video mode if ((palmode > 0 && adj < 0) || (palmode <= 0 && adj > 0)) { palmode += adj; cls(); drawmenu(); } break; case 21: // logperiod if ((logperiod > 0 && adj < 0) || (logperiod < 300 && adj > 0)) logperiod += adj; break; case 22: // Reset homeset if (adj > 0) { homeset = 0; initgps(1); } break; case 23: // mahmult if ((mahmult > 0.00001 && adj < 0) || (mahmult < 1.0 && adj > 0)) mahmult += 0.00001 * adj; break; case 24: // imult2 if ((imult2 > 0.001 && adj < 0) || (imult2 < 1.0 && adj > 0)) imult2 += 0.001 * adj; break; case 25: // calibrate ammeter if ((safezonex > 0 && adj < 0) || (safezonex < 40 && adj > 0)) { safezonex += adj; safezoney = safezonex; } break; case 26: // disable flashing warning on the Voltage1 reading disabv1 = (adj==1); break; case 27: // draw artificial horizon line drawahrs = (adj==1); enableUART2int(drawahrs); break; case CONFIGLIMIT: // save config if (adj > 0) { // Write to EEPROM dest = 0x7FFC00; for (i=0; i= 4) xxx = 0; send_BOB(out); } */ void changebaud(int ubrgconst) { Delay_ms(50); send_PMTK("251,38400"); // Change baud rate to faster speed Delay_ms(100); U1BRG = ubrgconst; // set to 38400 baud } void main(void) { int i = 0, iChar, logcount, pktincoming = 0, tick, logtick; float oldsecond; double oldalt, oldhdg, P; unsigned long src, logdest, logdate; // Initialize hardware PWMCON1 = 0x770; TRISE = 0; PORTEbits.RE0 = 0; // Reset BOB-4 Delay_ms(1); PORTEbits.RE0 = 1; Uart1_Init(4800L); U1MODEbits.ALTIO = 1; // Use alternate pins for RX/TX since SPI is needed Uart2_Init(57600L); // Baudrate used by the IMU U2MODEbits.UARTEN = 1; imudata = &attd; ppm7 = ppm8 = ppm2 = ppm1 = PWMNEUTRAL; PTMR = 0; // Set time base? - should be about 20ms? (15-bits) // This is a free running 15-bit counter that counts up to PTPER // Default is counting upwards PTPER = 24999; // Set period (15-bits) - 20ms PTCON = 0x8008; PDC1 = PDC2 = PDC3 = PWMNEUTRAL; // Set duty cycle register to match 1.5ms startbuf = 0; endbuf = 0; sError = 0; Setup_GP_Ports(); Setup_Interrupts(); AN3SUM = 0; // Initialize the following section only if this is a normal reset, not a Watchdog time-out if (!RCONbits.WDTO) { wp[0].lat = wp[0].lon = wp[0].alt = 0; // strcpy(wp[0].name,"HOME"); starthour = startminute = startsecond = 0; //elhour = elminute = elsecond = 0; speedmax = altmax = distmax = curmax = 0; mAhRAW = 0; homeset = 0; logdest = LOGSTART; logdate = 0; logcount = 0; } // else homeset = 1; voltage1 = elevator = rudder = elevtemp = ruddtemp = 0; // pdop = vdop = palt = 0; oldtimer7 = oldtimer8 = oldtimer2 = oldtimer1 = 0; lon = lat = speed = hdg = alt = galt = rateofturn = rateofclimb = hdop = mAh = 0; //altrate homedist = homehdg = absdist = oldalt = oldhdg = 0; hour = minute = 0; second = oldsecond = 0.0; AN0RAW = AN1RAW = AN3RAW = AN4RAW = 0; lock = lonq = latq = ' '; maxdataon = tempdataon = gpsdataon = GSVchanged = sv = autopilotinfo = tick = logtick = setupchanged = 0; setuppage = setupselect = an3count = 0; for (i = 0; i < 12; i++) { Sats[i].prn = 0; Sats[i].snr = 0; } x1 = x2 = y1 = y2 = x3 = y3 = 0; pitch = roll = 0; passthrough = displayon = 1; quarter = 4; // Setup SPI port - not greater than 64/2 Prescale or lower than 4/6 Spi1_Init_Advanced(_SPI_MASTER, _SPI_8_BIT, _SPI_PRESCALE_PRI_4, _SPI_PRESCALE_SEC_8, _SPI_SS_DISABLE, _SPI_DATA_SAMPLE_MIDDLE, _SPI_CLK_IDLE_LOW, _SPI_IDLE_2_ACTIVE); // SPI1CONbits.SPIFSD = 0; // enable _SS pin as output // SPI1CONbits.FRMEN = 1; // enable Framed mode // EEPROM block src = 0x7FFC00; for (i=0; i 0) { i--; xprintf(outbuffer2, "GPS/OSD STARTUP %d", i); centre_BOB(outbuffer2); Delay_ms(1000); // Allow GPS to warm-up } cls(); U1STAbits.UTXEN = 1; // Enable TX U1MODEbits.UARTEN = 1; // Enable UART - overrides TRISC settings changebaud(129); // 9600 baud changebaud(21); changebaud(32); // 32 31 38400 baud // changebaud(10); // 10 10 115200 baud, 21 for 57600 IFS0bits.U1RXIF = 0; U1STAbits.OERR = 0; centre_BOB("Waiting for GPS data ..."); while (getccb() >= 0); // Clear read buffer while (!peekccb()) { // Wait for something new data asm { clrwdt } } Delay_ms(200); cls(); resetmode(); send_PMTK("000"); Delay_ms(100); if (RCONbits.WDTO) initgps(0); else initgps(1); RCON = 0; // reset RCON Delay_ms(100); send_PMTK("301,2"); // Enable WAAS mode send_PMTK("220,200"); // 5Hz data rate // send_PMTK("300,200,0,0,0,0"); i = 0; pktincoming = 0; enableUART2int(drawahrs); while (1) { while (peekccb()) { asm { clrwdt } iChar = getccb(); if (iChar >= 0) { if (i > PACKETMAX-1) { // check for packet overflow, should not exceed 255 bytes i = 0; // force end of session if overflow pktincoming = 0; } else switch(iChar) { case '$': // start of incoming packet i = 0; // reset packet size pktincoming = 1; // indicate packet in session break; case 13: packetstr[i++] = 0; // null termination break; case 10: packetstr[i] = 0; // make sure packet is closed if (pktincoming != 0) { pktincoming = 0; // End the packet session if (process_NMEA(packetstr) > 0) { alt = galt - wp[0].alt; // Or can be galt if using GPS altitude-- if (oldsecond != second) { if (oldsecond > second) oldsecond -= 60; // 59.0 59.2 59.6 59.8 00.0 00.2 00.4 00.6 tick++; logtick++; rateofclimb = alt - oldalt; rateofturn = hdg - oldhdg; if (rateofturn > 180) rateofturn -= 360; else if (rateofturn < -180) rateofturn += 360; rateofturn /= (second - oldsecond); rateofclimb /= (second - oldsecond); oldsecond = second; oldalt = alt; oldhdg = hdg; } if (homeset == 0) sethome(); else { wayhome(0); if (sv > 3) { if (speed > speedmax) speedmax = speed; if (absdist > distmax) distmax = absdist; if (alt > altmax) altmax = alt; } if (passthrough == 0 && setuppage == 0) autopilot(1, tick); else autopilot(0, tick); tick = 0; if (logperiod > 0 && logtick >= logperiod) { logtick = 0; if ((speed >= 0.5 || logcount == 0) && logdest <= LOGEND) { logdate += utcdate + 1000000*minute + 100000000*hour; if (logcount == 0) { logdest -= 2; write_EEP(&logdest, (unsigned int *)&logcount, 1); // Need a dummy write to get things started right write_EEP(&logdest, (unsigned int *)&logdate, 2); } logcount++; logposition(&logdest, logcount); } } } } } break; default: if (pktincoming != 0) packetstr[i++] = iChar; break; } } } if ((quarter >= 1) && (buffspace() >= 128)) { // Current set to 32/7 = 4.5Hz quarter = 0; voltage1 = (voltage1 + (double)AN0RAW*VMULT1)/2; // Switch to AN0 in new board revision, 3 on old board voltage2 = (voltage2 + (double)AN1RAW*VMULT2)/2; //if (j >= CURSAMPS) j = 0; //avgcur[j++] = AN3RAW; //tempcur = 0; //for (k = 0; k < CURSAMPS; k++) tempcur += avgcur[k]; //current2 = (double)tempcur/CURSAMPS * imult2; current2 = (double)AN3AVE * imult2; if (current2 > curmax) curmax = current2; mAh = (double)mAhRAW * mahmult; // full-scale is 1024 for 75A if ((ppm2 < 2800) && homeset) { // autopilot mode if (setuppage == 1) { setuppage = 0; gsvchanged = 1; cls(); } else { passthrough = 0; displayon = 1; DRAWOSD(); } } else { passthrough = 1; if (ppm2 > 5100) ppm2 = PWMNEUTRAL; if (ppm2 > 4700) { // Turn off screen data if (displayon == 1) { cls(); displayon = 0; } } else if (ppm2 > 4500) { // Configuration menu screen if (displayon == 1) { cls(); displayon = 0; } if (setuppage == 0) { setuppage = 1; ppm7neutral = ppm7; ppm8neutral = ppm8; drawmenu(); } dosetup(); } else { // Regular screen data if (setuppage == 1) { setuppage = 0; cls(); } else { displayon = 1; // while (buffspace() < 128); #ifdef DEBUG if (!U2STAbits.UTXBF) U2TXREG = 'a'; // poll attitude data from IMU #else if (drawahrs && !U2STAbits.UTXBF) U2TXREG = 'a'; // poll attitude data from IMU #endif DRAWOSD(); } } } buffspace(); } } }