mirrors
/
gnss-sdr
mirror of https://github.com/gnss-sdr/gnss-sdr
1
0
Fork 0
Code Issues Releases Wiki Activity

Adding Local frame ENU velocity and COG to PVT monitor serialization

This commit is contained in:
Javier Arribas 2023-09-28 10:01:03 +02:00
parent 6cb3ae99b3
commit a39144105c
4 changed files with 36 additions and 2 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
docs/protobuf/monitor_pvt.proto
View File

@ -43,4 +43,10 @@ double vdop = 28; // Vertical Dilution of Precision
double user_clk_drift_ppm = 29; // User clock drift [ppm]
string utc_time = 30; // PVT UTC time (rfc 3339 datetime string)
double vel_e = 31; // Velocity East component in Local frame, in m/s
double vel_n = 32; // Velocity Nord component in Local frame, in m/s
double vel_u = 33; // Velocity Up component in Local frame, in m/s
double cog = 34; // Course Over Ground (cog) [deg]
}

13
src/algorithms/PVT/libs/monitor_pvt.h
View File

@ -64,6 +64,13 @@ public:
double longitude;
// GEO user position Height [m]
double height;
// East, Nord, Up (ENU) Velocity [m/s]
double vel_e;
double vel_n;
double vel_u;
// Course Over Ground (COG) [deg]
double cog;
// NUMBER OF VALID SATS
uint8_t valid_sats;
@ -137,6 +144,12 @@ public:
ar& BOOST_SERIALIZATION_NVP(user_clk_drift_ppm);
ar& BOOST_SERIALIZATION_NVP(utc_time);
ar& BOOST_SERIALIZATION_NVP(vel_e);
ar& BOOST_SERIALIZATION_NVP(vel_n);
ar& BOOST_SERIALIZATION_NVP(vel_u);
ar& BOOST_SERIALIZATION_NVP(cog);
}
};

10
src/algorithms/PVT/libs/rtklib_solver.cc
View File

@ -1598,7 +1598,7 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
ecef2pos(pvt_sol.rr, pos.data());
ecef2enu(pos.data(), &pvt_sol.rr[3], enuv.data());
this->set_speed_over_ground(norm_rtk(enuv.data(), 2));
double new_cog;
double new_cog = -9999.0; // COG not estimated due to insuficient velocity
if (ground_speed_ms >= 1.0)
{
new_cog = atan2(enuv[0], enuv[1]) * R2D;
@ -1680,6 +1680,14 @@ bool Rtklib_Solver::get_PVT(const std::map<int, Gnss_Synchro> &gnss_observables_
this->set_rx_vel({enuv[0], enuv[1], enuv[2]});
// ENU vel [m/s]
d_monitor_pvt.vel_e = enuv[0];
d_monitor_pvt.vel_n = enuv[1];
d_monitor_pvt.vel_u = enuv[2];
// Course Over Ground (cog) [deg]
d_monitor_pvt.cog = new_cog;
const double clock_drift_ppm = pvt_sol.dtr[5] / SPEED_OF_LIGHT_M_S * 1e6;
this->set_clock_drift_ppm(clock_drift_ppm);

9
src/algorithms/PVT/libs/serdes_monitor_pvt.h
View File

@ -113,7 +113,10 @@ public:
monitor_.set_vdop(monitor->vdop);
monitor_.set_user_clk_drift_ppm(monitor->user_clk_drift_ppm);
monitor_.set_utc_time(monitor->utc_time);
monitor_.set_vel_e(monitor->vel_e);
monitor_.set_vel_n(monitor->vel_n);
monitor_.set_vel_u(monitor->vel_u);
monitor_.set_cog(monitor->cog);
monitor_.SerializeToString(&data);
return data;
}
@ -152,6 +155,10 @@ public:
monitor.vdop = mon.vdop();
monitor.user_clk_drift_ppm = mon.user_clk_drift_ppm();
monitor.utc_time = mon.utc_time();
monitor.vel_e = mon.vel_e();
monitor.vel_n = mon.vel_n();
monitor.vel_u = mon.vel_u();
monitor.cog = mon.cog();
return monitor;
}