Added a function to set the offsets of the GPS to the current position in both...

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

@@ -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(); 

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;