diff --git a/include/gnss_utils/utils/satellite.h b/include/gnss_utils/utils/satellite.h index 4cfda619d786bef11a5640ea73a950a1408ac70b..0b15acdb2f68ef1fa6e316888d31e54a0d8c5a3c 100644 --- a/include/gnss_utils/utils/satellite.h +++ b/include/gnss_utils/utils/satellite.h @@ -35,6 +35,7 @@ namespace GnssUtils double clock_bias; double clock_drift; int svh; + int eph_mode; }; } // namespace GnssUtils diff --git a/src/utils/satellite.cpp b/src/utils/satellite.cpp index c2fc0de8eb5142938b460a04714ecd728a74036e..8abc4af4d1c7df8a6a403fbd0e3ffe628697db37 100644 --- a/src/utils/satellite.cpp +++ b/src/utils/satellite.cpp @@ -16,69 +16,121 @@ namespace GnssUtils { double computeSatElevation(const Eigen::Vector3d& receiver_ecef, const Eigen::Vector3d& sat_ecef) { - // ecef 2 geodetic - Eigen::Vector3d receiver_geo; - ecef2pos(receiver_ecef.data(), receiver_geo.data()); - - // receiver-sat vector ecef - Eigen::Vector3d receiver_sat_ecef = sat_ecef - receiver_ecef; - - // receiver-sat vector enu (receiver ecef as origin) - Eigen::Vector3d receiver_sat_enu; - ecef2enu(receiver_geo.data(), // geodetic position {lat,lon} (rad) - receiver_sat_ecef.data(), // vector in ecef coordinate {x,y,z} - receiver_sat_enu.data()); // vector in local tangental coordinate {e,n,u} - - // elevation - return atan2(receiver_sat_enu(2), - sqrt(receiver_sat_enu(0) * receiver_sat_enu(0) + receiver_sat_enu(1) * receiver_sat_enu(1))); + // ecef 2 geodetic + Eigen::Vector3d receiver_geo; + ecef2pos(receiver_ecef.data(), receiver_geo.data()); + + // receiver-sat vector ecef + Eigen::Vector3d receiver_sat_ecef = sat_ecef - receiver_ecef; + + // receiver-sat vector enu (receiver ecef as origin) + Eigen::Vector3d receiver_sat_enu; + ecef2enu(receiver_geo.data(), // geodetic position {lat,lon} (rad) + receiver_sat_ecef.data(), // vector in ecef coordinate {x,y,z} + receiver_sat_enu.data()); // vector in local tangental coordinate {e,n,u} + + // elevation + return atan2(receiver_sat_enu(2), + sqrt(receiver_sat_enu(0) * receiver_sat_enu(0) + receiver_sat_enu(1) * receiver_sat_enu(1))); } Satellites computeSatellites(const Observations& obs, const Navigation& nav, const int& eph_opt) { - double rs[6 * obs.size()], dts[2 * obs.size()], var[obs.size()]; - int svh[obs.size()]; - -// std::cout << "computing position of sats: "; -// for (auto&& obs_ref : obs.getObservations()) -// std::cout << (int)obs_ref.sat << " "; -// std::cout << std::endl; - - // compute positions - satposs( - obs.getObservations().front().time, obs.data(), obs.size(), &nav.getNavigation(), eph_opt, rs, dts, var, svh); - - // fill Satellites - Satellites sats; - //std::cout << "filling satellites: \n"; - for (int i = 0; i < obs.size(); i++) - { - auto sat_pair = sats.emplace(obs.getObservationByIdx(i).sat, // Key - Satellite({satsys(obs.getObservationByIdx(i).sat,NULL), - obs.getObservationByIdx(i).sat, // Constructor... - (Eigen::Vector3d() << rs[6*i], rs[6*i+1], rs[6*i+2]).finished(), - (Eigen::Vector3d() << rs[6*i+3], rs[6*i+4], rs[6*i+5]).finished(), - var[i], - dts[2*i], - dts[2*i+1], - (svh[i]==-2 ? 0 : svh[i])})); // -2 = ephemeris without SBAS correction (remove negative error code) - - assert(sat_pair.second && "satellite already computed"); - - //std::cout << "\tsat: " << sat_pair.first->second.sat << std::endl - // << "\t\tpos: " << sat_pair.first->second.pos.transpose() << std::endl - // << "\t\tvel: " << sat_pair.first->second.vel.transpose() << std::endl - // << "\t\tvar: " << sat_pair.first->second.var << std::endl - // << "\t\tclock bias: " << sat_pair.first->second.clock_bias << std::endl - // << "\t\tclock drift: " << sat_pair.first->second.clock_drift << std::endl - // << "\t\tsvh: " << sat_pair.first->second.svh << std::endl; - // if (sat_pair.first->second.pos_ == Eigen::Vector3d::Zero()) - // std::cout << "ephemeris not available for sat " << sat_pair.first->second.sat_ << std::endl; - } - - return sats; + double rs[6 * obs.size()], dts[2 * obs.size()], var[obs.size()]; + double rs_brdc[6 * obs.size()], dts_brdc[2 * obs.size()], var_brdc[obs.size()]; + int svh[obs.size()]; + int svh_brdc[obs.size()]; + + // std::cout << "computing position of sats: "; + // for (auto&& obs_ref : obs.getObservations()) + // std::cout << (int)obs_ref.sat << " "; + // std::cout << std::endl; + + // compute positions + // first compute all broadcasted ephemeris + satposs(obs.getObservations().front().time, + obs.data(), + obs.size(), + &nav.getNavigation(), + 0, + rs_brdc, + dts_brdc, + var_brdc, + svh_brdc); + /* DEBUG + std::cout << "Sats positions with broadcasted ephemeris: \n"; + for (int i = 0; i < obs.size(); i++) + { + if (rs_brdc[6*i] == 0 and rs_brdc[6*i+1] == 0 and rs_brdc[6*i+2] == 0) + std::cout << "\tsat: " << (int)obs.getObservationByIdx(i).sat << " ephemeris not available" << std::endl; + else + { + std::cout << "\tsat: " << (int)obs.getObservationByIdx(i).sat << std::endl + << "\t\tpos: " << rs_brdc[6*i] << " " << rs_brdc[6*i+1] << " " << rs_brdc[6*i+2] << std::endl + << "\t\tvel: " << rs_brdc[6*i+3] << " " << rs_brdc[6*i+4] << " " << rs_brdc[6*i+5] << std::endl + << "\t\tvar: " << var_brdc[i] << std::endl + << "\t\tclock bias: " << dts_brdc[2*i] << std::endl + << "\t\tclock drift: " << dts_brdc[2*i+1] << std::endl + << "\t\tsvh: " << svh_brdc[i] << std::endl; + } + } + //*/ + + // compute SBAS ephemeris + if (eph_opt != 0) + satposs(obs.getObservations().front().time, + obs.data(), + obs.size(), + &nav.getNavigation(), + eph_opt, + rs, + dts, + var, + svh); + + // fill Satellites + Satellites sats; + //std::cout << "computeSatellites: filling satellites: \n"; + for (int i = 0; i < obs.size(); i++) + { + bool brdc_eph = eph_opt == 0 or (rs[6*i] == 0 and rs[6*i+1] == 0 and rs[6*i+2] == 0); + + auto sat_pair = sats.emplace(obs.getObservationByIdx(i).sat, // Key + Satellite({satsys(obs.getObservationByIdx(i).sat,NULL), + obs.getObservationByIdx(i).sat, // Constructor... + brdc_eph ? + (Eigen::Vector3d() << rs_brdc[6*i],rs_brdc[6*i+1],rs_brdc[6*i+2]).finished() : + (Eigen::Vector3d() << rs[6*i],rs[6*i+1],rs[6*i+2]).finished(), + brdc_eph ? + (Eigen::Vector3d() << rs_brdc[6*i+3], rs_brdc[6*i+4], rs_brdc[6*i+5]).finished() : + (Eigen::Vector3d() << rs[6*i+3], rs[6*i+4], rs[6*i+5]).finished() , + brdc_eph ? var_brdc[i] : var[i], + brdc_eph ? dts_brdc[2*i] : dts[2*i], + brdc_eph ? dts_brdc[2*i+1] : dts[2*i+1], + brdc_eph ? svh_brdc[i] : svh[i], + brdc_eph ? 0 : 2})); + + assert(sat_pair.second && "satellite already computed"); + + /* DEBUG + if (sat_pair.first->second.pos == Eigen::Vector3d::Zero()) + std::cout << "\tsat: " << sat_pair.first->second.sat << " ephemeris not available" << std::endl; + else + { + std::cout << "\tsat: " << sat_pair.first->second.sat << std::endl + << "\t\tpos: " << sat_pair.first->second.pos.transpose() << std::endl + << "\t\tvel: " << sat_pair.first->second.vel.transpose() << std::endl + << "\t\tvar: " << sat_pair.first->second.var << std::endl + << "\t\tclock bias: " << sat_pair.first->second.clock_bias << std::endl + << "\t\tclock drift: " << sat_pair.first->second.clock_drift << std::endl + << "\t\tsvh: " << sat_pair.first->second.svh << std::endl + << "\t\teph: " << (sat_pair.first->second.eph_mode == 2 ? "SBAS" : "BRDC") << std::endl; + }//*/ + } + + return sats; } } // namespace GnssUtils