/******************************************************************************\ ** setup.c general functions for the Main Spray PCB, v1.0 ** ***************************************************************************** ** ** Oct2008, created ** ***************************************************************************** ** ** \******************************************************************************/ #include // Persistor BIOS and I/O Definitions #include // Persistor PicoDOS Definitions #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include #include "cf2_qsm332.h" #include "spi_cmd.h" #include "gpio_mux.h" #include "pitch_roll.h" #include "setup.h" // globals extern char Verbose; // switch for displaying more info extern char IRQ_Flag; // interrupt flag: says what caused the interrupt extern short iparam [ MAX_PARAM ]; // global parameter settings extern struct all_param all; //***************************************************************************** // functions specific to Spray 2008 CF2 controller //***************************************************************************** void init_pins ( void ) //*************************************************** { // initialize the pins to the desired states uchar mirrorpins[] = { 1, 39, 0 }; PIOSet ( RES_WDOG ); // set the reset line HIGH to the wdog msp430 PIOMirrorList(mirrorpins); // configure SPI with levels = low = off for Spray sense PIOClear ( 15 ); // pin15 = PCS2 PIOClear ( 17 ); // pin17 = PCS3 PIOClear ( 19 ); // pin19 = PCS1 PIOClear ( 21 ); // pin21 = PCS0 PIOClear ( 16 ); // pin16 = SCK SPI PIOClear ( 18 ); // pin18 = MOSI SPI PIORead ( 20 ); // pin20 = MISO SPI // spare TPU: unassigned PIOClear ( TPU9 ); PIOClear ( TPU10 ); PIOClear ( TPU11 ); PIOClear ( TPU12 ); PIOClear ( TPU13 ); // TPU serial Receive channels PIORead ( TPUPinFromChan( FROM_SAT ) ); PIORead ( TPUPinFromChan( FROM_MUX1 ) ); PIORead ( TPUPinFromChan( FROM_MUX2 ) ); PIORead ( TPUPinFromChan( FROM_SBE ) ); // TPU serial Transmit channels PIOSet ( TPUPinFromChan( TO_SAT ) ); PIOSet ( TPUPinFromChan( TO_MUX1 ) ); PIOSet ( TPUPinFromChan( TO_MUX2 ) ); PIOSet ( TPUPinFromChan( TO_SBE ) ); backup_v7(); // set alternative voltage regulator return; } //*************************************************************************** void backup_v7 ( void ) //***************************************************** { // presently just a shell PIOClear ( V7_BKUP ); //this leaves OFF // ideally, we want to detect when circuitry is cycling in this regulator // and set an eeprom/ram value that says it is time just to use it return; } //*************************************************************************** //***************************************************************************** //**** low-power hibernate/wait functions ************************************* //***************************************************************************** short go_lowpower ( short irq_enable ) //************************************* { // will wait in low-power until something wakes it up: // irq_enable defines which interrupts to enable // = PIT_ONLY = 0 = just use PIT, don't enable any others // = PIT_IRQ2 = 1 enable IRQ2 as well // = PIT_IRQ2_RXD = 2 = also enable RXD // this draws ~ 0.3 mA, and is only awakened by rs232 or by // IRQ2 = controlled by msp430 = CF2_ATTN line // returns IRQ_Flag = what woke it up, see below // set up to go to sleep ====================================== IRQ_Flag = 0; // reset flag: 0= no interrupts yet // it is set by the various ISR's // IRQ2_F 2 // awoken from IRQ2 = CF2_ATTN line from msp430 // IRQ4_F 4 // awoken by char detected on rs232 Rcv // SPUR_F 8 // awoken by spurious interrupt // it will remain 0 if serviced by the PIT cdrain(); // wait for completion of last SCI_Tx coflush(); // discard any output garbage characters ciflush(); // discard any input garbage characters cdrain(); // wait for completion of last SCI_Tx QSMStop(); // shut down the QSM TPURun(0); // turn off the TPU module DelayMilliSecs( 10 ); // give everything time to finish up??? // PERSISTOR flash notice for SiliconDrive cards *(ushort *) 0xffffe00c = 0xF000; // force CF card into Card-1 active mode // next line is to disable irq's, which are re-enabled by LPStop // CPUWriteInterruptMask(IEV_DISABLE); // disables all interrupts CPUWriteInterruptMask(5); if ( irq_enable > PIT_ONLY && !all.exc->wdog_error ) { // then include IRQ2 as well = CF2_ATTN line // it is only included if wdog_err = FALSE PinBus(IRQ2); } if ( irq_enable == PIT_IRQ2_RXD ) // enable rxd irq = irq4rxd PinBus(IRQ4RXD); // configuring as bus fnx turns on ISR // but interrupt mask ignores irq4, irq2 until FullStop // therefore, no spurious interrupts! EIAForceOff(true); // Force off the RS232 transmitter on the CF2 // put in low-power state ========================================= LPStopCSE( FullStop ); // we're here until PIT awakens // back here after awakened! ====================================== // Delay10us(); // slight delay // could be awake either from PIT, RxD, or IRQ2 // turn off all ISR pins (do here as don't know which ISR was called) PinIO(IRQ4RXD); // turns off ISR PinIO(IRQ2); // turns off ISR EIAForceOff(false); // Force RS232 transmitters on QSMRun(); //DelayMilliSecs( 10 ); TPURun(1); // turn back on ciflush(); // discard any input garbage characters // next line re-enables the interrupts CPUWriteInterruptMask(IEV_MASK_NONE); return ( IRQ_Flag ); } //*************************************************************************** // Following is for periodic interrupt, based on PIT ************************** // The hibernate_task function will be called by the PIT. // If you need to have to PIT // do anything periodically then stick it in here, otherwise, it doesn't need // to do anything but exist! void hibernate_task(void) { // periodic task can be added here if wanted. } //*************************************************************************** short hibernate( long *time, short dt, short irq_en) //*********************** { // set alarm to time+dt, update time to time=time+dt // this is used for synchronous sampling every dt seconds, // therefore *time should always be now, or in the recent past // dt is limited to MAX_HIBERNATE = 13 s = max for using PIT51ms // irq_en sets whether to use just PIT, or PIT+CF2_Attn, or PIT+Attn+Rxd // returns -1= no time to hibernate // else returns IRQ_Flag = what woke it up: // 0 = PIT, (normal) // IRQ2_F 2 = awoken from IRQ2 = CF2_ATTN line from msp430 // IRQ4_F 4 = awoken by char detected on rs232 Rcv long tnow, tnew; short n, irq_flag; tnew = *time + dt; // time to hibernate until tnow = RTCGetTime(0,0); // gets present time dt = tnew - tnow; // sequential hibernate if (dt<1) //=================================================== { // then not long enough to wait *time = tnow; // update the time return ( -1 ); } //=========================================================== if (dt > MAX_HIBERNATE ) // out-of-bounds dt = MAX_HIBERNATE; // limit n = dt*1000/ 51; // # of 51ms ticks to wait //cprintf("%d\n",n); if (n>255) n=255; // keep in bounds PITSet51msPeriod(n); // will wake up in dt seconds irq_flag = go_lowpower( irq_en ); // go to low-power PITSet51msPeriod(PITOff); // disable the timer *time = RTCGetTime(0,0); // reset to the new time return ( irq_flag ); } //*************************************************************************** short sleep_check(short tmax, short PIT_set, short do_wdog) //***************** { // look for answer up to tmax seconds // allows a wakeup dependent upon PIT_set (PIT_ONLY, PIT_IRQ2, PIT_IRQ2_RXD), // will service the watchdog if do_wdog = TRUE\ // set tmax = STAY_LOWPWR, then will do an infinite loop // returns AWAKE_RXD if has been awakened by rs232 short ch = 0; short dt = 10; // wake up every 10 s short ms = 200; // = milliseconds to wait for ans on keyboard wakeup short irq_flag; long tnow, tstop; tnow = RTCGetTime(0,0); // present time tstop = tnow + tmax; // time to stop cprintf("will wake up every %d s and display '.', ", dt ); if (tmax == STAY_LOWPWR ) //---------------------------------- { // then we want to do an infinite loop // waiting to be awoken tstop = tnow + 60; // set tstop to 60s ahead of tnow PIT_set = PIT_IRQ2_RXD; // force to awaken from rs232 cprintf(" you must "); } //---------------------------------------------------------- else cprintf(" for %d s \n", tmax ); if ( PIT_set == PIT_IRQ2_RXD ) cprintf("hit '.' to exit \n"); cdrain(); // wait for completion of last SCI_Tx // cdrain seems necessary to keep the CF2 from hanging // on an intermittent basis. Could use further bench-testing while ( ch != '.' && tnow '); ch = SCIRxGetCharWithTimeout(ms); //**wait ms for w or W // SCITxPutChar('-'); } //--------------------------------------------------------- else if ( irq_flag == IRQ2_F ) //---------------------------- { // interrupt from msp430, cprintf("Wake-up from CF2_ATTN\n"); service_wdog(); // service the CF2 } //--------------------------------------------------------- else if ( irq_flag == SPUR_F ) // --------------------------- { // spurious interrupt // special action here ??? cprintf("Spurious int \n"); } //--------------------------------------------------------- if (do_wdog) service_wdog(); // service the watchdog cdrain(); // wait for completion of last SCI_Tx if (tmax == STAY_LOWPWR ) tstop = tnow + 60; // keep tstop>tnow do_wdog = 0; } //===================================================================== if ( ch=='.' ) // then awoken by rs232 irq_flag = 1; else irq_flag = 0; return( irq_flag ); // 1 means it was awakened correctly } //*************************************************************************** short check_tmo ( char *c , short tmo ) //************************************ { // tmo =#s since last user input, //*c = pointer to last char read in (=-1 if no char was inputted over the last s) // returns the new tmo // and also changes *ch to upper case if needed char ch; ch = *c; if ( ch < 0 ) tmo++; //increment the timeout counter else // could be valid { ch = ch_l2u( ch ); // make upper-case tmo = 0; // reset } if (tmo>300) // has been >5 minutes with no user { cprintf("No user input for 300s, going to sleep...\n"); sleep_check ( STAY_LOWPWR, PIT_IRQ2_RXD, FALSE ); // go to low power // we're here when we're awake ch = 'U'; // update the menu tmo = 0; // reset } *c = ch; // update the value before returning return ( tmo ); } //*************************************************************************** void Reset_CF2 ( void ) //**************************************************** { // try to shut down everything in an orderly manner before resetting // this is called after we are done with surface comms flash_close (); // close the flash file BIOSReset(); // causes the reset return; } //**************************************************************************** void init_all ( void ) //***************************************************** { // initialize the params in the all struct to default settings // this is done regardless of op_mode float tau; all.necho = 0; // no echo bytes to send back all.roll_stat = 0; // roll status all.exc->wdog_error = 0; // NO error with wdog circuit all.exc->abort = 0; // flag to go to the abort code all.exc->reset = 0; // flag to reset all.gps->ovalid = FALSE; // assume no valid old GPS all.gps->valid = FALSE; // assume no valid GPS all.flt->pmin = PR_MIN; // set to default surface cnts tau = 2*DHDR*T_ACT; all.flt->tau = tau; //=time constant for heading error all.flt->etau = ETAU_M*tau; //=time constant for integrated error esum MAY07 CHANGE all.flt->esum = 0; // integrated error all.flt->ediv = all.flt->etau; // feb11, keep copy here; gets modified by shore cmds all.flt->em = 0; // feb11 init to 0 all.flt->rm = 0; // feb11 init to 0 all.sbd->send_waypts = FALSE; // don't send the waypt file all.sbd->send_param = FALSE; // don't send the parameter list all.sbd->save_waypt = FALSE; // don't save the waypt file all.sbd->save_route = FALSE; // don't save the parameter list all.sbd->calc_route = FALSE; // don't calculate a new route all.ubx->index = 0; all.ubx->valid[0] = 0; all.ubx->valid[1] = 0; init_accum_err( ); init_serr( ); all.exc->gps_cold = 0; //dec11, init to NOT do cold-reset of GPS return; } //*************************************************************************** void display_dbg ( void ) //************************************************** { // dec14: display some of all parameters // used to see what changes before/after reset printf("==================================================\n"); printf("ipp, klifo, necho = %d, %d, %d \n",all.ppb->ipp, all.klifo, all.necho); printf("exc_stat, roll_stat, ncyc = %d %d %d \n", all.exc->exc_stat, all.roll_stat, all.ncyc); printf("exc->wdog_error, n_reset = %d %d \n", all.exc->wdog_error, all.exc->n_reset); printf("==================================================\n"); return; } //*************************************************************************** void init_sensors ( void ) //************************************************** { // initialize the params on a normal entry (op_mode = 0 or -1) // these are NOT done if it is an abnormal // this way, if some sensors are on during an abnormal reset, they stay on. // the below flags control what is acquired in take_data() all.dat->sbe_pro_on = 0; // set OFF all.dat->take_adp = 0; // set OFF all.adp->on_adp = 0; // set OFF all.ppb->ipp = 0; // reset ping-pong buffer to empty return; } //*************************************************************************** void init_accum_err ( void ) //*********************************************** { // initialize error accumulators : should only be done once // at the start of the mission short i; for (i=0;i<2;i++) { // all.wing_err[i] = 0; // can be used to decide that wing has failed all.wing_now[i] = STBD_WING; // initialize wing to use stbd } return; } //*************************************************************************** void init_serr ( void ) //**************************************************** { // initialize the spi error accumulators : should be done every dive short i; for (i=0;i<5;i++) { all.exc->serr[i] = 0; } return; } //*************************************************************************** void toggle_verbose ( void ) //************************************************ { // toggle the global verbose setting // and display the new setting Verbose ^= 1; cprintf("Verbose mode is "); if (Verbose) cprintf("ON\n"); else cprintf("OFF\n"); return; } //*************************************************************************** //***************************************************************************** short setup_tpu_uart_all ( void ) { // jan13: used to set up the TPU_UART = TU arrays, // set up so it can be called multiple times if things get corrupted // ---returns --- // error = 0 = OK, // 1 = error short i, err=0; TUPort *tup; // handle for TPU UART calls long taddr; // TUChParams tuart_cfg; // used to display the config info // init the local array to pre-compiler values struct tpu_uart tu_port[N_TU_PORTS] = { FROM_SAT , TO_SAT , BAUD_SAT , 0, FROM_MUX1, TO_MUX1, BAUD_MUX1, 0, FROM_MUX2, TO_MUX2, BAUD_MUX2, 0, FROM_SBE, TO_SBE, BAUD_SBE, 0 }; // 4 TPU uart ports, defined as above for (i=0;i0 ) // also tested that TUClose is OK even if tup==0 { // already open, close it cprintf("Closing UART %d addr=%lx \n", i, taddr ); TUClose( tup ); } // now open again, so we know it has the correct settings tup = TUOpen( tu_port[i].rxch, tu_port[i].txch, tu_port[i].baud, 0); taddr = (long) tup; // convert to long cprintf(" TUP: %2d RX: %2u TX: %2u BAUD: %6lu ADDR: %lu\n", i, tu_port[i].rxch, tu_port[i].txch, tu_port[i].baud, taddr); if ( tup == 0 ) { //ERROR!! cprintf("TPU UART OPEN ERROR, port#=%d\n",i); err = 1; } // save all of the info to the global all.tu_port[i].tup = tup; all.tu_port[i].rxch = tu_port[i].rxch; all.tu_port[i].txch = tu_port[i].txch; all.tu_port[i].baud = tu_port[i].baud; /* used for displaying settings TUGetCurrentParams( tu_port[i].tup, &tuart_cfg ); // get the config cprintf("%ld %d%c %d pr=%d %d, size =%d %d %d\n", tuart_cfg.baud, tuart_cfg.bits, tuart_cfg.parity, tuart_cfg.autobaud, // not implemented tuart_cfg.rxpri, tuart_cfg.txpri, tuart_cfg.rxqsz, tuart_cfg.txqsz, tuart_cfg.tpfbsz ); */ } //iiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiiii return ( err ); } //*************************************************************************** //***************************************************************************** short setup_one_tpu_uart ( short i ) { // jan13: used to set up the TPU_UART for channel i // set up so it can be called multiple times if things get corrupted // ---returns --- // error = 0 = OK, // 1 = error short err=0; TUPort *tup; // handle for TPU UART calls long taddr; // init the local to pre-compiler values struct tpu_uart tu_port[N_TU_PORTS] = { FROM_SAT , TO_SAT , BAUD_SAT , 0, FROM_MUX1, TO_MUX1, BAUD_MUX1, 0, FROM_MUX2, TO_MUX2, BAUD_MUX2, 0, FROM_SBE, TO_SBE, BAUD_SBE, 0 }; // 4 TPU uart ports, defined as above if ( i<0 || i >= N_TU_PORTS ) { // NOT LEGAL PORT: declared in setup.h cprintf("Illegal tu_port chan=%d\n", i ); err = 1; } else //--- else OK to proceed ----------------------------------- { tup = all.tu_port[i].tup; // present handle taddr = (long) tup; // convert to long if ( taddr >0 ) { // already open, close it cprintf("Closing UART %d addr=%lx \n", i, taddr ); TUClose( tup ); } // now open again, so we know it has the correct settings tup = TUOpen( tu_port[i].rxch, tu_port[i].txch, tu_port[i].baud, 0); taddr = (long) tup; // convert to long cprintf(" TUP: %lX RX: %2u TX: %2u BAUD: %6lu ADDR: %lu\n", tu_port[i].tup, tu_port[i].rxch, tu_port[i].txch, tu_port[i].baud, taddr); if ( tup == 0 ) { //ERROR!! cprintf("TPU UART OPEN ERROR, port#=%d\n",i); err = 1; } // save all of the info to the global all.tu_port[i].tup = tup; all.tu_port[i].rxch = tu_port[i].rxch; all.tu_port[i].txch = tu_port[i].txch; all.tu_port[i].baud = tu_port[i].baud; } //-------------------------------------------------------------- return ( err ); } //*************************************************************************** //***************************************************************************** //***************************************************************************** // functions to mimic TT8 calls //***************************************************************************** void SerInFlush ( void ) //*************************************************** { // flush the serial input buffer while ( SCIRxQueuedCount() ) SCIRxFlush(); // delete any old stuff return; } //*************************************************************************** void DelayMilliSecs ( long ms ) //******************************************** { // wait for the desired millisecs long i; for (i=0;i30) tmo=30; // limit time-out ms = tmo*1000; // #ms for the first char //if (Verbose) cprintf("tgets wait = %d ms\n",ms); while ( ok ) //===================================================== { //keep reading while ok reply = TURxGetByteWithTimeout( tup, ms ); //get a byte // cprintf("%02x ",reply); //value>0 unless not rcv'd, then=-1 ok = ( reply>=0 && reply !=eol ); // good char, store if (ok) //------------------------------------------------ { c = reply; // store as char *cs++ = c; // store the byte i++; // increment the counter ms = 50; // for rest of char, use 50ms timing ok = (i30) tmo=30; // limit time-out to 30s ms = 50; // #ms to wait between char nms = tmo*(1000/50); //max #loops to wait for (k=0; k=0 && reply !=eol ); // good char, store if (ok) //------------------------------------------------ { c = reply; // store as char *cs++ = c; // store the byte i++; // increment the counter } //------------------------------------------------------ done = ( i>=n || reply==eol ); if (done) break; // no more room, or found the eol } //================================================================== *cs = '\0'; //* tack on the end of string marker *found = reply==eol; // true if eol char was the last read in. return ( i ); } //*************************************************************************** //***************************************************************************** //***************************************************************************** // generic functions //***************************************************************************** short YesNo ( void ) //*** return TRUE if answer is 'y' or 'Y'**************** { char ch; short yes; ch = SCIRxGetChar ( ); yes = ( ( ch =='Y') || ( ch == 'y' ) ); return(yes); } //*************************************************************************** short hex2dec( char c ) //***************************************************** { // convert hex '0'...'9', 'A'...'F' to decimal value // returns the decimal value = 0..15 of the hex character short n; n = -1; // initialize to illegal value if ( c>='0' && c <='9' ) { n = c - '0'; } // 0..9 else if ( c>='a' && c<='f' ) { n = c - 'a' + 10; } else if ( c>='A' && c<='F' ) { n = c - 'A' + 10; } return (n); } //*************************************************************************** //***************************************************************************** short fget_line(FILE *fp, char *line, short n) //************************** { // get line from input file // and drop the LF at the end ('0x0a') // returns #bytes read in // returns -1 if no line (=EOF) line[0]=0; // set to NULL length fgets( line,n,fp ); // reads up to a Newline = '\n' = LF = 0x0a n = strlen(line); // n=#actual char read in if ( n<1) return(-1); // did not read in any line n--; // point to the last char while ( ( (line[n]<' ') || (line[n]>'~') ) && (n>-1) ) { // throw away garbage at the end line[n--] = 0; // set =NULL & decrement #bytes } n = strlen(line); return(n); } //*************************************************************************** short get_num(short *x) //***************************************************** { // get a number from stdin // return 1 if good, 0 if bad // stores number in *x char line[80]; short n, status=0; SerInFlush(); gets(line); n = sscanf(line,"%d",x); if (n==1) { cprintf(" input value = %d\n",*x); status = 1; } else { cprintf(" Bad value\n"); } return(status); } //*************************************************************************** short get_substr( char *str, char *out, short imax) //************************* { // read in from the str, passing up blank spaces, // and return the first sub-str to out[] // returns # of char read from str, // so subsequent calls can parse the line // returns 0 if NO substring was found short i=0, k=0, n, valid; n = strlen(str); out[0] = 0; // set to NULL str do //================================================================ { // look for a valid char valid = ( str[i]>' ' ) && ( str[i] < 127 ); if (!valid) i++; // if no good, look for the next } while ( i' ' ) && ( str[i] < 127 ); } //================================================================= // i=next blank char into str out[k] = 0; // append the NULL char return( i ); } //*************************************************************************** short read_shorts( char *str, short imax, short *x ) //************************ { // read up to imax values and store into x // returns # of values actually read short i=0; // number actually read in short k=0, n; char s[40]; // sub-string of answer comma2blank(str); //--convert commas to white spaces do //====================================================== { // read in until not valid n = get_substr( &str[k], s, 40); //read in a substring //cprintf("%d %s___%s\n",n, &str[k], s ); if (n>0) //---------------------------------------- { // then we did read in a substr k += n; // next entry into str to start searching n = strlen(s); // substr length if (n>0) // verifies it is a valid substr { //nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn n = sscanf(s,"%d",&x[i]); // read in the value // cprintf("__%d %s %d\n",i,s,x[i] ); if (n==1) i++; //yes, read in OK } //nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn } //----------------------------------------------- // n=0 if no string, or illegal number } while (n>0 && i0) //---------------------------------------- { // then we did read in a substr k += n; // next entry into str to start searching n = strlen(s); // substr length if (n>0) // verifies it is a valid substr { //nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn n = sscanf(s,"%x",&x[i]); // read in the value // cprintf("__%d %s %d\n",i,s,x[i] ); if (n==1) i++; //yes, read in OK } //nnnnnnnnnnnnnnnnnnnnnnnnnnnnnnn } //----------------------------------------------- // n=0 if no string, or illegal number } while (n>0 && i='a' && ch<='z' ) ch -= 32; // convert to upper return(ch); } //*************************************************************************** short bswap( short v ) //****************************************************** { // swap bytes short msb, lsb, val; msb = ( v & 0xff00 ) >> 8; lsb = v & 0x00ff; val = (lsb << 8) + msb; // swapped! () around lsb<<8 are needed //printf ("%4x %2x %2x %4x\n",v, msb, lsb, val); return ( val ); } //*************************************************************************** void swap2ch ( uchar *a, uchar *b ) //**************************************** { // xfr 2 bytes of a to b, reversing the order b[1] = a[0]; b[0] = a[1]; return; } //*************************************************************************** void swap4ch ( uchar *a, uchar *b ) //**************************************** { // xfr 4 bytes of a to b, reversing the order // for (i=0;i<4;i++) b[n-1-i] = a[i]; // probably less code to do the loop? 4 lines seems pretty minimal b[3] = a[0]; b[2] = a[1]; b[1] = a[2]; b[0] = a[3]; return; } //*************************************************************************** short b2_store(short i, unsigned char ch[], short n) //*********************** { // store n to the next 2 bytes in ch array // this does not care if you're on a word boundary or not ch[i++] = ( n & 0xff00 ) >> 8; // msb, shifted right ch[i++] = n & 0x00ff; // lsb return(i); } //*************************************************************************** short b4_store(short i, unsigned char ch[], long n) //*********************** { // store n to the next 4 bytes in ch array // this does not care if you're on a word boundary or not unsigned char *b; // pointer to a single char short k; b = ( unsigned char *) &n; // point to the first byte of n for ( k=0; k<4; k++ ) ch[i++] = *b++; // store the 4 bytes return(i); } //*************************************************************************** //***************************************************************************** //***************************************************************************** //************************************************************** // Oct06, code for atan, sin, cos functions //************************************************************** //************************************************************** // atan_2 fnx equivalent *************************************** #define N_ATAN 25 // # of points in the atan look-up table #define R_ATAN_MAX 1.01 // max ratio for atan to create the table below #define MX_ATAN 0.002 // multiplier used to create the table below // OCT06 tests show the atan table, atan_find performs: // 50% faster ( 1000 cycles in 6 s: 6 ms per call ) // 20% less space ( 950 bytes versus 1130 ) // accuracy is <0.02 degrees error const short atan_tab[N_ATAN] = { 0, 1205, 2406, 3598, 4778, 5941, 7085, 8207, 9303, 10372, 11411, 12420, 13397, 14341, 15253, 16131, 16977, 17790, 18572, 19323, 20043, 20734, 21397, 22033, 22643 }; float atan_find( float yt, float xt ) //************************ { // use atan table to look up atan2 value // return the arctan [degrees], range = +/- 180 degrees float theta, r, dt, f; short s1=1, s2=1, s3=1, t2=0, t3=0, i; if ( yt< 0 ) // then in lower two quadrants { s1 = -1; yt = -yt; } // set sign, force to upper two quadrants if ( xt< 0 ) // then in left quadrant { s2 = -1; t2 = 180; xt = -xt; } // set sign/offset, force to right quad if ( yt > xt ) // then in the 45-90 degree octant { // force to the 0-45 deg octant s3 = -1; // set sign t3 = 90; // offset if (yt==0) r=0; else r = xt/yt; // ratio, always 0<=r <= 1 } else // else we're in the 0-45 deg octant { if (xt==0) r=0; else r = yt/xt; } // ratio // now compute the index for the lookup table f = r/R_ATAN_MAX *( N_ATAN - 1); // float value of index to table lookup i = ( short ) f; // truncates to the nearest integer // make sure i is legal for lookup entry if (i<0) i=0; if (i>= (N_ATAN-1) ) i = N_ATAN-2; // linear-interpolate dt = atan_tab[i+1] - atan_tab[i]; f= f - i; // fraction to interpolate beyond i theta = ( atan_tab[i] + f*dt )*MX_ATAN; // guess at theta, degrees // now get back into the correct octant & quadrant if (s3<0 ) theta = t3 - theta; // right octant, range=0-90 if (s2<0 ) theta = t2 - theta; // range = 0-180 if (s1<0 ) theta = -theta; // range = -180..180 return(theta); } //************************************************************ //************************************************************** //sine function follows **************************************** //************************************************************** //sin lookup takes 3.3 ms per call, and requires 1kB //sin() fnx takes 6.6 ms per call, and requires 11kB for all of //the required libraries. //accuracy: error is < 6e-5 for any angle #define N_SIN 91 // # points in the table #define SIN_MAX 91 // max degrees in table below #define MX_SIN 0.00004 // multiplier used to create the table below const short sin_tab[N_SIN] = { //--------------------------------- 0, 436, 872, 1308, 1744, 2179, 2613, 3047, 3479, 3911, 4341, 4770, 5198, 5624, 6048, 6470, 6891, 7309, 7725, 8139, 8551, 8959, 9365, 9768, 10168, 10565, 10959, 11350, 11737, 12120, 12500, 12876, 13248, 13616, 13980, 14339, 14695, 15045, 15392, 15733, 16070, 16401, 16728, 17050, 17366, 17678, 17983, 18284, 18579, 18868, 19151, 19429, 19700, 19966, 20225, 20479, 20726, 20967, 21201, 21429, 21651, 21865, 22074, 22275, 22470, 22658, 22839, 23013, 23180, 23340, 23492, 23638, 23776, 23908, 24032, 24148, 24257, 24359, 24454, 24541, 24620, 24692, 24757, 24814, 24863, 24905, 24939, 24966, 24985, 24996, 25000 }; //--------------------------------------------------------- float sin_find( float xt ) //********************************** { // input xt = degrees, +/- 180 // return the sin value float s, dt, f; short s1=1, i; // get value between +/- 360 degrees if ( xt>360 ) { // get back to legal value i= ( short ) ( xt/ 360); xt = xt - i*360; } // is now < 360 if ( xt< -360 ) { // get back to legal value i= ( short ) ( -xt/ 360); xt = xt + i*360; } // is now >-360 if (xt> 180) xt = xt - 360; // limit to +/-180 if (xt<-180) xt = xt + 360; if ( xt< 0 ) // then in lower two quadrants { s1 = -1; xt = -xt; } // set sign, force to upper two quadrants // now make sure if ( xt > 90 ) // then in left quadrant { xt = 180 - xt; } // set sign/offset, force to right quad // now compute the index for the lookup table // hardwired for a 91-datum table, 1 entry per degree // f = xt/SIN_MAX * ( N_SIN - 1); // float value of index to table lookup i = ( short ) xt; // truncates to the nearest integer // make sure i is legal for lookup entry if (i<0) i=0; if (i>= (N_SIN-1) ) i = N_SIN-2; // linear-interpolate dt = sin_tab[i+1] - sin_tab[i]; f= xt - i; // fraction to interpolate beyond i s = ( sin_tab[i] + f*dt )*MX_SIN; // guess at sin(xt) // now get back into the correct octant & quadrant if (s1<0 ) s = -s; // range = -180..180 return(s); } //************************************************************ float cos_find ( float xt ) //********************************** { // input xt = degrees // returns cos(xt) float ct; ct = sin_find( xt - 90 ); ct = -ct; return(ct); } // *********************************************************** float tan_find ( float xt ) //********************************** { // compute the tangent, based on the lookup tables float ct, st; ct = cos_find( xt ); if ( ct == 0 ) return( 999 ); // avoid divide by zero st = sin_find( xt ); ct = st / ct; // = tangent return(ct); } //************************************************************ //************************************************************** // arcsine fnx ************************************************* //************************************************************** //*** oct06 tests show the following code is: //size =894+50 bytes code+table //asin() size = 3200 bytes // 2.2k reduction in code size //speed: runs >2x faster on average, taking 5 ms/call //accuracy: biggest error = 0.033 degrees for 40-50 deg range #define N_ASIN 16 // # of points in the asin look-up table #define R_ASIN_MAX 0.80 // max ratio for asin to create the table below #define MX_ASIN 0.005 // multiplier used to create the table below const short asin_tab[N_ATAN] = { 0, 611, 1225, 1841, 2464, 3093, 3733, 4384, 5051, 5737, 6446, 7184, 7958, 8779, 9660, 10626 }; float asin_find ( float yt ) //******************************** { // input yt = -1..1 // return degrees: if out-of-bounds, returns +/-90 degrees float theta, dt, f; short s1=1, t2=0, i; if ( yt< 0 ) // then in lower half { s1 = -1; yt = -yt; } // set sign, force to upper half if ( yt>=1 ) //at or beyond a legal value { theta = 90*s1; return(theta); } // lookup table is only for yt = sin(t) = 0..0.8. // if y(t)>0.707, compute using yt = cos(t) to find t, return 90-t if ( yt>0.707 ) { // then we're past 45 degrees yt = sqrt( 1 - yt*yt ); // = cos(t) t2 = 1; // use t2 as flag that we're getting cos(t) } // now compute the index for the lookup table f = yt/R_ASIN_MAX *( N_ASIN -1); // float value of index to table lookup i = ( short ) f; // truncates to the nearest integer // make sure i is legal for lookup entry if (i<0) i=0; if (i>= (N_ASIN-1) ) i = N_ASIN-2; // linear-interpolate dt = asin_tab[i+1] - asin_tab[i]; f= f - i; // fraction to interpolate beyond i theta = ( asin_tab[i] + f*dt )*MX_ASIN; // guess at theta, degrees // correct if theta = cos(yt) if (t2>0 ) theta = 90 - theta; // theta = sin(yt) if (s1<0 ) theta = -theta; // range = -90..90 return(theta); } //************************************************************ //***END TRIG FNX's *********************************************************** //***************************************************************************** //***************************************************************************** // added functions to limit heading to 0..360 deg range short ilimit360(short x) //**************************************************** { // make sure heading x is within 0..360 degree limit if (x>360) x -= 360; if (x<0) x += 360; return(x); } //*************************************************************************** float limit360(float x) //***************************************************** { // make sure heading x is within 0..360 degree limit if (x>360) x -= 360; if (x<0) x += 360; return(x); } //*************************************************************************** short ilimit180(short x) //**************************************************** { // make sure heading diff stays within +/-180 if (x<-180) x +=360; if (x> 180) x -=360; return(x); } //*************************************************************************** float limit180(float x) //***************************************************** { // make sure heading diff stays within +/-180 if (x<-180) x +=360; if (x> 180) x -=360; return(x); } //*************************************************************************** float flt_abs(float x) //****************************************************** { // return absolute value of x, floating-point type if (x<0) x=-x; return(x); } //***************************************************************************