Changed gps position topic mostly to SI units and float, removed counters and added specifig timestamps

3 files changed, 70 insertions, 74 deletions

diff --git a/apps/drivers/gps/gps.cpp b/apps/drivers/gps/gps.cpp
index 28dc949d4..8c2775adb 100644
--- a/apps/drivers/gps/gps.cpp
+++ b/apps/drivers/gps/gps.cpp
@@ -543,9 +543,9 @@ GPS::print_info()
 			break;
 	}
 	warnx("port: %s, baudrate: %d, status: %s", _port, _baudrate, (_config_needed) ? "NOT OK" : "OK");
-	if (_report.timestamp != 0) {
+	if (_report.timestamp_position != 0) {
 		warnx("position lock: %dD, last update %4.2f seconds ago", (int)_report.fix_type,
-			(double)((float)(hrt_absolute_time() - _report.timestamp) / 1000000.0f));
+			(double)((float)(hrt_absolute_time() - _report.timestamp_position) / 1000000.0f));
 		warnx("lat: %d, lon: %d, alt: %d", _report.lat, _report.lon, _report.alt);
 		warnx("update rate: %6.2f Hz", (double)_rate);
 	}
diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp
index 440ec74c5..eec1df980 100644
--- a/apps/drivers/gps/ubx.cpp
+++ b/apps/drivers/gps/ubx.cpp
@@ -37,6 +37,7 @@
 
 #include <unistd.h>
 #include <stdio.h>
+#include <math.h>
 #include <string.h>
 #include <assert.h>
 #include <systemlib/err.h>
@@ -52,7 +53,7 @@ _config_state(UBX_CONFIG_STATE_PRT),
 _waiting_for_ack(false),
 _new_nav_posllh(false),
 _new_nav_timeutc(false),
-_new_nav_dop(false),
+//_new_nav_dop(false),
 _new_nav_sol(false),
 _new_nav_velned(false)
 {
@@ -211,10 +212,10 @@ UBX::configure(uint8_t *buffer, int &length, const unsigned max_length, bool &ba
 					cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
 					cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_TIMEUTC;
 					break;
-				case UBX_CONFIG_STATE_MSG_NAV_DOP:
-					cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
-					cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
-					break;
+//				case UBX_CONFIG_STATE_MSG_NAV_DOP:
+//					cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
+//					cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_DOP;
+//					break;
 				case UBX_CONFIG_STATE_MSG_NAV_SVINFO:
 					cfg_msg_packet.msgClass_payload = UBX_CLASS_NAV;
 					cfg_msg_packet.msgID_payload = UBX_MESSAGE_NAV_SVINFO;
@@ -316,10 +317,10 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
 						_message_id = NAV_TIMEUTC;
 						break;
 
-					case UBX_MESSAGE_NAV_DOP:
-						_decode_state = UBX_DECODE_GOT_MESSAGEID;
-						_message_id = NAV_DOP;
-						break;
+//					case UBX_MESSAGE_NAV_DOP:
+//						_decode_state = UBX_DECODE_GOT_MESSAGEID;
+//						_message_id = NAV_DOP;
+//						break;
 
 					case UBX_MESSAGE_NAV_SVINFO:
 						_decode_state = UBX_DECODE_GOT_MESSAGEID;
@@ -412,11 +413,11 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
 									gps_position->lon = packet->lon;
 									gps_position->alt = packet->height_msl;
 
-									gps_position->counter_pos_valid++;
-									gps_position->counter++;
+									gps_position->eph_m = (float)packet->hAcc * 1e-2f; // from mm to m
+									gps_position->epv_m = (float)packet->vAcc * 1e-2f; // from mm to m
 
 									/* Add timestamp to finish the report */
-									gps_position->timestamp = hrt_absolute_time();
+									gps_position->timestamp_position = hrt_absolute_time();
 
 									_new_nav_posllh = true;
 									/* set flag to trigger publishing of new position */
@@ -439,11 +440,10 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
 								if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
 
 									gps_position->fix_type = packet->gpsFix;
-									gps_position->s_variance = packet->sAcc;
-									gps_position->p_variance = packet->pAcc;
+									gps_position->s_variance_m_s = packet->sAcc;
+									gps_position->p_variance_m = packet->pAcc;
 
-									gps_position->counter++;
-									gps_position->timestamp = hrt_absolute_time();
+									gps_position->timestamp_variance = hrt_absolute_time();
 
 
 									_new_nav_sol = true;
@@ -457,29 +457,28 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
 								break;
 							}
 
-							case NAV_DOP: {
-//								printf("GOT NAV_DOP MESSAGE\n");
-								gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer;
-
-								//Check if checksum is valid and the store the gps information
-								if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
-
-									gps_position->eph =  packet->hDOP;
-									gps_position->epv =  packet->vDOP;
-
-									gps_position->counter++;
-									gps_position->timestamp = hrt_absolute_time();
-
-									_new_nav_dop = true;
-
-								} else {
-									warnx("NAV_DOP: checksum invalid");
-								}
-
-								// Reset state machine to decode next packet
-								decodeInit();
-								break;
-							}
+//							case NAV_DOP: {
+////								printf("GOT NAV_DOP MESSAGE\n");
+//								gps_bin_nav_dop_packet_t *packet = (gps_bin_nav_dop_packet_t *) _rx_buffer;
+//
+//								//Check if checksum is valid and the store the gps information
+//								if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
+//
+//									gps_position->eph_m =  packet->hDOP;
+//									gps_position->epv =  packet->vDOP;
+//
+//									gps_position->timestamp_posdilution = hrt_absolute_time();
+//
+//									_new_nav_dop = true;
+//
+//								} else {
+//									warnx("NAV_DOP: checksum invalid");
+//								}
+//
+//								// Reset state machine to decode next packet
+//								decodeInit();
+//								break;
+//							}
 
 							case NAV_TIMEUTC: {
 //								printf("GOT NAV_TIMEUTC MESSAGE\n");
@@ -501,8 +500,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
 									gps_position->time_gps_usec = (uint64_t)epoch * 1000000; //TODO: test this
 									gps_position->time_gps_usec += (uint64_t)(packet->time_nanoseconds * 1e-3f);
 
-									gps_position->counter++;
-									gps_position->timestamp = hrt_absolute_time();
+									gps_position->timestamp_time = hrt_absolute_time();
 
 									_new_nav_timeutc = true;
 
@@ -593,8 +591,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
 									}
 
 									gps_position->satellites_visible = satellites_used++; // visible ~= used but we are interested in the used ones
-									gps_position->counter++;
-									gps_position->timestamp = hrt_absolute_time();
+									gps_position->timestamp_satellites = hrt_absolute_time();
 
 									// as this message arrives only with 1Hz and is not essential, we don't take it into account for the report
 
@@ -614,18 +611,17 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
 								//Check if checksum is valid and the store the gps information
 								if (_rx_ck_a == packet->ck_a && _rx_ck_b == packet->ck_b) {
 
-									gps_position->vel = (uint16_t)packet->speed;
-									gps_position->vel_n = packet->velN / 100.0f;
-									gps_position->vel_e = packet->velE / 100.0f;
-									gps_position->vel_d = packet->velD / 100.0f;
+									gps_position->vel_m_s   = (float)packet->speed * 1e-2f;
+									gps_position->vel_n_m_s = (float)packet->velN * 1e-2f;
+									gps_position->vel_e_m_s = (float)packet->velE * 1e-2f;
+									gps_position->vel_d_m_s = (float)packet->velD * 1e-2f;
+									gps_position->cog_rad   = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f;
 									gps_position->vel_ned_valid = true;
-									gps_position->cog = (uint16_t)((float)(packet->heading) * 1e-3f);
-
-									gps_position->counter++;
-									gps_position->timestamp = hrt_absolute_time();
+									gps_position->timestamp_velocity = hrt_absolute_time();
 
 									_new_nav_velned = true;
 
+
 								} else {
 									warnx("NAV_VELNED: checksum invalid");
 								}
@@ -691,12 +687,12 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
 										break;
 									case UBX_CONFIG_STATE_MSG_NAV_TIMEUTC:
 										if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
-											_config_state = UBX_CONFIG_STATE_MSG_NAV_DOP;
-										break;
-									case UBX_CONFIG_STATE_MSG_NAV_DOP:
-										if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
 											_config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO;
 										break;
+//									case UBX_CONFIG_STATE_MSG_NAV_DOP:
+//										if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
+//											_config_state = UBX_CONFIG_STATE_MSG_NAV_SVINFO;
+//										break;
 									case UBX_CONFIG_STATE_MSG_NAV_SVINFO:
 										if (packet->clsID == UBX_CLASS_CFG && packet->msgID == UBX_MESSAGE_CFG_MSG)
 											_config_state = UBX_CONFIG_STATE_MSG_NAV_SOL;
@@ -766,7 +762,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
 	}
 
 	/* return 1 when position updates and the remaining packets updated at least once */
-	if(_new_nav_posllh &&_new_nav_timeutc && _new_nav_dop && _new_nav_sol && _new_nav_velned) {
+	if(_new_nav_posllh &&_new_nav_timeutc /*&& _new_nav_dop*/ && _new_nav_sol && _new_nav_velned) {
 
 		/* we have received everything, this most probably means that the configuration is fine */
 		config_needed = false;
@@ -774,7 +770,7 @@ UBX::parse(uint8_t b, struct vehicle_gps_position_s *gps_position, bool &config_
 		/* Reset the flags */
 		_new_nav_posllh = false;
 		_new_nav_timeutc = false;
-		_new_nav_dop = false;
+//		_new_nav_dop = false;
 		_new_nav_sol = false;
 		_new_nav_velned = false;
 
diff --git a/apps/drivers/gps/ubx.h b/apps/drivers/gps/ubx.h
index d3c6c6d4c..43cded02f 100644
--- a/apps/drivers/gps/ubx.h
+++ b/apps/drivers/gps/ubx.h
@@ -54,7 +54,7 @@
 #define UBX_MESSAGE_NAV_POSLLH 0x02
 #define UBX_MESSAGE_NAV_SOL 0x06
 #define UBX_MESSAGE_NAV_TIMEUTC 0x21
-#define UBX_MESSAGE_NAV_DOP 0x04
+//#define UBX_MESSAGE_NAV_DOP 0x04
 #define UBX_MESSAGE_NAV_SVINFO 0x30
 #define UBX_MESSAGE_NAV_VELNED 0x12
 //#define UBX_MESSAGE_RXM_SVSI 0x20
@@ -140,18 +140,18 @@ typedef struct {
 	uint8_t ck_b;
 } gps_bin_nav_timeutc_packet_t;
 
-typedef struct {
-	uint32_t time_milliseconds; 	/**<  GPS Millisecond Time of Week */
-	uint16_t gDOP; 					/**< Geometric DOP (scaling 0.01) */
-	uint16_t pDOP; 					/**< Position DOP (scaling 0.01) */
-	uint16_t tDOP; 					/**< Time DOP (scaling 0.01) */
-	uint16_t vDOP; 					/**< Vertical DOP (scaling 0.01) */
-	uint16_t hDOP; 					/**< Horizontal DOP (scaling 0.01) */
-	uint16_t nDOP; 					/**< Northing DOP (scaling 0.01) */
-	uint16_t eDOP; 					/**< Easting DOP (scaling 0.01) */
-	uint8_t ck_a;
-	uint8_t ck_b;
-} gps_bin_nav_dop_packet_t;
+//typedef struct {
+//	uint32_t time_milliseconds; 	/**<  GPS Millisecond Time of Week */
+//	uint16_t gDOP; 					/**< Geometric DOP (scaling 0.01) */
+//	uint16_t pDOP; 					/**< Position DOP (scaling 0.01) */
+//	uint16_t tDOP; 					/**< Time DOP (scaling 0.01) */
+//	uint16_t vDOP; 					/**< Vertical DOP (scaling 0.01) */
+//	uint16_t hDOP; 					/**< Horizontal DOP (scaling 0.01) */
+//	uint16_t nDOP; 					/**< Northing DOP (scaling 0.01) */
+//	uint16_t eDOP; 					/**< Easting DOP (scaling 0.01) */
+//	uint8_t ck_a;
+//	uint8_t ck_b;
+//} gps_bin_nav_dop_packet_t;
 
 typedef struct {
 	uint32_t time_milliseconds; 	/**<  GPS Millisecond Time of Week */
@@ -311,7 +311,7 @@ typedef enum {
 	NAV_POSLLH,
 	NAV_SOL,
 	NAV_TIMEUTC,
-	NAV_DOP,
+//	NAV_DOP,
 	NAV_SVINFO,
 	NAV_VELNED,
 //	RXM_SVSI,
@@ -371,7 +371,7 @@ private:
 	unsigned			_payload_size;
 	bool				_new_nav_posllh;
 	bool				_new_nav_timeutc;
-	bool				_new_nav_dop;
+//	bool				_new_nav_dop;
 	bool				_new_nav_sol;
 	bool				_new_nav_velned;
 };