owlps/owlps-positioner/autocalibrationline.cc

/*
 * This file is part of the Owl Positioning System (OwlPS) project.
 * It is subject to the copyright notice and license terms in the
 * COPYRIGHT.t2t file found in the top-level directory of this
 * distribution and at
 *   https://code.lm7.fr/mcy/owlps/src/master/COPYRIGHT.t2t
 * No part of the OwlPS Project, including this file, may be copied,
 * modified, propagated, or distributed except according to the terms
 * contained in the COPYRIGHT.t2t file; the COPYRIGHT.t2t file must be
 * distributed along with this file, either separately or by replacing
 * this notice by the COPYRIGHT.t2t file's contents.
 */


#include "autocalibrationline.hh"

#include "point3d.hh"
#include "capturepoint.hh"
#include "stock.hh"
#include "configuration.hh"
#include "posexcept.hh"

#include <iostream>
#include <sstream>

using namespace std ;



/* *** Constructors *** */


/**
 * @param _point The coordinates of the reference point to generate.
 * @param _cp1 The first capture point (receiver) to use the measurements
 * of to generate the reference points; it cannot be null.
 * @param _cp2 The second receiver capture point (optional).
 */
AutocalibrationLine::AutocalibrationLine(
  const Point3D &_point,
  const CapturePoint *const _cp1, const CapturePoint *const _cp2):
  Autocalibration(_point), cp1(_cp1), cp2(_cp2),
  trx_cp(nullptr), trx_rp(nullptr)
{
  assert(cp1) ;
}



/* *** Operations *** */


void AutocalibrationLine::generate_ss()
{
  /* With AutocalibrationLine, we want to generate measurements only
   * for the CP(s) we want to use. */
  assert(&rx->second) ;
  if (&rx->second != cp1 && &rx->second != cp2)
    return ;

  /* Choose the reference CP: try the other extremity first */
  if (&rx->second == cp2) // rx is cp2
    trx_cp = cp1 ;
  else if (cp2) // rx is cp1 and cp2 is defined
    trx_cp = cp2 ;
  else // rx is cp1 and cp2 is not defined
    /* Choose another reference CP */
    sort_reference_cps() ;

  /* Get the ReferencePoint corresponding to trx_cp */
  try
    {
      trx_rp = &Stock::get_reference_point(trx_cp->get_coordinates()) ;
    }
  catch (element_not_found &e)
    {
      ostringstream oss ;
      oss << "No measurements from transmitter CP "
          << trx_cp->get_mac_addr()
          << ": cannot generate RP " << point
          << " (receiver CP: " << rx->second.get_mac_addr() << ")." ;
      throw autocalibration_error(oss.str()) ;
    }
  assert(trx_rp) ;

  compute_ss() ;
}


/**
 * The reference (transmitter) CPs are first filtered (floor, coverage),
 * then sorted according to the signal strength RX receives from them.
 */
void AutocalibrationLine::sort_reference_cps()
{
  // Transmitter capture points, sorted by coverage with rx
  std::map<float, const CapturePoint*> sorted_ref_cps ;

  const Point3D &rx_coord = rx->second.get_coordinates() ;

  for (auto ref = Stock::cps.begin() ; ref != Stock::cps.end() ; ++ref)
    {
      if (ref == rx)
        continue ;

      /* Skip the CP if it is not at the same floor than the
       * receiver CP */
      const Point3D &ref_coord = ref->second.get_coordinates() ;
      if (ref_coord.get_z() != rx_coord.get_z())
        continue ;

      /* Skip the CP if it is not in coverage with the receiver CP */
      float coverage =
        rx->second.received_calibration_from_cp(ref->second) ;
      if (coverage < 1) // Less than 1% coverage is ridiculous!
        continue ;

      /* Store the CP according to the SS rx receives from it */
      const ReferencePoint &ref_rp =
        Stock::get_reference_point(ref->second.get_coordinates()) ;
      const vector<CalibrationRequest*> &ref_cr = ref_rp.get_requests() ;
      /* Search for the first measurement made by RX in ref_rp */
      const Measurement *rx_measurement = nullptr ;
      const string &rx_mac = rx->second.get_mac_addr() ;
      for (auto cr = ref_cr.begin() ; cr != ref_cr.end() ; ++cr)
        {
          rx_measurement = (*cr)->get_measurement(rx_mac) ;
          if (rx_measurement)
            break ;
        }
      // We skip CPs in bad coverage, so we should have a measurement
      assert(rx_measurement) ;
      float avg_ss = rx_measurement->get_average_dbm() ;
      sorted_ref_cps[avg_ss] = &ref->second ;
    }

  if (sorted_ref_cps.empty())
    {
      ostringstream oss ;
      oss << "No CPs in coverage with receiver CP "
          << rx->second.get_mac_addr() << " to generate RP "
          << point << "." ;
      throw autocalibration_error(oss.str()) ;
    }

  // Select the last CP from sorted_ref_cps (the CP with the highest SS)
  trx_cp = sorted_ref_cps.rbegin()->second ;
}


void AutocalibrationLine::compute_multi_packet_ss()
{
  const vector<CalibrationRequest*> &ref_cr = trx_rp->get_requests() ;

  /* Search for the first measurement made by RX in trx_rp */
  const Measurement *rx_measurement = nullptr ;
  vector<CalibrationRequest*>::const_iterator cr ;
  const string &rx_mac = rx->second.get_mac_addr() ;
  for (cr = ref_cr.begin() ; cr != ref_cr.end() ; ++cr)
    {
      rx_measurement = (*cr)->get_measurement(rx_mac) ;
      if (rx_measurement)
        break ;
    }

  /* The selected reference CP is supposed to be in coverage of RX,
   * but it is possible that it is not, in case we use cp1 and cp2
   * and no calibration request was transmitted in both directions
   * yet. */
  if (! rx_measurement)
    {
      ostringstream oss ;
      oss << "Transmitter CP " << trx_cp->get_mac_addr()
          << " is not in coverage with receiver CP "
          << rx->second.get_mac_addr() << " to generate RP "
          << point << "." ;
      throw autocalibration_error(oss.str()) ;
    }

  /* Generate the SS for each packet */
  bool packet_generated = false ;
  for (pkt_id_t pkt_id = 1 ; pkt_id <= (*cr)->get_nb_packets() ;
       ++pkt_id)
    if (compute_single_packet_ss(pkt_id))
      packet_generated = true ;

  if (! packet_generated)
    {
      if (Configuration::is_configured("verbose"))
        cerr << "No packet has been generated for RX CP " << rx_mac
             << "... failing back to single packet request. Calibration"
             << " request involved : " << **cr << "\n" ;
      compute_single_packet_ss(0) ;
    }
}


/**
 * @param pkt_id The number of the packet to create. Must be 0 if all
 * the generated reference points contain only one packet.
 */
bool AutocalibrationLine::compute_single_packet_ss(pkt_id_t pkt_id)
{
  /* Distance RX-P */
  const Point3D &rx_coord = rx->second.get_coordinates() ;
  float point_dst = rx_coord.distance(point) ;

  /* Compute the SS */
  double rx_ss = compute_single_ss(pkt_id, point_dst) ;
  if (rx_ss > 127) // error
    return false ;

  /* Add the SS to the measurements' list */
  const string &rx_mac = rx->second.get_mac_addr() ;
  // 0 is not a valid packet ID, so we use 1 if we generate only one
  // packet per reference point
  if (pkt_id == 0)
    pkt_id = 1 ;
  // The Measurement measurements[rx_mac] is created if needed
  measurements[rx_mac].add_ss(pkt_id, rx_ss) ;
  // Useful only if the measurement was just created, but it is not
  // worth a test
  measurements[rx_mac].set_cp(&rx->second) ;

  return true ;
}


double AutocalibrationLine::
compute_single_ss(const pkt_id_t pkt_id, const float point_dst)
{
  /* Friis index */
  const string &rx_mac = rx->second.get_mac_addr() ;
  float friis_idx ;
  if (pkt_id == 0)
    friis_idx = trx_rp->friis_index_for_cp(rx_mac) ;
  else
    {
      friis_idx = trx_rp->friis_index_for_cp(rx_mac, pkt_id) ;
      if (friis_idx == 0)
        return 128 ; // The packet pkt_id does not exist in trx_rp
    }

  /* SS */
  // Constant term
  double common_ss = rx->second.friis_constant_term() ;
  double ss =
    common_ss +
    trx_cp->get_trx_power() +
    trx_cp->get_antenna_gain() -
    10 * friis_idx * log10(point_dst) ;

  return ss ;
}