From 253323601a6fb32ed67345c332adf0ec8910e816 Mon Sep 17 00:00:00 2001
From: Daniel Agar <daniel@agar.ca>
Date: Wed, 1 Aug 2018 15:08:01 -0400
Subject: [PATCH] wind_estimator status print last publication

---
 .../wind_estimator/wind_estimator_main.cpp    | 30 ++++++++++++++-----
 1 file changed, 22 insertions(+), 8 deletions(-)

diff --git a/src/modules/wind_estimator/wind_estimator_main.cpp b/src/modules/wind_estimator/wind_estimator_main.cpp
index fbe32ebd65..45b44978ed 100644
--- a/src/modules/wind_estimator/wind_estimator_main.cpp
+++ b/src/modules/wind_estimator/wind_estimator_main.cpp
@@ -32,13 +32,14 @@
  ****************************************************************************/
 
 #include <drivers/drv_hrt.h>
-#include <matrix/math.hpp>
 #include <ecl/airdata/WindEstimator.hpp>
+#include <matrix/math.hpp>
+#include <parameters/param.h>
+#include <perf/perf_counter.h>
 #include <px4_module.h>
 #include <px4_module_params.h>
 #include <px4_workqueue.h>
-#include <parameters/param.h>
-#include <perf/perf_counter.h>
+#include <uORB/Subscription.hpp>
 #include <uORB/topics/airspeed.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_attitude.h>
@@ -46,6 +47,8 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/wind_estimate.h>
 
+using namespace time_literals;
+
 #define SCHEDULE_INTERVAL	100000	/**< The schedule interval in usec (10 Hz) */
 
 using matrix::Dcmf;
@@ -89,6 +92,8 @@ private:
 	perf_counter_t _perf_elapsed{};
 	perf_counter_t _perf_interval{};
 
+	int	_instance{-1};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::WEST_W_P_NOISE>) wind_p_noise,
 		(ParamFloat<px4::params::WEST_SC_P_NOISE>) tas_scale_p_noise,
@@ -200,13 +205,13 @@ WindEstimatorModule::cycle()
 	vehicle_attitude_s att = {};
 
 	if (orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &att) == PX4_OK) {
-		att_valid = (time_now_usec - att.timestamp < 1000 * 1000) && (att.timestamp > 0);
+		att_valid = (time_now_usec - att.timestamp < 1_s) && (att.timestamp > 0);
 	}
 
 	vehicle_local_position_s lpos = {};
 
 	if (orb_copy(ORB_ID(vehicle_local_position), _vehicle_local_position_sub, &lpos) == PX4_OK) {
-		lpos_valid = (time_now_usec - lpos.timestamp < 1000 * 1000) && (lpos.timestamp > 0) && lpos.v_xy_valid;
+		lpos_valid = (time_now_usec - lpos.timestamp < 1_s) && (lpos.timestamp > 0) && lpos.v_xy_valid;
 	}
 
 	// update wind and airspeed estimator
@@ -224,7 +229,7 @@ WindEstimatorModule::cycle()
 		airspeed_s airspeed = {};
 
 		if (orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed) == PX4_OK) {
-			airspeed_valid = (time_now_usec - airspeed.timestamp < 1000 * 1000) && (airspeed.timestamp > 0);
+			airspeed_valid = (time_now_usec - airspeed.timestamp < 1_s) && (airspeed.timestamp > 0);
 		}
 
 		if (airspeed_valid) {
@@ -252,8 +257,7 @@ WindEstimatorModule::cycle()
 		wind_est.beta_innov_var = _wind_estimator.get_beta_innov_var();
 		wind_est.tas_scale = _wind_estimator.get_tas_scale();
 
-		int instance;
-		orb_publish_auto(ORB_ID(wind_estimate), &_wind_est_pub, &wind_est, &instance, ORB_PRIO_DEFAULT);
+		orb_publish_auto(ORB_ID(wind_estimate), &_wind_est_pub, &wind_est, &_instance, ORB_PRIO_DEFAULT);
 	}
 
 	perf_end(_perf_elapsed);
@@ -322,6 +326,16 @@ int WindEstimatorModule::print_status()
 	perf_print_counter(_perf_elapsed);
 	perf_print_counter(_perf_interval);
 
+	if (_instance > -1) {
+		unsigned instance = _instance;
+		uORB::Subscription<wind_estimate_s> est{ORB_ID(wind_estimate), instance};
+		est.update();
+
+		print_message(est.get());
+	} else {
+		PX4_INFO("never published");
+	}
+
 	return 0;
 }