owlps/owlps-positioning/src/inputudpsocket.cc

#include "inputudpsocket.hh"
#include "posexcept.hh"
#include "stock.hh"
#include "configuration.hh"

#include <owlps.h>

#include <iostream>
#include <cstdio> // For perror()
#include <boost/tr1/unordered_map.hpp>

using namespace std ;
using std::tr1::unordered_map ;



/* *** Constructors *** */


InputUDPSocket::InputUDPSocket(const uint_fast16_t _port):
  listening_port(_port)
{
  if (! init_socket())
    throw error_opening_input_file("UDP socket") ;
}


InputUDPSocket::~InputUDPSocket()
{
  close_socket() ;
}



/* *** Operations *** */


/**
 * @return \em true if the socket were successfully opened.
 * @return \em false in case of error.
 */
bool InputUDPSocket::init_socket()
{
  sockfd = owl_create_udp_listening_socket(listening_port) ;
  return sockfd >= 0 ;
}


/**
 * Normally, the socket is closed automatically by the destructor. Use
 * this if you want to close the socket prematurely.
 * #sockfd is set to -1, even in case of error.
 * @return \em true if the socket were successfully closed or were not
 * opened.
 * @return \em false in case of error.
 */
bool InputUDPSocket::close_socket()
{
  if (sockfd >= 0)
    {
      if (close(sockfd))
        {
          perror("Cannot close UDP socket") ;
          return false ;
        }

      sockfd = -1 ;
    }

  return true ;
}


/**
 * This function reads the next Request in the UDP input socket.
 * It cannot read calibration requests.
 *
 * #sockfd should be opened before processing requests; otherwise,
 * #current_request is \link Request::clear() cleared\endlink and a
 * blank Request is returned.
 *
 * @return The read Request, or a blank Request in case of error (socket
 * not opened, bad packet format).
 */
const Request& InputUDPSocket::get_next_request()
{
  clear_current_request() ;

  if (eof())
    return *current_request ;

  struct sockaddr_in client; // UDP client structure
  socklen_t client_len = sizeof(client) ; // Size of clients

  // Read the main request data
  owl_request request ;
  ssize_t nread = recvfrom(sockfd, &request, sizeof(request), 0,
                           (struct sockaddr *) &client, &client_len) ;
  if (nread <= 0)
    {
      cerr << "No request received!" << endl ;
      return *current_request ;
    }

  // Endianess conversions
  request.request_time = owl_ntoh_timestamp(request.request_time) ;
  request.nb_info = ntohs(request.nb_info) ;

  // Mobile MAC
  string mac_mobile(
    owl_mac_bytes_to_string(request.mobile_mac_addr_bytes)) ;
  PosUtil::to_upper(mac_mobile) ;
  const Mobile &mobile = Stock::find_create_mobile(mac_mobile) ;
  current_request->set_mobile(&mobile) ;

  // Request type
  current_request->set_type(request.type) ;

  // Timestamp
  current_request->set_time_sent(Timestamp(request.request_time)) ;

  // Position
  Point3D position(owl_ntohf(request.x_position),
                   owl_ntohf(request.y_position),
                   owl_ntohf(request.z_position)) ;

  // Read {MAC_AP;SS} mcouples (request_info)
  unordered_map<string, Measurement> measurements ;
  owl_request_info request_info ;
  for (int i = 0 ; i < request.nb_info ; ++i)
    {
      nread = recvfrom(sockfd, &request_info, sizeof(request_info), 0,
                       (struct sockaddr *) &client, &client_len) ;
      if (nread <= 0)
        {
          cerr << "No request info received!" << endl ;
          current_request->clear() ;
          return *current_request ;
        }
      string mac_ap(
        owl_mac_bytes_to_string(request_info.ap_mac_addr_bytes)) ;
      PosUtil::to_upper(mac_ap) ;

      if (! Configuration::bool_value("positioning.accept-new-aps") &&
          ! Stock::ap_exists(mac_ap))
        continue ;

      const AccessPoint &ap = Stock::find_create_ap(mac_ap) ;
      measurements[mac_ap].set_ap(&ap) ;
      measurements[mac_ap].add_ss(static_cast<int_fast8_t>
                                  (request_info.antenna_signal_dbm)) ;
    }

  current_request->set_measurements(measurements) ;

  // Calibration request?
  if ((request.type == OWL_REQUEST_CALIBRATION ||
       request.type == OWL_REQUEST_AUTOCALIBRATION)
      &&
      (Configuration::bool_value("positioning.accept-new-aps") ||
       Stock::ap_exists(mac_mobile)))
    {
      AccessPoint &transmitter =
        const_cast<AccessPoint&>(Stock::find_create_ap(mac_mobile)) ;

      // If an autocalibration request does not contain the coordinates
      // of the AP, we use the current coordinates of the AP as
      // ReferencePoint.
      if (request.type == OWL_REQUEST_AUTOCALIBRATION && ! position)
        position = transmitter.get_coordinates() ;

      // Update the AP's coordinates if allowed. A 'false' position
      // (i.e. 0;0;0) is only set for calibration requests, to avoid
      // setting the coordinates to 0;0;0 if an autocalibration
      // request that does not contain the coordinates is received.
      else if (Configuration::
               bool_value("positioning.update-ap-coordinates-online"))
        transmitter.set_coordinates(position) ;

      const ReferencePoint &reference_point =
        Stock::find_create_reference_point(position) ;
      current_request_to_calibration_request(&reference_point,
                                             Direction(request.direction),
                                             request.type) ;
    }

  // We set the real coordinates (if found) only for non-calibration
  // requests
  else if (position)
    current_request->set_real_position(position) ;

  return *current_request ;
}