283 lines
8.7 KiB
C++
283 lines
8.7 KiB
C++
/*
|
|
* 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 "positioning.hh"
|
|
#include "realposition.hh"
|
|
#include "fbcm.hh"
|
|
#include "frbhmbasic.hh"
|
|
#include "interlinknetworks.hh"
|
|
#include "nss.hh"
|
|
#include "request.hh"
|
|
#include "result.hh"
|
|
#include "resultlist.hh"
|
|
#include "timestamp.hh"
|
|
#include "configuration.hh"
|
|
#include "posexcept.hh"
|
|
#include "stock.hh"
|
|
|
|
#include <owlps.h>
|
|
|
|
#include <iostream>
|
|
#include <unordered_set>
|
|
|
|
using namespace std ;
|
|
|
|
|
|
|
|
/* *** Constructors *** */
|
|
|
|
|
|
Positioning::Positioning()
|
|
{
|
|
initialise_algorithms() ;
|
|
loop() ;
|
|
}
|
|
|
|
|
|
Positioning::~Positioning()
|
|
{
|
|
for (auto i = algorithms.begin() ; i != algorithms.end() ; ++i)
|
|
delete *i ;
|
|
algorithms.clear() ;
|
|
}
|
|
|
|
|
|
|
|
/* *** Operations *** */
|
|
|
|
|
|
void Positioning::initialise_algorithms()
|
|
{
|
|
if (! Configuration::is_configured("positioning.algorithm"))
|
|
throw missing_configuration(
|
|
"No positioning algorithm specified in configuration!") ;
|
|
|
|
const vector<string> &algo_names =
|
|
Configuration::string_vector_value("positioning.algorithm") ;
|
|
|
|
// We will store the names of the stored algorithms in a set, to
|
|
// avoid multiple occurrences of the same algorithm:
|
|
unordered_set<string> stored_algos ;
|
|
|
|
for (auto i = algo_names.begin() ; i != algo_names.end() ; ++i)
|
|
{
|
|
if (*i == "Real")
|
|
{
|
|
auto stored = stored_algos.insert("Real") ;
|
|
if (stored.second) // Check if there was no previous instance
|
|
algorithms.insert(algorithms.begin(), new RealPosition) ;
|
|
/* Note: in order to compute the errors of the other
|
|
* algorithms, Real must be the first for each request,
|
|
* so we add it at the begining. */
|
|
}
|
|
|
|
else if (*i == "FBCM")
|
|
{
|
|
auto stored = stored_algos.insert("FBCM") ;
|
|
if (stored.second) // Check if there was no previous instance
|
|
{
|
|
/* Generate the Friis indexes only if the autocalibration
|
|
* is not activated (if it is, Friis indexes will be
|
|
* regenerated each time FBCM wants to computes a position)
|
|
*/
|
|
if (! Configuration::autocalibration_enabled())
|
|
Stock::update_all_friis_indexes() ;
|
|
algorithms.push_back(new FBCM) ;
|
|
}
|
|
}
|
|
|
|
else if (*i == "FRBHMBasic")
|
|
{
|
|
auto stored = stored_algos.insert("FRBHMBasic") ;
|
|
if (stored.second) // Check if there was no previous instance
|
|
algorithms.push_back(new FRBHMBasic) ;
|
|
// TODO: Pre-compute all per-ReferencePoint friis indexes?
|
|
}
|
|
|
|
else if (*i == "InterlinkNetworks")
|
|
{
|
|
auto stored = stored_algos.insert("InterlinkNetworks") ;
|
|
if (stored.second) // Check if there was no previous instance
|
|
algorithms.push_back(new InterlinkNetworks) ;
|
|
}
|
|
|
|
else if (*i == "NSS")
|
|
{
|
|
auto stored = stored_algos.insert("NSS") ;
|
|
if (stored.second) // Check if there was no previous instance
|
|
algorithms.push_back(new NSS) ;
|
|
}
|
|
|
|
else
|
|
throw bad_configuration(
|
|
"The specified positioning_algorithm \""+ *i +
|
|
"\" is unknown!") ;
|
|
}
|
|
}
|
|
|
|
|
|
void Positioning::loop()
|
|
{
|
|
vector<PositioningAlgorithm*>::const_iterator algo ;
|
|
|
|
while (! input.eof() && owl_run)
|
|
{
|
|
const Request &request = input.get_next_request() ;
|
|
if (! request)
|
|
continue ;
|
|
|
|
Point3D real_position ;
|
|
bool compute_error = false ;
|
|
ResultList results(&request) ;
|
|
for (algo = algorithms.begin() ; algo != algorithms.end() ; ++algo)
|
|
{
|
|
const string &algo_name = (*algo)->get_name() ;
|
|
|
|
Result res ;
|
|
try
|
|
{
|
|
res = (*algo)->compute(request) ;
|
|
}
|
|
catch (exception &e)
|
|
{
|
|
cerr
|
|
<< "Cannot compute with algorithm "
|
|
<< algo_name << ": \""
|
|
<< e.what() << "\"\n" ;
|
|
continue ;
|
|
}
|
|
|
|
// Apply filtering if needed
|
|
filter(request, res) ;
|
|
|
|
if (compute_error)
|
|
res.compute_error(real_position) ;
|
|
else if (algo_name == "Real")
|
|
{
|
|
compute_error = true ;
|
|
real_position = res.get_position() ;
|
|
}
|
|
|
|
results.add(res) ;
|
|
}
|
|
|
|
// If filtering is enabled, update the last known position(s) of
|
|
// the current mobile
|
|
bool do_filter =
|
|
Configuration::float_value("positioning.filter.max-speed") > 0 ;
|
|
if (do_filter)
|
|
{
|
|
const Mobile *const mobile = request.get_mobile() ;
|
|
const_cast<Mobile*>(mobile)->set_last_results(results) ;
|
|
}
|
|
|
|
output.write(results) ;
|
|
}
|
|
}
|
|
|
|
|
|
/**
|
|
* @param[in] request The request from which `result` has been computed.
|
|
* @param[in,out] result The unfiltered Result, that will be updated by
|
|
* this function if needed.
|
|
*/
|
|
void Positioning::filter(const Request &request, Result &result)
|
|
{
|
|
/* Don't apply filter if filtering is disabled */
|
|
float max_speed =
|
|
Configuration::float_value("positioning.filter.max-speed") ;
|
|
if (max_speed <= 0)
|
|
return ;
|
|
|
|
/* Don't apply filter if it is the first result we compute for this
|
|
* mobile (hence we don't have previous results to compare to) */
|
|
const Mobile *const mobile = request.get_mobile() ;
|
|
const ResultList &last_results = mobile->get_last_results() ;
|
|
if(last_results.empty())
|
|
return ;
|
|
|
|
/* Don't apply filter if the "algorithm" is the real position */
|
|
string algo_name = result.get_algorithm() ;
|
|
if (algo_name == "Real")
|
|
return ;
|
|
|
|
/* Print the common message in verbose mode */
|
|
const Timestamp ×tamp = result.get_request()->get_time_sent() ;
|
|
if (Configuration::is_configured("verbose"))
|
|
cerr
|
|
<< "Filtering (algorithm " << algo_name << "): "
|
|
<< "for request sent at " << timestamp
|
|
<< " by mobile " << mobile->get_mac_addr()
|
|
<< " (maximum speed is " << max_speed << " km/h), " ;
|
|
|
|
/* Use alternative maximum speed if the reset distance is configured
|
|
* and the mobile is within this distance */
|
|
// Get the unfiltered coordinates:
|
|
const Point3D &pos = result.get_position() ;
|
|
float reset_distance =
|
|
Configuration::float_value("positioning.filter.cp-reset-distance") ;
|
|
if (reset_distance > 0)
|
|
{
|
|
float dist_closest_cp = Stock::distance_from_closest_cp(pos) ;
|
|
assert(dist_closest_cp >= 0) ;
|
|
if (dist_closest_cp <= reset_distance)
|
|
{
|
|
if (Configuration::is_configured("verbose"))
|
|
cerr
|
|
<< "the mobile appears to be within the reset distance"
|
|
<< " of " << reset_distance
|
|
<< " m from a CP" ;
|
|
max_speed =
|
|
Configuration::float_value("positioning.filter.max-speed-cp") ;
|
|
if (max_speed <= 0)
|
|
{
|
|
if (Configuration::is_configured("verbose"))
|
|
cerr
|
|
<< " and the alternative speed is \"unlimited\";"
|
|
<< " no filtering.\n" ;
|
|
return ;
|
|
/* Note: saying that the mobile's speed is unlimited is
|
|
* another way to say that the filter is disabled. */
|
|
}
|
|
if (Configuration::is_configured("verbose"))
|
|
cerr
|
|
<< "; maximum speed set to alternative speed ("
|
|
<< max_speed << " km/h).\n" ;
|
|
}
|
|
}
|
|
|
|
/* Okay, now let's get to it! */
|
|
|
|
const Result &prev_result = last_results.get_result_for_algo(algo_name) ;
|
|
|
|
// Calculate the travel time (duration)
|
|
const Timestamp &prev_timestamp =
|
|
last_results.get_request()->get_time_sent() ;
|
|
Timestamp duration = prev_timestamp.elapsed(timestamp) ;
|
|
|
|
// Get the previous coordinates
|
|
const Point3D &prev_pos = prev_result.get_position() ;
|
|
|
|
// Recompute the result by interpolation
|
|
Point3D filtered_pos(prev_pos.interpolate(pos, max_speed, duration)) ;
|
|
if (filtered_pos != pos)
|
|
{
|
|
if (Configuration::is_configured("verbose"))
|
|
cerr << pos << " was replaced by " << filtered_pos << ".\n" ;
|
|
result.set_position(filtered_pos) ;
|
|
}
|
|
else if (Configuration::is_configured("verbose"))
|
|
cerr << "the original position looked good enough and was kept.\n" ;
|
|
}
|