ekf2 : move to new vision topic

src/modules/ekf2/ekf2_main.cpp

@@ -69,7 +69,6 @@
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/vision_position_estimate.h>
 #include <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_global_position.h>
 #include <uORB/topics/wind_estimate.h>
@@ -407,7 +406,8 @@ void Ekf2::task_main()
 	int params_sub = orb_subscribe(ORB_ID(parameter_update));
 	int optical_flow_sub = orb_subscribe(ORB_ID(optical_flow));
 	int range_finder_sub = orb_subscribe(ORB_ID(distance_sensor));
-	int ev_pos_sub = orb_subscribe(ORB_ID(vision_position_estimate));
+	int ev_pos_sub = orb_subscribe(ORB_ID(vehicle_vision_position));
+	int ev_att_sub = orb_subscribe(ORB_ID(vehicle_vision_attitude));
 	int vehicle_land_detected_sub = orb_subscribe(ORB_ID(vehicle_land_detected));
 	int status_sub = orb_subscribe(ORB_ID(vehicle_status));
 
@@ -429,7 +429,8 @@ void Ekf2::task_main()
 	optical_flow_s optical_flow = {};
 	distance_sensor_s range_finder = {};
 	vehicle_land_detected_s vehicle_land_detected = {};
-	vision_position_estimate_s ev = {};
+	vehicle_local_position_s ev_pos = {};
+	vehicle_attitude_s ev_att = {};
 	vehicle_status_s vehicle_status = {};
 
 	while (!_task_should_exit) {
@@ -465,6 +466,7 @@ void Ekf2::task_main()
 		bool range_finder_updated = false;
 		bool vehicle_land_detected_updated = false;
 		bool vision_position_updated = false;
+		bool vision_attitude_updated = false;
 		bool vehicle_status_updated = false;
 
 		orb_copy(ORB_ID(sensor_combined), sensors_sub, &sensors);
@@ -508,7 +510,13 @@ void Ekf2::task_main()
 		orb_check(ev_pos_sub, &vision_position_updated);
 
 		if (vision_position_updated) {
-			orb_copy(ORB_ID(vision_position_estimate), ev_pos_sub, &ev);
+			orb_copy(ORB_ID(vehicle_vision_position), ev_pos_sub, &ev_pos);
+		}
+
+		orb_check(ev_att_sub, &vision_attitude_updated);
+
+		if (vision_attitude_updated) {
+			orb_copy(ORB_ID(vehicle_vision_attitude), ev_att_sub, &ev_att);
 		}
 
 		// in replay mode we are getting the actual timestamp from the sensor topic
@@ -653,32 +661,20 @@ void Ekf2::task_main()
 
 		// get external vision data
 		// if error estimates are unavailable, use parameter defined defaults
-		if (vision_position_updated) {
+		if (vision_position_updated || vision_attitude_updated) {
 			ext_vision_message ev_data;
-			ev_data.posNED(0) = ev.x;
-			ev_data.posNED(1) = ev.y;
-			ev_data.posNED(2) = ev.z;
-			Quaternion q(ev.q);
+			ev_data.posNED(0) = ev_pos.x;
+			ev_data.posNED(1) = ev_pos.y;
+			ev_data.posNED(2) = ev_pos.z;
+			Quaternion q(ev_att.q);
 			ev_data.quat = q;
 
-			// position measurement error
-			if (ev.pos_err >= 0.001f) {
-				ev_data.posErr = ev.pos_err;
-
-			} else {
-				ev_data.posErr = _default_ev_pos_noise;
-			}
-
-			// angle measurement error
-			if (ev.ang_err >= 0.001f) {
-				ev_data.angErr = ev.ang_err;
-
-			} else {
-				ev_data.angErr = _default_ev_ang_noise;
-			}
+			// position measurement error from parameters. TODO : use covariances from topic
+			ev_data.posErr = _default_ev_pos_noise;
+			ev_data.angErr = _default_ev_ang_noise;
 
 			// use timestamp from external computer, clocks are synchronized when using MAVROS
-			_ekf.setExtVisionData(ev.timestamp, &ev_data);
+			_ekf.setExtVisionData(vision_position_updated ? ev_pos.timestamp : ev_att.timestamp, &ev_data);
 		}
 
 		orb_check(vehicle_land_detected_sub, &vehicle_land_detected_updated);
@@ -1086,17 +1082,18 @@ void Ekf2::task_main()
 				replay.asp_timestamp = 0;
 			}
 
-			if (vision_position_updated) {
-				replay.ev_timestamp = ev.timestamp;
-				replay.pos_ev[0] = ev.x;
-				replay.pos_ev[1] = ev.y;
-				replay.pos_ev[2] = ev.z;
-				replay.quat_ev[0] = ev.q[0];
-				replay.quat_ev[1] = ev.q[1];
-				replay.quat_ev[2] = ev.q[2];
-				replay.quat_ev[3] = ev.q[3];
-				replay.pos_err = ev.pos_err;
-				replay.ang_err = ev.ang_err;
+			if (vision_position_updated || vision_attitude_updated) {
+				replay.ev_timestamp = vision_position_updated ? ev_pos.timestamp : ev_att.timestamp;
+				replay.pos_ev[0] = ev_pos.x;
+				replay.pos_ev[1] = ev_pos.y;
+				replay.pos_ev[2] = ev_pos.z;
+				replay.quat_ev[0] = ev_att.q[0];
+				replay.quat_ev[1] = ev_att.q[1];
+				replay.quat_ev[2] = ev_att.q[2];
+				replay.quat_ev[3] = ev_att.q[3];
+				// TODO : switch to covariances from topic later
+				replay.pos_err = _default_ev_pos_noise;
+				replay.ang_err = _default_ev_ang_noise;
 
 			} else {
 				replay.ev_timestamp = 0;