Merge pull request #401 from PX4/sdlog2_LPOS

sdlog2: position & velocity valid, postion global and landed flags added...

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -259,7 +259,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 						local_pos.ref_timestamp = hrt_absolute_time();
 						local_pos.z_valid = true;
 						local_pos.v_z_valid = true;
-						local_pos.global_z = true;
+						local_pos.z_global = true;
 					}
 				}
 			}
@@ -597,7 +597,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			local_pos.timestamp = t;
 			local_pos.xy_valid = can_estimate_xy && gps_valid;
 			local_pos.v_xy_valid = can_estimate_xy;
-			local_pos.global_xy = local_pos.xy_valid && gps_valid;	// will make sense when local position sources (e.g. vicon) will be implemented
+			local_pos.xy_global = local_pos.xy_valid && gps_valid;	// will make sense when local position sources (e.g. vicon) will be implemented
 			local_pos.x = x_est[0];
 			local_pos.vx = x_est[1];
 			local_pos.y = y_est[0];
@@ -610,9 +610,9 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 			orb_publish(ORB_ID(vehicle_local_position), vehicle_local_position_pub, &local_pos);
 
 			/* publish global position */
-			global_pos.valid = local_pos.global_xy;
+			global_pos.valid = local_pos.xy_global;
 
-			if (local_pos.global_xy) {
+			if (local_pos.xy_global) {
 				double est_lat, est_lon;
 				map_projection_reproject(local_pos.x, local_pos.y, &est_lat, &est_lon);
 				global_pos.lat = (int32_t)(est_lat * 1e7);
@@ -630,7 +630,7 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				global_pos.relative_alt = -local_pos.z;
 			}
 
-			if (local_pos.global_z) {
+			if (local_pos.z_global) {
 				global_pos.alt = local_pos.ref_alt - local_pos.z;
 			}
 

src/modules/sdlog2/sdlog2.c

@@ -1046,6 +1046,9 @@ int sdlog2_thread_main(int argc, char *argv[])
 				log_msg.body.log_LPOS.ref_lat = buf.local_pos.ref_lat;
 				log_msg.body.log_LPOS.ref_lon = buf.local_pos.ref_lon;
 				log_msg.body.log_LPOS.ref_alt = buf.local_pos.ref_alt;
+				log_msg.body.log_LPOS.xy_flags = (buf.local_pos.xy_valid ? 1 : 0) | (buf.local_pos.v_xy_valid ? 2 : 0) | (buf.local_pos.xy_global ? 8 : 0);
+				log_msg.body.log_LPOS.z_flags = (buf.local_pos.z_valid ? 1 : 0) | (buf.local_pos.v_z_valid ? 2 : 0) | (buf.local_pos.z_global ? 8 : 0);
+				log_msg.body.log_LPOS.landed = buf.local_pos.landed;
 				LOGBUFFER_WRITE_AND_COUNT(LPOS);
 			}
 

src/modules/sdlog2/sdlog2_messages.h

@@ -109,6 +109,9 @@ struct log_LPOS_s {
 	int32_t ref_lat;
 	int32_t ref_lon;
 	float ref_alt;
+	uint8_t xy_flags;
+	uint8_t z_flags;
+	uint8_t landed;
 };
 
 /* --- LPSP - LOCAL POSITION SETPOINT --- */
@@ -278,7 +281,7 @@ static const struct log_format_s log_formats[] = {
 	LOG_FORMAT(ATSP, "ffff", "RollSP,PitchSP,YawSP,ThrustSP"),
 	LOG_FORMAT(IMU, "fffffffff", "AccX,AccY,AccZ,GyroX,GyroY,GyroZ,MagX,MagY,MagZ"),
 	LOG_FORMAT(SENS, "ffff", "BaroPres,BaroAlt,BaroTemp,DiffPres"),
-	LOG_FORMAT(LPOS, "ffffffLLf", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt"),
+	LOG_FORMAT(LPOS, "ffffffLLfBBB", "X,Y,Z,VX,VY,VZ,RefLat,RefLon,RefAlt,XYFlags,ZFlags,Landed"),
 	LOG_FORMAT(LPSP, "ffff", "X,Y,Z,Yaw"),
 	LOG_FORMAT(GPS, "QBffLLfffff", "GPSTime,FixType,EPH,EPV,Lat,Lon,Alt,VelN,VelE,VelD,Cog"),
 	LOG_FORMAT(ATTC, "ffff", "Roll,Pitch,Yaw,Thrust"),

src/modules/uORB/topics/vehicle_local_position.h

@@ -70,8 +70,8 @@ struct vehicle_local_position_s
 	/* Heading */
 	float yaw;
 	/* Reference position in GPS / WGS84 frame */
-	bool global_xy;			/**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
+	bool xy_global;			/**< true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon) */
-	bool global_z;			/**< true if z is valid and has valid global reference (ref_alt) */
+	bool z_global;			/**< true if z is valid and has valid global reference (ref_alt) */
 	uint64_t ref_timestamp;	/**< Time when reference position was set */
 	int32_t ref_lat;		/**< Reference point latitude in 1E7 degrees */
 	int32_t ref_lon;		/**< Reference point longitude in 1E7 degrees */