wind_estimator fix LPWORK/HPWORK confusion and cleanup

src/modules/wind_estimator/CMakeLists.txt

@@ -33,6 +33,8 @@
 px4_add_module(
 	MODULE modules__wind_estimator
 	MAIN wind_estimator
+	INCLUDES
+		${PX4_SOURCE_DIR}/src/lib/ecl
 	SRCS
 		wind_estimator_main.cpp
 )

src/modules/wind_estimator/wind_estimator_main.cpp

@@ -31,24 +31,28 @@
  *
  ****************************************************************************/
 
+#include <drivers/drv_hrt.h>
 #include <ecl/airdata/WindEstimator.hpp>
-
+#include <matrix/matrix/math.hpp>
 #include <px4_module.h>
 #include <px4_module_params.h>
 #include <px4_workqueue.h>
-#include <uORB/topics/vehicle_local_position.h>
+#include <systemlib/param/param.h>
-
+#include <systemlib/perf_counter.h>
-#include <drivers/drv_hrt.h>
-#include <matrix/matrix/math.hpp>
 #include <uORB/topics/airspeed.h>
-#include <uORB/topics/wind_estimate.h>
+#include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_local_position.h>
-#include <uORB/topics/parameter_update.h>
+#include <uORB/topics/vehicle_local_position.h>
-#include <systemlib/param/param.h>
+#include <uORB/topics/wind_estimate.h>
 
 #define SCHEDULE_INTERVAL	100000	/**< The schedule interval in usec (10 Hz) */
 
+using matrix::Dcmf;
+using matrix::Quatf;
+using matrix::Vector2f;
+using matrix::Vector3f;
+
 class WindEstimatorModule : public ModuleBase<WindEstimatorModule>, public ModuleParams
 {
 public:
@@ -60,20 +64,17 @@ public:
 	/** @see ModuleBase */
 	static int task_spawn(int argc, char *argv[]);
 
-	/** @see ModuleBase */
-	static WindEstimatorModule *instantiate(int argc, char *argv[]);
-
 	/** @see ModuleBase */
 	static int custom_command(int argc, char *argv[]);
 
 	/** @see ModuleBase */
 	static int print_usage(const char *reason = nullptr);
 
-	/**
+	// run the main loop
-	 * run the main loop
-	 */
 	void cycle();
 
+	int print_status() override;
+
 private:
 	static struct work_s	_work;
 
@@ -85,6 +86,9 @@ private:
 	int _airspeed_sub{-1};
 	int _param_sub{-1};
 
+	perf_counter_t _perf_elapsed{};
+	perf_counter_t _perf_interval{};
+
 	DEFINE_PARAMETERS(
 		(ParamFloat<px4::params::WEST_W_P_NOISE>) wind_p_noise,
 		(ParamFloat<px4::params::WEST_SC_P_NOISE>) tas_scale_p_noise,
@@ -113,6 +117,8 @@ WindEstimatorModule::WindEstimatorModule():
 	// initialise parameters
 	update_params();
 
+	_perf_elapsed = perf_alloc_once(PC_ELAPSED, "wind_estimator elapsed");
+	_perf_interval = perf_alloc_once(PC_INTERVAL, "wind_estimator interval");
 }
 
 WindEstimatorModule::~WindEstimatorModule()
@@ -123,6 +129,9 @@ WindEstimatorModule::~WindEstimatorModule()
 	orb_unsubscribe(_param_sub);
 
 	orb_unadvertise(_wind_est_pub);
+
+	perf_free(_perf_elapsed);
+	perf_free(_perf_interval);
 }
 
 int
@@ -134,12 +143,13 @@ WindEstimatorModule::task_spawn(int argc, char *argv[])
 	// wait until task is up & running
 	if (wait_until_running() < 0) {
 		_task_id = -1;
-		return -1;
+
+	} else {
+		_task_id = task_id_is_work_queue;
+		return PX4_OK;
 	}
 
-	_task_id = task_id_is_work_queue;
+	return PX4_ERROR;
-
-	return PX4_OK;
 }
 
 void
@@ -159,12 +169,17 @@ WindEstimatorModule::cycle_trampoline(void *arg)
 		_object = dev;
 	}
 
-	dev->cycle();
+	if (dev) {
+		dev->cycle();
+	}
 }
 
 void
 WindEstimatorModule::cycle()
 {
+	perf_count(_perf_interval);
+	perf_begin(_perf_elapsed);
+
 	bool param_updated;
 	orb_check(_param_sub, &param_updated);
 
@@ -172,49 +187,52 @@ WindEstimatorModule::cycle()
 		update_params();
 	}
 
-	vehicle_attitude_s vehicle_attitude = {};
+	bool lpos_valid = false;
-	vehicle_local_position_s vehicle_local_position = {};
+	bool att_valid = false;
-	airspeed_s airspeed = {};
+	bool airspeed_valid = false;
 
-	orb_copy(ORB_ID(vehicle_attitude), _vehicle_attitude_sub, &vehicle_attitude);
+	const hrt_abstime time_now_usec = hrt_absolute_time();
-	orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
-	orb_copy(ORB_ID(vehicle_local_position), _vehicle_local_position_sub, &vehicle_local_position);
 
-	hrt_abstime time_now_usec = hrt_absolute_time();
+	// validate required conditions for the filter to fuse measurements
+
+	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);
+	}
+
+	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;
+	}
 
 	// update wind and airspeed estimator
 	_wind_estimator.update(time_now_usec);
 
-	// validate required conditions for the filter to fuse measurements
+	if (lpos_valid && att_valid) {
-	bool fuse = time_now_usec - vehicle_local_position.timestamp < 1000 * 1000 && vehicle_local_position.timestamp;
-	fuse &= time_now_usec - vehicle_attitude.timestamp < 1000 * 1000;
-	fuse &= vehicle_local_position.v_xy_valid;
 
-	// additionally, for airspeed fusion we need to have recent measurements
+		Vector3f vI(lpos.vx, lpos.vy, lpos.vz);
-	bool fuse_airspeed = fuse && time_now_usec - airspeed.timestamp < 1000 * 1000;
+		Quatf q(att.q);
-
-
-	if (fuse) {
-
-		matrix::Dcmf R_to_earth(matrix::Quatf(vehicle_attitude.q));
-		matrix::Vector3f vI(vehicle_local_position.vx, vehicle_local_position.vy, vehicle_local_position.vz);
 
 		// sideslip fusion
-		_wind_estimator.fuse_beta(time_now_usec, &vI._data[0][0], vehicle_attitude.q);
+		_wind_estimator.fuse_beta(time_now_usec, vI, q);
 
-		if (fuse_airspeed) {
+		// additionally, for airspeed fusion we need to have recent measurements
-			matrix::Vector3f vel_var(vehicle_local_position.evh, vehicle_local_position.evh, vehicle_local_position.evv);
+		airspeed_s airspeed = {};
-			vel_var = R_to_earth * vel_var;
 
-			//airspeed fusion
+		if (orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed) == PX4_OK) {
-			_wind_estimator.fuse_airspeed(time_now_usec, airspeed.indicated_airspeed_m_s, &vI._data[0][0],
+			airspeed_valid = (time_now_usec - airspeed.timestamp < 1000 * 1000) && (airspeed.timestamp > 0);
-						      &vel_var._data[0][0]);
 		}
-	}
 
+		if (airspeed_valid) {
+			Vector3f vel_var{Dcmf(q) *Vector3f{lpos.evh, lpos.evh, lpos.evv}};
 
-	// if we fused either airspeed or sideslip we publish a wind_estimate message
+			// airspeed fusion
-	if (fuse) {
+			_wind_estimator.fuse_airspeed(time_now_usec, airspeed.indicated_airspeed_m_s, vI, Vector2f{vel_var(0), vel_var(1)});
+		}
+
+		// if we fused either airspeed or sideslip we publish a wind_estimate message
 		wind_estimate_s wind_est = {};
 
 		wind_est.timestamp = time_now_usec;
@@ -236,12 +254,14 @@ WindEstimatorModule::cycle()
 		orb_publish_auto(ORB_ID(wind_estimate), &_wind_est_pub, &wind_est, &instance, ORB_PRIO_DEFAULT);
 	}
 
+	perf_end(_perf_elapsed);
+
 	if (should_exit()) {
 		exit_and_cleanup();
 
 	} else {
 		/* schedule next cycle */
-		work_queue(HPWORK, &_work, (worker_t)&WindEstimatorModule::cycle_trampoline, this, USEC2TICK(SCHEDULE_INTERVAL));
+		work_queue(LPWORK, &_work, (worker_t)&WindEstimatorModule::cycle_trampoline, this, USEC2TICK(SCHEDULE_INTERVAL));
 	}
 }
 
@@ -254,18 +274,10 @@ void WindEstimatorModule::update_params()
 	_wind_estimator.set_tas_scale_p_noise(tas_scale_p_noise.get());
 	_wind_estimator.set_tas_noise(tas_noise.get());
 	_wind_estimator.set_beta_noise(beta_noise.get());
-
-}
-
-WindEstimatorModule *WindEstimatorModule::instantiate(int argc, char *argv[])
-{
-	// No arguments to parse. We also know that we should run as task
-	return new WindEstimatorModule();
 }
 
 int WindEstimatorModule::custom_command(int argc, char *argv[])
 {
-	/* start the FMU if not running */
 	if (!is_running()) {
 		int ret = WindEstimatorModule::task_spawn(argc, argv);
 
@@ -301,6 +313,14 @@ measured_airspeed = scale_factor * real_airspeed.
 	return 0;
 }
 
+int WindEstimatorModule::print_status()
+{
+	perf_print_counter(_perf_elapsed);
+	perf_print_counter(_perf_interval);
+
+	return 0;
+}
+
 extern "C" __EXPORT int wind_estimator_main(int argc, char *argv[]);
 
 int