ekf2: Changes required to enter POSCTL mode

Adds missing local position and global position data

src/lib/ecl

@@ -1 +1 @@
-Subproject commit eda69e727fab22c81c421b635cb34303b90b7968
+Subproject commit 0cb210d045d005d0775bc7b4c1631addb128c8e3

src/modules/ekf2/ekf2_main.cpp

@@ -70,6 +70,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/control_state.h>
+#include <uORB/topics/vehicle_global_position.h>
 
 #include <ecl/EKF/ekf.h>
 
@@ -111,6 +112,8 @@ public:
 
 	void print();
 
+	void print_status();
+
 private:
 	static constexpr float _dt_max = 0.02;
 	bool		_task_should_exit = false;		/**< if true, task should exit */
@@ -123,6 +126,7 @@ private:
 	orb_advert_t _att_pub;
 	orb_advert_t _lpos_pub;
 	orb_advert_t _control_state_pub;
+	orb_advert_t _vehicle_global_position_pub;
 
 	/* Low pass filter for attitude rates */
 	math::LowPassFilter2p _lp_roll_rate;
@@ -162,6 +166,11 @@ void Ekf2::print()
 	_ekf->printStoredIMU();
 }
 
+void Ekf2::print_status()
+{
+	warnx("position OK %s", (_ekf->position_is_valid()) ? "[YES]" : "[NO]");
+}
+
 void Ekf2::task_main()
 {
 	// subscribe to relevant topics
@@ -244,11 +253,12 @@ void Ekf2::task_main()
 			_ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s);
 		}
 
+		// run the EKF update
+		_ekf->update();
+
+		// generate vehicle attitude data
 		struct vehicle_attitude_s att;
-		struct vehicle_local_position_s lpos;
 		att.timestamp = hrt_absolute_time();
-		lpos.timestamp = hrt_absolute_time();
-		_ekf->update();
 
 		_ekf->copy_quaternion(att.q);
 		matrix::Quaternion<float> q(att.q[0], att.q[1], att.q[2], att.q[3]);
@@ -257,25 +267,65 @@ void Ekf2::task_main()
 		att.pitch = euler(1);
 		att.yaw = euler(2);
 
+		// generate vehicle local position data
+		struct vehicle_local_position_s lpos;
 		float pos[3] = {};
 		float vel[3] = {};
 
+		lpos.timestamp = hrt_absolute_time();
+
+		// Position in local NED frame
 		_ekf->copy_position(pos);
 		lpos.x = pos[0];
 		lpos.y = pos[1];
 		lpos.z = pos[2];
 
+		// Velocity in NED frame (m/s)
 		_ekf->copy_velocity(vel);
 		lpos.vx = vel[0];
 		lpos.vy = vel[1];
 		lpos.vz = vel[2];
 
+		// TODO: better status reporting
+		lpos.xy_valid = _ekf->position_is_valid();
+		lpos.z_valid = true;
+		lpos.v_xy_valid = _ekf->position_is_valid();
+		lpos.v_z_valid = true;
+
+		// Position of local NED origin in GPS / WGS84 frame
+		lpos.ref_timestamp = _ekf->_last_gps_origin_time_us; // Time when reference position was set
+ 		lpos.xy_global = _ekf->position_is_valid();// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
+ 		lpos.z_global = true;// true if z is valid and has valid global reference (ref_alt)
+		lpos.ref_lat = _ekf->_posRef.lat_rad * (double)180.0 * M_PI; // Reference point latitude in degrees
+		lpos.ref_lon = _ekf->_posRef.lon_rad * (double)180.0 * M_PI; // Reference point longitude in degrees
+		lpos.ref_alt = _ekf->_gps_alt_ref; // Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level		
+
+		// TODO: uORB definition does not define what this variable is. We have assumed it to be heading angle in radians lying on the range from +-pi
+		lpos.yaw = euler(2);
+
+		// TODO: Distance to surface. Units not defined.
+		lpos.dist_bottom = 0.0f; // Distance to bottom surface (ground)
+		lpos.dist_bottom_rate = 0.0f; // Distance to bottom surface (ground) change rate
+		lpos.surface_bottom_timestamp	= 0; // Time when new bottom surface found
+		lpos.dist_bottom_valid = false; // true if distance to bottom surface is valid
+
+ 		// TODO: uORB definition does not define what thes variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres 
+		// TODO: Should use sqrt of filter position variances
+		lpos.eph = gps.eph;
+		lpos.epv = gps.epv;
+
+		// publish vehicle local position data
+		if (_lpos_pub == nullptr) {
+			_lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos);
+		} else {
+			orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos);
+		}
+
+		// generate control state data
 		control_state_s ctrl_state = {};
 		ctrl_state.timestamp = hrt_absolute_time();
 		ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]);
-
 		ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]);
-
 		ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]);
 
 		ctrl_state.q[0] = q(0);
@@ -283,6 +333,14 @@ void Ekf2::task_main()
 		ctrl_state.q[2] = q(2);
 		ctrl_state.q[3] = q(3);
 
+		// publish control state data
+		if (_control_state_pub == nullptr) {
+			_control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
+		} else {
+			orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
+		}
+
+		// generate vehicle attitude data
 		att.q[0] = q(0);
 		att.q[1] = q(1);
 		att.q[2] = q(2);
@@ -293,25 +351,51 @@ void Ekf2::task_main()
 		att.pitchspeed = sensors.gyro_rad_s[1];
 		att.yawspeed = sensors.gyro_rad_s[2];
 
-		if (_control_state_pub == nullptr) {
-			_control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
-		} else {
-			orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
-		}
-
+		// publish vehicle attitude data
 		if (_att_pub == nullptr) {
 			_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
 		} else {
 			orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
 		}
 
-		if (_lpos_pub == nullptr) {
-			_lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos);
-		} else {
-			orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos);
-		}
+		// generate and publish global position data
+		struct vehicle_global_position_s global_pos;
+		if (_ekf->position_is_valid()) {
+			// TODO: local origin is currenlty at GPS height origin - this is different to ekf_att_pos_estimator
+
+			global_pos.timestamp = hrt_absolute_time(); // Time of this estimate, in microseconds since system start
+			global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds
+
+			double est_lat, est_lon;
+			map_projection_reproject(&_ekf->_posRef, lpos.x, lpos.y, &est_lat, &est_lon);
+			global_pos.lat = est_lat; // Latitude in degrees
+			global_pos.lon = est_lon; // Longitude in degrees
+
+			global_pos.alt = -pos[2]; // Altitude AMSL in meters
 
+			global_pos.vel_n = vel[0]; // Ground north velocity, m/s
+			global_pos.vel_e = vel[1]; // Ground east velocity, m/s
+			global_pos.vel_d = vel[2]; // Ground downside velocity, m/s
 
+			global_pos.yaw = euler(2); // Yaw in radians -PI..+PI.
+
+			global_pos.eph = gps.eph; // Standard deviation of position estimate horizontally
+			global_pos.epv = gps.epv; // Standard deviation of position vertically
+
+			// TODO: implement terrain estimator
+			global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84
+			global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid
+			// TODO use innovatun consistency check timouts to set this
+			global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning
+
+			global_pos.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m)
+
+			if (_vehicle_global_position_pub == nullptr) {
+				_vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
+			} else {
+				orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos);
+			}
+		}
 	}
 }
 
@@ -394,6 +478,7 @@ int ekf2_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "status")) {
 		if (ekf2::instance) {
 			PX4_WARN("running");
+			ekf2::instance->print_status();
 			return 0;
 
 		} else {