-
Joan Vallvé Navarro authoredJoan Vallvé Navarro authored
observations.cpp 7.45 KiB
#include "gnss_utils/gnss_utils.h"
#include "gnss_utils/observations.h"
using namespace GNSSUtils;
Observations::Observations()
{
}
Observations::Observations(const Observations& obs)
: sat_2_idx_(obs.sat_2_idx_)
, idx_2_sat_(obs.idx_2_sat_)
, obs_(obs.obs_.size())
{
// copy all elements
for (auto i = 0; i<obs.obs_.size(); i++)
{
obs_[i].time = obs.obs_[i].time;
obs_[i].eventime = obs.obs_[i].eventime;
obs_[i].timevalid = obs.obs_[i].timevalid;
obs_[i].sat = obs.obs_[i].sat;
obs_[i].rcv = obs.obs_[i].rcv;
std::copy(obs.obs_[i].SNR, obs.obs_[i].SNR + NFREQ+NEXOBS, obs_[i].SNR);
std::copy(obs.obs_[i].LLI, obs.obs_[i].LLI + NFREQ+NEXOBS, obs_[i].LLI);
std::copy(obs.obs_[i].code, obs.obs_[i].code + NFREQ+NEXOBS, obs_[i].code);
std::copy(obs.obs_[i].qualL, obs.obs_[i].qualL + NFREQ+NEXOBS, obs_[i].qualL);
std::copy(obs.obs_[i].qualP, obs.obs_[i].qualP + NFREQ+NEXOBS, obs_[i].qualP);
obs_[i].freq = obs.obs_[i].freq;
std::copy(obs.obs_[i].L, obs.obs_[i].L + NFREQ+NEXOBS, obs_[i].L);
std::copy(obs.obs_[i].P, obs.obs_[i].P + NFREQ+NEXOBS, obs_[i].P);
std::copy(obs.obs_[i].D, obs.obs_[i].D + NFREQ+NEXOBS, obs_[i].D);
}
}
Observations::~Observations()
{
}
void Observations::clearObservations()
{
this->obs_.clear();
this->idx_2_sat_.clear();
this->sat_2_idx_.clear();
}
void Observations::addObservation(const obsd_t & obs)
{
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
// copy obsd_t object
obsd_t copy_obs;
copy_obs.time = obs.time;
copy_obs.eventime = obs.eventime;
copy_obs.timevalid = obs.timevalid;
copy_obs.sat = obs.sat;
copy_obs.rcv = obs.rcv;
std::copy(obs.SNR, obs.SNR + NFREQ+NEXOBS, copy_obs.SNR);
std::copy(obs.LLI, obs.LLI + NFREQ+NEXOBS, copy_obs.LLI);
std::copy(obs.code, obs.code + NFREQ+NEXOBS, copy_obs.code);
std::copy(obs.qualL, obs.qualL + NFREQ+NEXOBS, copy_obs.qualL);
std::copy(obs.qualP, obs.qualP + NFREQ+NEXOBS, copy_obs.qualP);
copy_obs.freq = obs.freq;
std::copy(obs.L, obs.L + NFREQ+NEXOBS, copy_obs.L);
std::copy(obs.P, obs.P + NFREQ+NEXOBS, copy_obs.P);
std::copy(obs.D, obs.D + NFREQ+NEXOBS, copy_obs.D);
assert(!hasSatellite(copy_obs.sat) && "adding an observation of a satellite already stored!");
this->obs_.push_back(copy_obs);
idx_2_sat_.push_back(copy_obs.sat);
sat_2_idx_[copy_obs.sat] = obs_.size()-1;
//std::cout << "added observation of sat " << (int)obs.sat << " stored in idx " << sat_2_idx_[obs.sat] << std::endl;
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
}
void Observations::loadFromRinex(const std::string& rnx_file, gtime_t t_start, gtime_t t_end, double dt, const char* opt)
{
obs_t obs;
obs.data = (obsd_t *) malloc(sizeof(obsd_t)*MAXSAT);
obs.n = 0;
obs.nmax = MAXSAT;
//const char* opt = "";
auto stat = readrnxt(rnx_file.c_str(), 1, t_start, t_end, dt, opt, &obs, NULL, NULL);
if (stat == 1)
sortobs(&obs);
else
{
std::cout << "Couldn't load provided observation file, reason: " << (stat == 0 ? "no data" : "error") << std::endl;
return;
}
for (int i=0; i < obs.n; i++)
{
// std::cout << "time: " << time_str(obs.data[i].time, 3) << " | sat: " << int(obs.data[i].sat) << " | rcv: " << int(obs.data[i].rcv) <<
// " | SNR: " << int(obs.data[i].SNR[0]) << " | LLI: " << int(obs.data[i].LLI[0]) << " | code: " << int(obs.data[i].code[0]) <<
// " | L: " << obs.data[i].L[0] << " | P: " << obs.data[i].P[0] << " | D: " << obs.data[i].D[0] << std::endl;
addObservation(obs.data[i]);
}
free(obs.data);
}
void Observations::removeObservationByIdx(const int& _idx)
{
//std::cout << "removing observation of idx " << _idx << std::endl;
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
assert(obs_.size() > _idx);
assert(idx_2_sat_.size() > _idx);
assert(sat_2_idx_.count(idx_2_sat_.at(_idx)) != 0);
// remove obs from sat_2_idx map
sat_2_idx_.erase(idx_2_sat_.at(_idx));
// decrease the idx of the observations after the removed one
for (auto&& sat_idx : sat_2_idx_)
if (sat_idx.second > _idx)
sat_idx.second--;
// remove obs from obs and idx_2_sat vectors
obs_.erase(obs_.begin() + _idx);
idx_2_sat_.erase(idx_2_sat_.begin() + _idx);
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
}
void Observations::removeObservationBySat(const int& _sat)
{
//std::cout << "removing observation of sat " << _sat << std::endl;
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
assert(sat_2_idx_.count(_sat) != 0);
assert(hasSatellite(_sat));
assert(obs_.size() > sat_2_idx_.at(_sat));
assert(idx_2_sat_.size() > sat_2_idx_.at(_sat));
int idx = sat_2_idx_.at(_sat);
// remove obs from sat_2_idx map
sat_2_idx_.erase(_sat);
// decrease the idx of the observations after the removed one
for (auto&& sat_idx : sat_2_idx_)
if (sat_idx.second > idx)
sat_idx.second--;
// remove obs from obs and idx_2_sat vectors
obs_.erase(obs_.begin() + idx);
idx_2_sat_.erase(idx_2_sat_.begin() + idx);
assert(sat_2_idx_.size() == idx_2_sat_.size());
assert(sat_2_idx_.size() == obs_.size());
}
void Observations::print(obsd_t & _obs)
{
std::string msg = "Observation of satellite #" + std::to_string(_obs.sat);
GNSSUtils::print(msg);
std::cout << "Time [s]: " << _obs.time.time << " + " << _obs.time.sec << "\n";
printArray("SNR: ", _obs.SNR, ARRAY_SIZE(_obs.SNR));
printArray("LLI: ", _obs.LLI, ARRAY_SIZE(_obs.LLI));
printArray("code: ", _obs.code, ARRAY_SIZE(_obs.code));
printArray("L: ", _obs.L, ARRAY_SIZE(_obs.L));
printArray("P: ", _obs.P, ARRAY_SIZE(_obs.P));
printArray("D: ", _obs.D, ARRAY_SIZE(_obs.D));
}
void Observations::printBySat(const int& _sat_number)
{
print(getObservationBySat(_sat_number));
}
void Observations::printByIdx(const int& _idx)
{
print(getObservationByIdx(_idx));
}
void Observations::print()
{
for (auto obs : obs_)
{
print(obs);
}
}
void Observations::findCommonObservations(const Observations& obs_1, const Observations& obs_2,
Observations& common_obs_1, Observations& common_obs_2)
{
// clear and reserve
common_obs_1.clearObservations();
common_obs_2.clearObservations();
//std::cout << "obs 1 sats: ";
//for (auto&& obs_1_ref : obs_1.getObservations())
// std::cout << (int)obs_1_ref.sat << " ";
//std::cout << std::endl;
//std::cout << "obs 2 sats: ";
//for (auto&& obs_2_ref : obs_2.getObservations())
// std::cout << (int)obs_2_ref.sat << " ";
//std::cout << std::endl;
// iterate 1
for (auto&& obs_1_ref : obs_1.getObservations())
if (obs_2.hasSatellite(obs_1_ref.sat))
{
common_obs_1.addObservation(obs_1_ref);
common_obs_2.addObservation(obs_2.getObservationBySat(obs_1_ref.sat));
}
//std::cout << "common sats: ";
//for (auto&& obs_2_ref : common_obs_1.getObservations())
// std::cout << (int)obs_2_ref.sat << " ";
//std::cout << std::endl;
}