Solved a bug when the some of the raw messages were missing.

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");
 }
 

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

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;
   }
 }
 

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

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;
   }