drivers/ins/vectornav: publish estimator_status (only in VN_MODE 1)

src/drivers/ins/vectornav/VectorNav.cpp

@@ -355,6 +355,35 @@ void VectorNav::sensorCallback(VnUartPacket *packet)
 			_global_position_pub.publish(global_position);
 			perf_count(_global_position_pub_interval_perf);
 		}
+
+		// publish estimator_status (VN_MODE 1 only)
+		if (_param_vn_mode.get() == 1) {
+
+			estimator_status_s estimator_status{};
+			estimator_status.timestamp_sample = time_now_us;
+
+			float test_ratio = 0.f;
+
+			if (mode_aligning) {
+				test_ratio = 0.99f;
+
+			} else if (mode_tracking) {
+				// very good
+				test_ratio = 0.1f;
+			}
+
+			estimator_status.mag_test_ratio = test_ratio;
+			estimator_status.vel_test_ratio = test_ratio;
+			estimator_status.pos_test_ratio = test_ratio;
+			estimator_status.hgt_test_ratio = test_ratio;
+
+			estimator_status.accel_device_id = _px4_accel.get_device_id();
+			estimator_status.gyro_device_id = _px4_gyro.get_device_id();
+
+			estimator_status.timestamp = hrt_absolute_time();
+			_estimator_status_pub.publish(estimator_status);
+
+		}
 	}
 
 	// binary output 3

src/drivers/ins/vectornav/VectorNav.hpp

@@ -63,6 +63,7 @@ extern "C" {
 #include <px4_platform_common/px4_work_queue/ScheduledWorkItem.hpp>
 #include <uORB/Publication.hpp>
 #include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/estimator_status.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/sensor_baro.h>
 #include <uORB/topics/sensor_gps.h>
@@ -140,6 +141,8 @@ private:
 	uORB::PublicationMulti<vehicle_local_position_s> _local_position_pub;
 	uORB::PublicationMulti<vehicle_global_position_s> _global_position_pub;
 
+	uORB::Publication<estimator_status_s> _estimator_status_pub{ORB_ID(estimator_status)};
+
 	perf_counter_t _comms_errors{perf_alloc(PC_COUNT, MODULE_NAME": com_err")};
 	perf_counter_t _sample_perf{perf_alloc(PC_ELAPSED, MODULE_NAME": read")};