add PDOP generation

coverage.cc

@@ -8,10 +8,43 @@
 #include "fmt/printf.h"
 #include <fstream>
 
+#include <eigen3/Eigen/Dense>
+using Eigen::MatrixXd;
+
+
 using namespace std;
 extern GetterSetter<map<int, GalileoMessage::Almanac>> g_galileoalmakeeper;
 extern GetterSetter<svstats_t> g_statskeeper;
 
+xDOP getDOP(Point& us, vector<Point> sats)
+{
+  xDOP ret;
+  if(sats.size() < 4) {
+    return ret;
+  }
+  MatrixXd G(sats.size(), 4); // 4 columns
+
+  // (x1 - x)/R1    (y1 -y)/R1    (z1 - z)/R1   -1
+
+  for(size_t n = 0 ; n < sats.size(); ++n) {
+    const auto& s = sats[n];
+    auto R = Vector(us, s).length();
+
+    G(n, 0) = (s.x - us.x)/R;
+    G(n, 1) = (s.y - us.y)/R;
+    G(n, 2) = (s.z - us.z)/R;
+    G(n, 3) = -1;
+  }
+
+  //  cout<<"Matrix: "<<endl;
+  //  cout<<G<<endl;
+  MatrixXd Q = (G.transpose() * G).inverse();
+
+  ret.pdop = sqrt(Q(0,0) + Q(1,1) + Q(2,2)); // already squared
+  ret.tdop = sqrt(Q(3,3));
+  ret.gdop = sqrt(ret.pdop*ret.pdop + ret.tdop*ret.tdop);
+  return ret;
+};
 
 covmap_t emitCoverage()
 {
@@ -29,7 +62,7 @@ covmap_t emitCoverage()
 
     if(g.first < 0)
       continue;
-    if(svstats[{2,g.first,1}].completeIOD() && svstats[{2,g.first,1}].liveIOD().sisa == 255) {
+    if(svstats[{2,(uint32_t)g.first,1}].completeIOD() && svstats[{2,(uint32_t)g.first,1}].liveIOD().sisa == 255) {
       //      cout<<g.first<<" NAPA!"<<endl;
       continue;
     }
@@ -40,7 +73,7 @@ covmap_t emitCoverage()
     double phi = M_PI* latitude / 180;
     double longsteps = 1 + 360.0 * cos(phi);
     double step = 180.0 / longsteps;
-    vector<tuple<double, int, int, int>> latvect;
+    vector<tuple<double, int, int, int, double, double, double>> latvect;
     for(double longitude = -180; longitude < 180; longitude += step) { // east - west
       Point p;
       // phi = latitude, lambda = longitude
@@ -51,28 +84,38 @@ covmap_t emitCoverage()
       p.z = R * sin(phi);
 
       if(longitude == -180) {
-        auto longlat = getLongLat(p.x, p.y, p.z);
+        //        auto longlat = getLongLat(p.x, p.y, p.z);
         //        cout<<fmt::sprintf("%3.0f ", 180.0*longlat.second/M_PI);
       }
         
       
       int numsats5=0, numsats10=0, numsats20=0;
+      vector<Point> satposs5, satposs10, satposs20;
       for(const auto& s : sats) {
         //        double getElevationDeg(const Point& sat, const Point& our);
         double elev = getElevationDeg(s.second, p);
-        if(elev > 5.0)
+        if(elev > 5.0) {
+          satposs5.push_back(s.second);
           numsats5++;
-        if(elev > 10.0)
+        }
+        if(elev > 10.0) {
+          satposs10.push_back(s.second);
           numsats10++;
-        if(elev > 20.0)
+        }
+        if(elev > 20.0) {
+          satposs20.push_back(s.second);
           numsats20++;
+        }
       }
-      if(numsats20 < 4)
-        latvect.push_back(make_tuple(longitude, numsats5, numsats10, numsats20));
+      latvect.push_back(make_tuple(longitude,
+                                   numsats5, numsats10, numsats20,
+                                   getDOP(p, satposs5).pdop,
+                                   getDOP(p, satposs10).pdop, 
+                                   getDOP(p, satposs20).pdop
+                                   ));
       //      cmap << longitude <<" " <<latitude <<" " << numsats5 << " " <<numsats10<<" "<<numsats20<<endl;
     }
-    if(!latvect.empty())
-      ret.push_back(make_pair(latitude, latvect));
+    ret.push_back(make_pair(latitude, latvect));
   }
   return ret;
 }

navparse.cc

@@ -993,6 +993,10 @@ try
           jsdatum.push_back(get<1>(longpair));
           jsdatum.push_back(get<2>(longpair));
           jsdatum.push_back(get<3>(longpair));
+          
+          jsdatum.push_back((int)(10*get<4>(longpair)));
+          jsdatum.push_back((int)(10*get<5>(longpair)));
+          jsdatum.push_back((int)(10*get<6>(longpair)));
           jslongvect.push_back(jsdatum);
         }
         jslatvect.push_back(latvect.first);

navparse.hh

@@ -143,7 +143,17 @@ struct SVStat
 typedef std::map<SatID, SVStat> svstats_t;
 
 // a vector of pairs of latidude,vector<longitude,numsats>
-typedef vector<pair<double,vector<tuple<double, int, int, int> > > > covmap_t;
+typedef vector<pair<double,vector<tuple<double, int, int, int, double, double, double> > > > covmap_t;
 
 covmap_t emitCoverage();
 
+struct xDOP
+{
+  double gdop{-1};
+  double pdop{-1};
+  double tdop{-1};
+  //  double hdop;
+  //  double vdop;
+};
+
+xDOP getDOP(Point& us, vector<Point> sats);