WIP

include/gnss_utils/gnss_utils.h

@@ -42,6 +42,14 @@ namespace GNSSUtils
                                     Navigation & _navigation,
                                     const prcopt_t & _prcopt);
   
+//  ComputePosOutput computePosOwn(const Observations & _observations,
+//                                    Navigation & _navigation,
+//                                    const prcopt_t & _prcopt);
+//
+//  int pntposOwn(const obsd_t *obs, int n, const nav_t *nav,
+//                const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
+//                char *msg);
+
   Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef);
 }
 

src/examples/CMakeLists.txt

@@ -5,4 +5,4 @@ INCLUDE_DIRECTORIES(../../include)
 ADD_EXECUTABLE(gnss_utils_test gnss_utils_test.cpp)
 
 # link necessary libraries
-TARGET_LINK_LIBRARIES(gnss_utils_test gnss_utils)
+TARGET_LINK_LIBRARIES(gnss_utils_test ${gnss_utils_LIBRARIES})

src/gnss_utils.cpp

@@ -53,6 +53,132 @@ namespace GNSSUtils
     return output;
   }
 
+//  ComputePosOutput computePosOwn(const GNSSUtils::Observations & _observations,
+//                                       GNSSUtils::Navigation & _navigation,
+//                                       const prcopt_t & _prcopt)
+//    {
+//
+//    // Remove duplicated satellites
+//    uniqnav(&(_navigation.getNavigation()));
+//
+//    // Define error msg
+//    char msg[128] = "";
+//
+//    GNSSUtils::ComputePosOutput output;
+//    sol_t sol;
+//    sol = {{0}};
+//
+//    output.pos_stat = pntposOwn(_observations.getObservations().data(), _observations.getObservations().size(),
+//                              &(_navigation.getNavigation()),
+//                              &_prcopt, &sol, NULL, NULL, msg);
+//
+//    if (output.pos_stat == 0)
+//    {
+//    std::cout << "Bad computing: "  << msg << "\n";
+//    }
+//
+//    output.time = sol.time.time;
+//    output.time = sol.time.sec;
+//    output.pos  = Eigen::Vector3d(sol.rr);
+//    // std::cout << "Compute pos:  " << output.pos.transpose() << "\n";
+//    output.vel  = Eigen::Vector3d(&sol.rr[3]);
+//    output.pos_covar << sol.qr[0], sol.qr[3], sol.qr[5],
+//                      sol.qr[3], sol.qr[1], sol.qr[4],
+//                      sol.qr[5], sol.qr[3], sol.qr[2];
+//
+//    // XXX: segmentation fault here.
+//    // if (sol.dtr != NULL)
+//    // {
+//    //   output.rcv_bias << sol.dtr[0], sol.dtr[1], sol.dtr[2], sol.dtr[3], sol.dtr[4], sol.dtr[5];
+//    // }
+//
+//    output.type  = sol.type;
+//    output.stat  = sol.stat;
+//    output.ns    = sol.ns;
+//    output.age   = sol.age;
+//    output.ratio = sol.ratio;
+//    output.lat_lon = ecefToLatLon(output.pos);
+//
+//    return output;
+//    }
+//
+//  /* single-point positioning COPIED FROM RTKLIB ------------------------------
+//  * compute receiver position, velocity, clock bias by single-point positioning
+//  * with pseudorange and doppler observables
+//  * args   : obsd_t *obs      I   observation data
+//  *          int    n         I   number of observation data
+//  *          nav_t  *nav      I   navigation data
+//  *          prcopt_t *opt    I   processing options
+//  *          sol_t  *sol      IO  solution
+//  *          double *azel     IO  azimuth/elevation angle (rad) (NULL: no output)
+//  *          ssat_t *ssat     IO  satellite status              (NULL: no output)
+//  *          char   *msg      O   error message for error exit
+//  * return : status(1:ok,0:error)
+//  * notes  : assuming sbas-gps, galileo-gps, qzss-gps, compass-gps time offset and
+//  *          receiver bias are negligible (only involving glonass-gps time offset
+//  *          and receiver bias)
+//  *-----------------------------------------------------------------------------*/
+//  int pntposOwn(const obsd_t *obs, int n, const nav_t *nav,
+//                const prcopt_t *opt, sol_t *sol, double *azel, ssat_t *ssat,
+//                char *msg)
+//  {
+//      prcopt_t opt_=*opt;
+//      double *rs,*dts,*var,*azel_,*resp;
+//      int i,stat,vsat[MAXOBS]={0},svh[MAXOBS];
+//
+//      sol->stat=SOLQ_NONE;
+//
+//      if (n<=0) {strcpy(msg,"no observation data"); return 0;}
+//
+//      trace(3,"pntpos  : tobs=%s n=%d\n",time_str(obs[0].time,3),n);
+//
+//      sol->time=obs[0].time; msg[0]='\0';
+//
+//      rs=mat(6,n); dts=mat(2,n); var=mat(1,n); azel_=zeros(2,n); resp=mat(1,n);
+//
+//      if (opt_.mode!=PMODE_SINGLE) { /* for precise positioning */
+//  #if 0
+//          opt_.sateph =EPHOPT_BRDC;
+//  #endif
+//          opt_.ionoopt=IONOOPT_BRDC;
+//          opt_.tropopt=TROPOPT_SAAS;
+//      }
+//      /* satellite positons, velocities and clocks */
+//      satposs(sol->time,obs,n,nav,opt_.sateph,rs,dts,var,svh);
+//
+//      /* estimate receiver position with pseudorange */
+//      stat=estpos(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
+//
+//      /* raim fde */
+//      if (!stat&&n>=6&&opt->posopt[4]) {
+//          stat=raim_fde(obs,n,rs,dts,var,svh,nav,&opt_,sol,azel_,vsat,resp,msg);
+//      }
+//      /* estimate receiver velocity with doppler */
+//      //if (stat) estvel(obs,n,rs,dts,nav,&opt_,sol,azel_,vsat);
+//
+//      if (azel) {
+//          for (i=0;i<n*2;i++) azel[i]=azel_[i];
+//      }
+//      if (ssat) {
+//          for (i=0;i<MAXSAT;i++) {
+//              ssat[i].vs=0;
+//              ssat[i].azel[0]=ssat[i].azel[1]=0.0;
+//              ssat[i].resp[0]=ssat[i].resc[0]=0.0;
+//              ssat[i].snr[0]=0;
+//          }
+//          for (i=0;i<n;i++) {
+//              ssat[obs[i].sat-1].azel[0]=azel_[  i*2];
+//              ssat[obs[i].sat-1].azel[1]=azel_[1+i*2];
+//              ssat[obs[i].sat-1].snr[0]=obs[i].SNR[0];
+//              if (!vsat[i]) continue;
+//              ssat[obs[i].sat-1].vs=1;
+//              ssat[obs[i].sat-1].resp[0]=resp[i];
+//          }
+//      }
+//      free(rs); free(dts); free(var); free(azel_); free(resp);
+//      return stat;
+//  }
+
   Eigen::Vector3d ecefToLatLon(const Eigen::Vector3d & _ecef)
   {
     double pos[3];