Merge remote-tracking branch 'upstream/master' into linux

Signed-off-by: Mark Charlebois <charlebm@gmail.com>

Conflicts:
	src/modules/commander/accelerometer_calibration.cpp

ROMFS/px4fmu_common/init.d/rc.mc_apps

@@ -4,11 +4,21 @@
 # att & pos estimator, att & pos control.
 #
 
-# previously (2014) the system was relying on
+# The system is defaulting to INAV_ENABLED = 1
-# INAV, which defaults to 0 now. 
+# but users can alternatively try the EKF-based
+# filter by setting INAV_ENABLED = 0
 if param compare INAV_ENABLED 1
 then
-	attitude_estimator_ekf start
+	# The system is defaulting to EKF_ATT_ENABLED = 1
+	# and uses the older EKF filter. However users can
+	# enable the new quaternion based complimentary
+	# filter by setting EKF_ATT_ENABLED = 0.
+	if param compare EKF_ATT_ENABLED 1
+	then
+		attitude_estimator_ekf start
+	else
+		attitude_estimator_q start
+	fi
 	position_estimator_inav start
 else
 	ekf_att_pos_estimator start

makefiles/nuttx/config_px4fmu-v2_default.mk

@@ -77,6 +77,7 @@ MODULES 	+= modules/land_detector
 # Estimation modules (EKF/ SO3 / other filters)
 #
 MODULES		+= modules/attitude_estimator_ekf
+MODULES		+= modules/attitude_estimator_q
 MODULES		+= modules/ekf_att_pos_estimator
 MODULES		+= modules/position_estimator_inav
 

src/drivers/px4io/px4io.cpp

@@ -1743,20 +1743,14 @@ PX4IO::io_publish_pwm_outputs()
 	uint16_t ctl[_max_actuators];
 	int ret = io_reg_get(PX4IO_PAGE_SERVOS, 0, ctl, _max_actuators);
 
-	if (ret != OK){
+	if (ret != OK)
 		return ret;
-	}
-
-	unsigned maxouts = sizeof(outputs.output) / sizeof(outputs.output[0]);
-	unsigned actuator_max = (_max_actuators > maxouts) ? maxouts : _max_actuators;
-
 
 	/* convert from register format to float */
-	for (unsigned i = 0; i < actuator_max; i++){
+	for (unsigned i = 0; i < _max_actuators; i++)
 		outputs.output[i] = ctl[i];
-	}
 
-	outputs.noutputs = actuator_max;
+	outputs.noutputs = _max_actuators;
 
 	/* lazily advertise on first publication */
 	if (_to_outputs == 0) {
@@ -2087,13 +2081,13 @@ PX4IO::print_status(bool extended_status)
 		printf("vrssi %u\n", io_reg_get(PX4IO_PAGE_STATUS, PX4IO_P_STATUS_VRSSI));
 	}
 
-	printf("actuators (including S.BUS)");
+	printf("actuators");
 
 	for (unsigned i = 0; i < _max_actuators; i++)
 		printf(" %hi", int16_t(io_reg_get(PX4IO_PAGE_ACTUATORS, i)));
 
 	printf("\n");
-	printf("hardware servo ports");
+	printf("servos");
 
 	for (unsigned i = 0; i < _max_actuators; i++)
 		printf(" %u", io_reg_get(PX4IO_PAGE_SERVOS, i));

src/lib/mathlib/math/Matrix.hpp

@@ -135,6 +135,24 @@ public:
 	}
 #endif
 
+	/**
+	 * set row from vector
+	 */
+	void set_row(unsigned int row, const Vector<N> v) {
+		for (unsigned i = 0; i < N; i++) {
+			data[row][i] = v.data[i];
+		}
+	}
+
+	/**
+	 * set column from vector
+	 */
+	void set_col(unsigned int col, const Vector<M> v) {
+		for (unsigned i = 0; i < M; i++) {
+			data[i][col] = v.data[i];
+		}
+	}
+
 	/**
 	 * access by index
 	 */

src/lib/mathlib/math/Quaternion.hpp

@@ -93,6 +93,19 @@ public:
 			       data[0] * q.data[3] + data[1] * q.data[2] - data[2] * q.data[1] + data[3] * q.data[0]);
 	}
 
+	/**
+	 * division
+	 */
+	Quaternion operator /(const Quaternion &q) const {
+		float norm = q.length_squared();
+		return Quaternion(
+				(  data[0] * q.data[0] + data[1] * q.data[1] + data[2] * q.data[2] + data[3] * q.data[3]) / norm,
+			    (- data[0] * q.data[1] + data[1] * q.data[0] - data[2] * q.data[3] + data[3] * q.data[2]) / norm,
+			    (- data[0] * q.data[2] + data[1] * q.data[3] + data[2] * q.data[0] - data[3] * q.data[1]) / norm,
+			    (- data[0] * q.data[3] - data[1] * q.data[2] + data[2] * q.data[1] + data[3] * q.data[0]) / norm
+		);
+	}
+
 	/**
 	 * derivative
 	 */
@@ -108,6 +121,69 @@ public:
 		return Q * v * 0.5f;
 	}
 
+	/**
+	 * conjugate
+	 */
+	Quaternion conjugated() const {
+		return Quaternion(data[0], -data[1], -data[2], -data[3]);
+	}
+
+	/**
+	 * inversed
+	 */
+	Quaternion inversed() const {
+		float norm = length_squared();
+		return Quaternion(data[0] / norm, -data[1] / norm, -data[2] / norm, -data[3] / norm);
+	}
+
+	/**
+	 * conjugation
+	 */
+	Vector<3> conjugate(const Vector<3> &v) const {
+		float q0q0 = data[0] * data[0];
+		float q1q1 = data[1] * data[1];
+		float q2q2 = data[2] * data[2];
+		float q3q3 = data[3] * data[3];
+
+		return Vector<3>(
+				v.data[0] * (q0q0 + q1q1 - q2q2 - q3q3) +
+				v.data[1] * 2.0f * (data[1] * data[2] - data[0] * data[3]) +
+				v.data[2] * 2.0f * (data[0] * data[2] + data[1] * data[3]),
+
+				v.data[0] * 2.0f * (data[1] * data[2] + data[0] * data[3]) +
+				v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) +
+				v.data[2] * 2.0f * (data[2] * data[3] - data[0] * data[1]),
+
+				v.data[0] * 2.0f * (data[1] * data[3] - data[0] * data[2]) +
+				v.data[1] * 2.0f * (data[0] * data[1] + data[2] * data[3]) +
+				v.data[2] * (q0q0 - q1q1 - q2q2 + q3q3)
+		);
+	}
+
+	/**
+	 * conjugation with inversed quaternion
+	 */
+	Vector<3> conjugate_inversed(const Vector<3> &v) const {
+		float q0q0 = data[0] * data[0];
+		float q1q1 = data[1] * data[1];
+		float q2q2 = data[2] * data[2];
+		float q3q3 = data[3] * data[3];
+
+		return Vector<3>(
+				v.data[0] * (q0q0 + q1q1 - q2q2 - q3q3) +
+				v.data[1] * 2.0f * (data[1] * data[2] + data[0] * data[3]) +
+				v.data[2] * 2.0f * (data[1] * data[3] - data[0] * data[2]),
+
+				v.data[0] * 2.0f * (data[1] * data[2] - data[0] * data[3]) +
+				v.data[1] * (q0q0 - q1q1 + q2q2 - q3q3) +
+				v.data[2] * 2.0f * (data[2] * data[3] + data[0] * data[1]),
+
+				v.data[0] * 2.0f * (data[1] * data[3] + data[0] * data[2]) +
+				v.data[1] * 2.0f * (data[2] * data[3] - data[0] * data[1]) +
+				v.data[2] * (q0q0 - q1q1 - q2q2 + q3q3)
+		);
+	}
+
 	/**
 	 * imaginary part of quaternion
 	 */
@@ -115,35 +191,6 @@ public:
 		return Vector<3>(&data[1]);
 	}
 
-	/**
-	 * inverse of quaternion
-	 */
-	math::Quaternion inverse() {
-		Quaternion res;
-		memcpy(res.data,data,sizeof(res.data));
-		res.data[1] = -res.data[1];
-		res.data[2] = -res.data[2];
-		res.data[3] = -res.data[3];
-		return res;
-	}
-
-
-	/**
-	* rotate vector by quaternion
-	*/
-	Vector<3> rotate(const Vector<3> &w) {
-		Quaternion q_w;	// extend vector to quaternion
-		Quaternion q = {data[0],data[1],data[2],data[3]};
-		Quaternion q_rotated;	// quaternion representation of rotated vector
-		q_w(0) = 0;
-		q_w(1) = w.data[0];
-		q_w(2) = w.data[1];
-		q_w(3) = w.data[2];
-		q_rotated = q*q_w*q.inverse();
-		Vector<3> res = {q_rotated.data[1],q_rotated.data[2],q_rotated.data[3]};
-		return res;
-	}
-
 	/**
 	 * set quaternion to rotation defined by euler angles
 	 */
@@ -164,6 +211,17 @@ public:
 		data[3] = static_cast<float>(cosPhi_2 * cosTheta_2 * sinPsi_2 - sinPhi_2 * sinTheta_2 * cosPsi_2);
 	}
 
+	/**
+	 * create Euler angles vector from the quaternion
+	 */
+	Vector<3> to_euler() const {
+		return Vector<3>(
+			atan2f(2.0f * (data[0] * data[1] + data[2] * data[3]), 1.0f - 2.0f * (data[1] * data[1] + data[2] * data[2])),
+			asinf(2.0f * (data[0] * data[2] - data[3] * data[1])),
+			atan2f(2.0f * (data[0] * data[3] + data[1] * data[2]), 1.0f - 2.0f * (data[2] * data[2] + data[3] * data[3]))
+		);
+	}
+
 	/**
 	 * set quaternion to rotation by DCM
 	 * Reference: Shoemake, Quaternions, http://www.cs.ucr.edu/~vbz/resources/quatut.pdf

src/modules/attitude_estimator_ekf/attitude_estimator_ekf_params.c

@@ -95,6 +95,19 @@ PARAM_DEFINE_FLOAT(EKF_ATT_V4_R1, 10000.0f);
  */
 PARAM_DEFINE_FLOAT(EKF_ATT_V4_R2, 100.0f);
 
+/**
+ * EKF attitude estimator enabled
+ *
+ * If enabled, it uses the older EKF filter.
+ * However users can enable the new quaternion
+ * based complimentary filter by setting EKF_ATT_ENABLED = 0.
+ *
+ * @min 0
+ * @max 1
+ * @group Attitude EKF estimator
+ */
+PARAM_DEFINE_INT32(EKF_ATT_ENABLED, 1);
+
 /* magnetic declination, in degrees */
 PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);
 

src/modules/attitude_estimator_q/attitude_estimator_q_main.cpp

@@ -0,0 +1,479 @@
+/****************************************************************************
+ *
+ *   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 attitude_estimator_q_main.cpp
+ *
+ * Attitude estimator (quaternion based)
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <nuttx/config.h>
+#include <unistd.h>
+#include <stdlib.h>
+#include <stdio.h>
+#include <stdbool.h>
+#include <poll.h>
+#include <fcntl.h>
+#include <float.h>
+#include <nuttx/sched.h>
+#include <sys/prctl.h>
+#include <termios.h>
+#include <errno.h>
+#include <limits.h>
+#include <math.h>
+#include <uORB/uORB.h>
+#include <uORB/topics/debug_key_value.h>
+#include <uORB/topics/sensor_combined.h>
+#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/vehicle_control_mode.h>
+#include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/parameter_update.h>
+#include <drivers/drv_hrt.h>
+
+#include <lib/mathlib/mathlib.h>
+#include <lib/geo/geo.h>
+
+#include <systemlib/systemlib.h>
+#include <systemlib/param/param.h>
+#include <systemlib/perf_counter.h>
+#include <systemlib/err.h>
+
+extern "C" __EXPORT int attitude_estimator_q_main(int argc, char *argv[]);
+
+using math::Vector;
+using math::Matrix;
+using math::Quaternion;
+
+class AttitudeEstimatorQ;
+
+namespace attitude_estimator_q {
+AttitudeEstimatorQ *instance;
+}
+
+
+class AttitudeEstimatorQ {
+public:
+	/**
+	 * Constructor
+	 */
+	AttitudeEstimatorQ();
+
+	/**
+	 * Destructor, also kills task.
+	 */
+	~AttitudeEstimatorQ();
+
+	/**
+	 * Start task.
+	 *
+	 * @return		OK on success.
+	 */
+	int		start();
+
+	static void	task_main_trampoline(int argc, char *argv[]);
+
+	void		task_main();
+
+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		_params_sub = -1;
+	int		_global_pos_sub = -1;
+	orb_advert_t		_att_pub = -1;
+
+	struct {
+		param_t	w_acc;
+		param_t	w_mag;
+		param_t	w_gyro_bias;
+		param_t	mag_decl;
+		param_t	mag_decl_auto;
+		param_t	acc_comp;
+		param_t	bias_max;
+	}		_params_handles;		/**< handles for interesting parameters */
+
+	float		_w_accel = 0.0f;
+	float		_w_mag = 0.0f;
+	float		_w_gyro_bias = 0.0f;
+	float		_mag_decl = 0.0f;
+	bool		_mag_decl_auto = false;
+	bool		_acc_comp = false;
+	float		_bias_max = 0.0f;
+
+	Vector<3>	_gyro;
+	Vector<3>	_accel;
+	Vector<3>	_mag;
+
+	Quaternion	_q;
+	Vector<3>	_rates;
+	Vector<3>	_gyro_bias;
+
+	vehicle_global_position_s _gpos = {};
+	Vector<3>	_vel_prev;
+	Vector<3>	_pos_acc;
+	hrt_abstime _vel_prev_t = 0;
+
+	bool		_inited = false;
+
+	perf_counter_t _update_perf;
+	perf_counter_t _loop_perf;
+
+	void update_parameters(bool force);
+
+	int update_subscriptions();
+
+	void init();
+
+	void update(float dt);
+};
+
+
+AttitudeEstimatorQ::AttitudeEstimatorQ() {
+	_params_handles.w_acc		= param_find("ATT_W_ACC");
+	_params_handles.w_mag		= param_find("ATT_W_MAG");
+	_params_handles.w_gyro_bias	= param_find("ATT_W_GYRO_BIAS");
+	_params_handles.mag_decl	= param_find("ATT_MAG_DECL");
+	_params_handles.mag_decl_auto	= param_find("ATT_MAG_DECL_A");
+	_params_handles.acc_comp	= param_find("ATT_ACC_COMP");
+	_params_handles.bias_max	= param_find("ATT_BIAS_MAX");
+}
+
+/**
+ * Destructor, also kills task.
+ */
+AttitudeEstimatorQ::~AttitudeEstimatorQ() {
+	if (_control_task != -1) {
+		/* task wakes up every 100ms or so at the longest */
+		_task_should_exit = true;
+
+		/* wait for a second for the task to quit at our request */
+		unsigned i = 0;
+
+		do {
+			/* wait 20ms */
+			usleep(20000);
+
+			/* if we have given up, kill it */
+			if (++i > 50) {
+				task_delete(_control_task);
+				break;
+			}
+		} while (_control_task != -1);
+	}
+
+	attitude_estimator_q::instance = nullptr;
+}
+
+int AttitudeEstimatorQ::start() {
+	ASSERT(_control_task == -1);
+
+	/* start the task */
+	_control_task = task_spawn_cmd("attitude_estimator_q",
+				       SCHED_DEFAULT,
+				       SCHED_PRIORITY_MAX - 5,
+				       2500,
+				       (main_t)&AttitudeEstimatorQ::task_main_trampoline,
+				       nullptr);
+
+	if (_control_task < 0) {
+		warn("task start failed");
+		return -errno;
+	}
+
+	return OK;
+}
+
+void AttitudeEstimatorQ::task_main_trampoline(int argc, char *argv[]) {
+	attitude_estimator_q::instance->task_main();
+}
+
+void AttitudeEstimatorQ::task_main() {
+	warnx("started");
+
+    _sensors_sub = orb_subscribe(ORB_ID(sensor_combined));
+	_params_sub = orb_subscribe(ORB_ID(parameter_update));
+	_global_pos_sub = orb_subscribe(ORB_ID(vehicle_global_position));
+
+	update_parameters(true);
+
+	hrt_abstime last_time = 0;
+
+	struct pollfd fds[1];
+	fds[0].fd = _sensors_sub;
+	fds[0].events = POLLIN;
+
+	while (!_task_should_exit) {
+		int ret = poll(fds, 1, 1000);
+
+		if (ret < 0) {
+			// Poll error, sleep and try again
+			usleep(10000);
+			continue;
+		} else if (ret == 0) {
+			// Poll timeout, do nothing
+			continue;
+		}
+
+		update_parameters(false);
+
+		// Update sensors
+		sensor_combined_s sensors;
+		if (!orb_copy(ORB_ID(sensor_combined), _sensors_sub, &sensors)) {
+			_gyro.set(sensors.gyro_rad_s);
+			_accel.set(sensors.accelerometer_m_s2);
+			_mag.set(sensors.magnetometer_ga);
+		}
+
+		bool gpos_updated;
+		orb_check(_global_pos_sub, &gpos_updated);
+		if (gpos_updated) {
+			orb_copy(ORB_ID(vehicle_global_position), _global_pos_sub, &_gpos);
+			if (_mag_decl_auto && _gpos.eph < 20.0f && hrt_elapsed_time(&_gpos.timestamp) < 1000000) {
+				/* set magnetic declination automatically */
+				_mag_decl = math::radians(get_mag_declination(_gpos.lat, _gpos.lon));
+			}
+		}
+
+		if (_acc_comp && _gpos.timestamp != 0 && hrt_absolute_time() < _gpos.timestamp + 20000 && _gpos.eph < 5.0f) {
+			/* position data is actual */
+			if (gpos_updated) {
+				Vector<3> vel(_gpos.vel_n, _gpos.vel_e, _gpos.vel_d);
+
+				/* velocity updated */
+				if (_vel_prev_t != 0 && _gpos.timestamp != _vel_prev_t) {
+					float vel_dt = (_gpos.timestamp - _vel_prev_t) / 1000000.0f;
+					/* calculate acceleration in body frame */
+					_pos_acc = _q.conjugate_inversed((vel - _vel_prev) / vel_dt);
+				}
+				_vel_prev_t = _gpos.timestamp;
+				_vel_prev = vel;
+			}
+
+		} else {
+			/* position data is outdated, reset acceleration */
+			_pos_acc.zero();
+			_vel_prev.zero();
+			_vel_prev_t = 0;
+		}
+
+		// Time from previous iteration
+		uint64_t now = hrt_absolute_time();
+		float dt = (last_time > 0) ? ((now  - last_time) / 1000000.0f) : 0.0f;
+		last_time = now;
+
+		if (dt > _dt_max) {
+			dt = _dt_max;
+		}
+
+		update(dt);
+
+		Vector<3> euler = _q.to_euler();
+
+		struct vehicle_attitude_s att = {};
+		att.timestamp = sensors.timestamp;
+
+		att.roll = euler(0);
+		att.pitch = euler(1);
+		att.yaw = euler(2);
+
+		att.rollspeed = _rates(0);
+		att.pitchspeed = _rates(1);
+		att.yawspeed = _rates(2);
+
+		for (int i = 0; i < 3; i++) {
+			att.g_comp[i] = _accel(i) - _pos_acc(i);
+		}
+
+		/* copy offsets */
+		memcpy(&att.rate_offsets, _gyro_bias.data, sizeof(att.rate_offsets));
+
+		Matrix<3, 3> R = _q.to_dcm();
+
+		/* copy rotation matrix */
+		memcpy(&att.R[0], R.data, sizeof(att.R));
+		att.R_valid = true;
+
+		if (_att_pub < 0) {
+			_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
+		} else {
+			orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
+		}
+	}
+}
+
+void AttitudeEstimatorQ::update_parameters(bool force) {
+	bool updated = force;
+	if (!updated) {
+		orb_check(_params_sub, &updated);
+	}
+	if (updated) {
+		parameter_update_s param_update;
+		orb_copy(ORB_ID(parameter_update), _params_sub, &param_update);
+
+		param_get(_params_handles.w_acc, &_w_accel);
+		param_get(_params_handles.w_mag, &_w_mag);
+		param_get(_params_handles.w_gyro_bias, &_w_gyro_bias);
+		float mag_decl_deg = 0.0f;
+		param_get(_params_handles.mag_decl, &mag_decl_deg);
+		_mag_decl = math::radians(mag_decl_deg);
+		int32_t mag_decl_auto_int;
+		param_get(_params_handles.mag_decl_auto, &mag_decl_auto_int);
+		_mag_decl_auto = mag_decl_auto_int != 0;
+		int32_t acc_comp_int;
+		param_get(_params_handles.acc_comp, &acc_comp_int);
+		_acc_comp = acc_comp_int != 0;
+		param_get(_params_handles.bias_max, &_bias_max);
+	}
+}
+
+void AttitudeEstimatorQ::init() {
+	// Rotation matrix can be easily constructed from acceleration and mag field vectors
+	// 'k' is Earth Z axis (Down) unit vector in body frame
+	Vector<3> k = -_accel;
+	k.normalize();
+
+	// 'i' is Earth X axis (North) unit vector in body frame, orthogonal with 'k'
+	Vector<3> i = (_mag - k * (_mag * k));
+	i.normalize();
+
+	// 'j' is Earth Y axis (East) unit vector in body frame, orthogonal with 'k' and 'i'
+	Vector<3> j = k % i;
+
+	// Fill rotation matrix
+	Matrix<3, 3> R;
+	R.set_row(0, i);
+	R.set_row(1, j);
+	R.set_row(2, k);
+
+	// Convert to quaternion
+	_q.from_dcm(R);
+}
+
+void AttitudeEstimatorQ::update(float dt) {
+	if (!_inited) {
+		init();
+		_inited = true;
+	}
+
+	// Angular rate of correction
+	Vector<3> corr;
+
+	// Magnetometer correction
+	// Project mag field vector to global frame and extract XY component
+	Vector<3> mag_earth = _q.conjugate(_mag);
+	float mag_err = _wrap_pi(atan2f(mag_earth(1), mag_earth(0)) - _mag_decl);
+	// Project magnetometer correction to body frame
+	corr += _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, -mag_err)) * _w_mag;
+
+	// Accelerometer correction
+	// Project 'k' unit vector of earth frame to body frame
+	// Vector<3> k = _q.conjugate_inversed(Vector<3>(0.0f, 0.0f, 1.0f));
+	// Optimized version with dropped zeros
+	Vector<3> k(
+			2.0f * (_q(1) * _q(3) - _q(0) * _q(2)),
+			2.0f * (_q(2) * _q(3) + _q(0) * _q(1)),
+			(_q(0) * _q(0) - _q(1) * _q(1) - _q(2) * _q(2) + _q(3) * _q(3))
+	);
+
+	corr += (k % (_accel - _pos_acc).normalized()) * _w_accel;
+
+	// Gyro bias estimation
+	_gyro_bias += corr * (_w_gyro_bias * dt);
+	for (int i = 0; i < 3; i++) {
+		_gyro_bias(i) = math::constrain(_gyro_bias(i), -_bias_max, _bias_max);
+	}
+	_rates = _gyro + _gyro_bias;
+
+	// Feed forward gyro
+	corr += _rates;
+
+	// Apply correction to state
+	_q += _q.derivative(corr) * dt;
+
+	// Normalize quaternion
+	_q.normalize(); // TODO! NaN protection???
+}
+
+
+int attitude_estimator_q_main(int argc, char *argv[]) {
+	if (argc < 1) {
+		errx(1, "usage: attitude_estimator_q {start|stop|status}");
+	}
+
+	if (!strcmp(argv[1], "start")) {
+
+		if (attitude_estimator_q::instance != nullptr) {
+			errx(1, "already running");
+		}
+
+		attitude_estimator_q::instance = new AttitudeEstimatorQ;
+
+		if (attitude_estimator_q::instance == nullptr) {
+			errx(1, "alloc failed");
+		}
+
+		if (OK != attitude_estimator_q::instance->start()) {
+			delete attitude_estimator_q::instance;
+			attitude_estimator_q::instance = nullptr;
+			err(1, "start failed");
+		}
+
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "stop")) {
+		if (attitude_estimator_q::instance == nullptr) {
+			errx(1, "not running");
+		}
+
+		delete attitude_estimator_q::instance;
+		attitude_estimator_q::instance = nullptr;
+		exit(0);
+	}
+
+	if (!strcmp(argv[1], "status")) {
+		if (attitude_estimator_q::instance) {
+			errx(0, "running");
+
+		} else {
+			errx(1, "not running");
+		}
+	}
+
+	warnx("unrecognized command");
+	return 1;
+}

src/modules/attitude_estimator_q/attitude_estimator_q_params.c

@@ -0,0 +1,50 @@
+/****************************************************************************
+ *
+ *   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 attitude_estimator_q_params.c
+ *
+ * Parameters for attitude estimator (quaternion based)
+ *
+ * @author Anton Babushkin <anton.babushkin@me.com>
+ */
+
+#include <systemlib/param/param.h>
+
+PARAM_DEFINE_FLOAT(ATT_W_ACC, 0.2f);
+PARAM_DEFINE_FLOAT(ATT_W_MAG, 0.1f);
+PARAM_DEFINE_FLOAT(ATT_W_GYRO_BIAS, 0.1f);
+PARAM_DEFINE_FLOAT(ATT_MAG_DECL, 0.0f);		///< magnetic declination, in degrees
+PARAM_DEFINE_INT32(ATT_MAG_DECL_A, 1);		///< automatic GPS based magnetic declination
+PARAM_DEFINE_INT32(ATT_ACC_COMP, 2);		///< acceleration compensation
+PARAM_DEFINE_FLOAT(ATT_BIAS_MAX, 0.05f);		///< gyro bias limit, rad/s

src/modules/attitude_estimator_q/module.mk

@@ -0,0 +1,43 @@
+############################################################################
+#
+#   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.
+#
+############################################################################
+
+#
+# Attitude estimator (quaternion based)
+#
+
+MODULE_COMMAND	 = attitude_estimator_q
+
+SRCS		 = attitude_estimator_q_main.cpp \
+		   attitude_estimator_q_params.c
+
+MODULE_STACKSIZE = 1200

src/modules/commander/accelerometer_calibration.cpp

@@ -409,7 +409,33 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
  */
 calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&accel_avg)[max_accel_sens][detect_orientation_side_count][3], unsigned orient, unsigned samples_num)
 {
+	/* get total sensor board rotation matrix */
+	param_t board_rotation_h = param_find("SENS_BOARD_ROT");
+	param_t board_offset_x = param_find("SENS_BOARD_X_OFF");
+	param_t board_offset_y = param_find("SENS_BOARD_Y_OFF");
+	param_t board_offset_z = param_find("SENS_BOARD_Z_OFF");
+
+	float board_offset[3];
+	param_get(board_offset_x, &board_offset[0]);
+	param_get(board_offset_y, &board_offset[1]);
+	param_get(board_offset_z, &board_offset[2]);
+
+	math::Matrix<3, 3> board_rotation_offset;
+	board_rotation_offset.from_euler(M_DEG_TO_RAD_F * board_offset[0],
+			M_DEG_TO_RAD_F * board_offset[1],
+			M_DEG_TO_RAD_F * board_offset[2]);
+
+	int32_t board_rotation_int;
+	param_get(board_rotation_h, &(board_rotation_int));
+	enum Rotation board_rotation_id = (enum Rotation)board_rotation_int;
+	math::Matrix<3, 3> board_rotation;
+	get_rot_matrix(board_rotation_id, &board_rotation);
+
+	/* combine board rotation with offset rotation */
+	board_rotation = board_rotation_offset * board_rotation;
+
 	px4_pollfd_struct_t fds[max_accel_sens];
+	struct pollfd fds[max_accel_sens];
 
 	for (unsigned i = 0; i < max_accel_sens; i++) {
 		fds[i].fd = subs[i];
@@ -455,6 +481,13 @@ calibrate_return read_accelerometer_avg(int (&subs)[max_accel_sens], float (&acc
 		}
 	}
 
+	// rotate sensor measurements from body frame into sensor frame using board rotation matrix
+	for (unsigned i = 0; i < max_accel_sens; i++) {
+		math::Vector<3> accel_sum_vec(&accel_sum[i][0]);
+		accel_sum_vec = board_rotation * accel_sum_vec;
+		memcpy(&accel_sum[i][0], &accel_sum_vec.data[0], sizeof(accel_sum[i]));
+	}
+
 	for (unsigned s = 0; s < max_accel_sens; s++) {
 		for (unsigned i = 0; i < 3; i++) {
 			accel_avg[s][orient][i] = accel_sum[s][i] / counts[s];

src/modules/commander/commander_params.c

@@ -111,7 +111,8 @@ PARAM_DEFINE_FLOAT(BAT_CAPACITY, -1.0f);
  */
 PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);
 
- /** Datalink loss time threshold
+/**
+ * Datalink loss time threshold
  *
  * After this amount of seconds without datalink the data link lost mode triggers
  *
@@ -122,7 +123,8 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_EN, 0);
  */
 PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10);
 
-/** Datalink regain time threshold
+/**
+ * Datalink regain time threshold
  *
  * After a data link loss: after this this amount of seconds with a healthy datalink the 'datalink loss'
  * flag is set back to false
@@ -134,7 +136,8 @@ PARAM_DEFINE_INT32(COM_DL_LOSS_T, 10);
  */
 PARAM_DEFINE_INT32(COM_DL_REG_T, 0);
 
-/** Engine Failure Throttle Threshold
+/**
+ * Engine Failure Throttle Threshold
  *
  * Engine failure triggers only above this throttle value
  *
@@ -144,7 +147,8 @@ PARAM_DEFINE_INT32(COM_DL_REG_T, 0);
  */
 PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
 
-/** Engine Failure Current/Throttle Threshold
+/**
+ * Engine Failure Current/Throttle Threshold
  *
  * Engine failure triggers only below this current/throttle value
  *
@@ -154,7 +158,8 @@ PARAM_DEFINE_FLOAT(COM_EF_THROT, 0.5f);
  */
 PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
 
-/** Engine Failure Time Threshold
+/**
+ * Engine Failure Time Threshold
  *
  * Engine failure triggers only if the throttle threshold and the
  * current to throttle threshold are violated for this time
@@ -166,7 +171,8 @@ PARAM_DEFINE_FLOAT(COM_EF_C2T, 5.0f);
  */
 PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
 
-/** RC loss time threshold
+/**
+ * RC loss time threshold
  *
  * After this amount of seconds without RC connection the rc lost flag is set to true
  *
@@ -177,7 +183,8 @@ PARAM_DEFINE_FLOAT(COM_EF_TIME, 10.0f);
  */
 PARAM_DEFINE_FLOAT(COM_RC_LOSS_T, 0.5);
 
-/** Autosaving of params
+/**
+ * Autosaving of params
  *
  * If not equal to zero the commander will automatically save parameters to persistent storage once changed.
  * Default is on, as the interoperability with currently deployed GCS solutions depends on parameters

src/modules/mavlink/mavlink.c

@@ -50,18 +50,25 @@
 /**
  * MAVLink system ID
  * @group MAVLink
+ * @min 1
+ * @max 250
  */
 PARAM_DEFINE_INT32(MAV_SYS_ID, 1);
+
 /**
  * MAVLink component ID
  * @group MAVLink
+ * @min 1
+ * @max 50
  */
 PARAM_DEFINE_INT32(MAV_COMP_ID, 50);
+
 /**
  * MAVLink type
  * @group MAVLink
  */
 PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
+
 /**
  * Use/Accept HIL GPS message (even if not in HIL mode)
  *
@@ -70,6 +77,7 @@ PARAM_DEFINE_INT32(MAV_TYPE, MAV_TYPE_FIXED_WING);
  * @group MAVLink
  */
 PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
+
 /**
  * Forward external setpoint messages
  *
@@ -80,6 +88,18 @@ PARAM_DEFINE_INT32(MAV_USEHILGPS, 0);
  */
 PARAM_DEFINE_INT32(MAV_FWDEXTSP, 1);
 
+/**
+ * Test parameter
+ *
+ * This parameter is not actively used by the system. Its purpose is to allow
+ * testing the parameter interface on the communication level.
+ *
+ * @group MAVLink
+ * @min -1000
+ * @max 1000
+ */
+PARAM_DEFINE_INT32(MAV_TEST_PAR, 1);
+
 mavlink_system_t mavlink_system = {
 	100,
 	50

src/modules/mavlink/mavlink_main.cpp

@@ -506,6 +506,9 @@ void Mavlink::mavlink_update_system(void)
 		_param_system_type = param_find("MAV_TYPE");
 		_param_use_hil_gps = param_find("MAV_USEHILGPS");
 		_param_forward_externalsp = param_find("MAV_FWDEXTSP");
+
+		/* test param - needs to be referenced, but is unused */
+		(void)param_find("MAV_TEST_PAR");
 	}
 
 	/* update system and component id */

src/modules/position_estimator_inav/position_estimator_inav_params.c

@@ -291,8 +291,8 @@ PARAM_DEFINE_INT32(CBRK_NO_VISION, 0);
 /**
  * INAV enabled
  *
- * If set to 1, use INAV for position estimation
+ * If set to 1, use INAV for position estimation.
- * the system uses the combined attitude / position
+ * Else the system uses the combined attitude / position
  * filter framework.
  *
  * @min 0

src/modules/px4iofirmware/mixer.cpp

@@ -272,9 +272,8 @@ mixer_tick(void)
 
 	if (mixer_servos_armed && should_arm) {
 		/* update the servo outputs. */
-		for (unsigned i = 0; i < PX4IO_SERVO_HARDWARE_COUNT; i++) {
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
 			up_pwm_servo_set(i, r_page_servos[i]);
-		}
 
 		/* set S.BUS1 or S.BUS2 outputs */
 
@@ -286,9 +285,8 @@ mixer_tick(void)
 
 	} else if (mixer_servos_armed && should_always_enable_pwm) {
 		/* set the disarmed servo outputs. */
-		for (unsigned i = 0; i < PX4IO_SERVO_HARDWARE_COUNT; i++) {
+		for (unsigned i = 0; i < PX4IO_SERVO_COUNT; i++)
 			up_pwm_servo_set(i, r_page_servo_disarmed[i]);
-		}
 
 		/* set S.BUS1 or S.BUS2 outputs */
 		if (r_setup_features & PX4IO_P_SETUP_FEATURES_SBUS1_OUT)

src/modules/px4iofirmware/px4io.h

@@ -51,8 +51,7 @@
 /*
  * Constants and limits.
  */
-#define PX4IO_SERVO_COUNT		16
+#define PX4IO_SERVO_COUNT		8
-#define PX4IO_SERVO_HARDWARE_COUNT	8
 #define PX4IO_CONTROL_CHANNELS		8
 #define PX4IO_CONTROL_GROUPS		4
 #define PX4IO_RC_INPUT_CHANNELS		18

src/modules/px4iofirmware/registers.c

@@ -285,7 +285,7 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 	case PX4IO_PAGE_DIRECT_PWM:
 
 		/* copy channel data */
-		while ((offset < PX4IO_SERVO_COUNT) && (num_values > 0)) {
+		while ((offset < PX4IO_CONTROL_CHANNELS) && (num_values > 0)) {
 
 			/* XXX range-check value? */
 			if (*values != PWM_IGNORE_THIS_CHANNEL) {

src/modules/px4iofirmware/sbus.c

@@ -163,8 +163,8 @@ sbus1_output(uint16_t *values, uint16_t num_values)
 void
 sbus2_output(uint16_t *values, uint16_t num_values)
 {
-	// XXX S.BUS2 is not implemented, fall back to S.BUS1
+	char b = 'B';
-	sbus1_output(values, num_values);
+	write(sbus_fd, &b, 1);
 }
 
 bool

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -822,7 +822,8 @@ void VtolAttitudeControl::task_main()
 
 				/* Only publish if the proper mode(s) are enabled */
 				if(_v_control_mode.flag_control_attitude_enabled ||
-				   _v_control_mode.flag_control_rates_enabled)
+				   _v_control_mode.flag_control_rates_enabled ||
+				   _v_control_mode.flag_control_manual_enabled)
 				{
 					if (_actuators_0_pub > 0) {
 						orb_publish(ORB_ID(actuator_controls_0), _actuators_0_pub, &_actuators_out_0);

src/systemcmds/tests/test_mathlib.cpp

@@ -300,7 +300,7 @@ int test_mathlib(int argc, char *argv[])
 					R.from_euler(roll, pitch, yaw);
 					q.from_euler(roll, pitch, yaw);
 					vector_r = R * vector;
-					vector_q = q.rotate(vector);
+					vector_q = q.conjugate(vector);
 
 					for (int i = 0; i < 3; i++) {
 						if (fabsf(vector_r(i) - vector_q(i)) > tol) {
@@ -315,7 +315,7 @@ int test_mathlib(int argc, char *argv[])
 		// test some values calculated with matlab
 		tol = 0.0001f;
 		q.from_euler(M_PI_2_F, 0.0f, 0.0f);
-		vector_q = q.rotate(vector);
+		vector_q = q.conjugate(vector);
 		Vector<3> vector_true = {1.00f, -1.00f, 1.00f};
 
 		for (unsigned i = 0; i < 3; i++) {
@@ -326,7 +326,7 @@ int test_mathlib(int argc, char *argv[])
 		}
 
 		q.from_euler(0.3f, 0.2f, 0.1f);
-		vector_q = q.rotate(vector);
+		vector_q = q.conjugate(vector);
 		vector_true = {1.1566, 0.7792, 1.0273};
 
 		for (unsigned i = 0; i < 3; i++) {
@@ -337,7 +337,7 @@ int test_mathlib(int argc, char *argv[])
 		}
 
 		q.from_euler(-1.5f, -0.2f, 0.5f);
-		vector_q = q.rotate(vector);
+		vector_q = q.conjugate(vector);
 		vector_true = {0.5095, 1.4956, -0.7096};
 
 		for (unsigned i = 0; i < 3; i++) {
@@ -348,7 +348,7 @@ int test_mathlib(int argc, char *argv[])
 		}
 
 		q.from_euler(M_PI_2_F, -M_PI_2_F, -M_PI_F / 3.0f);
-		vector_q = q.rotate(vector);
+		vector_q = q.conjugate(vector);
 		vector_true = { -1.3660, 0.3660, 1.0000};
 
 		for (unsigned i = 0; i < 3; i++) {
@@ -359,4 +359,4 @@ int test_mathlib(int argc, char *argv[])
 		}
 	}
 	return rc;
-}
+}