Skip to content
GitLab
Commit b353344b authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

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

parent fb02d2d2
Hide whitespace changes
Inline Side-by-side
src/asterx1_gps.cpp
View file @ b353344b
......@@ -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
View file @ b353344b
......@@ -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
View file @ b353344b
......@@ -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
View file @ b353344b
......@@ -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
View file @ b353344b
......@@ -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;
}
......
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