HIL bugfixing

4 files changed, 43 insertions, 30 deletions

diff --git a/apps/drivers/gps/ubx.cpp b/apps/drivers/gps/ubx.cpp
index 0d4894b8d..c150f5271 100644
--- a/apps/drivers/gps/ubx.cpp
+++ b/apps/drivers/gps/ubx.cpp
@@ -33,7 +33,14 @@
  *
  ****************************************************************************/
 
-/* @file U-Blox protocol implementation */
+/**
+ * @file ubx.cpp
+ *
+ * U-Blox protocol implementation. Following u-blox 6/7 Receiver Description
+ * including Prototol Specification.
+ *
+ * @see http://www.u-blox.com/images/downloads/Product_Docs/u-blox6_ReceiverDescriptionProtocolSpec_%28GPS.G6-SW-10018%29.pdf
+ */
 
 #include <unistd.h>
 #include <stdio.h>
@@ -633,16 +640,17 @@ UBX::handle_message()
 		}
 
 		case NAV_VELNED: {
-//			printf("GOT NAV_VELNED MESSAGE\n");
 
 			if (!_waiting_for_ack) {
+				/* 35.15 NAV-VELNED (0x01 0x12) message (page 181 / 210 of reference manual */
 				gps_bin_nav_velned_packet_t *packet = (gps_bin_nav_velned_packet_t *) _rx_buffer;
 
 				_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->vel_n_m_s = (float)packet->velN * 1e-2f; /* NED NORTH velocity */
+				_gps_position->vel_e_m_s = (float)packet->velE * 1e-2f; /* NED EAST velocity */
+				_gps_position->vel_d_m_s = (float)packet->velD * 1e-2f; /* NED DOWN velocity */
 				_gps_position->cog_rad   = (float)packet->heading * M_DEG_TO_RAD_F * 1e-5f;
+				_gps_position->c_variance_rad = (float)packet->cAcc * M_DEG_TO_RAD_F * 1e-5f;
 				_gps_position->vel_ned_valid = true;
 				_gps_position->timestamp_velocity = hrt_absolute_time();
 			}
diff --git a/apps/examples/kalman_demo/KalmanNav.cpp b/apps/examples/kalman_demo/KalmanNav.cpp
index 6df454a55..4ef150da1 100644
--- a/apps/examples/kalman_demo/KalmanNav.cpp
+++ b/apps/examples/kalman_demo/KalmanNav.cpp
@@ -355,6 +355,8 @@ int KalmanNav::predictState(float dt)
 		float LDot = vN / R;
 		float lDot = vE / (cosLSing * R);
 		float rotRate = 2 * omega + lDot;
+
+		// XXX position prediction using speed
 		float vNDot = fN - vE * rotRate * sinL +
 			      vD * LDot;
 		float vDDot = fD - vE * rotRate * cosL -
diff --git a/apps/mavlink/mavlink_receiver.c b/apps/mavlink/mavlink_receiver.c
index a30f0bf3c..22c2fcdad 100644
--- a/apps/mavlink/mavlink_receiver.c
+++ b/apps/mavlink/mavlink_receiver.c
@@ -477,25 +477,27 @@ handle_message(mavlink_message_t *msg)
 
 			/* gps */
 			hil_gps.timestamp_position = gps.time_usec;
-//			hil_gps.counter = hil_counter++;
 			hil_gps.time_gps_usec = gps.time_usec;
 			hil_gps.lat = gps.lat;
 			hil_gps.lon = gps.lon;
 			hil_gps.alt = gps.alt;
-//			hil_gps.counter_pos_valid = hil_counter++;
 			hil_gps.eph_m = (float)gps.eph * 1e-2f; // from cm to m
 			hil_gps.epv_m = (float)gps.epv * 1e-2f; // from cm to m
 			hil_gps.s_variance_m_s = 5.0f;
 			hil_gps.p_variance_m = hil_gps.eph_m * hil_gps.eph_m;
 			hil_gps.vel_m_s = (float)gps.vel * 1e-2f; // from cm/s to m/s
 
-			/* gps.cog is in degrees 0..360 * 100, heading is -PI..PI */
-			float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f - M_PI_F;
+			/* gps.cog is in degrees 0..360 * 100, heading is -PI..+PI */
+			float heading_rad = gps.cog * M_DEG_TO_RAD_F * 1e-2f;
+			/* go back to -PI..PI */
+			if (heading_rad > M_PI_F)
+				heading_rad -= 2.0f * M_PI_F;
 			hil_gps.vel_n_m_s = (float)gps.vel * 1e-2f * cosf(heading_rad);
 			hil_gps.vel_e_m_s = (float)gps.vel * 1e-2f * sinf(heading_rad);
 			hil_gps.vel_d_m_s = 0.0f;
-			/* COG (course over ground) is speced as 0..360 degrees (compass) */
-			hil_gps.cog_rad = heading_rad + M_PI_F; // from deg*100 to rad
+			hil_gps.vel_ned_valid = true;
+			/* COG (course over ground) is speced as -PI..+PI */
+			hil_gps.cog_rad = heading_rad;
 			hil_gps.fix_type = gps.fix_type;
 			hil_gps.satellites_visible = gps.satellites_visible;
 
diff --git a/apps/uORB/topics/vehicle_gps_position.h b/apps/uORB/topics/vehicle_gps_position.h
index 5463a460d..0a7fb4e9d 100644
--- a/apps/uORB/topics/vehicle_gps_position.h
+++ b/apps/uORB/topics/vehicle_gps_position.h
@@ -55,38 +55,39 @@
  */
 struct vehicle_gps_position_s
 {
-	uint64_t timestamp_position;		/**< Timestamp for position information */
-	int32_t lat;						/**< Latitude in 1E7 degrees */
-	int32_t lon;						/**< Longitude in 1E7 degrees */
-	int32_t alt;						/**< Altitude in 1E3 meters (millimeters) above MSL  */
+	uint64_t timestamp_position;			/**< Timestamp for position information */
+	int32_t lat;					/**< Latitude in 1E7 degrees */
+	int32_t lon;					/**< Longitude in 1E7 degrees */
+	int32_t alt;					/**< Altitude in 1E3 meters (millimeters) above MSL  */
 	
 	uint64_t timestamp_variance;
 	float s_variance_m_s;				/**< speed accuracy estimate m/s */
-	float p_variance_m;					/**< position accuracy estimate m */
-	uint8_t fix_type; 					/**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.   */
+	float p_variance_m;				/**< position accuracy estimate m */
+	float c_variance_rad;				/**< course accuracy estimate rad */
+	uint8_t fix_type; 				/**< 0-1: no fix, 2: 2D fix, 3: 3D fix. Some applications will not use the value of this field unless it is at least two, so always correctly fill in the fix.   */
 
-	float eph_m;						/**< GPS HDOP horizontal dilution of position in m */
-	float epv_m;						/**< GPS VDOP horizontal dilution of position in m */
+	float eph_m;					/**< GPS HDOP horizontal dilution of position in m */
+	float epv_m;					/**< GPS VDOP horizontal dilution of position in m */
 
-	uint64_t timestamp_velocity;		/**< Timestamp for velocity informations */
-	float vel_m_s;						/**< GPS ground speed (m/s) */
-	float vel_n_m_s;					/**< GPS ground speed in m/s */
-	float vel_e_m_s;					/**< GPS ground speed in m/s */
-	float vel_d_m_s;					/**< GPS ground speed in m/s */
-	float cog_rad;						/**< Course over ground (NOT heading, but direction of movement) in rad */
-	bool vel_ned_valid;					/**< Flag to indicate if NED speed is valid */
+	uint64_t timestamp_velocity;			/**< Timestamp for velocity informations */
+	float vel_m_s;					/**< GPS ground speed (m/s) */
+	float vel_n_m_s;				/**< GPS ground speed in m/s */
+	float vel_e_m_s;				/**< GPS ground speed in m/s */
+	float vel_d_m_s;				/**< GPS ground speed in m/s */
+	float cog_rad;					/**< Course over ground (NOT heading, but direction of movement) in rad, -PI..PI */
+	bool vel_ned_valid;				/**< Flag to indicate if NED speed is valid */
 
 	uint64_t timestamp_time;			/**< Timestamp for time information */
 	uint64_t time_gps_usec;				/**< Timestamp (microseconds in GPS format), this is the timestamp which comes from the gps module   */
 
-	uint64_t timestamp_satellites;		/**< Timestamp for sattelite information */
+	uint64_t timestamp_satellites;			/**< Timestamp for sattelite information */
 	uint8_t satellites_visible;			/**< Number of satellites visible. If unknown, set to 255 */
 	uint8_t satellite_prn[20]; 			/**< Global satellite ID */
 	uint8_t satellite_used[20];			/**< 0: Satellite not used, 1: used for localization */
-	uint8_t satellite_elevation[20]; 	/**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
-	uint8_t satellite_azimuth[20];		/**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
+	uint8_t satellite_elevation[20]; 		/**< Elevation (0: right on top of receiver, 90: on the horizon) of satellite */
+	uint8_t satellite_azimuth[20];			/**< Direction of satellite, 0: 0 deg, 255: 360 deg. */
 	uint8_t satellite_snr[20];			/**< Signal to noise ratio of satellite   */
-	bool satellite_info_available;	/**< 0 for no info, 1 for info available */
+	bool satellite_info_available;			/**< 0 for no info, 1 for info available */
 };
 
 /**