updated ecl lib

cmake/configs/posix_sitl_default.cmake

@@ -32,23 +32,15 @@ set(config_module_list
 	modules/sensors
 	modules/simulator
 	modules/mavlink
+	modules/dataman
 	modules/attitude_estimator_ekf
-	modules/attitude_estimator_q
+	#modules/attitude_estimator_q
 	modules/ekf_att_pos_estimator
 	modules/position_estimator_inav
 	modules/navigator
 	modules/vtol_att_control
-	modules/mc_pos_control
-	modules/mc_att_control
-	modules/mc_pos_control_multiplatform
-	modules/mc_att_control_multiplatform
-	modules/land_detector
-	modules/fw_att_control
-	modules/fw_pos_control_l1
-	modules/dataman
-	modules/sdlog2
-	modules/commander
 	modules/controllib
+	modules/ekf2
 	lib/mathlib
 	lib/mathlib/math/filter
 	lib/conversion
@@ -59,6 +51,7 @@ set(config_module_list
 	lib/launchdetection
 	lib/terrain_estimation
 	lib/runway_takeoff
+	lib/ecl/EKF/tests/base
 	)
 
 set(config_extra_builtin_cmds

posix-configs/SITL/init/rcS_jmavsim_iris

@@ -61,3 +61,4 @@ mavlink stream -r 250 -s HIGHRES_IMU -u 14556
 mavlink stream -r 10 -s OPTICAL_FLOW_RAD -u 14556
 mavlink boot_complete
 sdlog2 start -r 100 -e -t -a
+ekf2 start

src/lib/ecl

@@ -1 +1 @@
-Subproject commit e5743d503c416693f11be3af13fdcd442e49cbce
+Subproject commit 8d6864304fa3a18f728730f9e49fa07640a87e72

src/modules/ekf2/CMakeLists.txt

@@ -0,0 +1,47 @@
+############################################################################
+#
+#   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# 1. Redistributions of source code must retain the above copyright
+#    notice, this list of conditions and the following disclaimer.
+# 2. Redistributions in binary form must reproduce the above copyright
+#    notice, this list of conditions and the following disclaimer in
+#    the documentation and/or other materials provided with the
+#    distribution.
+# 3. Neither the name PX4 nor the names of its contributors may be
+#    used to endorse or promote products derived from this software
+#    without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+# OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+# AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+#
+#############################################################################
+set(MODULE_CFLAGS)
+if (${OS} STREQUAL "nuttx")
+	list(APPEND MODULE_CFLAGS -Wframe-larger-than=30000)
+endif()
+px4_add_module(
+	MODULE modules__ekf2
+	MAIN ekf2
+	COMPILE_FLAGS ${MODULE_CFLAGS}
+	STACK 30000
+	SRCS
+		ekf2_main.cpp
+	DEPENDS
+		platforms__common
+	)
+# vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/ekf2/ekf2_main.cpp

@@ -0,0 +1,320 @@
+/****************************************************************************
+ *
+ *   Copyright (c) 2015 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file ekf2_main.cpp
+ * Implementation of the attitude and position estimator.
+ *
+ * @author Roman Bapst
+ */
+
+#include <px4_config.h>
+#include <px4_defines.h>
+#include <px4_tasks.h>
+#include <px4_posix.h>
+#include <px4_time.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <errno.h>
+#include <math.h>
+#include <poll.h>
+#include <time.h>
+#include <float.h>
+
+#include <arch/board/board.h>
+#include <systemlib/param/param.h>
+#include <systemlib/err.h>
+#include <systemlib/systemlib.h>
+#include <mathlib/mathlib.h>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
+#include <mavlink/mavlink_log.h>
+#include <platforms/px4_defines.h>
+#include <drivers/drv_hrt.h>
+
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_gps_position.h>
+#include <uORB/topics/airspeed.h>
+
+#include <ecl/EKF/estimator_base.h>
+
+
+extern "C" __EXPORT int ekf2_main(int argc, char *argv[]);
+
+
+class Ekf2;
+
+namespace ekf2
+{
+Ekf2 *instance;
+}
+
+
+class Ekf2
+{
+public:
+	/**
+	 * Constructor
+	 */
+	Ekf2();
+
+	/**
+	 * Destructor, also kills task.
+	 */
+	~Ekf2();
+
+	/**
+	 * Start task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+	static void	task_main_trampoline(int argc, char *argv[]);
+
+	void		task_main();
+
+	void print();
+
+private:
+	static constexpr float _dt_max = 0.02;
+	bool		_task_should_exit = false;		/**< if true, task should exit */
+	int		_control_task = -1;			/**< task handle for task */
+
+	int		_sensors_sub = -1;
+	int		_gps_sub = -1;
+	int		_airspeed_sub = -1;
+
+	EstimatorBase *_ekf;
+
+
+	void update_parameters(bool force);
+
+	int update_subscriptions();
+
+};
+
+Ekf2::Ekf2()
+{
+	_ekf = new EstimatorBase();
+}
+
+Ekf2::~Ekf2()
+{
+
+}
+
+void Ekf2::print()
+{
+	_ekf->printStoredGps();
+	_ekf->printStoredBaro();
+	_ekf->printStoredMag();
+	_ekf->printStoredIMU();
+}
+
+void Ekf2::task_main()
+{
+	// subscribe to relevant topics
+	_sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
+	_gps_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
+	_airspeed_sub = orb_subscribe(ORB_ID(airspeed));
+
+	px4_pollfd_struct_t fds[1];
+	fds[0].fd = _sensors_sub;
+	fds[0].events = POLLIN;
+
+	while (!_task_should_exit) {
+		int ret = px4_poll(fds, 1, 1000);
+
+		if (ret < 0) {
+			// Poll error, sleep and try again
+			usleep(10000);
+			continue;
+
+		} else if (ret == 0 || fds[0].revents != POLLIN) {
+			// Poll timeout or no new data, do nothing
+			continue;
+		}
+
+		bool gps_updated = false;
+		bool airspeed_updated = false;
+
+		sensor_combined_s sensors = {};
+		vehicle_gps_position_s gps = {};
+		airspeed_s airspeed = {};
+
+		orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors);
+
+		// update all other topics if they have new data
+		orb_check(_gps_sub, &gps_updated);
+
+		if (gps_updated) {
+			orb_copy(ORB_ID(vehicle_gps_position), _gps_sub, &gps);
+		}
+
+		orb_check(_airspeed_sub, &airspeed_updated);
+
+		if (airspeed_updated) {
+			orb_copy(ORB_ID(airspeed), _airspeed_sub, &airspeed);
+		}
+
+		hrt_abstime now = hrt_absolute_time();
+		// push imu data into estimator
+		_ekf->setIMUData(now, sensors.gyro_integral_dt[0], sensors.accelerometer_integral_dt[0],
+				 &sensors.gyro_integral_rad[0], &sensors.accelerometer_integral_m_s[0]);
+
+		// read mag data
+		_ekf->setMagData(sensors.magnetometer_timestamp[0], &sensors.magnetometer_ga[0]);
+
+		// read baro data
+		_ekf->setBaroData(sensors.baro_timestamp[0], &sensors.baro_alt_meter[0]);
+
+		// read gps data if available
+		if (gps_updated) {
+			struct gps_message gps_msg = {};
+			gps_msg.time_usec = gps.timestamp_position;
+			gps_msg.lat = gps.lat;
+			gps_msg.lon = gps.lon;
+			gps_msg.alt = gps.alt;
+			gps_msg.fix_type = gps.fix_type;
+			gps_msg.eph = gps.eph;
+			gps_msg.epv = gps.epv;
+			gps_msg.time_usec_vel = gps.timestamp_velocity;
+			gps_msg.vel_m_s = gps.vel_m_s;
+			gps_msg.vel_ned[0] = gps.vel_n_m_s;
+			gps_msg.vel_ned[1] = gps.vel_e_m_s;
+			gps_msg.vel_ned[2] = gps.vel_d_m_s;
+			gps_msg.vel_ned_valid = gps.vel_ned_valid;
+
+			_ekf->setGpsData(gps.timestamp_position, &gps_msg);
+		}
+
+		// read airspeed data if available
+		if (airspeed_updated) {
+			_ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s);
+		}
+	}
+}
+
+void Ekf2::task_main_trampoline(int argc, char *argv[])
+{
+	ekf2::instance->task_main();
+}
+
+int Ekf2::start()
+{
+	ASSERT(_control_task == -1);
+
+	/* start the task */
+	_control_task = px4_task_spawn_cmd("ekf2",
+					   SCHED_DEFAULT,
+					   SCHED_PRIORITY_MAX - 5,
+					   30000,
+					   (px4_main_t)&Ekf2::task_main_trampoline,
+					   nullptr);
+
+	if (_control_task < 0) {
+		PX4_WARN("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+int ekf2_main(int argc, char *argv[])
+{
+	if (argc < 1) {
+		PX4_WARN("usage: ekf2 {start|stop|status}");
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (ekf2::instance != nullptr) {
+			PX4_WARN("already running");
+			return 1;
+		}
+
+		ekf2::instance = new Ekf2();
+
+		if (ekf2::instance == nullptr) {
+			PX4_WARN("alloc failed");
+			return 1;
+		}
+
+		if (OK != ekf2::instance->start()) {
+			delete ekf2::instance;
+			ekf2::instance = nullptr;
+			PX4_WARN("start failed");
+			return 1;
+		}
+
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (ekf2::instance == nullptr) {
+			PX4_WARN("not running");
+			return 1;
+		}
+
+		delete ekf2::instance;
+		ekf2::instance = nullptr;
+		return 0;
+	}
+
+	if (!strcmp(argv[1], "print")) {
+		if (ekf2::instance != nullptr) {
+			ekf2::instance->print();
+			return 0;
+		}
+
+		return 1;
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (ekf2::instance) {
+			PX4_WARN("running");
+			return 0;
+
+		} else {
+			PX4_WARN("not running");
+			return 1;
+		}
+	}
+
+	PX4_WARN("unrecognized command");
+	return 1;
+}