diff --git a/src/asterx1_gps.cpp b/src/asterx1_gps.cpp
index 605a785ed0afe2853790a4321ef2a7518658a0e1..d8cefd7bd4db5f264956673cfbc5280bfdf35692 100644
--- a/src/asterx1_gps.cpp
+++ b/src/asterx1_gps.cpp
@@ -41,7 +41,7 @@ static const unsigned short CRC_16CCIT_LookUp[256] = {
CasteRx1::CasteRx1(const std::string &name,const std::string &hwPortName)
{
- this->serialPort=NULL;
+ this->serial_port=NULL;
this->serial_device=hwPortName;
this->gps_running=false;
/* initialize events */
@@ -75,9 +75,9 @@ CasteRx1::CasteRx1(const std::string &name,const std::string &hwPortName)
CasteRx1::~CasteRx1()
{
// stop data acquiaition
- this->stopAcquisition();
+ this->stop_acquisition();
// close the gps module
- this->closeDevice();
+ this->close_device();
// delete threads and events
this->thread_server->detach_thread(gps_process_data_thread_id);
this->thread_server->delete_thread(gps_process_data_thread_id);
@@ -100,81 +100,103 @@ CasteRx1::~CasteRx1()
this->new_gps_raw_block_event_id="";
this->event_server->delete_event(this->new_pvt_block_event_id);
this->new_pvt_block_event_id="";
- if(this->serialPort!=NULL)
+ if(this->serial_port!=NULL)
{
- delete this->serialPort;
- this->serialPort=NULL;
+ delete this->serial_port;
+ this->serial_port=NULL;
}
}
-void CasteRx1::openDevice()
+void CasteRx1::open_device()
{
- TRS232_config serialConfig;
+ TRS232_config serial_config;
try
{
- this->serialPort = new CRS232(this->serial_device); //creates a new object to manage serial connection
- this->serialPort->open((void*)&this->serial_device);
- serialConfig.baud = 115200;
- serialConfig.num_bits = 8;
- serialConfig.parity = none;
- serialConfig.stop_bits = 1;
- this->serialPort->config(&serialConfig);
+ this->serial_port = new CRS232(this->serial_device); //creates a new object to manage serial connection
+ this->serial_port->open((void*)&this->serial_device);
+ serial_config.baud = 115200;
+ serial_config.num_bits = 8;
+ serial_config.parity = none;
+ serial_config.stop_bits = 1;
+ this->serial_port->config(&serial_config);
// start the GPS processing data thread
this->thread_server->start_thread(this->gps_process_data_thread_id);
// configure the data streams
- this->configureStreams();
+ this->configure_streams();
}catch(CCommException &e){
- if(this->serialPort!=NULL)
+ if(this->serial_port!=NULL)
{
- delete this->serialPort;
- this->serialPort=NULL;
+ delete this->serial_port;
+ this->serial_port=NULL;
}
throw CasteRx1Exception(_HERE_,"Impossible to open the communications device");
}
}
-void CasteRx1::closeDevice()
+void CasteRx1::close_device()
{
// stop the internal acquisition thread if necessary
- this->stopAcquisition();
+ this->stop_acquisition();
// close the serial port
if(this->thread_server->get_thread_state(this->gps_process_data_thread_id)==active)
{
this->event_server->set_event(this->finish_thread_event_id);
this->thread_server->end_thread(this->gps_process_data_thread_id);
}
- if(this->serialPort!=NULL)
- this->serialPort->close();
+ if(this->serial_port!=NULL)
+ this->serial_port->close();
}
-void CasteRx1::startAcquisition()
+void CasteRx1::start_acquisition()
{
if(this->thread_server->get_thread_state(this->gps_process_data_thread_id)==active)
{
if(!this->gps_running)
{
- this->sendCommand("setSBFOutput,Stream1,USB1,Group1,OnChange\n");
- this->sendCommand("setSBFOutput,Stream2,USB1,Group2,OnChange\n");
- this->sendCommand("setSBFOutput,Stream3,USB1,Group3,OnChange\n");
- this->sendCommand("setSBFOutput,Stream4,USB1,Group4,OnChange\n");
+ this->send_command("setSBFOutput,Stream1,USB1,Group1,OnChange\n");
+ this->send_command("setSBFOutput,Stream2,USB1,Group2,OnChange\n");
+ this->send_command("setSBFOutput,Stream3,USB1,Group3,OnChange\n");
+ this->send_command("setSBFOutput,Stream4,USB1,Group4,OnChange\n");
this->gps_running=true;
}
}
}
-void CasteRx1::stopAcquisition()
+void CasteRx1::stop_acquisition()
{
if(this->gps_running)
{
- this->sendCommand("setSBFOutput,Stream1,USB1,none\n");
- this->sendCommand("setSBFOutput,Stream2,USB1,none\n");
- this->sendCommand("setSBFOutput,Stream3,USB1,none\n");
- this->sendCommand("setSBFOutput,Stream4,USB1,none\n");
+ this->send_command("setSBFOutput,Stream1,USB1,none\n");
+ this->send_command("setSBFOutput,Stream2,USB1,none\n");
+ this->send_command("setSBFOutput,Stream3,USB1,none\n");
+ this->send_command("setSBFOutput,Stream4,USB1,none\n");
this->gps_running=false;
}
}
+void CasteRx1::set_dynamics(gps_dyn_t dynamics)
+{
+ //set the receiver dynamics
+ switch(dynamics)
+ {
+ case dyn_low: this->send_command("setReceiverDynamics,Low\n");
+ break;
+ case dyn_moderate: this->send_command("setReceiverDynamics,Moderate\n");
+ break;
+ case dyn_high: this->send_command("setReceiverDynamics,High\n");
+ break;
+ case dyn_max: this->send_command("setReceiverDynamics,Max\n");
+ break;
+ }
+ this->current_dyn=dynamics;
+}
+
+gps_dyn_t CasteRx1::get_dynamics(void)
+{
+ return this->current_dyn;
+}
+
std::string CasteRx1::get_new_meas_data_event_id(void)
{
return this->new_meas_block_event_id;
@@ -316,7 +338,7 @@ void *CasteRx1::gps_process_data_thread(void *params)
bool end=false;
events.push_back(gps->finish_thread_event_id);
- events.push_back(gps->serialPort->get_rx_event_id());
+ events.push_back(gps->serial_port->get_rx_event_id());
while(!end)
{
event_id=gps->event_server->wait_first(events);
@@ -324,11 +346,20 @@ void *CasteRx1::gps_process_data_thread(void *params)
end=true;
else if(event_id==1)// new data available
{
- num=gps->serialPort->get_num_data();
- if(data!=NULL)
- delete[] data;
- data=new unsigned char[num];
- gps->serialPort->read(data,num);
+ try{
+ gps->gps_access.enter();
+ num=gps->serial_port->get_num_data();
+ if(data!=NULL)
+ delete[] data;
+ data=new unsigned char[num];
+ gps->serial_port->read(data,num);
+ gps->gps_access.exit();
+ }catch(CException &e){
+ std::cout << e.what() << std::endl;
+ if(data!=NULL)
+ delete[] data;
+ gps->gps_access.exit();
+ }
for(i=0;i<num;i++)
{
switch(state)
@@ -1173,7 +1204,7 @@ unsigned short int CasteRx1::compute_crc(unsigned char *data,unsigned short int
return crc;
}
-void CasteRx1::sendCommand(const std::string &cmd)
+void CasteRx1::send_command(const std::string &cmd)
{
std::list<std::string> events;
std::string reply;
@@ -1183,7 +1214,8 @@ void CasteRx1::sendCommand(const std::string &cmd)
events.push_back(this->new_reply_event_id);
try{
- this->serialPort->write((unsigned char *)cmd.c_str(),cmd.size());
+ this->gps_access.enter();
+ this->serial_port->write((unsigned char *)cmd.c_str(),cmd.size());
this->event_server->wait_all(events,500);
// compare the answer with the command
reply=this->reply_queue.front();// get the reply
@@ -1191,40 +1223,45 @@ void CasteRx1::sendCommand(const std::string &cmd)
if(reply.find("Invalid command!")!=std::string::npos)
{
/* handle invalid command */
+ this->gps_access.exit();
throw CasteRx1Exception(_HERE_,"Invalid command");
}
else if(reply.find(cmd.c_str(),0,cmd.size()-1)==std::string::npos)
{
/* handle unexpected answer */
+ this->gps_access.exit();
throw CasteRx1Exception(_HERE_,"Unexpected answer");
}
+ this->gps_access.exit();
}catch(CEventTimeoutException &e){
// no answer in the allowed time
+ this->gps_access.exit();
throw CasteRx1Exception(_HERE_,"Device did not answer in time");
}
}
-void CasteRx1::configureStreams(void)
+void CasteRx1::configure_streams(void)
{
// stop any incomming data
- this->sendCommand("setSBFOutput,all,USB1,none\n");
+ this->send_command("setSBFOutput,all,USB1,none\n");
//enables septentrio binary format (SBF) as output at USB1
- this->sendCommand("setDataInOut,USB1,CMD,SBF\n");
+ this->send_command("setDataInOut,USB1,CMD,SBF\n");
//enables ony the tracking of GPS sattelites
- this->sendCommand("setSatelliteTracking,GPS\n");
+ this->send_command("setSatelliteTracking,GPS\n");
//set the receiver dynamics
- this->sendCommand("setReceiverDynamics,Moderate\n");
+ this->send_command("setReceiverDynamics,Moderate\n");
+ this->current_dyn=dyn_moderate;
// create group 1 blocks (PVTCartesian,PosCovCartesian,DOP,PVTGeodetic)
- this->sendCommand("setSBFGroups,Group1,PVTCartesian+PosCovCartesian+VelCovCartesian+DOP+PVTGeodetic+PosCovGeodetic+VelCovGeodetic+EndOfPVT\n");
+ this->send_command("setSBFGroups,Group1,PVTCartesian+PosCovCartesian+VelCovCartesian+DOP+PVTGeodetic+PosCovGeodetic+VelCovGeodetic+EndOfPVT\n");
// create group 2 blocks (GPSNav,GPSAlm,GPSIon,GPSUtc)
#ifdef HAVE_GPSTK
- this->sendCommand("setSBFGroups,Group2,GPSRawCA\n");
+ this->send_command("setSBFGroups,Group2,GPSRawCA\n");
#else
- this->sendCommand("setSBFGroups,Group2,GPSNav+GPSAlm+GPSIon+GPSUtc\n");
+ this->send_command("setSBFGroups,Group2,GPSNav+GPSAlm+GPSIon+GPSUtc\n");
#endif
// create group 3 blocks (MeasEpoch,MeasExtra,EndOfMeas)
- this->sendCommand("setSBFGroups,Group3,MeasEpoch+MeasExtra+EndOfMeas\n");
+ this->send_command("setSBFGroups,Group3,MeasEpoch+MeasExtra+EndOfMeas\n");
// create group 4 blocks (ChannelStatus,ReceiverStatus,SatVisivility)
- this->sendCommand("setSBFGroups,Group4,ChannelStatus+ReceiverStatus+SatVisibility\n");
+ this->send_command("setSBFGroups,Group4,ChannelStatus+ReceiverStatus+SatVisibility\n");
}
diff --git a/src/asterx1_gps.h b/src/asterx1_gps.h
index 6c12575bac7dd9a620612d2d37af1fb9524f3e40..24e0760ca9eb896f69861dba0902064c22c86a79 100644
--- a/src/asterx1_gps.h
+++ b/src/asterx1_gps.h
@@ -15,14 +15,13 @@
#include "threadserver.h"
#include "commexceptions.h"
#include "eventexceptions.h"
+#include "mutex.h"
#include "gps_types.h"
#include "ctime.h"
typedef enum {header1,header2,get_cmd_reply,get_block_header,get_sbf_data} gps_state_t;
-//physical constants
-const double EARTH_RADIUS = 6378137.0;
-const double EARTH_EXCENTRICITY = 0.0818191908426220;
+typedef enum {dyn_low=0,dyn_moderate=1,dyn_high=2,dyn_max=3} gps_dyn_t;
//Device protocol constants (SBF protocol)
const unsigned short int PVTCartesian_ID = 4006;
@@ -45,10 +44,6 @@ const unsigned short int ReceiverStatus_ID = 4014;
const unsigned short int ChannelStatus_ID = 4013;
const unsigned short int SatVisibility_ID = 4012;
-//acquisition constants
-const int DATA_TIMEOUT = 11000; //msec
-const int CONFIG_TIMEOUT = 500; //msec
-
//data sizes
const unsigned int BLOCK_HEADER_SIZE = 8;
@@ -81,24 +76,34 @@ class CasteRx1
* Open and configure serial device to 8N1 Byte transmission.
* Launches a thread for serial in/out comms
*/
- void openDevice();
+ void open_device();
/** \brief Closes serial comm
*
* Closes serial comm's , stops comm thread and deletes comm object.
* If success , resets status bit DEVICE_OPEN.
*/
- void closeDevice();
+ void close_device();
/** \brief Starts basic data acquisition
*
* Configures the device to perform some signal processing basics for mobile robotics.
* Starts a mode of operation of continuos data flow from the device at a rate of 2Hz.
*/
- void startAcquisition();
+ void start_acquisition();
/** \brief Stops acquisition
*
* Stops continuous acquisition mode
*/
- void stopAcquisition();
+ void stop_acquisition();
+ /**
+ * \brief
+ *
+ */
+ void set_dynamics(gps_dyn_t dynamics);
+ /**
+ * \brief
+ *
+ */
+ gps_dyn_t get_dynamics(void);
/**
* \brief
*
@@ -180,7 +185,8 @@ class CasteRx1
*/
void get_pvt_dop_data(TPVTDOP &pvt_dop);
private:
- CRS232 *serialPort; /**<Object for serial comms. This object launch an independent thread managing byte I/O through serial port (USB or RS232)*/
+ CRS232 *serial_port; /**<Object for serial comms. This object launch an independent thread managing byte I/O through serial port (USB or RS232)*/
+ CMutex gps_access;
std::string serial_device;
/* internal queue for command replies */
std::queue<std::string> reply_queue;
@@ -193,6 +199,7 @@ class CasteRx1
std::string gps_process_data_thread_id;
bool gps_running;
CTime local_time;
+ gps_dyn_t current_dyn;
std::vector<unsigned short int> sat_channel;
/* header of the message currently being received */
@@ -339,12 +346,12 @@ class CasteRx1
* \brief
*
*/
- void configureStreams(void);
+ void configure_streams(void);
/**
* \brief
*
*/
- void sendCommand(const std::string &cmnd);
+ void send_command(const std::string &cmnd);
/**
computes the crc-ccitt of buf with polynomic 0x1021
diff --git a/src/asterx1_process.cpp b/src/asterx1_process.cpp
index d13d36809f1bedab8466c816d4c45ab24f6f1b03..c86dbd6cc4f91a8146225bf7dcd1e078f5705dfe 100644
--- a/src/asterx1_process.cpp
+++ b/src/asterx1_process.cpp
@@ -11,7 +11,7 @@ CAsteRx1Process::CAsteRx1Process()
for(i=0;i<37;i++)
{
this->ephemeris_data[i].sync=false;
- this->ephemeris_data[i].num_frames=0;
+ this->ephemeris_data[i].missing_frames.resize(3,true);
}
}
@@ -60,12 +60,12 @@ void CAsteRx1Process::generate_gps_nav(gpstk::EngEphemeris &ephemeris,TGPSNav &g
gps_nav.wnt_oe=gps_epoch.GPSModWeek();
}catch(gpstk::InvalidParameter &e){
/* ignore the Error for including subframes 4 and 5 */
- std::cout << "generate GPS NAV" << e.what() << std::endl;
+ //std::cout << "generate GPS NAV" << e.what() << std::endl;
}catch(gpstk::InvalidRequest &e){
/* this should not happen */
- std::cout << "generate GPS NAV" << e.what() << std::endl;
+ //std::cout << "generate GPS NAV" << e.what() << std::endl;
}catch(...){
- std::cout << "generate GPS NAV Exception" << std::endl;
+ //std::cout << "generate GPS NAV Exception" << std::endl;
}
}
@@ -85,44 +85,55 @@ void CAsteRx1Process::add_subframe(TGPSFrame &frame)
TGPSNav gps_nav;
try{
- this->ephemeris_data[frame.sat_id].ephemeris.addSubframe(frame.frame_bits,frame.wn,frame.sat_id,frame.track_id);
- if(!this->ephemeris_data[frame.sat_id].sync)
+ if(this->get_subframe_id(frame)<4)
{
- if(this->ephemeris_data[frame.sat_id].ephemeris.isData(1))// the first frame has been received for the first time
+ this->ephemeris_data[frame.sat_id].ephemeris.addSubframe(frame.frame_bits,frame.wn,frame.sat_id,frame.track_id);
+ if(!this->ephemeris_data[frame.sat_id].sync)
{
- this->ephemeris_data[frame.sat_id].sync=true;
- this->ephemeris_data[frame.sat_id].num_frames=2;
+ if(this->get_subframe_id(frame)==1)// the first frame has been received for the first time
+ {
+ this->ephemeris_data[frame.sat_id].sync=true;
+ this->ephemeris_data[frame.sat_id].missing_frames[this->get_subframe_id(frame)-1]=false;
+ }
}
- }
- else
- {
- this->ephemeris_data[frame.sat_id].num_frames--;
- if(this->ephemeris_data[frame.sat_id].num_frames==0)
+ else
{
- this->ephemeris_data[frame.sat_id].num_frames=3;
- // process the data and build a GPSNav structure
- this->generate_gps_nav(this->ephemeris_data[frame.sat_id].ephemeris,gps_nav);
- gps_nav.local_timestamp=frame.local_timestamp;
- gps_frames.wn=this->ephemeris_data[frame.sat_id].ephemeris.weeknum;
- gps_frames.sat_id=this->ephemeris_data[frame.sat_id].ephemeris.PRNID;
- gps_frames.track_id=this->ephemeris_data[frame.sat_id].ephemeris.tracker;
- for(i=0;i<3;i++)
+ this->ephemeris_data[frame.sat_id].missing_frames[this->get_subframe_id(frame)-1]=false;
+ if(!this->ephemeris_data[frame.sat_id].missing_frames[0] && !this->ephemeris_data[frame.sat_id].missing_frames[1] &&
+ !this->ephemeris_data[frame.sat_id].missing_frames[2])
{
- for(j=0;j<10;j++)
- gps_frames.subframes[i][j]=this->ephemeris_data[frame.sat_id].ephemeris.subframeStore[i][j];
+ this->ephemeris_data[frame.sat_id].missing_frames[0]=true;
+ this->ephemeris_data[frame.sat_id].missing_frames[1]=true;
+ this->ephemeris_data[frame.sat_id].missing_frames[2]=true;
+ // process the data and build a GPSNav structure
+ this->generate_gps_nav(this->ephemeris_data[frame.sat_id].ephemeris,gps_nav);
+ gps_nav.local_timestamp=frame.local_timestamp;
+ gps_frames.wn=this->ephemeris_data[frame.sat_id].ephemeris.weeknum;
+ gps_frames.sat_id=this->ephemeris_data[frame.sat_id].ephemeris.PRNID;
+ gps_frames.track_id=this->ephemeris_data[frame.sat_id].ephemeris.tracker;
+ for(i=0;i<3;i++)
+ {
+ for(j=0;j<10;j++)
+ gps_frames.subframes[i][j]=this->ephemeris_data[frame.sat_id].ephemeris.subframeStore[i][j];
+ }
+ this->gps_nav_data.push(gps_nav);
+ this->gps_raw_data.push(gps_frames);
}
- this->gps_nav_data.push(gps_nav);
- this->gps_raw_data.push(gps_frames);
}
}
}catch(gpstk::InvalidParameter &e){
/* ignore the Error for including subframes 4 and 5 */
- std::cout << "add subframe" << e.what() << std::endl;
+ this->ephemeris_data[frame.sat_id].missing_frames[0]=true;
+ this->ephemeris_data[frame.sat_id].missing_frames[1]=true;
+ this->ephemeris_data[frame.sat_id].missing_frames[2]=true;
}catch(gpstk::InvalidRequest &e){
/* this should not happen */
- std::cout << "add subframe" << e.what() << std::endl;
+ this->ephemeris_data[frame.sat_id].sync=false;
+ this->ephemeris_data[frame.sat_id].missing_frames[0]=true;
+ this->ephemeris_data[frame.sat_id].missing_frames[1]=true;
+ this->ephemeris_data[frame.sat_id].missing_frames[2]=true;
}catch(...){
- std::cout << "add subframe Exception" << std::endl;
+ //std::cout << "add subframe Exception" << std::endl;
}
}
diff --git a/src/asterx1_process.h b/src/asterx1_process.h
index 6a3df10d8586926e9fc28aada1f3a473276ed94c..9afcb98951a8054eed67ff56baaca828e9d388a3 100644
--- a/src/asterx1_process.h
+++ b/src/asterx1_process.h
@@ -8,7 +8,7 @@
typedef struct{
gpstk::EngEphemeris ephemeris;
bool sync;
- int num_frames;
+ std::vector<bool> missing_frames;
}TEphemerisData;
class CAsteRx1Process
diff --git a/src/examples/testasterx1.cpp b/src/examples/testasterx1.cpp
index 4decdaa1baf7e3e7afd810ffd635b1e3d926c77d..55a41ce4a825ec8426da09573ad87b38b0f9a904 100644
--- a/src/examples/testasterx1.cpp
+++ b/src/examples/testasterx1.cpp
@@ -52,7 +52,7 @@ int main(int argc, char **argv)
std::cout << "\n\n TEST AsteRx1 GPS RECEIVER \n\n" << std::endl;
try{
- gps.openDevice();
+ gps.open_device();
#ifdef HAVE_GPSTK
events.push_back(gps.get_new_meas_data_event_id());
events.push_back(gps.get_new_gps_raw_data_event_id());
@@ -65,7 +65,7 @@ int main(int argc, char **argv)
events.push_back(gps.get_new_gps_utc_data_event_id());
events.push_back(gps.get_new_pvt_data_event_id());
#endif
- gps.startAcquisition();
+ gps.start_acquisition();
for(;;)
{
index=event_server->wait_first(events);
@@ -153,7 +153,7 @@ int main(int argc, char **argv)
}
#endif
}
- gps.stopAcquisition();
+ gps.stop_acquisition();
}catch(CException &e){
std::cout << e.what() << std::endl;
}