Commit 5c39a47d authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

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.
parent 7b2e512d
......@@ -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();
......
......@@ -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;
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment