diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 69c932ef1650dddf0eaebb9bb128b6ee73e3fc23..279543f86a2a6ba9646bbdd06ad0be900f12af2e 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -1,8 +1,8 @@
# 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)
diff --git a/src/asterx1_gps.cpp b/src/asterx1_gps.cpp
index 5d437070e1aabee3256a5009feae09f563f7f270..e86f097b9e527a1c6760c021bb8cbec5287a91d0 100644
--- a/src/asterx1_gps.cpp
+++ b/src/asterx1_gps.cpp
@@ -1,4 +1,5 @@
#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");
}
}
diff --git a/src/asterx1_gps.h b/src/asterx1_gps.h
index 0dde8715073093870e827abb0380ebfd8317359e..1cbf719dd1480df0a0b4f1a98104cb54110b4d56 100644
--- a/src/asterx1_gps.h
+++ b/src/asterx1_gps.h
@@ -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;
diff --git a/src/asterx1exceptions.cpp b/src/asterx1exceptions.cpp
index 48601198744d9cc9711f1f338fbadb7080bde1c7..e0a65e4dea683427cdb46977d7b908b99f40f24e 100644
--- a/src/asterx1exceptions.cpp
+++ b/src/asterx1exceptions.cpp
@@ -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;
}
diff --git a/src/asterx1exceptions.h b/src/asterx1exceptions.h
index 0971abbae344f0932daa19ba294ed6f5646eeda5..bfbf1d523ee7d2608b863fae04d466d4c8790605 100644
--- a/src/asterx1exceptions.h
+++ b/src/asterx1exceptions.h
@@ -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
diff --git a/src/examples/testasterx1.cpp b/src/examples/testasterx1.cpp
index a64735ddd6b0a862ff32a31a909c0be80dff3765..a4d601c9eb845a57a34116726b6bfcaeadda2486 100644
--- a/src/examples/testasterx1.cpp
+++ b/src/examples/testasterx1.cpp
@@ -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();
diff --git a/src/gps_types.h b/src/gps_types.h
index cd5ec73fd315e3e2287e02574187a472ab025229..cb53834830f51bbf6b582af5f46d16cd5ca785d6 100644
--- a/src/gps_types.h
+++ b/src/gps_types.h
@@ -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;
diff --git a/src/stream_gps.cpp b/src/stream_gps.cpp
index d02a4b01bcc0ccce57fc3a7f75441d94099064f1..f9a7dd4a20bff0adc403348ba751261d487721ca 100644
--- a/src/stream_gps.cpp
+++ b/src/stream_gps.cpp
@@ -1,4 +1,5 @@
#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;
diff --git a/src/stream_gps.h b/src/stream_gps.h
index 597aacd827249f34b3920968e554490be63bf263..5298fe16319d22a4dac3d888ecff1fc9508009c0 100644
--- a/src/stream_gps.h
+++ b/src/stream_gps.h
@@ -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