Trajectory calculator

3 files changed, 251 insertions, 0 deletions

diff --git a/makefiles/config_px4fmu-v2_default.mk b/makefiles/config_px4fmu-v2_default.mk
index cd19fe907..919ee7397 100644
--- a/makefiles/config_px4fmu-v2_default.mk
+++ b/makefiles/config_px4fmu-v2_default.mk
@@ -88,6 +88,7 @@ MODULES		+= modules/attitude_estimator_ekf
 #MODULES		+= modules/ekf_att_pos_estimator
 MODULES		+= modules/position_estimator_inav
 #MODULES		+= examples/flow_position_estimator
+MODULES		+= modules/airdog/trajectory_calculator
 
 #
 # Vehicle Control
diff --git a/src/modules/airdog/trajectory_calculator/module.mk b/src/modules/airdog/trajectory_calculator/module.mk
new file mode 100644
index 000000000..6eafc4818
--- /dev/null
+++ b/src/modules/airdog/trajectory_calculator/module.mk
@@ -0,0 +1,3 @@
+MODULE_COMMAND = trajectory_calculator
+SRCS = trajectory_calculator.cpp
+CXXFLAGS += -std=gnu++11
\ No newline at end of file
diff --git a/src/modules/airdog/trajectory_calculator/trajectory_calculator.cpp b/src/modules/airdog/trajectory_calculator/trajectory_calculator.cpp
new file mode 100644
index 000000000..38c61beb4
--- /dev/null
+++ b/src/modules/airdog/trajectory_calculator/trajectory_calculator.cpp
@@ -0,0 +1,247 @@
+#include <nuttx/config.h>
+
+#include <geo/geo.h>
+#include <math.h>
+#include <mathlib/math/filter/LowPassFilter.hpp>
+#include <poll.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <sys/types.h>
+#include <systemlib/err.h>
+#include <systemlib/param/param.h>
+#include <systemlib/systemlib.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/home_position.h>
+#include <uORB/topics/trajectory.h>
+#include <uORB/topics/vehicle_local_position.h>
+
+namespace trajectory_calculator {
+
+// Global variables, controlling threaded execution
+static bool g_thread_continue = false;
+static bool g_thread_running = false;
+
+// Helper class for position analysis.
+// TODO! Conisider migrating to function
+class PointAnalyzer {
+private:
+	// TODO! Consider migrating to a vector filtering
+	math::LowPassFilter<float> x_filter, y_filter;
+	double radius;
+	float center_x, center_y;
+public:
+	PointAnalyzer(float filter_cutoff, float init_radius) :
+				x_filter(filter_cutoff),
+				y_filter(filter_cutoff),
+				radius(init_radius),
+				center_x(0.0f/0.0f),
+				center_y(center_x) {
+	}
+	~PointAnalyzer() {}
+
+	// Analyze provided point, filter its x and y, update center, if needed and return point type
+	uint8_t analyze(vehicle_local_position_s &point) {
+		double x, y;
+
+		point.x = x_filter.apply(point.timestamp, point.x);
+		point.y = y_filter.apply(point.timestamp, point.y);
+		if (!isfinite(center_x) || !isfinite(center_y)) {
+			center_x = point.x;
+			center_y = point.y;
+		}
+		x = point.x - center_x;
+		y = point.y - center_y;
+		// TODO! Consider variable radius based on speed
+		if (sqrt(x*x + y*y) < (double) radius) {
+			return 0; // still point
+		}
+		else {
+			center_x = point.x;
+			center_y = point.y;
+			return 1; // moving point
+		}
+	}
+};
+
+// Thread entry point. Calculates trajectory points and publishes them
+int calculate(int argc, char *argv[]) {
+	// TODO! Parameters instead of constants
+	float cutoff, radius;
+	if (param_get(param_find("AIRD_TRAJ_CUT"), &cutoff) != 0 ||
+			param_get(param_find("AIRD_TRAJ_RAD"), &radius) != 0) {
+		warnx("Could not read parameters!");
+		return -1;
+	}
+
+	// Prepare pollers
+	int pos_topic = orb_subscribe(ORB_ID(vehicle_local_position));
+	pollfd poll_data;
+	poll_data.fd = pos_topic;
+	poll_data.events = POLLIN;
+	int home_topic = orb_subscribe(ORB_ID(home_position));
+	int res;
+
+	orb_advert_t trajectory_pub = -1;
+	trajectory_s trajectory_data;
+
+	// TODO! We might be to the party too early - consider waiting for the first publication
+	vehicle_local_position_s local_position = {};
+	home_position_s home;
+
+	bool updated;
+	// We consider home position secondary, thus we need to initialize it with correct data
+	orb_copy(ORB_ID(home_position), home_topic, &home);
+
+	// Projection vars
+	struct map_projection_reference_s ref_point;
+	double est_lat, est_lon;
+	// Latitude and longitude in ref_point are converted, but we need to compare with local refs
+	double ref_lat = 0.0f/0.0f, ref_lon = ref_lat;
+
+	// Filtering object
+	PointAnalyzer analyzer(cutoff, radius);
+
+	g_thread_running = true;
+	while (g_thread_continue) {
+		// pace the daemon by global position topic, 100 HZ with a bit of overhead
+		res = poll(&poll_data, 1, 15);
+		if (res == 1) {
+			orb_copy(ORB_ID(vehicle_local_position), pos_topic, &local_position);
+			orb_check(home_topic, &updated);
+			if (updated) {
+				orb_copy(ORB_ID(home_position), home_topic, &home);
+			}
+
+			// TODO! Currently we trust local_position topic to not have incorrect refs
+			// TODO! Use xy_global to check for valid refs
+			if (local_position.ref_lat != ref_lat || local_position.ref_lon != ref_lon) {
+				map_projection_init(&ref_point, local_position.ref_lat, local_position.ref_lon);
+				ref_lat = local_position.ref_lat;
+				ref_lon = local_position.ref_lon;
+			}
+
+			// Filters and modifies local position while he's at it
+			trajectory_data.point_type = analyzer.analyze(local_position);
+
+			map_projection_reproject(&ref_point, local_position.x, local_position.y, &est_lat, &est_lon);
+
+			trajectory_data.alt = local_position.ref_alt - local_position.z;
+			trajectory_data.lat = est_lat;
+			trajectory_data.lon = est_lon;
+			trajectory_data.vel_d = local_position.vz;
+			trajectory_data.vel_e = local_position.vy;
+			trajectory_data.vel_n = local_position.vx;
+			trajectory_data.relative_alt = home.alt - trajectory_data.alt;
+			trajectory_data.timestamp = local_position.timestamp;
+			trajectory_data.heading = _wrap_2pi(local_position.yaw);
+
+			if (trajectory_pub < 0) {
+				trajectory_pub = orb_advertise(ORB_ID(trajectory), &trajectory_data);
+			} else {
+				orb_publish(ORB_ID(trajectory), trajectory_pub, &trajectory_data);
+			}
+		}
+		else {
+			warnx("Poll error!");
+		}
+	}
+
+	close(pos_topic);
+	close(home_topic);
+	close(trajectory_pub);
+	g_thread_running = false;
+	return 0;
+}
+
+// Helper function to start the thread
+int start() {
+	if (g_thread_running) {
+		return -1;
+	}
+	g_thread_continue = true;
+	// Beware of the stack size. With 500 bytes the process just hangs!
+	if (0 >= task_spawn_cmd("trajectory_daemon",
+			SCHED_DEFAULT,SCHED_PRIORITY_DEFAULT, 2000,
+			calculate,
+			(const char**) NULL)){
+		g_thread_continue = false;
+		return -2;
+	}
+	// TODO! Consider checking if the thread successfully started
+	return 0;
+}
+
+// Helper function to stop the thread
+int stop() {
+	int i = 0;
+
+	if (!g_thread_running) {
+		return -1;
+	}
+	g_thread_continue = false;
+	// Wait for the thread to actually stop, pausing the caller
+	while (g_thread_running) {
+		// Wait max of 10 seconds
+		if (i++ > 10) {
+			return -2;
+		}
+		sleep(1);
+	}
+	return 0;
+}
+
+} // End trajectory_calculator namespace
+
+inline void usage(const char *progname) {
+	printf("Usage: %s {start|stop|status}\n", progname);
+}
+
+extern "C" __EXPORT int trajectory_calculator_main(int argc, char *argv[]) {
+	using namespace trajectory_calculator;
+
+	if (argc != 2) {
+		usage(argv[0]);
+		return 1;
+	}
+	if (strcmp(argv[1], "start") == 0) {
+		switch (start()) {
+		case 0:
+			warnx("Started.");
+			break;
+		case -1:
+			errx(1, "Already running!");
+			break;
+		default:
+			errx(1, "Failed to start the calculator!");
+			break;
+		}
+	}
+	else if (strcmp(argv[1], "stop") == 0) {
+		switch (stop()) {
+		case 0:
+			warnx("Stopped.");
+			break;
+		case -1:
+			errx(1, "Not running!");
+			break;
+		default:
+			errx(1, "Failed to stop the calculator!");
+			break;
+		}
+	}
+	else if (strcmp(argv[1], "status") == 0) {
+		if (g_thread_running == 0) {
+			warnx("Not running.");
+		}
+		else {
+			warnx("Started.");
+		}
+	}
+	else {
+		warnx("Unknown argument: '%s'", argv[1]);
+		usage(argv[0]);
+		return 1;
+	}
+	return 0;
+}