From 5c39a47d4d980fbe8cce1452fb3c269d56ab85be Mon Sep 17 00:00:00 2001
From: =?UTF-8?q?Sergi=20Hern=C3=A0ndez=20Juan?= <shernand@iri.upc.edu>
Date: Thu, 15 Sep 2016 10:14:23 +0000
Subject: [PATCH] Added a function to set the offsets of the GPS to the current
position in both geodetic and cartesian coordinates. Added functions to get
these offsets.
---
src/asterx1_gps.cpp | 35 +++++++++++++++++++++++++++++++++++
src/asterx1_gps.h | 19 +++++++++++++++++++
2 files changed, 54 insertions(+)
diff --git a/src/asterx1_gps.cpp b/src/asterx1_gps.cpp
index 5ed0614..3eb689e 100644
--- a/src/asterx1_gps.cpp
+++ b/src/asterx1_gps.cpp
@@ -70,6 +70,9 @@ CasteRx1::CasteRx1(const std::string &name,const std::string &hwPortName)
this->thread_server->create_thread(this->gps_process_data_thread_id);
this->thread_server->attach_thread(this->gps_process_data_thread_id,this->gps_process_data_thread,this);
this->sat_channel.resize(256,-1);
+ // initialize offsets
+ this->geodetic_offsets.resize(3,0.0);
+ this->cartesian_offsets.resize(3,0.0);
}
CasteRx1::~CasteRx1()
@@ -205,6 +208,32 @@ gps_dyn_t CasteRx1::get_dynamics(void)
return this->current_dyn;
}
+void CasteRx1::set_zero(void)
+{
+ this->pvt_access.enter();
+ this->geodetic_offsets[0]=this->current_pvt_geodetic.latitude;
+ this->geodetic_offsets[1]=this->current_pvt_geodetic.longitude;
+ this->geodetic_offsets[2]=this->current_pvt_geodetic.h;
+ this->cartesian_offsets[0]=this->current_pvt_cartesian.x;
+ this->cartesian_offsets[1]=this->current_pvt_cartesian.y;
+ this->cartesian_offsets[2]=this->current_pvt_cartesian.z;
+ this->pvt_access.exit();
+}
+
+void CasteRx1::get_zero_geodetic(double &latitude,double &longitude,double &altitude)
+{
+ latitude=this->geodetic_offsets[0];
+ longitude=this->geodetic_offsets[1];
+ altitude=this->geodetic_offsets[2];
+}
+
+void CasteRx1::get_zero_cartesian(double &x,double &y,double &z)
+{
+ x=this->cartesian_offsets[0];
+ y=this->cartesian_offsets[1];
+ z=this->cartesian_offsets[2];
+}
+
std::string CasteRx1::get_new_meas_data_event_id(void)
{
return this->new_meas_block_event_id;
@@ -306,7 +335,13 @@ void CasteRx1::get_pvt_data(TPVTCartesian &pvt_cart,TPVTGeodetic &pvt_geo)
{
this->pvt_access.enter();
pvt_cart=this->current_pvt_cartesian;
+ pvt_cart.x-=this->cartesian_offsets[0];
+ pvt_cart.y-=this->cartesian_offsets[1];
+ pvt_cart.z-=this->cartesian_offsets[2];
pvt_geo=this->current_pvt_geodetic;
+ pvt_geo.latitude-=this->geodetic_offsets[0];
+ pvt_geo.longitude-=this->geodetic_offsets[1];
+ pvt_geo.h-=this->geodetic_offsets[2];
if(this->event_server->event_is_set(this->new_pvt_block_event_id))
this->event_server->reset_event(this->new_pvt_block_event_id);
this->pvt_access.exit();
diff --git a/src/asterx1_gps.h b/src/asterx1_gps.h
index 24e0760..bc2d017 100644
--- a/src/asterx1_gps.h
+++ b/src/asterx1_gps.h
@@ -104,6 +104,21 @@ class CasteRx1
*
*/
gps_dyn_t get_dynamics(void);
+ /**
+ * \brief
+ *
+ */
+ void set_zero(void);
+ /**
+ * \brief
+ *
+ */
+ void get_zero_geodetic(double &latitude,double &longitude,double &altitude);
+ /**
+ * \brief
+ *
+ */
+ void get_zero_cartesian(double &x,double &y,double &z);
/**
* \brief
*
@@ -202,6 +217,10 @@ class CasteRx1
gps_dyn_t current_dyn;
std::vector<unsigned short int> sat_channel;
+ // offset attributes
+ std::vector<double> geodetic_offsets;
+ std::vector<double> cartesian_offsets;
+
/* header of the message currently being received */
TGPSBlockHeader current_header;
TReceiverStatus receiver_status;
--
GitLab