core/gps.cpp

//
// Copyright 2011-2013, Per Zetterberg, KTH Royal Institute of Technology
//
// This program is free software: you can redistribute it and/or modify
// it under the terms of the GNU General Public License as published by
// the Free Software Foundation, either version 3 of the License, or
// (at your option) any later version.
//
// This program is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
// GNU General Public License for more details.
//
// You should have received a copy of the GNU General Public License
// along with this program.  If not, see <http://www.gnu.org/licenses/>.
//


#include "gps.hpp"
#include <iostream>
#include <boost/asio/io_service.hpp>
#include <boost/asio/serial_port.hpp>



unsigned int
gps_wait_until(const char* device_name, const unsigned int gps_time_combined ) {
  uint32_t hour, minute, second, combined;


  char data[GPS_CHAR_BUFFER_SIZE];
  std::string stored_gps_str, gps_line_str, time_str;
  unsigned long first_position, second_position, data_field_position;



  boost::asio::io_service io;
  boost::asio::serial_port port( io ); 


  port.open( device_name );   // or whatever the device might be 


  port.set_option( boost::asio::serial_port_base::baud_rate( 4800 ) );
  port.set_option( boost::asio::serial_port_base::character_size( 8 ) );



  port.set_option(  boost::asio::serial_port_base::
                    parity( boost::asio::serial_port_base::parity::none ) );

  port.set_option( boost::asio::serial_port_base::
                   stop_bits( boost::asio::serial_port_base::stop_bits::two ) );
  

  port.set_option( boost::asio::serial_port_base::
                 flow_control( boost::asio::serial_port_base::
                               flow_control::none ) );


  bool first_time=true;
  while (1) {
    usleep(10000);
    long n=port.read_some(boost::asio::buffer(data, GPS_CHAR_BUFFER_SIZE));
    data[n]=0;
    if (first_time) {
      if (gps_time_combined<100000)
        std::cout << "=========== GPS Started! =============== \n";
      first_time=false;
    }

    //std::cout << data; 
    stored_gps_str+=data;
    //std::cout << stored_gps_str << std::endl;
    first_position=stored_gps_str.find('$');
    //std::cout << "first_position=" << first_position << std::endl;
    if (!(first_position==std::string::npos)) {
      second_position=stored_gps_str.find('$',first_position+1);
      //std::cout << "second_position=" << second_position << std::endl;
      if (!(second_position==std::string::npos)) {
        gps_line_str=stored_gps_str.substr(first_position,second_position-first_position-1);
        stored_gps_str=stored_gps_str.substr(second_position);
        //std::cout << "================ start" << std::endl;
        //std::cout << gps_line_str;
        //std::cout << std::endl;
        //std::cout << "================ stop" << std::endl;
        // Process gps_line_str
        data_field_position=gps_line_str.find("GPRMC",0);
        if (!(data_field_position==std::string::npos)) {
          time_str=gps_line_str.substr(7,6);
          //std::cout << "T=" << time_str << std::endl;
          
          hour=atoi( time_str.substr(0,2).c_str());
          minute=atoi( time_str.substr(2,2).c_str());
          second=atoi( time_str.substr(4,2).c_str());

          //std::cout << "hour=" << hour << " minute=" 
          //    << minute << " second=" << second << " \n";
          
          combined=hour*60*60+minute*60+second;

          //std::cout << "combined=" << combined << std::endl;
          if (combined>=gps_time_combined)
            break;



        };
      };
    };
  };

  //std::cout << std::endl;
  port.close();


  return gps_time_combined;

}


unsigned int
gps_fix_indicator(const char* device_name) {



  char data[GPS_CHAR_BUFFER_SIZE];
  std::string stored_gps_str, gps_line_str, temp_str;
  unsigned long first_position, second_position, data_field_position;
  uint32_t fix;


  boost::asio::io_service io;
  boost::asio::serial_port port( io ); 



  port.open( device_name );   // or whatever the device might be 


  port.set_option( boost::asio::serial_port_base::baud_rate( 4800 ) );
  port.set_option( boost::asio::serial_port_base::character_size( 8 ) );



  port.set_option(  boost::asio::serial_port_base::
                    parity( boost::asio::serial_port_base::parity::none ) );

  port.set_option( boost::asio::serial_port_base::
                   stop_bits( boost::asio::serial_port_base::stop_bits::two ) );
  

  port.set_option( boost::asio::serial_port_base::
                 flow_control( boost::asio::serial_port_base::
                               flow_control::none ) );



  while (1) {
    long n=port.read_some(boost::asio::buffer(data, GPS_CHAR_BUFFER_SIZE));
    data[n]=0;
    //std::cout << data ;
    stored_gps_str+=data;
    //std::cout << stored_gps_str << std::endl;
    first_position=stored_gps_str.find('$');
    //std::cout << "first_position=" << first_position << std::endl;
    //std::cout << "std::string::npos=" << std::string::npos << std::endl;
    if (!(first_position==std::string::npos)) {
      second_position=stored_gps_str.find('$',first_position+1);
      //std::cout << "second_position=" << second_position << std::endl;
      if (!(second_position==std::string::npos)) {
        //std::cout << "str=" << stored_gps_str << std::endl;
        gps_line_str=stored_gps_str.substr(first_position,second_position-first_position-1);
        stored_gps_str=stored_gps_str.substr(second_position);
        //std::cout << "================ start" << std::endl;
        //std::cout << gps_line_str;
        //std::cout << std::endl;
        //std::cout << "================ stop" << std::endl;
        // Process gps_line_str
        data_field_position=gps_line_str.find("GPGGA",0);
        if (!(data_field_position==std::string::npos)) {

          //std::cout << "T=" << gps_line_str << std::endl;
          temp_str=gps_line_str;
          for (int i1=0;i1<6;i1++) {
            temp_str=temp_str.substr(temp_str.find(',')+1);
          };

          fix=atoi( temp_str.substr(0,1).c_str());
          //std::cout << "R=" << temp_str.substr(0,1) << std::endl;

          
          break;



        };
      };
    };
  };

  //std::cout << std::endl;
  port.close();


  return fix;

}