/* DragonOSD Firmware version 4.6 Date: 12 Nov 2007 Copyright Daniel Wee 2007 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. */ #include #include #include #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 256 // a power of 2 for fast index manipulation. #define BUFOVFL 1 #define CARRIAGE_RETURN 0x0D #define LINE_FEED 0x0A #define PACKETMAX 100 #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 1 // 5 #define APROTGAIN 48 #define APROCGAIN 40.0 #define SPEEDLOWLIMIT 5.0 #define CRUISEALT 100.0 #ifndef VMULT1 #define VMULT1 0.02592 //0.01464 #define VMULT2 0.02592 //0.01464 #define IMULT2 0.072303 //0.10955 // 0.19584 // 0.244140625 // For +-120A full-scale 10-bits #define MAHMULT 0.0002574 // 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 66 // 44 #define CURSAMPS 2 #define CONFIGLIMIT 27 #define DEFAULTVIDEO 1 // PAL=1, NTSC=0 #define DEFLOGPERIOD 15 #define LOGSTART 0x7FFC5A // reserve 94 first bytes for other stuff #define LOGEND 0x7FFFF4 #define PI 3.141592653589793 #define SAFEZONE 0 typedef struct { double lon; double lat; double alt; char name[6]; } Waypoint; typedef struct { UINT prn; UINT snr; } SVStruct; 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 *); void bootloader(void); 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; double lon, lat, speed, hdg, alt, galt, hdop, homedist, homehdg, absdist, altrate, voltage2, current2, mAh, curmax; double speedmax, altmax, distmax, hdopvalid, voltage1, rateofturn, rateofclimb, palt; // double homelat, homelon, homealt; // double vdop, pdop; int stabilized, sv, elevator, rudder, starthour, startminute, startsecond, elevtemp, ruddtemp, metric, setupchanged; int hour, minute, second, elhour, elminute, elsecond, setupselect, displayon, packeterror; // ds2450present int maxdataon, gpsdataon, GSVchanged, passthrough, autopilotinfo, homeset, setuppage, palmode, logperiod; int elenorm, rudnorm1, rudnorm2, disabv1; unsigned long utcdate; //int test = 0; unsigned long mAhRAW; int AN0RAW, AN1RAW, AN3RAW, an3offset; unsigned char lock, lonq, latq, quarter; unsigned char outbuffer1[60]; unsigned char outbuffer2[60]; unsigned char packetstr[PACKETMAX]; // double testhdg; AsyncStruct as; SVStruct Sats[12]; //int vsprintf(char *buf, const char *fmt, va_list args); //int xprintf(char *buf, const char *fmt, ...); //#include "sprintf.c" 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) { ADPCFG = 0x0034; // All ADC pins as analog inputs // ADPCFGbits.PCFG2 = 1; // except _SS1/AN2 as output // ADPCFGbits.PCFG4 = 1; // and IC7 // ADPCFGbits.PCFG5 = 1; // and IC8 ADCHS = 0x0303; // ADCHSbits.CH0SA = 3; // AN3 points to ADCBUF0 // ADCHSbits.CH0SB = 3; // Same but on MUX B ADCON1 = 0x004C; // 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 ADCON2 = 0x0202; // 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 // ADCON3bits.SAMC = 20; // Auto sample 4 Tad ADCON3 = 0x0007; // ADCON3bits.ADCS = 7; // Conversion clock 8 Tcy // ADCSSL = 11; // PORTB = 0; TRISB = 0xFFFF; // 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 // TRISC = 0; // Unnecessary for the moment // PORTC = 0xFFFF; // LATC = 0xFFFF; // TRISCbits.TRISC14 = 1; PORTD = 0; TRISD = 0xFFFF; // Set as inputs (if LCD used, set as outputs) TRISE = 0; // All as outputs (mostly for LCD), RE8 as SCK // TRISEbits.TRISE4 = 1; // used to handshake with RTSO on the BOB-4 TRISF = 0; // PORTFbits.RF2 = 1; // TRISFbits.TRISF2 = 1; // SDI/RF2 set as input } /* 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 = 1; // U1RXIP - Rx interrupt priority IFS0bits.U1RXIF = 0; // Clear U1RXIF - Rx flag IEC0bits.U1RXIE = 1; // U1RXIE - 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 = 9766; // Match every 0.25 seconds (19531) 9766 TMR3 = 0; /* T3CONbits.TCKPS = 2; // Set prescaler to 1:256 (1, 8, 64, 256) (3 = 256, 2 = 64) T3CONbits.TCS = 0; // Use internal clock = 80MHz/4 = 20MHz T3CONbits.TON = 1; // Start timer */ T3CON = 0x8020; 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 = 1; // 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 = 1; // 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 = 1; // 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 = 1; // 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 { if (IFS0bits.ADIF) { if (ADCON2bits.BUFS) { AN0RAW = ADCBUF1; AN1RAW = ADCBUF2; AN3RAW = ADCBUF0; } else { AN0RAW = ADCBUF9; AN1RAW = ADCBUFA; AN3RAW = ADCBUF8; } AN3RAW -= an3offset; if (AN3RAW < 0) AN3RAW = 0; mAhRAW += AN3RAW; quarter++; IFS0bits.ADIF = 0; // Clear interrupt } } void IC1interrupt() org 0x16 { /* long scratch; if (IFS0bits.IC1IF) { while (IC1CONbits.ICBNE) scratch = IC1BUF; if (PORTDbits.RD0) oldtimer1 = scratch; // get the last in the FIFO else { ppm1 = scratch - oldtimer1; if (ppm1 < 0) ppm1 += 65536; // Used to set gain for ROT } IFS0bits.IC1IF = 0; // clear interrupt } */ asm { btss IFS0, 1 // test IC1IF, if set then do 0020 else 0055 goto IC1L0055 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 // bra nz, IC1L0045 // subr _rudnorm2, W0 // ppm8 - ruddir*ruddtemp IC1L0045: mov W0, PDC3 IC1L0050: bclr IFS0, #1 // clear IC1IF IC1L0055: } } void IC2interrupt() org 0x1C { /* long scratch; if (IFS0bits.IC2IF) { while (IC2CONbits.ICBNE) scratch = IC2BUF; if (PORTDbits.RD1) oldtimer2 = scratch; // get the last in the FIFO else { ppm2 = scratch - oldtimer2; if (ppm2 < 0) ppm2 += 65536; // Used to control mode } IFS0bits.IC2IF = 0; // clear interrupt } */ asm { // goto IC2L0050 btss IFS0, 4 // test IC2IF, if set then do 0020 else 0055 goto IC2L0055 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 { /* long scratch; if (IFS1bits.IC7IF) { while (IC7CONbits.ICBNE) scratch = IC7BUF; if (PORTBbits.RB4) oldtimer7 = scratch; // get the last in the FIFO else { ppm7 = scratch - oldtimer7; if (ppm7 < 0) ppm7 += 65536; // control for elevator if (passthrough) PDC1 = ppm7; } IFS1bits.IC7IF = 0; // clear interrupt } */ asm { btss IFS1, 1 // test IC7IF, if set then do 0020 else 0055 goto IC7L0055 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 // bra nz, IC7L0045 // subr _elenorm, W0 // ppm7 - eledir*elevtemp IC7L0045: mov W0, PDC1 IC7L0050: bclr IFS1, #1 // clear IC7IF IC7L0055: } } void IC8interrupt() org 0x38 { /* long scratch; if (IFS1bits.IC8IF) { while (IC8CONbits.ICBNE) scratch = IC8BUF; if (PORTBbits.RB5) oldtimer8 = scratch; // get the last in the FIFO else { ppm8 = scratch - oldtimer8; if (ppm8 < 0) ppm8 += 65536; if (passthrough) PDC2 = ppm8; // control for rudder } IFS1bits.IC8IF = 0; // clear interrupt } */ asm { btss IFS1, 2 // test IC8IF, if set then do 0020 else 0055 goto IC8L0055 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 // bra nz, IC8L0045 // add _rudnorm1, W0 // ppm8 + ruddir*ruddtemp IC8L0045: mov W0, PDC2 IC8L0050: bclr IFS1, #2 // clear IC8IF IC8L0055: } } void U1RXinterrupt() org 0x26 { // Receive Byte... if (IFS0bits.U1RXIF) { if (U1STAbits.OERR == 0) // error-handling { // No errors... as.ccbuf[as.endbuf] = U1RXREG; as.endbuf++; as.endbuf &= (SBUFF_SIZE-1); if ( ((as.endbuf+1)&(SBUFF_SIZE-1)) == as.startbuf ) { // Buffer Overflow!... as.sError = BUFOVFL; as.startbuf = 0; as.endbuf = 0; } } // Clear Interrupt... else U1STAbits.OERR = 0; // clear error flag IFS0bits.U1RXIF = 0; } } // Retrieves a byte from the circular buffer... int getccb(void) { int ret_value; ret_value = -1; if ( as.endbuf != as.startbuf ) { ret_value = (int)as.ccbuf[as.startbuf]; as.startbuf++; as.startbuf %= SBUFF_SIZE; } return(ret_value); } // Tells whether or not a byte is available in the circular buffer... BYTE peekccb(void) { return( as.endbuf != as.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]; // unsigned char outbuffer[60]; int i; checksum = 2; // Already computed TalkerID checksum "PMTK" i = 0; while (Pkt[i] != 0) { checksum ^= Pkt[i++]; } sprintf(Chk,"%02x", checksum); i = 0; while (Chk[i] != 0) { Chk[i] = toupper(Chk[i]); i++; } sprintf(outbuffer2, "$PMTK%s*%s\r\n", Pkt, Chk); Uart_write_str(outbuffer2); } /* void shell_sort(void) { int i, j, increment, temp1, temp2, size; size = 0; while (Sats[size].prn != 0 && size < 12) size++; increment = 6; // Half of array size while (increment > 0) { for (i=increment; i < size; i++) // Array is size 12 { j = i; temp1 = Sats[i].prn; temp2 = Sats[i].snr; while ((j >= increment) && (Sats[j-increment].prn > temp1)) { Sats[j].prn = Sats[j - increment].prn; Sats[j].snr = Sats[j - increment].snr; j = j - increment; } Sats[j].prn = temp1; Sats[j].snr = temp2; } increment = (int) (increment / 2.2); if (increment == 2) increment = 1; } } */ unsigned int buffspace(void) { asm { 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 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 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 } } unsigned int send_BOB(unsigned char *bobstr) { unsigned int slen, sbuf; slen = strlen(bobstr); while (slen > buffspace()) Delay_ms(5); asm { 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: } } /* int _doprntf(char * pb, const char *format, register va_list args); void bprintf(char *format, ...) { va_list args; // unsigned char outbuffer[60]; va_start(args, format); // vsprintf(outbuffer, format, args); _doprntf(outbuffer1, format, args); // va_end(args); send_BOB(outbuffer1); } */ void pattrib(int attrib) { sprintf(outbuffer1,"\033[%dm",attrib); send_BOB(outbuffer1); } void resetmode(void) { pattrib(69); } void cls(void) { send_BOB("\033[2J"); } void cursorH(int x, int y) { sprintf(outbuffer1, "\033[0z\033[%d;%dH", y, x); send_BOB(outbuffer1); } void cursor(int x, int y) { sprintf(outbuffer1, "\033[0z\033[%d;%dx", x, y); send_BOB(outbuffer1); } void cursor4(int x, int y) { sprintf(outbuffer1, "\033[4z\033[%d;%dx", x, y); send_BOB(outbuffer1); } 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; sprintf(outbuffer1,"\033[1z\033[%d;%dx", x, y); send_BOB(outbuffer1); } void centre_BOB(unsigned char *bobstr) { unsigned int i; if (displayon) { i = (40 - strlen(bobstr))/2; cursorH(i,8+palmode); send_BOB(bobstr); } } void DRAWOSD(void) { int i, j, lefthdg, dat, xref; double scratch; unsigned char altarrow, utfhigh; // unsigned char attrib[3]; // This section handles the time display at the top left cursor(1+safezonex,11+safezoney); if (homeset == 0) { sprintf(outbuffer1,"%02d:%02d\033[4z%02d UTC", hour, minute, second); send_BOB(outbuffer1); } else { i = 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; sprintf(outbuffer1,"%02d:%02d\033[4z%02d", lefthdg, j, i); send_BOB(outbuffer1); } // send_BOB(outbuffer1); // This section handles the altitude and speed info at the top right group cursor(375-safezonex,11+safezoney); if (metric) sprintf(outbuffer1,"%#6.1f\033[1zkph", speed); else sprintf(outbuffer1,"%#6.1f\033[1zmph", speed * KMTOMILE); send_BOB(outbuffer1); utfhigh = 0x0C4; if (altrate < 0.05 && altrate > -0.05) { altarrow = 0x0A0; utfhigh = 0x0C0; } else if (altrate > 0) altarrow = 0x082; else altarrow = 0x081; if (alt < altratio*homedist) pattrib(65); else resetmode(); cursor(375-safezonex,24+safezoney); if (metric) sprintf(outbuffer1,"%#6.1f\033[1zm", alt); else sprintf(outbuffer1,"%#6.1f\033[1zft", alt * MTOFT); send_BOB(outbuffer1); resetmode(); sprintf(outbuffer1,"\033[0z%c%c", utfhigh, altarrow); // This section handles the voltage display at the top left if (!disabv1 && voltage1 < vcutoff) pattrib(65); else resetmode(); cursor(1+safezonex,24+safezoney); sprintf(outbuffer1,"%#5.2f\033[1zV", voltage1); send_BOB(outbuffer1); resetmode(); // This section handles the voltage and current group at the bottom left dat = 208 + 48*palmode - safezoney; xref = 1 + safezonex; if (voltage2 > 1) { cursor(xref,dat); if (voltage2 < vcutoff2) pattrib(65); else resetmode(); sprintf(outbuffer1,"%#5.2f\033[1zV\033[69m\033[4z EXT", voltage2); send_BOB(outbuffer1); cursor(xref,dat+13); sprintf(outbuffer1,"%#5.1f\033[1zA", current2); send_BOB(outbuffer1); cursor(xref,dat+26); if (mAh > mahcutoff) pattrib(65); else resetmode(); sprintf(outbuffer1,"%5.0f\033[1zmAh", mAh); send_BOB(outbuffer1); 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(399 - safezonex,dat); if (homedist < showhome) { if (metric) sprintf(outbuffer1,"%4.0f\033[1zm", homedist); else sprintf(outbuffer1,"%#4.2f\033[1zml", homedist * KMTOMILE / 1000); send_BOB(outbuffer1); } else send_BOB("\033[7X"); // send_BOB(outbuffer1); cursor(363 - safezonex,dat+13); send_BOB("\033[4zLOS \033[0z"); if (absdist > distcutoff) pattrib(65); else resetmode(); if (metric) sprintf(outbuffer1,"%5.0f\033[1zm", absdist); else sprintf(outbuffer1,"%#5.2f\033[1zml", absdist * KMTOMILE / 1000); send_BOB(outbuffer1); 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); sprintf(outbuffer1,"\033[4zSPEED \033[0z%#6.1f\033[1zkph", speedmax); send_BOB(outbuffer1); cursor(xref,dat+26); sprintf(outbuffer1,"\033[4z ALT \033[0z%#6.1f\033[1zm", altmax); send_BOB(outbuffer1); cursor(xref,dat+39); sprintf(outbuffer1,"\033[4zRANGE \033[0z%6.0f\033[1zm", distmax); } else { cursor(xref,dat+13); sprintf(outbuffer1,"\033[4zSPEED \033[0z%#6.1f\033[1zmph", speedmax * KMTOMILE); send_BOB(outbuffer1); cursor(xref,dat+26); sprintf(outbuffer1,"\033[4z ALT \033[0z%#6.1f\033[1zft", altmax * MTOFT); send_BOB(outbuffer1); cursor(xref,dat+39); sprintf(outbuffer1,"\033[4zRANGE \033[0z%#6.2f\033[1zml", distmax * KMTOMILE / 1000); } send_BOB(outbuffer1); if (voltage2 > 1) { cursor(xref,dat+52); sprintf(outbuffer1,"\033[4z CURR \033[0z%#6.1f\033[1zA", curmax); send_BOB(outbuffer1); } 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) sprintf(outbuffer1,"\033[65m%2d\033[69m\033[4z SATS", sv); else sprintf(outbuffer1,"%2d\033[4z SATS", sv); send_BOB(outbuffer1); cursor(230,11+safezoney); sprintf(outbuffer1,"%#10.6f\033[4z%c", lat, latq); send_BOB(outbuffer1); cursor(230,24+safezoney); sprintf(outbuffer1,"%#10.6f\033[4z%c", lon, lonq); send_BOB(outbuffer1); cursor(110,24+safezoney); if (hdop > 0) { sprintf(outbuffer1,"\033[4zHDOP \033[0z%#-6.1f", hdop); send_BOB(outbuffer1); } 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 recticle 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) { sprintf(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); sprintf(outbuffer1," %s ", outbuffer2+3); send_BOB(outbuffer1); // This section handles the center heading readout above the compass lefthdg = (double)(homehdg - hdg); cursor4(xref+90,dat); // * if (lefthdg == 0 && homeset) sprintf(outbuffer1,"\033[74m%03.0f", hdg); else sprintf(outbuffer1,"%03.0f", hdg); send_BOB(outbuffer1); resetmode(); if (homeset) { // This section handles the "H" home indicator if (lefthdg < 0) lefthdg += 360; if (lefthdg > 180) lefthdg = 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) sprintf(outbuffer1," \033[71m%s\033[69m ", outbuffer2); else if (lefthdg < 7 || lefthdg > -7) sprintf(outbuffer1," %s ", outbuffer2); // else if (lefthdg < 7 || lefthdg > -7) sprintf(outbuffer1," \033[65m%s\033[69m ", outbuffer2); else sprintf(outbuffer1," %s ", outbuffer2); send_BOB(outbuffer1); // This section handles the indicator arrows at either ends of the compass if (lefthdg == 0) { cursor(xref-7,dat+10); // * send_BOB("\xC0\xA0"); /* utfhigh = 0x0C0; altarrow = 0x0A0; sprintf(outbuffer1,"%c%c", utfhigh, altarrow); send_BOB(outbuffer1); */ cursor(xref+191,dat+10); // * send_BOB("\xC0\xA0"); /* sprintf(outbuffer1,"%c%c", utfhigh, altarrow); send_BOB(outbuffer1); */ } else { if (lefthdg < 0) { cursor(xref-7,dat+10); // * if (lefthdg < -90) { // utfhigh = 0x0C3; // altarrow = 0x0BF; send_BOB("\xC3\xBF"); } else { // utfhigh = 0x0C4; // altarrow = 0x080; send_BOB("\xC4\x80"); } // sprintf(outbuffer1,"%c%c", utfhigh, altarrow); // 186 = 192-6+6(space)-6(12-width font) // send_BOB(outbuffer1); cursor(xref+191,dat+10); // * send_BOB("\xC0\xA0"); /* utfhigh = 0x0C0; altarrow = 0x0A0; sprintf(outbuffer1,"%c%c", utfhigh, altarrow); send_BOB(outbuffer1); */ } if (lefthdg > 0) { cursor(xref+191,dat+10); // * if (lefthdg > 90) { // utfhigh = 0x0C4; // altarrow = 0x080; send_BOB("\xC4\x80"); } else { // utfhigh = 0x0C3; // altarrow = 0x0BF; send_BOB("\xC3\xBF"); } // sprintf(outbuffer1,"%c%c", utfhigh, altarrow); // send_BOB(outbuffer1); // utfhigh = 0x0C0; // altarrow = 0x0A0; cursor(xref-7,dat+10); // * send_BOB("\xC0\xA0"); // sprintf(outbuffer1,"%c%c", utfhigh, altarrow); // send_BOB(outbuffer1); } } } 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; } sprintf(outbuffer1,"\xC4%c\033[4z \033[0z", utfhigh); send_BOB(outbuffer1); } cursorH(11,dat+1); for (i = 0; i < 12; i++) { if (Sats[i].snr >= 49) utfhigh = 0x096; else utfhigh = 0x090 + (Sats[i].snr) / 7; sprintf(outbuffer1,"\xC4%c\033[4z \033[0z", utfhigh); send_BOB(outbuffer1); } cursorH(11,dat+2); send_BOB("\033[4z"); for (i = 0; i < 12; i++) { if (Sats[i].prn != 0) { sprintf(outbuffer1,"%02d ", Sats[i].prn); send_BOB(outbuffer1); } 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); 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)); 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"); sprintf(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); sprintf(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; // Delay_ms(200); 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 = second; homeset = 1; // send_PMTK("314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0"); initgps(0); resetmode(); } else { stabilized--; sprintf(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) { 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 (y < 0 && x < 0) return -PI + atan(y/x); // if (y < 0 && x == 0) return -PI/2; if (x == 0) { if (y > 0) return PI/2; // if (y == 0) return 0; } // if (y > 0 && x == 0) return PI/2; // if (y == 0 && x == 0) return 0; return 0; } void wayhome(int wpx) { // double templon, templat; double rlona, rlonb, rlata, rlatb; rlona = lon * PI/180; rlata = lat * PI/180; if (lonq == 'W') rlona = -rlona; if (latq == 'S') rlata = -rlata; // rlona = templon * PI/180; rlonb = wp[wpx].lon * PI/180; // rlata = templat * PI/180; rlatb = wp[wpx].lat * PI/180; // homedist = sqrt(pow(templon-wp[wpx].lon,2) + pow(templat-wp[wpx].lat,2)) * 60000.0 * NAUTKM; // convert to minutes (nautical mile) 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 = atan2(wp[wpx].lon-templon,wp[wpx].lat-templat)*180/3.141592653589793; 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 + homehdg; elhour = elminute = 0; elsecond = second - startsecond; if (elsecond < 0) { elsecond += 60; elminute--; } elminute += minute - startminute; if (elminute < 0) { elminute += 60; elhour--; } elhour += hour - starthour; if (elhour < 0) elhour += 24; if (sv > 3) { if (speed > speedmax) speedmax = speed; if (absdist > distmax) distmax = absdist; if (alt > altmax) altmax = alt; } // altcutoff = (homedist * LOWALTRATIO); } void autopilot(int flag, int tick) { int dat; double alterror, hdgerror, desiredroc, desiredrot; dat = 50; if (flag == 0) { if (autopilotinfo) { autopilotinfo = elevtemp = ruddtemp = 0; // send_BOB("\033[4z\033[2;50x\033[45X"); // send_BOB("\033[4z\033[2;63x\033[45X"); cursor(183,dat); send_BOB("\033[9X"); } } else { if (autopilotinfo == 0) { autopilotinfo = 1; cursor(183,dat); send_BOB("\033[79mAUTOPILOT"); } if (tick >= 1) { alterror = cruisealt-alt; if (alterror > maxaltchg) alterror = maxaltchg; 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 - hdgerror; else if (hdgerror < -180) hdgerror += 360; if (hdgerror > MAXHDGCHANGE) hdgerror = MAXHDGCHANGE; // limit the size of the error else if (hdgerror < -MAXHDGCHANGE) hdgerror = -MAXHDGCHANGE; 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; // sprintf(outbuffer1,"\033[4z\033[2;50xROC:%-5.1f DROC:%-5.1f ELE:%-6d pp7:%-5ld", rateofclimb, desiredroc, elevator, ppm7); // send_BOB(outbuffer1); // sprintf(outbuffer1,"\033[4z\033[2;63xROT:%-5.1f DROT:%-5.1f RDR:%-6d pp8:%-5ld", rateofturn, desiredrot, rudder, ppm8); // send_BOB(outbuffer1); } elevtemp += (elevator - elevtemp) / 3; ruddtemp += (rudder - ruddtemp) / 3; // dat = ppm7 - (elevtemp * eledir); // if (dat < 0) elenorm = ppm7; // else if (dat > 5000) elenorm = ppm7 + 5000; // else elenorm = elevtemp * eledir; // dat = ppm8 + (ruddtemp * ruddir); // if (dat < 0) rudnorm1 = ppm8; // else if (dat > 5000) rudnorm1 = 5000 - ppm8; // else rudnorm1 = ruddtemp * ruddir; // dat = ppm1 - (ruddtemp * ruddir); // if (dat < 0) rudnorm2 = ppm7; // else if (dat > 5000) rudnorm2 = ppm1 - 5000; // else rudnorm2 = ruddtemp * ruddir; PDC1 = ppm7 - elenorm; PDC2 = ppm8 + rudnorm1; PDC3 = ppm1 - rudnorm2; // For dual servo driven ailerons } } void menulabel(int sel) { cursor1(sel); if (sel == setupselect) pattrib(74); else resetmode(); switch(sel) { case 0: sprintf(outbuffer1," MAXRANGE \033[0z%6.0f ", distcutoff); break; case 1: sprintf(outbuffer1," LOWALTRATIO \033[0z%6.2f ", altratio); break; case 2: sprintf(outbuffer1," VALIDHDOP \033[0z%6.1f ", validhdop); break; case 3: sprintf(outbuffer1," MINVALID \033[0z%6d ", minvalid); break; case 4: sprintf(outbuffer1," SHOWHOMELIM \033[0z%6.0f ", showhome); break; case 5: sprintf(outbuffer1," ROCLIMIT \033[0z%6.1f ", roclimit); break; case 6: sprintf(outbuffer1," ROCSTEPGAIN \033[0z%6.0f ", rocstepgain); break; case 7: sprintf(outbuffer1," APROCGAIN \033[0z%6.1f ", aprocgain); break; case 8: sprintf(outbuffer1," ROTLIMIT \033[0z%6.1f ", rotlimit); break; case 9: sprintf(outbuffer1," ROTSTEPGAIN \033[0z%6.0f ", rotstepgain); break; case 10: sprintf(outbuffer1," APROTGAIN \033[0z%6.1f ", aprotgain); break; case 11: sprintf(outbuffer1," SPEEDLOWLIM \033[0z%6.1f ", speedlowlimit); break; case 12: sprintf(outbuffer1," CRUISEALT \033[0z%6.0f ", cruisealt); break; case 13: sprintf(outbuffer1," MAXALTCHG \033[0z%6.0f ", maxaltchg); break; case 14: sprintf(outbuffer1," MAXHDGCHG \033[0z%6.0f ", maxhdgchg); break; case 15: sprintf(outbuffer1," MAHWARNING \033[0z%6.0f ", mahcutoff); break; case 16: sprintf(outbuffer1," ELESERVODIR \033[0z%6s ", (eledir==1?"NOR":"REV")); break; case 17: sprintf(outbuffer1," RDRSERVODIR \033[0z%6s ", (ruddir==1?"NOR":"REV")); break; case 18: sprintf(outbuffer1," UNITS %8s ", (metric?"METRIC":"IMPERIAL")); break; case 19: sprintf(outbuffer1," AMPSZERO \033[0z%6d ", an3offset); break; case 20: sprintf(outbuffer1," VIDEOMODE %8s ", (palmode?"PAL":"NTSC")); break; case 21: sprintf(outbuffer1," LOGPERIOD \033[0z%6d ", logperiod); break; case 22: sprintf(outbuffer1," HOMELOC %8s ", (homeset?"SET":"UNSET")); break; case 23: sprintf(outbuffer1," MAHMULT \033[0z%7.5f ", mahmult); break; case 24: sprintf(outbuffer1," AMPMULT \033[0z%6.3f ", imult2); break; case 25: sprintf(outbuffer1," SAFEZONE \033[0z%6d ", safezonex); break; case 26: sprintf(outbuffer1," VOLT1LOWARN \033[0z%6s ", (disabv1?"OFF":" ON")); break; case CONFIGLIMIT: sprintf(outbuffer1," SAVE CONFIG \033[0z%6s ", (setupchanged?"*":"DONE")); break; default: break; } send_BOB(outbuffer1); } void drawmenu(void) { int k; for (k = 0; k <= CONFIGLIMIT; k++) menulabel(k); } void dosetup(void) { // double adj; // double tmp1; // double *tmp2; int adj, oldselection; // int tmp3, *tmp4; unsigned long dest; /* unsigned int *test[25] = { &distcutoff, &altratio, &validhdop, &minvalid, &showhome, &roclimit, &rocstepgain, &aprocgain, &rotlimit, &rotstepgain, &aprotgain,&speedlowlimit, &cruisealt, &maxhdgchg, &maxaltchg, &mahcutoff, &eledir, &ruddir, &metric, &an3offset, &palmode, &logperiod, 0, &mahmult, &imult2 }; */ oldselection = setupselect; if (ppm7 < 3550) { setupselect += eledir; if (setupselect < 0) setupselect = 0; if (setupselect > CONFIGLIMIT) setupselect = CONFIGLIMIT; } else if (ppm7 > 3950) { setupselect -= eledir; if (setupselect < 0) setupselect = 0; if (setupselect > CONFIGLIMIT) setupselect = CONFIGLIMIT; } else { adj = 0; if (ppm8 > 3950) adj = 1; else if (ppm8 < 3550) 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 < 9999 && 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 < 9999 && 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 > 5 && adj < 0) || (logperiod < 60 && 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 < 0.1 && 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: // calibrate ammeter disabv1 = (adj==1); break; case CONFIGLIMIT: // save config if (adj > 0) { // Write to EEPROM dest = 0x7FFC00; write_EEP(&dest, (unsigned int *)&distcutoff, 2); write_EEP(&dest, (unsigned int *)&altratio, 2); write_EEP(&dest, (unsigned int *)&validhdop, 2); write_EEP(&dest, (unsigned int *)&minvalid, 1); write_EEP(&dest, (unsigned int *)&showhome, 2); write_EEP(&dest, (unsigned int *)&roclimit, 2); write_EEP(&dest, (unsigned int *)&rocstepgain, 2); write_EEP(&dest, (unsigned int *)&aprocgain, 2); write_EEP(&dest, (unsigned int *)&rotlimit, 2); write_EEP(&dest, (unsigned int *)&rotstepgain, 2); write_EEP(&dest, (unsigned int *)&aprotgain, 2); write_EEP(&dest, (unsigned int *)&speedlowlimit, 2); write_EEP(&dest, (unsigned int *)&cruisealt, 2); write_EEP(&dest, (unsigned int *)&maxhdgchg, 2); write_EEP(&dest, (unsigned int *)&maxaltchg, 2); write_EEP(&dest, (unsigned int *)&mahcutoff, 2); write_EEP(&dest, (unsigned int *)&eledir, 1); write_EEP(&dest, (unsigned int *)&ruddir, 1); write_EEP(&dest, (unsigned int *)&metric, 1); write_EEP(&dest, (unsigned int *)&an3offset, 1); write_EEP(&dest, (unsigned int *)&palmode, 1); write_EEP(&dest, (unsigned int *)&logperiod, 1); write_EEP(&dest, (unsigned int *)&mahmult, 2); write_EEP(&dest, (unsigned int *)&imult2, 2); write_EEP(&dest, (unsigned int *)&safezonex, 2); setupchanged = 0; drawmenu(); } break; default: break; } menulabel(setupselect); } } if (oldselection != setupselect) { menulabel(oldselection); menulabel(setupselect); } // if (setupchanged) menulabel(setupselect); resetmode(); } /* unsigned int read_DS2450(void) { unsigned int result; int i = 0; Ow_Reset(&PORTE, 2); Ow_Write(&PORTE, 2, 0xCC); // Skip ROM Ow_Write(&PORTE, 2, 0x3C); // Convert Ow_Write(&PORTE, 2, 0x08); // Input mask - Channel D - faster Ow_Write(&PORTE, 2, 0x40); // Read-out control - preset to 0 - Channel D Ow_Read(&PORTE, 2); // Read out the 2 CRC16 bytes Ow_Read(&PORTE, 2); while ((Ow_Read(&PORTE, 2) != 0xFF) && (i++ < 10)); Delay_ms(1); Ow_Reset(&PORTE, 2); Ow_Write(&PORTE, 2, 0xCC); // Skip ROM Ow_Write(&PORTE, 2, 0xAA); // Read memory Ow_Write(&PORTE, 2, 0x06); // at address 0x0006 - Channel D conversion result Ow_Write(&PORTE, 2, 0x00); // result = Ow_Read(&PORTE, 2); result = (Ow_Read(&PORTE, 2) << 8) | result; return(result); } */ void changebaud(int ubrgconst) { Delay_ms(50); send_PMTK("251,115200"); // Change baud rate to faster speed Delay_ms(100); U1BRG = ubrgconst; // set to 38400 baud } void main(void) { int i = 0, iChar, logcount; int pktincoming = 0; int oldsecond, tick, logtick; double oldalt, oldhdg; unsigned long src, logdest, logdate; double P; int avgcur[CURSAMPS], j, k, tempcur; PWMCON1 = 0x770; TRISE = 0; PORTEbits.RE0 = 0; // Reset BOB-4 Delay_ms(1); PORTEbits.RE0 = 1; // bootloader(); Uart1_Init(4800L); U1MODEbits.ALTIO = 1; // Use alternate pins for RX/TX since SPI is needed for (j = 0; j < CURSAMPS; j++) avgcur[j] = 0; j = disabv1 = 0; Setup_GP_Ports(); Setup_Interrupts(); oldtimer7 = oldtimer8 = oldtimer2 = oldtimer1 = 0; ppm7 = ppm8 = ppm2 = ppm1 = 3750; metric = 1; lon = lat = speed = hdg = alt = galt = altrate = rateofturn = rateofclimb = hdop = mAh = 0; // pdop = vdop = palt = 0; homedist = homehdg = absdist = oldalt = oldhdg = 0; wp[0].lat = wp[0].lon = wp[0].alt = 0; strcpy(wp[0].name,"HOME"); hour = minute = second = starthour = startminute = startsecond = elhour = elminute = elsecond = 0; speedmax = altmax = distmax = voltage1 = elevator = rudder = elevtemp = ruddtemp = curmax = 0; lock = lonq = latq = ' '; ppm7 = ppm8 = ppm2 = ppm1 = 3750; AN0RAW = AN1RAW = AN3RAW = mAhRAW = 0; maxdataon = gpsdataon = GSVchanged = homeset = sv = autopilotinfo = tick = logtick = setupchanged = 0; oldsecond = setuppage = setupselect = logcount = 0; logdest = LOGSTART; logdate = 0; // ds2450present = passthrough = displayon = 1; quarter = 4; as.startbuf = 0; as.endbuf = 0; as.sError = 0; // 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_ENABLE, _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 /* Ow_Reset(&PORTE, 2); Ow_Write(&PORTE, 2, 0xCC); // Skip ROM Ow_Write(&PORTE, 2, 0x55); // Write memory Ow_Write(&PORTE, 2, 0x1C); // Location 0x001C Ow_Write(&PORTE, 2, 0x00); Ow_Write(&PORTE, 2, 0x40); // Indicate powered by Vcc Ow_Read(&PORTE, 2); // Read out the 2 CRC16 bytes Ow_Read(&PORTE, 2); Ow_Read(&PORTE, 2); // Some random read? Ow_Reset(&PORTE, 2); Ow_Write(&PORTE, 2, 0xCC); // Skip ROM Ow_Write(&PORTE, 2, 0x55); // Write memory Ow_Write(&PORTE, 2, 0x0E); // Location 0x000E - Channel D Ow_Write(&PORTE, 2, 0x00); Ow_Write(&PORTE, 2, 0x00); // 16-bits conversion Ow_Read(&PORTE, 2); // Read out the 2 CRC16 bytes Ow_Read(&PORTE, 2); Ow_Read(&PORTE, 2); // Some random read? Ow_Write(&PORTE, 2, 0x00); // Range is 5.12V Ow_Read(&PORTE, 2); // Read out the 2 CRC16 bytes Ow_Read(&PORTE, 2); Ow_Read(&PORTE, 2); // Some random read? */ // EEPROM block src = 0x7FFC00; read_EEP(&src, (unsigned int *)&distcutoff, 2); read_EEP(&src, (unsigned int *)&altratio, 2); read_EEP(&src, (unsigned int *)&validhdop, 2); read_EEP(&src, (unsigned int *)&minvalid, 1); read_EEP(&src, (unsigned int *)&showhome, 2); read_EEP(&src, (unsigned int *)&roclimit, 2); read_EEP(&src, (unsigned int *)&rocstepgain, 2); read_EEP(&src, (unsigned int *)&aprocgain, 2); read_EEP(&src, (unsigned int *)&rotlimit, 2); read_EEP(&src, (unsigned int *)&rotstepgain, 2); read_EEP(&src, (unsigned int *)&aprotgain, 2); read_EEP(&src, (unsigned int *)&speedlowlimit, 2); read_EEP(&src, (unsigned int *)&cruisealt, 2); read_EEP(&src, (unsigned int *)&maxhdgchg, 2); read_EEP(&src, (unsigned int *)&maxaltchg, 2); read_EEP(&src, (unsigned int *)&mahcutoff, 2); read_EEP(&src, (unsigned int *)&eledir, 1); read_EEP(&src, (unsigned int *)&ruddir, 1); read_EEP(&src, (unsigned int *)&metric, 1); read_EEP(&src, (unsigned int *)&an3offset, 1); read_EEP(&src, (unsigned int *)&palmode, 1); read_EEP(&src, (unsigned int *)&logperiod, 1); read_EEP(&src, (unsigned int *)&mahmult, 2); read_EEP(&src, (unsigned int *)&imult2, 2); read_EEP(&src, (unsigned int *)&safezonex, 1); safezoney = safezonex; if (minvalid < 0 || minvalid > 151 || ppm7 < 3450) { aprotgain = APROTGAIN; aprocgain = APROCGAIN; rotlimit = ROTLIMIT; roclimit = ROCLIMIT; rotstepgain = ROTSTEPGAIN; rocstepgain = ROCSTEPGAIN; speedlowlimit = SPEEDLOWLIMIT; maxhdgchg = MAXHDGCHANGE; maxaltchg = MAXALTCHANGE; minvalid = MINVALIDREADINGS; showhome = SHOWHOMEDIST; validhdop = VALIDHDOP; ruddir = eledir = 1; // assume normal servo direction cruisealt = CRUISEALT; mahcutoff = HIGHMAH; altratio = LOWALTRATIO; distcutoff = MAXRANGE; metric = 1; an3offset = AN3OFFSET; palmode = DEFAULTVIDEO; // Defaults to PAL logperiod = DEFLOGPERIOD; mahmult = MAHMULT; imult2 = IMULT2; safezonex = safezoney = SAFEZONE; } vcutoff = LOWV2S; vcutoff2 = LOWV2S; hdopvalid = validhdop; stabilized = minvalid; stabilized++; // flag the first printing for (i = 0; i < 12; i++) { Sats[i].prn = 0; Sats[i].snr = 0; } // Setup PWM ports - Choose Edge-aligned mode /* PWMCON1bits.PMOD1 = 1; // All PWM pins in independent (non-complementary) mode PWMCON1bits.PMOD2 = 1; PWMCON1bits.PMOD3 = 1; PWMCON1bits.PEN1L = 0; // Enable all as High PWM output (not regular I/O), Low PWM as I/O. PWMCON1bits.PEN2L = 0; // Defaults to enabled as per configuration bits (Edit Project) PWMCON1bits.PEN3L = 0; // RST_PWMPIN PWMCON1bits.PEN1H = 1; PWMCON1bits.PEN2H = 1; PWMCON1bits.PEN3H = 1; */ 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 /* PTCONbits.PTCKPS = 2; // set master clock prescaler = 20MHz /1 /4 /16 /64 (0-3) PTCONbits.PTMOD = 0; // Free-running mode PTCONbits.PTEN = 1; // Enable PWM */ PTCON = 0x8008; PDC1 = PDC2 = PDC3 = 3750; // Set duty cycle register to match 1.5ms // Lcd_Custom_Config(&PORTE, 5,4,3,2, &PORTD, 1,6,0); // Lcd_Custom_Cmd(LCD_CURSOR_OFF); // Lcd_Custom_Cmd(LCD_CLEAR); Delay_ms(150); cls(); sprintf(outbuffer1,"\033[16;%dv", palmode); send_BOB(outbuffer1); send_BOB("\033[21;0v\033[32;1v"); // Clear OSD screen Delay_ms(100); // Allow GPS to warm-up resetmode(); send_BOB("\033[1zDragonOSD Version 4.6 28/11/2007 Copyright Daniel Wee"); Delay_ms(400); // Allow GPS to warm-up i = 2; // 5-seconds countdown delay while (i > 0) { i--; sprintf(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(31); changebaud(10); 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(); Delay_ms(5); resetmode(); send_PMTK("000"); Delay_ms(100); initgps(1); // send_PMTK("314,0,1,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0"); // if (homeset == 0) send_PMTK("314,0,1,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0"); // else send_PMTK("314,0,1,0,1,0,0,0,0,0,0,0,0,0,0,0,0,0"); // Don't need GSV if coming out of Watchdog reset Delay_ms(100); send_PMTK("313,1"); // enable SBAS/WAAS send_PMTK("220,200"); // 5Hz data rate // send_PMTK("300,200,0,0,0,0"); voltage1 = (double)AN0RAW * VMULT1; // Switch to AN0 in new board revision voltage2 = (double)AN1RAW * VMULT2; if (voltage1 < 8.5) vcutoff = LOWV2S; else vcutoff = LOWV3S; if (voltage2 < 8.5) vcutoff2 = LOWV2S; else if (voltage2 < 12.8) vcutoff2 = LOWV3S; else vcutoff2 = LOWV4S; i = 0; pktincoming = 0; 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) { /* if (ds2450present) { P = ((double)(read_DS2450())/65536.0+0.095)/0.911925; // pressure in kPa palt = (1.0 - pow(P,0.190263)) * 44330.32172; // alt in metres } */ altrate = alt; alt = galt - wp[0].alt; // Or can be galt if using GPS altitude-- altrate = alt - altrate; if (oldsecond != second) { tick++; logtick++; rateofclimb = alt - oldalt; rateofturn = hdg - oldhdg; if (rateofturn > 180) rateofturn -= 360; else if (rateofturn < -180) rateofturn += 360; oldsecond = second; oldalt = alt; oldhdg = hdg; } if (homeset == 0) sethome(); else { wayhome(0); if (passthrough == 0 && setuppage == 0) { autopilot(1, tick); // Number of seconds lag (match below) } else autopilot(0, tick); // if (tick >= 1) tick = 0; // Number of seconds lag before clearing 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 > 4) { 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; 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 = 3750; 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; // setupselect = 0; drawmenu(); } dosetup(); } else { // Regular screen data if (setuppage == 1) { setuppage = 0; cls(); } else { displayon = 1; DRAWOSD(); } } } buffspace(); } } }