mavlink_receiver : Add external estimator interface

src/modules/mavlink/mavlink_receiver.cpp

@@ -121,6 +121,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_pos_sp_triplet_pub(nullptr),
 	_att_pos_mocap_pub(nullptr),
 	_vision_position_pub(nullptr),
+	_vision_attitude_pub(nullptr),
 	_telemetry_status_pub(nullptr),
 	_rc_pub(nullptr),
 	_manual_pub(nullptr),
@@ -133,6 +134,7 @@ MavlinkReceiver::MavlinkReceiver(Mavlink *parent) :
 	_gps_inject_data_pub(nullptr),
 	_command_ack_pub(nullptr),
 	_control_mode_sub(orb_subscribe(ORB_ID(vehicle_control_mode))),
+	_global_ref_timestamp(0),
 	_hil_frames(0),
 	_old_timestamp(0),
 	_hil_last_frame(0),
@@ -216,6 +218,18 @@ MavlinkReceiver::handle_message(mavlink_message_t *msg)
 		handle_message_vision_position_estimate(msg);
 		break;
 
+	case MAVLINK_MSG_ID_ATTITUDE_QUATERNION_COV:
+		handle_message_attitude_quaternion_cov(msg);
+		break;
+
+	case MAVLINK_MSG_ID_LOCAL_POSITION_NED_COV:
+		handle_message_local_position_ned_cov(msg);
+		break;
+
+	case MAVLINK_MSG_ID_GPS_GLOBAL_ORIGIN:
+		handle_message_gps_global_origin(msg);
+		break;
+
 	case MAVLINK_MSG_ID_RADIO_STATUS:
 		handle_message_radio_status(msg);
 		break;
@@ -1045,6 +1059,93 @@ MavlinkReceiver::handle_message_set_actuator_control_target(mavlink_message_t *m
 
 }
 
+void
+MavlinkReceiver::handle_message_gps_global_origin(mavlink_message_t *msg)
+{
+	mavlink_gps_global_origin_t origin;
+	mavlink_msg_gps_global_origin_decode(msg, &origin);
+	
+	if (!globallocalconverter_initialized()) {
+		/* Set reference point conversion of local coordiantes <--> global coordinates */
+		globallocalconverter_init((double)origin.latitude * 1.0e-7, (double)origin.longitude * 1.0e-7,
+					  (float)origin.altitude * 1.0e-3f, hrt_absolute_time());
+		_global_ref_timestamp = hrt_absolute_time();
+
+	}
+}
+
+void
+MavlinkReceiver::handle_message_attitude_quaternion_cov(mavlink_message_t *msg)
+{
+	mavlink_attitude_quaternion_cov_t att;
+	mavlink_msg_attitude_quaternion_cov_decode(msg, &att);
+	
+	struct vehicle_attitude_s vision_attitude = {};
+
+	vision_attitude.timestamp = sync_stamp(att.time_usec);
+
+	vision_attitude.q[0] = att.q[0];
+	vision_attitude.q[1] = att.q[1];
+	vision_attitude.q[2] = att.q[2];
+	vision_attitude.q[3] = att.q[3];
+
+	vision_attitude.rollspeed = att.rollspeed;
+	vision_attitude.pitchspeed = att.pitchspeed;
+	vision_attitude.yawspeed = att.yawspeed;
+
+	// TODO : full covariance matrix
+
+	int instance_id = 0;
+	orb_publish_auto(ORB_ID(vehicle_vision_attitude), &_vision_attitude_pub, &vision_attitude, &instance_id, ORB_PRIO_HIGH);
+
+}
+
+void
+MavlinkReceiver::handle_message_local_position_ned_cov(mavlink_message_t *msg)
+{
+	mavlink_local_position_ned_cov_t pos;
+	mavlink_msg_local_position_ned_cov_decode(msg, &pos);
+
+	struct vehicle_local_position_s vision_position = {};
+	
+	vision_position.timestamp = sync_stamp(pos.time_usec);
+	
+	// Use the estimator type to identify the external estimate
+	vision_position.estimator_type = pos.estimator_type;
+	
+	vision_position.xy_valid = true;
+	vision_position.z_valid = true;
+	vision_position.v_xy_valid = true;
+	vision_position.v_z_valid = true;
+
+	vision_position.x = pos.x;
+	vision_position.y = pos.y;
+	vision_position.z = pos.z;
+
+	vision_position.vx = pos.vx;
+	vision_position.vy = pos.vy;
+	vision_position.vz = pos.vz;
+	
+	vision_position.ax = pos.ax;
+	vision_position.ay = pos.ay;
+	vision_position.az = pos.az;
+
+	// Low risk change for now. TODO : full covariance matrix
+	vision_position.eph = sqrtf(fmaxf(pos.covariance[0],pos.covariance[9]));
+	vision_position.epv = sqrtf(pos.covariance[17]);
+	
+	vision_position.xy_global = globallocalconverter_initialized();
+	vision_position.z_global = globallocalconverter_initialized();
+	vision_position.ref_timestamp = _global_ref_timestamp;
+	globallocalconverter_getref(&vision_position.ref_lat, &vision_position.ref_lon, &vision_position.ref_alt);
+
+	vision_position.dist_bottom_valid = false;
+	
+	int instance_id = 0;
+	orb_publish_auto(ORB_ID(vehicle_vision_position), &_vision_position_pub, &vision_position, &instance_id, ORB_PRIO_HIGH);
+
+}
+
 void
 MavlinkReceiver::handle_message_vision_position_estimate(mavlink_message_t *msg)
 {

src/modules/mavlink/mavlink_receiver.h

@@ -59,7 +59,6 @@
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <uORB/topics/position_setpoint_triplet.h>
 #include <uORB/topics/att_pos_mocap.h>
-#include <uORB/topics/vision_position_estimate.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
 #include <uORB/topics/optical_flow.h>
@@ -126,6 +125,9 @@ private:
 	void handle_message_set_mode(mavlink_message_t *msg);
 	void handle_message_att_pos_mocap(mavlink_message_t *msg);
 	void handle_message_vision_position_estimate(mavlink_message_t *msg);
+	void handle_message_gps_global_origin(mavlink_message_t *msg);
+	void handle_message_attitude_quaternion_cov(mavlink_message_t *msg);
+	void handle_message_local_position_ned_cov(mavlink_message_t *msg);
 	void handle_message_quad_swarm_roll_pitch_yaw_thrust(mavlink_message_t *msg);
 	void handle_message_set_position_target_local_ned(mavlink_message_t *msg);
 	void handle_message_set_actuator_control_target(mavlink_message_t *msg);
@@ -218,6 +220,7 @@ private:
 	orb_advert_t _pos_sp_triplet_pub;
 	orb_advert_t _att_pos_mocap_pub;
 	orb_advert_t _vision_position_pub;
+	orb_advert_t _vision_attitude_pub;
 	orb_advert_t _telemetry_status_pub;
 	orb_advert_t _rc_pub;
 	orb_advert_t _manual_pub;
@@ -231,6 +234,7 @@ private:
 	orb_advert_t _gps_inject_data_pub;
 	orb_advert_t _command_ack_pub;
 	int _control_mode_sub;
+	uint64_t _global_ref_timestamp;
 	int _hil_frames;
 	uint64_t _old_timestamp;
 	uint64_t _hil_last_frame;

src/modules/systemlib/system_params.c

@@ -93,7 +93,7 @@ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);
 /**
  * Set multicopter estimator group
  *
- * Set the group of estimators used for multicopters and vtols
+ * Set the group of estimators used for multicopters and VTOLs
  *
  * @value 1 local_position_estimator, attitude_estimator_q
  * @value 2 ekf2