Commit 0ec3090f authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added exceptions.

Completed the functions to stream the GPS information.
Minor changes to the GPS data types.
parent f57f0bb0
# driver source files
SET(sources asterx1_gps.cpp stream_gps.cpp)
SET(sources asterx1_gps.cpp stream_gps.cpp asterx1exceptions.cpp)
# application header files
SET(headers asterx1_gps.h stream_gps.h)
SET(headers asterx1_gps.h stream_gps.h asterx1exceptions.h)
# locate the the necessary dependencies
FIND_PACKAGE(iriutils)
......
#include "asterx1_gps.h"
#include "asterx1exceptions.h"
#include "gps_types.h"
#include "ctime.h"
......@@ -124,6 +125,7 @@ void CasteRx1::openDevice()
delete this->serialPort;
this->serialPort=NULL;
}
throw CasteRx1Exception(_HERE_,"Impossible to open the communications device");
}
}
......@@ -510,7 +512,6 @@ void CasteRx1::process_sat_visibility(unsigned char *data,unsigned short int len
TGPSSatVisibility gps_sat_visibility;
unsigned short int index=0,i;
TGPSSatInfo gps_sat_info;
TSatInfo sat_info;
memcpy((unsigned char *)&gps_sat_visibility,data,sizeof(TGPSSatVisibility));
index+=sizeof(TGPSSatVisibility);
......@@ -518,14 +519,14 @@ void CasteRx1::process_sat_visibility(unsigned char *data,unsigned short int len
{
memcpy((unsigned char *)&gps_sat_info,&data[index],sizeof(TGPSSatInfo));
index+=gps_sat_visibility.length_sat;
sat_info.gps_timestamp.wnc=gps_sat_visibility.timestamp.wnc;
sat_info.gps_timestamp.tow=gps_sat_visibility.timestamp.tow;
sat_info.local_timestamp=this->local_time.getTimeInSeconds();
sat_info.sat_id=gps_sat_info.sat_id;
sat_info.azimuth=gps_sat_info.azimuth*0.01;
sat_info.elevation=gps_sat_info.elevation*0.01;
sat_info.rise_set=(rise_set_t)gps_sat_info.rise_set;
sat_info.sat_info=(sat_info_t)gps_sat_info.sat_info;
this->sat_info.gps_timestamp.wnc=gps_sat_visibility.timestamp.wnc;
this->sat_info.gps_timestamp.tow=gps_sat_visibility.timestamp.tow;
this->sat_info.local_timestamp=this->local_time.getTimeInSeconds();
this->sat_info.sat_id=gps_sat_info.sat_id;
this->sat_info.azimuth=gps_sat_info.azimuth*0.01;
this->sat_info.elevation=gps_sat_info.elevation*0.01;
this->sat_info.rise_set=(rise_set_t)gps_sat_info.rise_set;
this->sat_info.sat_info=(sat_info_t)gps_sat_info.sat_info;
}
}
......@@ -810,7 +811,6 @@ void CasteRx1::process_pvt_cartesian(unsigned char *data,unsigned short int leng
this->current_pvt_cartesian.time_system=(sys_time_t)gps_pvt_cartesian.time_system;
this->current_pvt_cartesian.datum=(datum_t)gps_pvt_cartesian.datum;
this->current_pvt_cartesian.num_sat=gps_pvt_cartesian.num_sat;
this->current_pvt_cartesian.wa_corr_info=gps_pvt_cartesian.wa_corr_info;
if(gps_pvt_cartesian.corr_info.orbit_clock_corr==1)
this->current_pvt_cartesian.orbit_sat_clock_corr=true;
else
......@@ -1023,10 +1023,10 @@ void CasteRx1::process_dop(unsigned char *data,unsigned short int length)
this->current_pvt_dop.gps_timestamp.tow=gps_pvt_dop.timestamp.tow;
this->current_pvt_dop.local_timestamp=this->local_time.getTimeInSeconds();
this->current_pvt_dop.num_sat=gps_pvt_dop.num_sat;
this->current_pvt_dop.p_dop=gps_pvt_dop.p_dop;
this->current_pvt_dop.t_dop=gps_pvt_dop.t_dop;
this->current_pvt_dop.t_dop=gps_pvt_dop.h_dop;
this->current_pvt_dop.v_dop=gps_pvt_dop.v_dop;
this->current_pvt_dop.p_dop=gps_pvt_dop.p_dop/100.0;
this->current_pvt_dop.t_dop=gps_pvt_dop.t_dop/100.0;
this->current_pvt_dop.t_dop=gps_pvt_dop.h_dop/100.0;
this->current_pvt_dop.v_dop=gps_pvt_dop.v_dop/100.0;
this->current_pvt_dop.hpl=gps_pvt_dop.hpl;
this->current_pvt_dop.vpl=gps_pvt_dop.vpl;
this->pvt_access.exit();
......@@ -1074,18 +1074,16 @@ void CasteRx1::sendCommand(const std::string &cmd)
if(reply.find("Invalid command!")!=std::string::npos)
{
/* handle invalid command */
std::cout << "Invalid Command" << std::endl;
throw CasteRx1Exception(_HERE_,"Invalid command");
}
else if(reply.find(cmd.c_str(),0,cmd.size()-1)==std::string::npos)
{
/* handle unexpected answer */
std::cout << "Unexpected answer" << std::endl;
throw CasteRx1Exception(_HERE_,"Unexpected answer");
}
else
std::cout << "Command ok" << std::endl;
}catch(CEventTimeoutException &e){
// no answer in the allowed time
std::cout << e.what() << std::endl;
throw CasteRx1Exception(_HERE_,"Device did not answer in time");
}
}
......
......@@ -186,6 +186,7 @@ class CasteRx1
/* header of the message currently being received */
TGPSBlockHeader current_header;
TReceiverStatus receiver_status;
TSatInfo sat_info;
/* Meas blocks attributes */
CMutex meas_access;
......
......@@ -15,13 +15,13 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#include "hokuyoexceptions.h"
#include "asterx1exceptions.h"
#include <string.h>
#include <stdio.h>
const std::string hokuyo_exception_msg="[CHokuyo class] - ";
const std::string asterx1_exception_msg="[CasteRx1 class] - ";
CHokuyoException::CHokuyoException(const std::string& where,const std::string& error_msg):CException(where,hokuyo_exception_msg)
CasteRx1Exception::CasteRx1Exception(const std::string& where,const std::string& error_msg):CException(where,asterx1_exception_msg)
{
this->error_msg+=error_msg;
}
......
......@@ -16,49 +16,23 @@
// You should have received a copy of the GNU Lesser General Public License
// along with this program. If not, see <http://www.gnu.org/licenses/>.
#ifndef _HOKUYO_EXCEPTIONS
#define _HOKUYO_EXCEPTIONS
#ifndef _ASTERX1_EXCEPTIONS
#define _ASTERX1_EXCEPTIONS
#include "exceptions.h"
/**
* \brief Mutual exclusion exception class
*
* This class implements the exceptions for the CMutex class. It does not add
* any feature to the base clase CException, but it is implemented to easyly
* distinguish between different types of exceptions.
*
* In addition to the general exception message added by the base class, this
* class adds the "[CHokuyo class] - " string to the user message to identify
* the class that generated the exception.
* \brief
*
*/
class CHokuyoException : public CException
class CasteRx1Exception : public CException
{
public:
/**
* \brief Constructor
*
* The constructor calls the base class constructor to add the general
* exception identifier and then adds the class identifier string
* "[CHokuyo class]" and the supplied error message. The total exception
* message will look like this:
*
* \verbatim
* [Exception caught] - <where>
* Error: [CHokuyo class] - <error message>
* \endverbatim
*
* \param where a null terminated string with the information about the name
* of the function, the source code filename and the line where
* the exception was generated. This string must be generated
* by the _HERE_ macro.
*
* \param error_msg a null terminated string that contains the error message.
* This string may have any valid character and there is no
* limit on its length.
*/
CHokuyoException(const std::string& where,const std::string& error_msg);
CasteRx1Exception(const std::string& where,const std::string& error_msg);
};
#endif
......
......@@ -63,29 +63,41 @@ int main(int argc, char **argv)
else if(index==1)
{
myGps.get_gps_nav_data(gps_nav);
std::cout << gps_nav << std::endl;
// std::cout << gps_nav << std::endl;
}
else if(index==2)
{
myGps.get_gps_alm_data(gps_alm);
std::cout << gps_alm << std::endl;
// std::cout << gps_alm << std::endl;
}
else if(index==3)
{
myGps.get_gps_ion_data(gps_ion);
std::cout << gps_ion << std::endl;
// std::cout << gps_ion << std::endl;
}
else if(index==4)
{
myGps.get_gps_utc_data(gps_utc);
std::cout << gps_utc << std::endl;
// std::cout << gps_utc << std::endl;
}
else if(index==5)
{
std::cout << "GPS PVT" << std::endl;
myGps.get_pvt_data(pvt_cartesian,pvt_geodetic);
myGps.get_pvt_cov_data(pvt_pos_cov_cartesian,pvt_pos_cov_geodetic,pvt_vel_cov_cartesian,pvt_vel_cov_geodetic);
std::cout << "Cartesian:" << std::endl;
std::cout << pvt_cartesian << std::endl;
std::cout << "Position covariance:" << std::endl;
std::cout << pvt_pos_cov_cartesian << std::endl;
std::cout << "Velocity covariance:" << std::endl;
std::cout << pvt_vel_cov_cartesian << std::endl;
std::cout << "Geodetic:" << std::endl;
std::cout << pvt_geodetic << std::endl;
std::cout << "Position covariance:" << std::endl;
std::cout << pvt_pos_cov_geodetic << std::endl;
std::cout << "Velocity covariance:" << std::endl;
std::cout << pvt_vel_cov_geodetic << std::endl;
myGps.get_pvt_dop_data(pvt_dop);
std::cout << pvt_dop << std::endl;
}
}
myGps.stopAcquisition();
......
......@@ -295,7 +295,6 @@ typedef struct{
sys_time_t time_system;
datum_t datum;
unsigned int num_sat;
unsigned int wa_corr_info;
bool orbit_sat_clock_corr;
bool range_corr_info;
bool ion_info;
......@@ -365,10 +364,10 @@ typedef struct
TBlockTimeStamp gps_timestamp;
double local_timestamp;
unsigned int num_sat;
unsigned int p_dop;
unsigned int t_dop;
unsigned int h_dop;
unsigned int v_dop;
double p_dop;
double t_dop;
double h_dop;
double v_dop;
double hpl;
double vpl;
}TPVTDOP;
......
#include "stream_gps.h"
#include <iomanip>
std::ostream& operator<< (std::ostream& out, TMeasEpoch &meas_epoch)
{
......@@ -169,6 +170,207 @@ std::ostream& operator<< (std::ostream& out, TGPSUtc &gps_utc)
return out;
}
std::ostream& operator<< (std::ostream& out, TPVTCartesian &pvt_cartesian)
{
out << "PVT mode: ";
switch(pvt_cartesian.mode)
{
case no_pvt: out << "No PVT avaialbe" << std::endl;
break;
case stand_alone_pvt: out << "stand alone" << std::endl;
break;
case diff_pvt: out << "Differential" << std::endl;
break;
case fixed_loc: out << "Fixed location" << std::endl;
break;
case rtk_fixed_amb: out << "RTK with fixed ambiguities" << std::endl;
break;
case rtk_float_amb: out << "RTK with float a,biguities" << std::endl;
break;
case sbas_pvt: out << "SBAS aided" << std::endl;
break;
default: out << std::endl;
break;
}
if(pvt_cartesian.solution==pvt_3d)
out << "3D PVT Solution" << std::endl;
else
out << "2D PVT Solution" << std::endl;
out << "Error: ";
switch(pvt_cartesian.error)
{
case no_error: out << "none" << std::endl;
break;
case not_enough_meas: out << "Not enough measurements" << std::endl;
break;
case not_enough_eph: out << "Not enough ephemerides available" << std::endl;
break;
case dop_too_large: out << "DOP too large" << std::endl;
break;
case res_too_large: out << "Sum of squared residuals too large" << std::endl;
break;
case no_conv: out << "No convergence" << std::endl;
break;
case not_enough_meas_outliers: out << "Not enough measurements after outlier rejection" << std::endl;
break;
case export_laws: out << "Position output prohibited due to export laws" << std::endl;
break;
case not_enough_diff_corr: out << "Not enough differential correlation available" << std::endl;
break;
case no_base_coord: out << "Base station coordinates unavailable" << std::endl;
break;
default: out << std::endl;
break;
}
out << "Position: " << std::endl;
out << " x: " << pvt_cartesian.x << " m" << std::endl;
out << " y: " << pvt_cartesian.y << " m" << std::endl;
out << " z: " << pvt_cartesian.z << " m" << std::endl;
out << "Velocity: " << std::endl;
out << " x: " << pvt_cartesian.vx << " m/s" << std::endl;
out << " y: " << pvt_cartesian.vy << " m/s" << std::endl;
out << " z: " << pvt_cartesian.vz << " m/s" << std::endl;
out << "Undulation: " << pvt_cartesian.undulation << " m" << std::endl;
out << "Course over ground: " << pvt_cartesian.cog << " degrees" << std::endl;
out << "Receiver clock bias: " << pvt_cartesian.rx_clk_bias << " ms" << std::endl;
out << "Receiver clock drift: " << pvt_cartesian.rx_clk_drift << " ppm" << std::endl;
if(pvt_cartesian.time_system==gps_time)
out << "Using GPS time" << std::endl;
else
out << "Using Galileo time" << std::endl;
out << "Datum: ";
switch(pvt_cartesian.datum)
{
case wgs_84: out << "WGS84" << std::endl;
break;
case galileo: out << "Galileo" << std::endl;
break;
case pz_90_02: out << "PZ-90.02" << std::endl;
break;
case user_datum1: out << "User datum 1" << std::endl;
break;
case user_datum2: out << "User datum 2" << std::endl;
break;
default: out << std::endl;
break;
}
out << "Used " << pvt_cartesian.num_sat << " satellites" << std::endl;
return out;
}
std::ostream& operator<< (std::ostream& out, TPVTGeodetic &pvt_geodetic)
{
out << "PVT mode: ";
switch(pvt_geodetic.mode)
{
case no_pvt: out << "No PVT avaialbe" << std::endl;
break;
case stand_alone_pvt: out << "stand alone" << std::endl;
break;
case diff_pvt: out << "Differential" << std::endl;
break;
case fixed_loc: out << "Fixed location" << std::endl;
break;
case rtk_fixed_amb: out << "RTK with fixed ambiguities" << std::endl;
break;
case rtk_float_amb: out << "RTK with float a,biguities" << std::endl;
break;
case sbas_pvt: out << "SBAS aided" << std::endl;
break;
default: out << std::endl;
break;
}
if(pvt_geodetic.solution==pvt_3d)
out << "3D PVT Solution" << std::endl;
else
out << "2D PVT Solution" << std::endl;
out << "Error: ";
switch(pvt_geodetic.error)
{
case no_error: out << "none" << std::endl;
break;
case not_enough_meas: out << "Not enough measurements" << std::endl;
break;
case not_enough_eph: out << "Not enough ephemerides available" << std::endl;
break;
case dop_too_large: out << "DOP too large" << std::endl;
break;
case res_too_large: out << "Sum of squared residuals too large" << std::endl;
break;
case no_conv: out << "No convergence" << std::endl;
break;
case not_enough_meas_outliers: out << "Not enough measurements after outlier rejection" << std::endl;
break;
case export_laws: out << "Position output prohibited due to export laws" << std::endl;
break;
case not_enough_diff_corr: out << "Not enough differential correlation available" << std::endl;
break;
case no_base_coord: out << "Base station coordinates unavailable" << std::endl;
break;
default: out << std::endl;
break;
}
out << "Position:" << std::endl;
out << " Latitude: " << pvt_geodetic.latitude << " rad" << std::endl;
out << " Longitude: " << pvt_geodetic.longitude << " rad" << std::endl;
out << " Height: " << pvt_geodetic.h << " m" << std::endl;
out << "Velocity: " << std::endl;
out << " North: " << pvt_geodetic.vn << " m/s" << std::endl;
out << " East: " << pvt_geodetic.ve << " m/s" << std::endl;
out << " Up: " << pvt_geodetic.vu << " m/s" << std::endl;
out << "Undulation: " << pvt_geodetic.undulation << " m" << std::endl;
out << "Course over ground: " << pvt_geodetic.cog << " degrees" << std::endl;
out << "Receiver clock bias: " << pvt_geodetic.rx_clk_bias << " ms" << std::endl;
out << "Receiver clock drift: " << pvt_geodetic.rx_clk_drift << " ppm" << std::endl;
if(pvt_geodetic.time_system==gps_time)
out << "Using GPS time" << std::endl;
else
out << "Using Galileo time" << std::endl;
out << "Datum: ";
switch(pvt_geodetic.datum)
{
case wgs_84: out << "WGS84" << std::endl;
break;
case galileo: out << "Galileo" << std::endl;
break;
case pz_90_02: out << "PZ-90.02" << std::endl;
break;
case user_datum1: out << "User datum 1" << std::endl;
break;
case user_datum2: out << "User datum 2" << std::endl;
break;
default: out << std::endl;
break;
}
out << "Used " << pvt_geodetic.num_sat << " satellites" << std::endl;
return out;
}
std::ostream& operator<< (std::ostream& out, TPVTCov &pvt_cov)
{
out << "Covariance:" << std::endl;
out << std::fixed << std::setw(11) << std::setprecision(6) << "/ " << pvt_cov.cov_00 << " " << pvt_cov.cov_01 << " " << pvt_cov.cov_02 << " " << pvt_cov.cov_03 << " \\" << std::endl;
out << std::fixed << std::setw(11) << std::setprecision(6) << "| " << pvt_cov.cov_01 << " " << pvt_cov.cov_11 << " " << pvt_cov.cov_12 << " " << pvt_cov.cov_13 << " |" << std::endl;
out << std::fixed << std::setw(11) << std::setprecision(6) << "| " << pvt_cov.cov_02 << " " << pvt_cov.cov_12 << " " << pvt_cov.cov_22 << " " << pvt_cov.cov_23 << " |" << std::endl;
out << std::fixed << std::setw(11) << std::setprecision(6) << "\\ " << pvt_cov.cov_03 << " " << pvt_cov.cov_13 << " " << pvt_cov.cov_23 << " " << pvt_cov.cov_33 << " /" << std::endl;
return out;
}
std::ostream& operator<< (std::ostream& out, TPVTDOP &pvt_dop)
{
out << "Used " << pvt_dop.num_sat << " satellites" << std::endl;
out << "Position dilution of precision: " << pvt_dop.p_dop << std::endl;
out << "time dilution of precision: " << pvt_dop.t_dop << std::endl;
out << "Horitzontal dilution of precision: " << pvt_dop.h_dop << std::endl;
out << "Vertical dilution of precision: " << pvt_dop.v_dop << std::endl;
out << "Horitzontal protection level: " << pvt_dop.hpl << std::endl;
out << "Vertical protection level: " << pvt_dop.vpl << std::endl;
return out;
}
// ChannelStatus
/* std::cout << "gps timestamp: " << this->channel_status.gps_timestamp << std::endl;
std::cout << "num channles: " << this->channel_status.info.size() << std::endl;
......
......@@ -12,5 +12,9 @@ std::ostream& operator<< (std::ostream& out, TGPSNav &gps_nav);
std::ostream& operator<< (std::ostream& out, TGPSAlm &gps_alm);
std::ostream& operator<< (std::ostream& out, TGPSIon &gps_ion);
std::ostream& operator<< (std::ostream& out, TGPSUtc &gps_utc);
std::ostream& operator<< (std::ostream& out, TPVTCartesian &pvt_cartesian);
std::ostream& operator<< (std::ostream& out, TPVTGeodetic &pvt_geodetic);
std::ostream& operator<< (std::ostream& out, TPVTCov &pvt_cov);
std::ostream& operator<< (std::ostream& out, TPVTDOP &pvt_dop);
#endif
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