/****************************************************************************** // nav.c handles the navigation functions // //***************************************************************************** // // Jan2009, created // // //==== Future Expansion ======================================================= // last update: Jan2009 // // 1. // 2. verify that waypoint and route exceptions are handled OK. // ******************************************************************************/ #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 "setup.h" #include "nav.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; // internal prototypes void test_navigation( void ); //***************************************************************************** void accum_dr( float pa, float h, float dp ) { // calculate dr over this interval and accumulate // pa = pitch // h = heading (magnetic) // dp = change in pressure struct nav_param *nav; float t; nav = all.nav; // point to the nav sub-struct // compute the glide angle pa = flt_abs(pa); // abs value // if (pa<0) pa = -pa; //=abs value pa = pa + ATTACK_ANGLE; // rough estimate on glide angle if (pa<12) pa = 12; // set to a minimum value // pa = pa*0.0175; // glide angle, in rad t= tan_find( pa ); // tangent dp = flt_abs(dp); // take abs(dp) h = ( h - iparam[ IA_mag_var ] ); // True, in degrees dp = dp / t; // horiz glide dist. [m] nav->drx += (dp*sin_find( h ) ); // dist. traveled east nav->dry += (dp*cos_find( h ) ); // dist. traveled north return; } //*************************************************************************** short xy2rb( float dx, float dy, float *range) //***************************** { // convert dx, dy into range and bearing...returns bearing // bearing range = 0..360 double bear; short theta; *range = (float) sqrt( dx*dx + dy*dy ); // bear = atan2(dx,dy)*57.3; bear = atan_find(dx,dy); theta = ilimit360( (short) bear ); // limit to range = 0..360 return( theta ); } //*************************************************************************** short LonLat2rb(float x1, float y1, float x2, float y2, float *range) { // convert Lat, Lon from X1 to X2 to range and bearing...returns bearing // x1, x2, y1, y2 are decimal degrees // output bearing range = 0..360 float dx, dy, clat, y; short bear; y = (y1 + y2)/2; // average latitude clat = cos_find( y )*111.1; // average longitude scalar dx = (x2 - x1)*clat; // km dy = (y2 - y1)*111.1; // km bear = xy2rb(dx, dy, range); return(bear); } //*************************************************************************** void check_waypt( void ) // *************************************************** { // see if we are 'at' the waypoint, & update to the next one. // Special case is when waypt = 0 = Home location. // This fnx is only called from 'get_theta_.' float wlat1, wlon1; float lat_now, lon_now; float range, diff, circle, max_dx; short ir, iw, theta, n, detect, iradius; short at_waypt; // used to decide if we're at the waypt short at_end = FALSE; // set true when at the end-of-route short at_new_waypt = FALSE; // is set if we arrive at a new waypt struct nav_param *nav; struct route_param *route; struct s_waypt *waypt; struct gps_param *gps; gps = all.gps; // point to the gps structure if (!gps->valid) return; // gps is NOT valid, return immediately verify_route(); //makes sure route is legal, & heads for home if bad nav = all.nav; // point to the nav parameters route = nav->route; // point to the route params waypt = nav->waypt; // point to the waypoint list n = iparam[ IA_num_route ]; // = #of points in the route // compute a watch-circle radius based on last dive: used for auto-mode range=0 max_dx = ((float)all.eng->zmax) *0.003; // =last dive depth * 3.0 glide ratio // max_dx [km] = max expected horizontal distance traveled for 1/2 dive. iradius = 1 + (short) max_dx; // equivalent to rounding max_dx up //=1 if zmax<333, =2 if zmax<666, =3 if zmax<999, =4 if zmax<1333 if (iradius<1) iradius=1; // make sure that it is legal if (iradius>4) iradius=4; // make sure that it is legal printf("Auto-Watch-Circle R[km] =%4.1f = %d\n",max_dx, iradius); //get the present lat and lon in decimal degrees lat_now = gps->xlat; lon_now = gps->xlon; nav->xlat = lat_now; //save values in nav structure as well nav->xlon = lon_now; // printf("present lat, lon = %8.3f %8.3f\n",lat_now,lon_now); // compute bearing to HOME =waypt[0] & save just in case theta = LonLat2rb(lon_now,lat_now, waypt[0].lon, waypt[0].lat, &range); nav->home = theta; // True heading to home waypt ir = iparam[ IA_route_now ]; // point to the present index into the route // ir=0 = go HOME at_waypt = TRUE; // set TRUE to enter the while loop while (at_waypt) // &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&& { // check waypt & increment to next if we're there // if we reach the end-of-route, will also break out of this loop iw = route->rlist[ir]; // points to the waypoint index if ( iw<0 || iw>=MAX_WAYPTS || ir<0 || ir>n ) { // then either ir or iw are out-of range: something is broken iw =0; // = HOME waypoint index ir =0; // = head HOME iparam[ IA_route_now ] = 0; // = HOME route entry iparam[ IA_route_dir ] = 1; // make sure direction is legal iparam[ IA_steer_pt ] = 0; // turn off using the steering pt // exception is noted on shore via heading for HOME! } // end if bad indices // get information for this waypt wlat1 = waypt[iw].lat; wlon1 = waypt[iw].lon; if (route->circle[ir] == 0 ) // then auto-ranging circle = (float) iradius; // use circle based on glide depth else circle = (float) route->circle[ir]; detect = route->detect[ir]; // compute range & bearing from our present location theta = LonLat2rb(lon_now,lat_now,wlon1,wlat1,&range); // check the route-line bearing, and set if undefined if (nav->bear < 0 || nav->bear>360 ) //-------------- { // bearing has yet to be defined ----------------- if ( route->blist[ir] == 0) { // then we want auto-ranging nav->bear = theta; // set to our present bearing to the waypt if (nav->owlat<-90 || nav->owlat>90) { // then ill-defined old waypt settings nav->owlat = lat_now; nav->owlon = lon_now; nav->range = range; } } else // else we want to use a manual value, defined by blist { nav->bear = route->blist[ir]; } } // end if bear<0 ---------------------------------- if (at_new_waypt) //--------------------------------- { // then we want to keep track of old info for steering pt calcs nav->owlat = lat_now; nav->owlon = lon_now; nav->range = range; nav->sbear = theta; //=bearing to new waypt } // end if ----------------------------------------- if (at_end) break; // if reached a route endpt, quit checking // this exits us out of an infinite loop if we're within all waypts // but gets us far enough to set the new nav-bear, iw, ir values // check to see if we're at this waypoint diff = theta - nav->bear; // difference between waypt path & path NOW diff = limit180(diff); // limit to +/- 180 degrees diff = flt_abs(diff); // abs value // if (diff<0) diff = -diff; // take absolute value printf("waypt %2d : route_angle= %5d, range=%5.1f D-theta=%5.1f\n", iw,nav->bear,range, diff); // if diff>90, then the waypt is aft of abeam ---------------- switch(detect) //--------------------------------------------- { // depending on route detect, decide if we're there case 0 : at_waypt = range90) ); break; case 2 : // use bisector angle only at_waypt = diff>90; break; } //end switch --------------------------------------------- if (at_waypt) // then we're at this waypt ------------------- { // try the next waypt ------------------------------------- at_new_waypt = TRUE; // we have arrived @ a new waypt printf("At Route %2d, Waypt %2d\n",ir, iw); iparam[ IA_man_theta ] = -1; // turn OFF manual steering // so we head to the next waypt. nav->bear = -999; // =we don't know the new bearing // if we're at the home waypt, abort !! if (ir==0) { all.exc->abort = TRUE; // HOME exception return; } // return now ir += iparam[ IA_route_dir ]; // point to next waypt // check to see if we're past an endpt ^^^^^^^^^^^^^^^^^^^ if ( ir>n || ir<1 ) // ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ { // then we're at an endpt of the route at_end= TRUE; // set TRUE that we're at the end-of-route switch( iparam[ IA_route_end ]) //++++++++++++++++++ { // depending on end choice, take appropriate step case 0 : // go to the HOME waypoint ir = iw = 0; // go to HOME waypt break; case 1 : // repeat transect from the other end if (ir<1) ir = n; //going from 1 to npts else ir=1; //else going from npts to 1 break; case 2 : // reverse route if (ir<1) ir = 2; //going from 1 to 2 else ir =n-1; //else going from npts to npts-1 // switch route direction iparam[ IA_route_dir ] = -iparam[ IA_route_dir ]; break; case 3 : // stay at the last waypt if (ir<1) ir=1; else ir = n; break; case 4 : // means we're done with mission all.exc->abort = TRUE; break; } // end switch on end-of-route options +++++++++++++ } // end if at end of route ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ // --------------------------------------------------------- } // end deemed that at a waypt ------------------------------ } // end while not at waypt &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&& // we're here because we found a waypt that we're NOT at, or at the end-of-route // Save the new values: note ir, iw have already been bounds-checked iparam[ IA_route_now ] = ir; // save the new waypoint value nav->wlat = waypt[iw].lat; nav->wlon = waypt[iw].lon; printf("waypt %d : lat %8.3f lon %8.3f \n",iw,nav->wlat,nav->wlon); return; } //*************************************************************************** void get_theta( void ) // *************************************************** { // *** compute new heading given a new position in gps // updates nav->theta with new heading=Magnetic // nav->true_theta = True // 0901, this is called BEFORE the dive_num is incremented: handle approp. float lat_now, lon_now, clat, dr, set; float droute, dd, d2w, wlat, wlon; float x, dx1, dy1, dx2, dy2, dr2; float dxs, dys; short phi, theta, theta2, eps, gamma, use_set, nman; short steer_pt, iprof, set_correct, man_theta; struct gps_param *gps; struct nav_param *nav; nav = all.nav; // assign same pointer value gps = all.gps; // same here iprof = iparam [ EA_prof_num ] +1; //0901 +1 added // the next profile #: the dive_num is incr. AFTER get_theta, // therefore the +1 above. // This allows surface() to update bad heading params, // which happens before the dive_num is incremented. nman = iparam[ IA_steer_nprof ]; // last dive# to use steering pt if (nman > 0 && nman >= iprof) { steer_pt = iparam [ IA_steer_pt ]; cprintf("Using steering pt = %d\n", steer_pt ); } else //nman 1 if we want to steer relative to current: 'current-crossing' // set_correct = 0 if want to do neither nman = iparam[ IA_man_nprof]; // last dive# to steer manually if (nman > 0 && nman >= iprof) { man_theta = iparam[ IA_man_theta ]; cprintf("Using manual theta = %d\n", man_theta ); } else //nman valid) //&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&& { // valid fix, compute new direction check_waypt( ); // if needed, update waypt lat_now = nav->xlat; //values are from check_waypt() lon_now = nav->xlon; clat = cos_find( lat_now )*111.1; // #km in one degree Lat. theta = LonLat2rb(lon_now,lat_now,nav->wlon,nav->wlat,&dr); // theta = angle(True) to the next waypoint (wlat, wlon) nav->dr = dr; // range to waypoint, in km printf("now= %8.3f %8.3f, wpt= %8.3f %8.3f, range=%5.1f, true_theta=%d\n", lat_now, lon_now, nav->wlat, nav->wlon, dr, theta ); // compute an alternate steering point rather than go to waypoint -------- if (steer_pt>0) // ------------------------------------------------------- { // then find distance down route line by km = steer_pt droute = nav->range; // distance between waypts on the route line if (droute<0.1) droute = 0.1; // make sure it is OK so no divide-by-zero eps = nav->bear - theta; // angle difference between route line and present angle d2w = dr*cos_find( eps ); // distance to waypoint, along the route line dd = d2w - steer_pt; // distance between steer_pt & waypt, along route line // if >0, then steer_pt is before waypt, else, it is beyond the waypt dd = ( droute - dd)/ droute; //= fraction along route line to steering point // if dd>1, then steer beyond waypt, if dd<0, then steer before last waypt wlat = nav->owlat + dd*( nav->wlat - nav->owlat); // new latitude steering pt wlon = nav->owlon + dd*( nav->wlon - nav->owlon); theta = LonLat2rb(lon_now,lat_now,wlon,wlat,&dr); // theta = new angle to head for // print out debug info printf("steer pt = %6.3f %6.3f range=%6.1f\n",wlat, wlon, dr); } // end if steer_pt ---------------------------------------------------- // steer pt now acts as new waypoint: we have new theta, // correct for current if requested ===================================== if ( (set_correct !=0 ) && (nav->ovalid) ) //============================ { //correct only if the last gps was good & we want to do it // compute the distance traveled over the ground dx1 = (lon_now - nav->olon)*clat; // x km traveled over last dive dy1 = (lat_now - nav->olat)*111.1; // compute the distance traveled by dead-reckoning dx2 = nav->drx*0.001; // in km dy2 = nav->dry*0.001; dr2 = (float) sqrt(dx2*dx2 + dy2*dy2); // dead-reckoning distance, km if (dr2 <0.1) dr2 = 0.1; // keep >0 // estimate the amount of set dxs = dx1 - dx2; // est. set east-west, km dys = dy1 - dy2; // est. set N-S, km phi = xy2rb(dxs, dys, &set); // phi = direction of the current // set = magnitude of the set printf("Set direction & strength = %4d %4.1f km\n",phi, set); if ( set_correct ==1) //------------------------------------------ { // correct heading by the set correction gamma = theta - phi; // current angle relative to desired course x = (set/dr2)*sin_find( gamma ); // law of sines, x = sin(eps) // eps = angle between the dr vector and the desired course if ( x>1 || x<-1 ) // then we can't correct to get back: current is too large { // do our best to get back to the route-line by going 90 all.exc->exc_stat |= EXC_HIGH_CURRENT; //jan13 // 0x1000 = flag that we can't maintain against set if (x>1) eps = 90; // max correction else eps = -90; // these will be clipped in the code below /* old correction: if can't buck, then cross if (x>1) eps = 90 - gamma; // go perpendicular to the current else eps = -90 - gamma; /**/ } //xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx else // x is in bounds, compute the angle { eps = asin_find(x); // returns degrees // eps = asin(x)*57.3; } //xxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxxx } //------------------------------------------------------------- else { // else, set_correct is the desired angle relative to the current direction x = phi + set_correct; // new heading relative to the current x = limit360(x); // keep within 0..360 value // jun05, make so bounds are the same as above: relative to theta eps = x - theta; // difference between desired and straight-line } // end if..else use_set ==1 // we have eps = difference between desired & straightline course eps = limit180(eps); // make sure it's within 180 degrees if ( eps < iparam[ IA_set_min]) eps = iparam[ IA_set_min]; // limit correction if ( eps > iparam[ IA_set_max]) eps = iparam[ IA_set_max]; // limit correction theta2 = theta + eps; //=new course to steer printf("head = %d, %d w/o & w/set\n",theta, theta2); theta = ilimit360(theta2); } //====================================================================== // end if use_set !=0 and the last fix was valid ========================= //** save values for next time nav->olat = lat_now; nav->olon = lon_now; nav->ovalid = TRUE; } //&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&& else // position is not valid, let nav->bad_gps decide new heading { // bad_gps is set in surface, dependent upon IA_no_gps value theta = nav->bad_gps; // True heading if no valid fix : if =-1, do circle if (theta==-3) //flag that we want to head HOME theta = nav->home; cprintf("position NOT valid, using nav->bad_gps = %d\n", theta); nav->ovalid = FALSE; //set flag that we don't have valid position } // end if valid position &&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&&& if ( man_theta == -2 ) theta=-1; // just do circles if ( man_theta > -1 ) theta = man_theta; // manually steer !!! // 0702, moved here so manually steering does not require good GPS if ( theta > -1 ) { // then valid nav->true_theta = theta; // true course to steer theta += iparam[ IA_mag_var ]; // add in the local magnetic variation theta = ilimit360(theta); // limit to 0..360 } else // theta==-1: man_theta=-2, or bad_gps = -1 { nav->true_theta = -1; } // set as flag that we do circles nav->theta = theta; printf("Mag_head = %d, pitch = %d\n",nav->theta, iparam[ IA_glide_angle ]); return; } //*************************************************************************** void test_navigation( void ) // *********************************************** // test navigation function { float wlat, wlon; short i, drx, dry, iprof; char line[80]; struct nav_param *nav; all.eng->zmax = 1002; // set to a known qty nav = all.nav; // point to same spot if (nav->route->valid != 99) { // then not a valid route, quit now printf("Route is not valid! \n"); return; } // end not valid printf("give decimal latitude & longitude for present position >\n"); SerInFlush(); // flush input gets(line); i = sscanf(line,"%f %f", &wlat, &wlon); printf("give profile #, drx, dry (m) >\n"); SerInFlush(); // flush input gets(line); i = sscanf(line,"%d %d %d", &iprof, &drx, &dry); all.nav->drx = drx, all.nav->dry = dry; iparam [ EA_prof_num ] = iprof; all.gps->valid = TRUE; all.gps->xlat = wlat; all.gps->xlon = wlon; get_theta( ); printf("range (km) to waypt, heading (M) to steer= %6.1f %d\n", nav->dr, nav->theta ); //** save values for next time nav->olat = nav->xlat; nav->olon = nav->xlon; nav->ovalid = TRUE; return; } //*************************************************************************** void change_1_route( void ) // *********************************************** { // change 1 route index values char line[80]; short narg, x[6]; cprintf("Enter route index, waypt index,approach angle," " detection, radius >\n"); SerInFlush(); // flush input gets(line); narg = read_shorts( line, 5, x ); // read in the params cmd_7 ( 0, narg, x); // and parse return; } //*************************************************************************** void set_route( void ) // **************************************************** { // set route parameters : User input // route->rlist[] indices go from 1..n struct route_param *route; short i, m, n, k, angle, detect, rad; char line[80]; route = all.nav->route; n = iparam [ IA_num_route ]; // # of points in the route cprintf("Use cmd C to update the waypoint list.\n"); cprintf("#Waypts in Route = %d\n",n); if ( n<1 || n>(MAX_WAYPTS-1) ) { cprintf("Illegal #route points\n"); return; } cprintf("Enter Waypoint index, approach angle, arrival-detect, & watch-circle R\n"); cprintf("Approach angle=0 = auto-set, else 1-360=degrees T heading to waypoint\n"); cprintf("Detect values: 0=range only, 1=range+angle, 2=angle only\n"); cprintf("Watch-circle radius values: 0=auto-range, else = [km] for radius\n"); route->valid = 99; // 99=valid; start w/assuming it is valid route->rlist[0] = 0; // 1st position always points to HOME=0 route->blist[0] = 0; // and always use auto-bearing route->detect[0] = 0; // use range-only route->circle[0] = 0; // auto-ranging // get user info for the route list i=1; // first route index is always 1 do { // get the route while entries are valid cprintf(" Route pt %d: Give waypt[i],approach angle, detection, radius >\n", i); SerInFlush(); // flush input gets(line); m = sscanf(line,"%d %d %d %d", &k, &angle, &detect, &rad ); if ( m==4 && (k>0 && k-1 && angle<361) && (detect>-1 && detect<4) && (rad>-1 && rad<20) ) { // then all are valid route->rlist[i] = k; // point to this waypoint route->blist[i] = angle; // with this approach angle route->detect[i] = detect; // arrival-detect choice route->circle[i] = rad; // watch-circle radius } else route->valid = 0; // set flag that route is not valid } while ( i++valid == 99 ); verify_route(); // make sure the route makes sense if (route->valid != 99) { cprintf("Set Route failed! Try again.\n"); } else { display_route( FALSE ); } // show the route, but don't write to file return; } //*************************************************************************** //***************************************************************************** short insert_waypt(struct s_waypt *waypt, short iw, float xlat, float xlon) { // take waypt index=iw, xlat, xlon, verify ranges // & if valid, update the waypoint list // returns 1 if good, else 0 short good = FALSE; // assume it is bad if ( ( iw>=0 && iw-90 && xlat<90 ) // lat is OK && ( xlon>-180.01 && xlon<180.01 ) // lon is OK ) { // values are good good = TRUE; waypt[iw].valid= 99; // set to VALID flag waypt[iw].lat = xlat; waypt[iw].lon = xlon; printf("%3d %9.3f %9.3f : Waypoint is now set\n",iw,xlat,xlon); if (iw ==0) cprintf("HOME waypoint is now set\n"); } else printf("Bad waypt values\n"); return(good); } //*************************************************************************** void change_waypt( struct s_waypt *waypt ) // ******************************** { // Change the lat/lon values for a single waypoint short i, stat=0, n; float xlat, xlon; char line[80]; cprintf("\nGive waypoint #, decimal Lat, Lon >\n"); SerInFlush(); // flush input gets(line); n = sscanf(line,"%d %f %f",&i,&xlat, &xlon ); waypt[i].valid = 0; // set to NOT valid if (n==3) { // we read in right # of values stat = insert_waypt(waypt,i, xlat, xlon); // returns TRUE if good } // end if n==3 // else we're here because of a bad value/illegal input if (!stat) printf("-1 -1 -1 : BAD INPUT LINE %s\n",line); return; } //*************************************************************************** void waypt_display( short yes_file ) // *************************************** { // display the waypoint list // if *fp is valid, write to a file as well short i; char s[80]; // output string char name[20]; struct s_waypt *waypt; FILE *fp; waypt = all.nav->waypt; if (yes_file) //------------------------------------ { // then open a file to write out sprintf(name,"%04d.wpt", iparam[ EA_serno ] ); fp = fopen(name,"w"); // over-write an old one if needed if (fp==NULL) cprintf("could not open %s\n",s); write_header( fp ); } //------------------------------------------------- cprintf("# i Lat Lon\n"); for (i=0;isave_waypt = FALSE; // set flag that it is saved. } return; } //*************************************************************************** void store_waypt(float x, short *xdeg, short *xf) //************************** { // store a float in 2 shorts : degrees + fraction // x = float input, decimal degrees // xdeg = signed degrees // xf = unsigned fraction of degrees *1000 // used by lifo_waypts to store values as ints short xsgn=1, xmin, deg; if (x<0) { xsgn = -1; x = -x; } // keep sign, make x>0 deg = x; // unsigned integer of degrees xmin = ( x - deg)*1000.; // fraction of degree, *1000 *xdeg = deg*xsgn; // make output the signed value *xf = xmin; // and store the fractional value as well return; } //*************************************************************************** short fix_bad_route ( void ) //************************************************ { // if the route-valid flag is bad, // set the route to head HOME. // if HOME is not valid, also set that // returns TRUE if had to fix it short ierr = 0, n, k, ok; struct s_waypt *waypt; struct route_param *route; struct nav_param *nav; nav = all.nav; route = nav->route; waypt = nav->waypt; n = iparam [ IA_num_route ]; // # of waypoints in the route k = iparam [ IA_route_now ]; ok = ( n=0 && k<=n ); // legal route->rlist [ 0 ] = 0; // make sure 0-entry points to HOME if ( route->valid != 99 || !ok ) //=============================== { // NOT VALID, force to HOME iparam [ IA_route_now ] = 0; // route-entry=0=points to HOME iparam [ IA_route_dir ] = 1; iparam [ IA_route_end ] = 3; iparam [ IA_num_route ] = 1; // only one point! if ( waypt[0].valid !=99 ) { // HOME is NOT a VALID waypoint! waypt[0].lat = 0.0; waypt[0].lon = 0.0; waypt[0].valid = 99; // set valid } cprintf("Not Valid Route. Setting to head HOME\n"); route->valid = 99; // fixed ierr = TRUE; } //============================================================= return ( ierr ); } //*************************************************************************** short verify_route( void ) //************************************************** { // verify that the route's waypoints are not garbage // returns TRUE if OK, else FALSE short i,n, iw, i_ok=TRUE; struct s_waypt *waypt; struct route_param *route; struct nav_param *nav; nav = all.nav; route = nav->route; waypt = nav->waypt; fix_bad_route(); // if route is not valid, sets to head HOME n = iparam [ IA_num_route ]; // # of waypoints in the route i =1; // verify that the route->waypt indices make sense do //=============================================================== { // step thru i and verify iw = route->rlist[i]; // waypoint in the route i_ok = (iw>=0 && iw-90 && waypt[iw].lat<90 && waypt[iw].lon>-180 && waypt[iw].lon<180 ); } while (i_ok && i++calc_route = FALSE; //clear flag that might have been set via shore cmd return ( i_ok ); } //*************************************************************************** void display_route( short yes_file ) // *************************************** { // display the route and write to file if wanted short ir, iw, n; struct s_waypt *waypt; struct route_param *route; struct nav_param *nav; char name[20]; char s[80]; FILE *fp; nav = all.nav; route = nav->route; waypt = nav->waypt; n = iparam [ IA_num_route ]; // # of waypoints in the route if (yes_file) //------------------------------------ { // then open a file to write out sprintf(name,"%04d.rte", iparam[ EA_serno ] ); fp = fopen(name,"w"); // over-write an old one if needed if (fp==NULL) cprintf("could not open %s\n",s); write_header( fp ); } //------------------------------------------------- sprintf(s,"@R %3d %3d %3d %3d " "=#wpts in rte, next wpt, end-rte-flag, dir.", n, iparam[ IA_route_now ], iparam[ IA_route_end ], iparam[ IA_route_dir ] ); cprintf("%s\n",s); if ( yes_file && fp !=NULL ) fprintf(fp,"%s\r\n", s); sprintf(s,"# ir,iw, approach_angle, detection, range"); cprintf("%s\n",s); if ( yes_file && fp !=NULL ) fprintf(fp,"%s\r\n", s); for (ir=1; ir<=n;ir++) //=========================================== { // Display the route info for all points iw = route->rlist[ir]; sprintf(s, "@r%02d %02d %1d %1d %3d", ir,iw, route->blist[ir], route->detect[ir], route->circle[ir] ); cprintf("%s\n",s); if ( yes_file && fp !=NULL ) fprintf(fp,"%s\r\n", s); } //================================================================ if ( yes_file ) { cprintf("route list written to %s\n",name); fclose( fp ); // close the file all.sbd->save_route = FALSE; // set flag that it has been saved. } cprintf("Steering Mode Settings:\n"); cprintf("use_set = %d\n", iparam [ IA_use_set ] ); cprintf("steer_current = %d, tmo = %d\n",iparam [ IA_cross ] , iparam[IA_set_nprof ] ); cprintf("manual theta = %d, tmo = %d \n",iparam[ IA_man_theta ], iparam[IA_man_nprof ]); cprintf("steer pt km =%d, tmo = %d \n",iparam[IA_steer_pt], iparam[IA_steer_nprof] ); cprintf("Heading limits: min = %d, max = %d \n", iparam[ IA_set_min], iparam[ IA_set_max]); return; } //*************************************************************************** short read_wpt( void ) //***************************************************** { // read the waypoint list from the file // where XXXX is the serial # // returns TRUE if no errors short ok = true, err; short sn; FILE *fp; char name[20]; sn = (short) VEEFetchLong( "SERNO", NO_VEE_VAL ); if (sn == NO_VEE_VAL) //------------------------------------ { // then NOT found !! sn = 999; // set to default cprintf("Illegal s/n, setting to default = %d", sn); } //------------------------------------------------------- // compose the waypt name ------------------------------------- { sprintf(name,"%04d.wpt",sn); } //--- open the file ------------------------------------------- fp = fopen(name, "r"); // read if (fp) //----------------------------------------------------- { // then it is valid, we can read in err = rd_wpt( fp ); // read in all lines if (err) { cprintf("WPT err = %d\n",err); ok= FALSE; } } else { cprintf("Waypt File %s could not be opened \n",name); ok = FALSE; // open error } all.sbd->save_waypt = FALSE; // clear flag fclose( fp ); return ( ok ); } //*************************************************************************** short rd_wpt(FILE *fp ) //***************************************************** { // read in from file until no more lines // returns 0 if OK, else error status // err=1 = bad input // err=3 = incorrect #arg short n, num, err=0, ok; char str[80]; short iw; float xlat, xlon; struct s_waypt *wpt; wpt = all.nav->waypt; do //==================================================================== { // read until EOF n = fget_line( fp, str, 80 ); // read up to 80 char if (n>6 && str[0] == '@' ) //++++++++++++++++++++++++++++++++ { // then could be a legal line num = sscanf(str,"@w%d %f %f",&iw, &xlat, &xlon ); if (num==3) //333333333333333333333333333333333333 { // then read in the correct # of arguments ok = insert_waypt(wpt,iw, xlat, xlon); // returns TRUE if good if (!ok) err = 1; // any bad value sets return value } else err=3; //333333333333333333333333333333333333 } //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++ } while (n>=0); //======================================================= return ( err ); } //*************************************************************************** short read_rte( void ) //***************************************************** { // read the route list from the file // where XXXX is the serial # // returns TRUE if no errors short ok = true, err; short sn; FILE *fp; char name[20]; sn = (short) VEEFetchLong( "SERNO", NO_VEE_VAL ); if (sn == NO_VEE_VAL) //------------------------------------ { // then NOT found !! sn = 999; // set to default cprintf("Illegal s/n, setting to default = %d", sn); } //------------------------------------------------------- // compose the route name ------------------------------------- { sprintf(name,"%04d.rte",sn); } //--- open the file ------------------------------------------- fp = fopen(name, "r"); // read if (fp) //----------------------------------------------------- { // then it is valid, we can read in err = rd_rte( fp ); // read in all lines if (err) { cprintf("RTE err = %d\n",err); ok= FALSE; } } else { cprintf("File %s could not be opened \n",name); ok = FALSE; // open error } all.sbd->save_route = FALSE; // clear flag all.sbd->calc_route = FALSE; // clear flag fclose( fp ); return ( ok ); } //*************************************************************************** short rd_rte(FILE *fp ) //***************************************************** { // read in from file until no more lines // returns 0 if OK, else error status // err=1 = bad input // err=3 = incorrect #arg short n, err=0, e; char str[80]; // short ir, iw, a, d, r, num; do //==================================================================== { // read until EOF n = fget_line( fp, str, 80 ); // read up to 80 char if (n>6 && str[0] == '@' ) //++++++++++++++++++++++++++++++++ { // then could be a legal line if ( str[1] == 'R' ) //-------------------------------- { // main route parameters // fake into the 'W' command str[0] = 'W'; str[1] = ' '; e = parse_cmd( str ); if (e) err=1; } else if ( str[1] == 'r' ) //------------------------- { // then individual route values //num= sscanf(str,"@r%d %d %d %d %d", // &ir, &iw, &a, &d, &r ); //sprintf(str,"7 %d %d %d %d %d",ir,iw,a,d,r); // fake into '7' command str[0] = '7'; str[1] = ' '; e = parse_cmd( str ); if (e) err=2; } //--------------------------------------------------- } //+++++++++++++++++++++++++++++++++++++++++++++++++++++++++ } while (n>=0); //======================================================= return ( err ); } //*************************************************************************** short init_wpt_rte ( short reset_param ) //*********************************** { // read in the waypt and route files if available // also initializes other params // dec11, reset_param=TRUE, then will re-init waypoints, route to not-valid. // ..this is meant to happen on normal start-up, but NOT on spurious, remote // ..so the last waypoint/route info stays in RAM if files are corrupted. // returns error // 0 = no error // 1 = could not read waypt file // 2 = could not read route file // 3 = could not verify the route short ierr = 0, ok=0, iw; struct s_waypt *waypt; struct route_param *route; struct nav_param *nav; nav = all.nav; route = nav->route; waypt = nav->waypt; nav->bad_gps = iparam [ IA_no_gps ]; // what to do if no good gps nav->ovalid = 0; // assume no valid old fix is stored. nav->owlat =-99; nav->owlon =-99; nav->drx = 0; // reset the dead-reckoning nav->dry = 0; if ( reset_param ) //======================================== { // then normal start-up. before reading in, // init to not-valid values for (iw=0; iw< MAX_WAYPTS; iw++ ) { // this sets HOME as not valid as well waypt[iw].valid = -1; // NOT valid waypt[iw].lat = 0; waypt[iw].lon = 0; route->blist [iw] = 0; route->rlist [iw] = 0; // 0=HOME waypt route->detect[iw] = 2; route->circle[iw] = 0; } ok = FALSE; // waypt and route are NOT ok: not set } //========================================================== // this allows old data in RAM to stay on a remote reset, // even if the files cannot be read. else //======================================================= { // it is out of a mid-mission reset. // see if the route seems to be valid. ok = ( route->valid ==99 ); // TRUE if could be valid route if ( ok ) // could be valid ok = verify_route(); // confirm if (ok) display_route( FALSE ); // display the route } //========================================================== // ok = TRUE if the route verified OK if ( ok ) return (0); // return now, no errors // else the route is NOT valid, // try reading in from the files ok = read_wpt( ); // returns TRUE good file read if ( ok ) //================================================= { // then read in the route ok = read_rte(); if ( ok ) //------------------------------------- { // verify and display if ( verify_route() ) display_route( FALSE ); else ierr = 3; } else //---------------------------------------- { // set flag that route is not valid ierr = 2; } //--------------------------------------------- } //========================================================== else { // no valid waypt file ===================================== // initialized to NOT valid settings above if op_mode==0 ierr = 1; } //========================================================== if (Verbose) cprintf("init_wpt_rte error = %d\n",ierr); if (ierr) fix_bad_route(); // this will set route to HOME return ( ierr ); } //*************************************************************************** void menu_nav(void); void test_nav( void ) //******************************************************* { char ch=' '; short val, tmo=0; cprintf(" Test of NAV : %s \n", __DATE__); all.nav->bear = -999; // set to value = NOT Determined yet menu_nav(); while ( ch !='Q') //======================================================= { SerInFlush(); // flush any characters if (ch !=' ' && ch>0 ) cprintf("N_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) { //+++++++++++++++++++++++++++++++++++++++++ case 'Q' : break; // will quit the program case '?' : break; // will display the directory prompt case 'U' : menu_nav(); break; case -1 : break; // wait for another character case '+' : toggle_verbose(); // toggle verbose mode break; case 'C' : change_waypt(all.nav->waypt); break; // change waypoint case 'D' : cprintf("Write to file as well ? \n"); val = YesNo(); display_route( val ); break; case 'F' : init_wpt_rte( true ); // read in the waypt and route break; case 'N' : test_navigation( ); break; case 'S' : // set the entire route printf("Resets the entire route: This will erase the old route\n"); printf("Do you want to continue (y/n)? >\n"); if (YesNo() ) set_route( ); break; case 'R' : change_1_route( ); break; // change one route point case 'W' : // display the waypt list cprintf("Write to file as well ? \n"); val = YesNo(); waypt_display( val ); break; default : cprintf(" input unknown \n"); } // end switch +++++++++++++++++++++++++++++++++++++++++ } // end while ------------------------------------------------ return; } //*************************************************************************** void menu_nav(void) { //print out menu selections ************************************************* cprintf(" NAV MENU Q.uit Nav : U.pdate menu\n" " C.hange Waypt : D.isplay Route : N.avigate tests\n" " R.oute edit(1) : S.et Route : W.aypt Display\n" " F.ile Read .rte, .wpt\n" ); return; } //*************************************************************************** //*** end *********************************************************************