//---OPTIONS---
#define THRESHOLD_DISTANCE 15 //Proximity to get to waypoint in FEET
/**
  *Checks if we are "close enough" to waypoint to approach next one
  *If we are at last waypoint, stop!
  *
**/
void check_threshold()
{
   //---Local Variables---
   float threshold=0;
   //---Initialize---
   threshold =(float)0.000164578834 * (float)THRESHOLD_DISTANCE; //threshold in nautical miles
   
   //---Check for Autonomous Mode---
   if(PARAMS.mode == 'A')
   {
      //perform check
      if(WAYPOINTS.distance <= threshold)
      {
            //go to next waypoint
            WAYPOINTS.current++;
      }
      //are we at the last waypoint?
      if( WAYPOINTS.point[WAYPOINTS.current].lon == (float)0 || WAYPOINTS.point[WAYPOINTS.current].lat == (float)0)
      {
         //we're at last waypoint, STOP!
         PARAMS.mode = 'K'; //kill mode
         //set motors to 0!
         //set motor 1 command;
         //for roboteq 
         strcpy(ROBO.M1,"A00");
         //set motor 1 command
         //for roboteq
         strcpy(ROBO.M2,"B00");
         //OK, update motors
         update_robo();
      }
   }
}