/****************************************************************************** // gps.c : 13Nov2008 JTS // control of the GPS // // original includes add-ons from LAR/MCM for UBLOX compatibility // jul09v3 includes trying ant switch again in get_gps if >2 minutes w/o fix // 0903.5 nov11 added #sat used>3 for a good fix. // //==== Future Expansion ======================================================= // last update: Jan2009 // 1. // //****************************************************************************/ #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 "tcm2.h" #include "pitch_roll.h" #include "gpio_mux.h" #include "setup.h" #include "gps.h" #include "ublox.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; short GPS_Baud = 96; //=== internal prototypes =============================== void gps_menu ( void ); void gps_debug ( void ); void gps_d_menu ( void ); //! GPS_TYPE define here #define GPS_TYPE 2 // define type of GPS #if GPS_TYPE == 2 #define GBAUD 9600L #define GPARITY 'O' #define GTYPE "U-blox Neo5" #define XS 1.0 // scalar for snr min & avg values #endif short gps_on( void ) //**************************************************** { // power up the gps module // returns error with SPI comms short ierr = 0; struct spi_param *ss; ss = all.sspi; // points to the spi comms struct setup_spi ( CS_GPIO, ss ); //---feb12 set up the gps baud rate set_gps_baud ( GPS_Baud ); ierr = spi_aux_pwr( AUX_ON_GPS, ss ); // gps now has power ierr = spi_mux( 1, SER_GPS, ss ); // turn on rs232 & configure mux return ( ierr ); } //*************************************************************************** short gps_off( void ) //*************************************************** { // turn off the gps module // returns error with SPI comms short ierr = 0; struct spi_param *ss; TUPort *tup; ss = all.sspi; // points to the spi comms struct setup_spi ( CS_GPIO, ss ); tup = all.tu_port[TU_GPS].tup; // correct tpu uart port pointer // can send any commands before putting to sleep reset_gps_baud (); //---feb12, convert baud rate back ierr = spi_aux_pwr( -AUX_ON_GPS, ss ); DelayMilliSecs(10); //** wait ierr = spi_mux( 1, SER_AUX1, ss ); //switches MUX to AUX1, disconnecting the GPS lines return ( ierr ); } //*************************************************************************** //---feb12, hooks to set, reset GPS baud void set_gps_baud ( short baud ) //******************************************** { // set the baud rate for the GPS serial comms // baud = baud rate/100: 96 = 9600 short istat = 0; long b100; TUChParams tuart_cfg; TUPort *tup; tup = all.tu_port[TU_GPS].tup; // correct tpu uart port pointer TUGetCurrentParams( tup, &tuart_cfg ); // get the config // GPS uses mux1, shared with ADP and Polyplus b100 = baud*100; cprintf("Setting the baud to %ld\n", b100); tuart_cfg.baud = b100; // change to new value TUConfigure( tup, &tuart_cfg ); // and set return; } //*************************************************************************** void reset_gps_baud ( void ) //************************************************ { //reset the baud rate to the default short istat = 0; TUChParams tuart_cfg; TUPort *tup; tup = all.tu_port[TU_GPS].tup; // correct tpu uart port pointer TUGetCurrentParams( tup, &tuart_cfg ); // get the config tuart_cfg.baud = BAUD_MUX1; // change back; should be 9600 TUConfigure( tup, &tuart_cfg ); // and store return; } //*************************************************************************** short get_gps ( short wc ) //************************************************ { // get a gps fix // return 1 if valid, else 0 // wc = wing control flag: 0=NO, 1=YES, try periodically to readjust wing unsigned long tnow, tstart, tend, tnext, twc = 120; short hdop, ngood, iwing; struct gps_param *gps; long twait=10; // check every twait s short n_used; //nov11, #satellites used in the fix short irq_flag; //dec14, hibernate output gps = all.gps; // point to gps structure if (gps->valid) //======================================================= { // true if the last time we got a valid fix gps->ovalid = TRUE; gps->olat = gps->xlat; gps->olon = gps->xlon; } //===================================================================== tstart = tnow = RTCGetTime(0,0); tend = tnow + iparam [ IA_gps_tm ]*60; // try GPS up to this time cprintf("#minutes to try gps=%d\n", iparam [ IA_gps_tm ] ); tnext = tnow + twc; // next time to check the wing iwing = all.wing_now[GPS_UNIT]; // the wing we're presently using. if (iwing != PORT_WING) iwing = STBD_WING; // set to a legal value gps_on( ); DelayMilliSecs(2000); // wait 2 s for GPS to come up UBX_init( ); // set the UBX to a known state if ( all.exc->gps_cold ) //==================== { //dec11, force a reset DelayMilliSecs(2000); UBX_cold(); cprintf(" UBX cold reset performed \n"); all.exc->gps_cold = 0; DelayMilliSecs(2000); UBX_init(); } //=========================================== // initialize parameters hdop = gps->hdop = 2500; // hdop is used to decide whether fix is good gps->nsat = 0; gps->smin = 250; gps->savg = 0; gps->smax = 0; ngood = 0; // # of good fixes //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ //++++ keep trying until a valid fix ++++++++++++++++++++++++++++++++++++++ while ( RTCGetTime(0,0) < tend ) { // keep trying or until hdop is accurate enough //--dec14, handle irq from the wdog irq_flag = hibernate( &tnow, twait, PIT_IRQ2 ); //wait twait seconds if ( irq_flag == IRQ2_F ) //---------------------------- { // interrupt from msp430, //cprintf("Wake-up from CF2_ATTN\n"); service_wdog(); // service the CF2 } //---------------------------------------------------- n_used = check_UBX( ); // nov11 n_used = #satellites used in the fix if (n_used>2) hdop = gps->hdop; // if successful, update hdop if ( hdop<500 && n_used>3 ) //============================ { // got a reasonable fix; nov11 added n_used>3 = high-quality ngood++; // increment # of good fixes if (ngood==2) { // after 2nd fix, wait 20s more //--- sep11, update time first, then update end-time UBX_SetRTC(); // update the RTClock based on GPS time tnow = RTCGetTime(0,0); tend = tnow + 20; tnext = tend + 10; // don't worry about the wing anymore } } //======================================================== if ( tnow>tnext && wc==1 ) //------------------------------ { // then want wing-control & time to do it // jul09 v3 make sure gps is on & switch is right gps_on(); // make sure gps is on & mux is correctly set up ant_switch(GPS_UNIT,iwing); // config ant switch for right unit, wing if ( iwing == PORT_WING ) cprintf("PORT "); else cprintf("STBD "); cprintf(" wing control for GPS\n"); roll_wing(iwing,0); // roll the specified wing up,0=don't seek UBX_init( ); // set the UBX to a known state tnext += twc; // set flag for next control time service_wdog(); // dec14 service the CF2 } //-------------------------------------------------------- } // end while ++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++ tend = tnow - tstart; gps->time = (short) (tend/10); // time to acquire, [10 s] incr. gps->valid = (hdop<900 || ngood>0); // 0902, went from hdop<500 to <900, as fixes seem fine. // OR if already deemed we got a good fix (gets around variable hdop). gps->wing = iwing; // used in the sbd gps message // if (gps->valid) set_time(gps); // update RTC to UTC // could keep track of wing error here as well // if want to decide if a wing antenna is bad/poor gps_off( ); // turn off gps return ( gps->valid ); } //*************************************************************************** void show_gps ( void ) //**************************************************** { // display gps structure values short n; struct gps_param *gps; //oct13 update for +/-0 degree situations float xdlat, xdlon; gps = all.gps; // 0901 nsat = satellite info only: no rstat // n = gps->nsat; //rstat = ( gps->nsat & 0xf0 ) >> 4; // upper 4 bits, shift right by 4 if (gps->valid) { printf("NEW"); } else { printf("OLD"); } //oct13, changed to floats, & not quite zero if on equator xdlat = ((float)gps->lat_deg+.001)*gps->lat_sign; // signed latitude xdlon = ((float)gps->lon_deg+.001)*gps->lon_sign; // signed longitude printf("week %6d day %1d, %02d:%02d:%02d %+3.0f:%06.3f %+4.0f:%06.3f ", gps->gps_week, gps->gps_day, gps->utc_hr, gps->utc_min, gps->utc_sec, xdlat, gps->lat_min, xdlon, gps->lon_min ); printf(" %1d %4d %4d %4d %4.1f \n", n, gps->smin, gps->savg, gps->smax, gps->hdop*0.1 ); return; } //*************************************************************************** short calc_surf_drift ( void ) //********************************************** { // calculate the surface drift // returns 1=good // 0=either/both fixes NOT valid: no good short good; long dx; struct ublox_param *ubx; float ylat, clat; ubx = all.ubx; good = ( ubx->valid[0]==1 && ubx->valid[1] == 1 ); if ( good ) //=========================================================== { // both fixes are valid: process // ( dx*1e-7 *60 (nm/deg) * 1852 (m/nm) )*90 = 1.000 m ylat = ubx->lat[1]*1e-7; // decimal degrees clat = cos_find( ylat ); // cos of latitude dx = (ubx->lat[1] - ubx->lat[0] )/90; ubx->dy = (short) dx; // scaling = 1m per count dx = (ubx->lon[1] - ubx->lon[0] )/90; dx = dx*clat; // scale from fraction of long. degrees to m. ubx->dx = (short) dx; dx = ubx->ti[1] - ubx->ti[0]; ubx->dt = (short) dx; } else //================================================================ { // not valid ubx->dx = 0; ubx->dy = 0; ubx->dt = 0; } //===================================================================== return (good); } //*************************************************************************** void test_gps ( void ) //************************************************* { // test the gps functions char ch; short err, tmo=0, n, val; struct gps_param *gps; struct ublox_param *ubx; gps = all.gps; // point to gps structure; ubx = all.ubx; gps_off( ); gps->hdop = 2500; gps->nsat = 0; gps->smin = 250; gps->savg = 0; gps->smax = 0; // max time comes from iparam [ IA_gps_tm ] cprintf(" Test of GPS %s : %s \n",GTYPE, __DATE__); gps_menu(); while ( ch !='Q') //======================================================= { SerInFlush(); // flush any characters if (ch !=' ' && ch>0 ) cprintf("G_1>\n"); // display directory prompt ch = -1; // illegal value - means it timed out ch = SCIRxGetCharWithTimeout( 1000 ); // get a new character tmo = check_tmo ( &ch, tmo ); // increments the tmo counter if ch<0, and // goes to sleep mode after max_tmo. Returns *ch as upper-case switch (ch) { // call the desired function case 'Q' : break; // will quit the program case '?' : break; // displays the directory prompt case 'U' : gps_menu(); break; case 'D' : gps_debug(); break; case -1 : break; // wait for another character case '+' : toggle_verbose(); // toggle verbose mode break; case 'W' : gps_on( ); cprintf("gps is on\n"); break; case 'O' : gps_off( ); cprintf("gps is off\n"); break; case 'C' : calc_surf_drift ( ); cprintf("Surf dx, dy, dt = %d %d %d\n", ubx->dx, ubx->dy, ubx->dt); cprintf("Give 0=start, 1=end for next GPS fix\n"); n = get_num ( &val ); if (n==1) ubx->index = val; break; case 'G' : check_UBX( ); break; case 'I' : err = UBX_init ( ); cprintf("gps is initialized "); if (err) cprintf(" OK\n"); else cprintf(" BAD\n"); break; case 'L' : show_gps ( ); break; case 'P' : // err = ant_select ( ANT_GPS_2_PORT ); ant_switch ( GPS_UNIT, PORT_WING ); cprintf("port wing antenna to GPS \n"); //if (err !=SPI_CRC_OK) cprintf("port ant error=%c \n",err); break; case 'S' : //err = ant_select ( ANT_GPS_2_STBD ); ant_switch ( GPS_UNIT, STBD_WING ); cprintf("stbd wing antenna to GPS \n"); //if (err !=SPI_CRC_OK) cprintf("stbd ant error=%c \n",err); break; default : cprintf(" input unknown \n"); } // end switch +++++++++++++++++++++++++++++++++++++++++++++++++++++++ } //=== end while ======================================================== gps_off( ); // make sure gps is off return; } //************************************************************************** void gps_menu(void) //********************************************************* { //print out menu selections cprintf(" GPS MENU \n" "Q.uit GPS test : U.pdate menu : D.ebug Menu\n" "G.et Position : O.ff gps : W.akeup GPS\n" "I.nit GPS : L.ast Pos. Display : C.alc Drift \n" ); cprintf( "P.ort Wing : S.tbd Wing\n"); return; } //************************************************************************** void gps_debug ( void ) //************************************************ { // test the gps functions char ch; short err, tmo=0, n, val; struct gps_param *gps; gps = all.gps; // point to gps structure; GPS_Baud = 96; //---Feb12, play around w/GPS baud rate gps_off( ); gps->hdop = 2500; gps->nsat = 0; gps->smin = 250; gps->savg = 0; gps->smax = 0; // max time comes from iparam [ IA_gps_tm ] cprintf(" Debug GPS %s : %s \n",GTYPE, __DATE__); gps_d_menu(); while ( ch !='Q') //======================================================= { SerInFlush(); // flush any characters if (ch !=' ' && ch>0 ) cprintf("G_2>\n"); // display directory prompt ch = -1; // illegal value - means it timed out ch = SCIRxGetCharWithTimeout( 1000 ); // get a new character tmo = check_tmo ( &ch, tmo ); // increments the tmo counter if ch<0, and // goes to sleep mode after max_tmo. Returns *ch as upper-case switch (ch) { // call the desired function case 'Q' : break; // will quit the program case '?' : break; // displays the directory prompt case 'U' : gps_d_menu(); break; case 'D' : gps_debug(); break; case -1 : break; // wait for another character case '+' : toggle_verbose(); // toggle verbose mode break; case 'W' : gps_on( ); cprintf("gps is on\n"); break; case 'O' : gps_off( ); cprintf("gps is off\n"); break; case 'B' : cprintf(" give baud rate/100 (i.e. 96) \n"); n = get_num(&val); if (n==1) //then a legal read of value { // send command set_gps_baud( val); GPS_Baud = val; } break; case 'C' : UBX_cold( ); cprintf("GPS cold reset sent\n"); break; case 'G' : check_UBX( ); break; case 'I' : err = UBX_init ( ); cprintf("gps is initialized "); if (err) cprintf(" OK\n"); else cprintf(" BAD\n"); break; case 'R' : UBX_SetRTC( ); break; case 'P' : ant_switch ( GPS_UNIT, PORT_WING ); cprintf("port wing antenna to GPS \n"); break; case 'S' : ant_switch ( GPS_UNIT, STBD_WING ); cprintf("stbd wing antenna to GPS \n"); break; case 'X' : gps_off(); // make sure is off get_gps( 1 ); show_gps( ); break; default : cprintf(" input unknown \n"); } // end switch +++++++++++++++++++++++++++++++++++++++++++++++++++++++ } //=== end while ======================================================== gps_off( ); // turn off the gps cprintf("GPS is off\n"); return; } //************************************************************************** void gps_d_menu(void) //******************************************************* { //print out menu selections cprintf(" GPS Debug Menu \n" "Q.uit Debug : U.pdate menu : C.old Reset \n" "G.et Position : O.ff gps : W.akeup GPS\n" "I.nit GPS : X. execute cycle : R.TC set from GPS\n" ); cprintf("P.ort Wing : S.tbd Wing : B.aud Rate \n"); return; } //************************************************************************** //*** end *********************************************************************