Merged master

src/modules/commander/PreflightCheck.cpp

@@ -71,7 +71,50 @@
 
 namespace Commander
 {
-static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
+
+static int check_calibration(int fd, const char* param_template, int &devid);
+
+int check_calibration(int fd, const char* param_template, int &devid)
+{
+	bool calibration_found;
+
+	/* new style: ask device for calibration state */
+	int ret = px4_ioctl(fd, SENSORIOCCALTEST, 0);
+
+	calibration_found = (ret == OK);
+	
+	devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
+
+	char s[20];
+	int instance = 0;
+
+	/* old style transition: check param values */
+	while (!calibration_found) {
+		sprintf(s, param_template, instance);
+		param_t parm = param_find(s);
+
+		/* if the calibration param is not present, abort */
+		if (parm == PARAM_INVALID) {
+			break;
+		}
+
+		/* if param get succeeds */
+		int calibration_devid;
+		if (!param_get(parm, &(calibration_devid))) {
+
+			/* if the devid matches, exit early */
+			if (devid == calibration_devid) {
+				calibration_found = true;
+				break;
+			}
+		}
+		instance++;
+	}
+
+	return !calibration_found;
+}
+
+static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
 {
 	bool success = true;
 
@@ -88,13 +131,9 @@ static bool magnometerCheck(int mavlink_fd, unsigned instance, bool optional)
 		return false;
 	}
 
-	int calibration_devid;
-	int ret;
-	int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
-	sprintf(s, "CAL_MAG%u_ID", instance);
-	param_get(param_find(s), &(calibration_devid));
+	int ret = check_calibration(fd, "CAL_MAG%u_ID", device_id);
 
-	if (devid != calibration_devid) {
+	if (ret) {
 		mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: MAG #%u UNCALIBRATED", instance);
 		success = false;
@@ -115,7 +154,7 @@ out:
 	return success;
 }
 
-static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic)
+static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional, bool dynamic, int &device_id)
 {
 	bool success = true;
 
@@ -132,13 +171,9 @@ static bool accelerometerCheck(int mavlink_fd, unsigned instance, bool optional,
 		return false;
 	}
 
-	int calibration_devid;
-	int ret;
-	int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
-	sprintf(s, "CAL_ACC%u_ID", instance);
-	param_get(param_find(s), &(calibration_devid));
+	int ret = check_calibration(fd, "CAL_ACC%u_ID", device_id);
 
-	if (devid != calibration_devid) {
+	if (ret) {
 		mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: ACCEL #%u UNCALIBRATED", instance);
 		success = false;
@@ -184,7 +219,7 @@ out:
 	return success;
 }
 
-static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
+static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
 {
 	bool success = true;
 
@@ -201,13 +236,9 @@ static bool gyroCheck(int mavlink_fd, unsigned instance, bool optional)
 		return false;
 	}
 
-	int calibration_devid;
-	int ret;
-	int devid = px4_ioctl(fd, DEVIOCGDEVICEID, 0);
-	sprintf(s, "CAL_GYRO%u_ID", instance);
-	param_get(param_find(s), &(calibration_devid));
+	int ret = check_calibration(fd, "CAL_GYRO%u_ID", device_id);
 
-	if (devid != calibration_devid) {
+	if (ret) {
 		mavlink_and_console_log_critical(mavlink_fd,
 						 "PREFLIGHT FAIL: GYRO #%u UNCALIBRATED", instance);
 		success = false;
@@ -228,7 +259,7 @@ out:
 	return success;
 }
 
-static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
+static bool baroCheck(int mavlink_fd, unsigned instance, bool optional, int &device_id)
 {
 	bool success = true;
 
@@ -245,6 +276,20 @@ static bool baroCheck(int mavlink_fd, unsigned instance, bool optional)
 		return false;
 	}
 
+	device_id = -1000;
+
+	// TODO: There is no baro calibration yet, since no external baros exist
+	// int ret = check_calibration(fd, "CAL_BARO%u_ID");
+
+	// if (ret) {
+	// 	mavlink_and_console_log_critical(mavlink_fd,
+	// 					 "PREFLIGHT FAIL: BARO #%u UNCALIBRATED", instance);
+	// 	success = false;
+	// 	goto out;
+	// }
+
+//out:
+
 	px4_close(fd);
 	return success;
 }
@@ -311,49 +356,110 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 
 	/* ---- MAG ---- */
 	if (checkMag) {
+		bool prime_found = false;
+		int32_t prime_id = 0;
+		param_get(param_find("CAL_MAG_PRIME"), &prime_id);
+
 		/* check all sensors, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_mag_count; i++) {
 			bool required = (i < max_mandatory_mag_count);
+			int device_id = -1;
 
-			if (!magnometerCheck(mavlink_fd, i, !required) && required) {
+			if (!magnometerCheck(mavlink_fd, i, !required, device_id) && required) {
 				failed = true;
 			}
+
+			if (device_id == prime_id) {
+				prime_found = true;
+			}
+		}
+
+		/* check if the primary device is present */
+		if (!prime_found && prime_id != 0) {
+			mavlink_log_critical(mavlink_fd, "warning: primary compass not operational");
+			failed = true;
 		}
 	}
 
 	/* ---- ACCEL ---- */
 	if (checkAcc) {
+		bool prime_found = false;
+		int32_t prime_id = 0;
+		param_get(param_find("CAL_ACC_PRIME"), &prime_id);
+
 		/* check all sensors, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_accel_count; i++) {
 			bool required = (i < max_mandatory_accel_count);
+			int device_id = -1;
 
-			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic) && required) {
+			if (!accelerometerCheck(mavlink_fd, i, !required, checkDynamic, device_id) && required) {
 				failed = true;
 			}
+
+			if (device_id == prime_id) {
+				prime_found = true;
+			}
+		}
+
+		/* check if the primary device is present */
+		if (!prime_found && prime_id != 0) {
+			mavlink_log_critical(mavlink_fd, "warning: primary accelerometer not operational");
+			failed = true;
 		}
 	}
 
 	/* ---- GYRO ---- */
 	if (checkGyro) {
+		bool prime_found = false;
+		int32_t prime_id = 0;
+		param_get(param_find("CAL_GYRO_PRIME"), &prime_id);
+
 		/* check all sensors, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_gyro_count; i++) {
 			bool required = (i < max_mandatory_gyro_count);
+			int device_id = -1;
 
-			if (!gyroCheck(mavlink_fd, i, !required) && required) {
+			if (!gyroCheck(mavlink_fd, i, !required, device_id) && required) {
 				failed = true;
 			}
+
+			if (device_id == prime_id) {
+				prime_found = true;
+			}
+		}
+
+		/* check if the primary device is present */
+		if (!prime_found && prime_id != 0) {
+			mavlink_log_critical(mavlink_fd, "warning: primary gyro not operational");
+			failed = true;
 		}
 	}
 
 	/* ---- BARO ---- */
 	if (checkBaro) {
+		bool prime_found = false;
+		int32_t prime_id = 0;
+		param_get(param_find("CAL_BARO_PRIME"), &prime_id);
+
 		/* check all sensors, but fail only for mandatory ones */
 		for (unsigned i = 0; i < max_optional_baro_count; i++) {
 			bool required = (i < max_mandatory_baro_count);
+			int device_id = -1;
 
-			if (!baroCheck(mavlink_fd, i, !required) && required) {
+			if (!baroCheck(mavlink_fd, i, !required, device_id) && required) {
 				failed = true;
 			}
+
+			if (device_id == prime_id) {
+				prime_found = true;
+			}
+		}
+
+		// TODO there is no logic in place to calibrate the primary baro yet
+		// // check if the primary device is present 
+		if (!prime_found && prime_id != 0) {
+			mavlink_log_critical(mavlink_fd, "warning: primary barometer not operational");
+			failed = true;
 		}
 	}
 
@@ -367,12 +473,13 @@ bool preflightCheck(int mavlink_fd, bool checkMag, bool checkAcc, bool checkGyro
 	/* ---- RC CALIBRATION ---- */
 	if (checkRC) {
 		if (rc_calibration_check(mavlink_fd) != OK) {
+			mavlink_log_critical(mavlink_fd, "RC calibration check failed");
 			failed = true;
 		}
 	}
 
 	/* ---- Global Navigation Satellite System receiver ---- */
-	if(checkGNSS) {
+	if (checkGNSS) {
 		if(!gnssCheck(mavlink_fd)) {
 			failed = true;
 		}

src/modules/commander/accelerometer_calibration.cpp

@@ -156,6 +156,10 @@ static const int ERROR = -1;
 
 static const char *sensor_name = "accel";
 
+static int32_t device_id[max_accel_sens];
+static int device_prio_max = 0;
+static int32_t device_id_primary = 0;
+
 calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel_offs)[max_accel_sens][3], float (&accel_T)[max_accel_sens][3][3], unsigned *active_sensors);
 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);
 int mat_invert3(float src[3][3], float dst[3][3]);
@@ -172,7 +176,6 @@ typedef struct  {
 int do_accel_calibration(int mavlink_fd)
 {
 	int fd;
-	int32_t device_id[max_accel_sens];
 
 	mavlink_and_console_log_info(mavlink_fd, CAL_QGC_STARTED_MSG, sensor_name);
 
@@ -259,6 +262,8 @@ int do_accel_calibration(int mavlink_fd)
 		
 		bool failed = false;
 
+		failed = failed || (OK != param_set_no_notification(param_find("CAL_ACC_PRIME"), &(device_id_primary)));
+
 		/* set parameters */
 		(void)sprintf(str, "CAL_ACC%u_XOFF", i);
 		failed |= (OK != param_set_no_notification(param_find(str), &(accel_scale.x_offset)));
@@ -370,6 +375,15 @@ calibrate_return do_accel_calibration_measurements(int mavlink_fd, float (&accel
 		struct accel_report arp = {};
 		(void)orb_copy(ORB_ID(sensor_accel), worker_data.subs[i], &arp);
 		timestamps[i] = arp.timestamp;
+
+		// Get priority
+		int32_t prio;
+		orb_priority(worker_data.subs[i], &prio);
+
+		if (prio > device_prio_max) {
+			device_prio_max = prio;
+			device_id_primary = device_id[i];
+		}
 	}
 
 	if (result == calibrate_return_ok) {

src/modules/commander/gyro_calibration.cpp

@@ -168,6 +168,9 @@ int do_gyro_calibration(int mavlink_fd)
 		1.0f,	// z scale
 	};
 	
+	int device_prio_max = 0;
+	int32_t device_id_primary = 0;
+
 	for (unsigned s = 0; s < max_gyros; s++) {
 		char str[30];
 		
@@ -199,6 +202,15 @@ int do_gyro_calibration(int mavlink_fd)
 	
 	for (unsigned s = 0; s < max_gyros; s++) {
 		worker_data.gyro_sensor_sub[s] = orb_subscribe_multi(ORB_ID(sensor_gyro), s);
+
+		// Get priority
+		int32_t prio;
+		orb_priority(worker_data.gyro_sensor_sub[s], &prio);
+
+		if (prio > device_prio_max) {
+			device_prio_max = prio;
+			device_id_primary = worker_data.device_id[s];
+		}
 	}
 
 	int cancel_sub  = calibrate_cancel_subscribe();
@@ -258,9 +270,12 @@ int do_gyro_calibration(int mavlink_fd)
 	}
 
 	if (res == OK) {
+
 		/* set offset parameters to new values */
 		bool failed = false;
 
+		failed = failed || (OK != param_set_no_notification(param_find("CAL_GYRO_PRIME"), &(device_id_primary)));
+
 		for (unsigned s = 0; s < max_gyros; s++) {
 			if (worker_data.device_id[s] != 0) {
 				char str[30];

src/modules/commander/mag_calibration.cpp

@@ -73,6 +73,10 @@ static constexpr unsigned int calibration_sides = 6;			///< The total number of
 static constexpr unsigned int calibration_total_points = 240;		///< The total points per magnetometer
 static constexpr unsigned int calibraton_duration_seconds = 42; 	///< The total duration the routine is allowed to take
 
+int32_t	device_ids[max_mags];
+int device_prio_max = 0;
+int32_t device_id_primary = 0;
+
 calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mags]);
 
 /// Data passed to calibration worker routine
@@ -108,7 +112,6 @@ int do_mag_calibration(int mavlink_fd)
 	
 	// Determine which mags are available and reset each
 
-	int32_t	device_ids[max_mags];
 	char str[30];
 
 	for (size_t i=0; i<max_mags; i++) {
@@ -434,6 +437,15 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 					result = calibrate_return_error;
 					break;
 				}
+
+				// Get priority
+				int32_t prio;
+				orb_priority(worker_data.sub_mag[cur_mag], &prio);
+
+				if (prio > device_prio_max) {
+					device_prio_max = prio;
+					device_id_primary = device_ids[cur_mag];
+				}
 			}
 		}
 	}
@@ -550,6 +562,9 @@ calibrate_return mag_calibrate_all(int mavlink_fd, int32_t (&device_ids)[max_mag
 	}
 	
 	if (result == calibrate_return_ok) {
+
+		(void)param_set_no_notification(param_find("CAL_MAG_PRIME"), &(device_id_primary));
+
 		for (unsigned cur_mag=0; cur_mag<max_mags; cur_mag++) {
 			if (device_ids[cur_mag] != 0) {
 				int fd_mag = -1;

src/modules/controllib/blocks.cpp

@@ -39,6 +39,7 @@
 
 #include <math.h>
 #include <stdio.h>
+#include <float.h>
 
 #include "blocks.hpp"
 
@@ -51,6 +52,7 @@ int basicBlocksTest()
 	blockLimitSymTest();
 	blockLowPassTest();
 	blockHighPassTest();
+	blockLowPass2Test();
 	blockIntegralTest();
 	blockIntegralTrapTest();
 	blockDerivativeTest();
@@ -198,6 +200,47 @@ int blockHighPassTest()
 	return 0;
 }
 
+float BlockLowPass2::update(float input)
+{
+	if (!isfinite(getState())) {
+		setState(input);
+	}
+
+	if (fabsf(_lp.get_cutoff_freq() - getFCutParam()) > FLT_EPSILON) {
+		_lp.set_cutoff_frequency(_fs, getFCutParam());
+	}
+	_state = _lp.apply(input);
+	return _state;
+}
+
+int blockLowPass2Test()
+{
+	printf("Test BlockLowPass2\t\t: ");
+	BlockLowPass2 lowPass(NULL, "TEST_LP", 100);
+	// test initial state
+	ASSERT(equal(10.0f, lowPass.getFCutParam()));
+	ASSERT(equal(0.0f, lowPass.getState()));
+	ASSERT(equal(0.0f, lowPass.getDt()));
+	// set dt
+	lowPass.setDt(0.1f);
+	ASSERT(equal(0.1f, lowPass.getDt()));
+	// set state
+	lowPass.setState(1.0f);
+	ASSERT(equal(1.0f, lowPass.getState()));
+	// test update
+	ASSERT(equal(1.06745527f, lowPass.update(2.0f)));
+
+	// test end condition
+	for (int i = 0; i < 100; i++) {
+		lowPass.update(2.0f);
+	}
+
+	ASSERT(equal(2.0f, lowPass.getState()));
+	ASSERT(equal(2.0f, lowPass.update(2.0f)));
+	printf("PASS\n");
+	return 0;
+};
+
 float BlockIntegral::update(float input)
 {
 	// trapezoidal integration

src/modules/controllib/blocks.hpp

@@ -45,6 +45,7 @@
 #include <stdlib.h>
 #include <math.h>
 #include <mathlib/math/test/test.hpp>
+#include <mathlib/math/filter/LowPassFilter2p.hpp>
 
 #include "block/Block.hpp"
 #include "block/BlockParam.hpp"
@@ -163,6 +164,36 @@ protected:
 
 int __EXPORT blockHighPassTest();
 
+/**
+ * A 2nd order low pass filter block which uses the default px4 2nd order low pass filter
+ */
+class __EXPORT BlockLowPass2 : public Block
+{
+public:
+// methods
+	BlockLowPass2(SuperBlock *parent, const char *name, float sample_freq) :
+		Block(parent, name),
+		_state(0.0 / 0.0 /* initialize to invalid val, force into is_finite() check on first call */),
+		_fCut(this, ""), // only one parameter, no need to name
+		_fs(sample_freq),
+		_lp(_fs, _fCut.get())
+	{};
+	virtual ~BlockLowPass2() {};
+	float update(float input);
+// accessors
+	float getState() { return _state; }
+	float getFCutParam() { return _fCut.get(); }
+	void setState(float state) { _state = _lp.reset(state); }
+protected:
+// attributes
+	float _state;
+	control::BlockParamFloat _fCut;
+	float _fs;
+	math::LowPassFilter2p _lp;
+};
+
+int __EXPORT blockLowPass2Test();
+
 /**
  * A rectangular integrator.
  * A limiter is built into the class to bound the
@@ -263,6 +294,7 @@ public:
 	void setU(float u) {_u = u;}
 	float getU() {return _u;}
 	float getLP() {return _lowPass.getFCut();}
+	float getO() { return _lowPass.getState(); }
 protected:
 // attributes
 	float _u; /**< previous input */

src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h

@@ -177,6 +177,7 @@ private:
     hrt_abstime _last_accel;
     hrt_abstime _last_mag;
     unsigned _prediction_steps;
+    uint64_t _prediction_last;
 
     struct sensor_combined_s            _sensor_combined;
 

src/modules/ekf_att_pos_estimator/CMakeLists.txt

@@ -39,6 +39,7 @@ endif()
 px4_add_module(
 	MODULE modules__ekf_att_pos_estimator
 	MAIN ekf_att_pos_estimator
+	STACK 3000
 	COMPILE_FLAGS ${MODULE_CFLAGS}
 	SRCS
 		ekf_att_pos_estimator_main.cpp

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -157,6 +157,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
 	_last_accel(0),
 	_last_mag(0),
 	_prediction_steps(0),
+	_prediction_last(0),
 
 	_sensor_combined{},
 
@@ -997,11 +998,12 @@ void AttitudePositionEstimatorEKF::updateSensorFusion(const bool fuseGPS, const
 	_covariancePredictionDt += _ekf->dtIMU;
 
 	// only fuse every few steps
-	if (_prediction_steps < MAX_PREDICITION_STEPS) {
+	if (_prediction_steps < MAX_PREDICITION_STEPS && ((hrt_absolute_time() - _prediction_last) < 20 * 1000)) {
 		_prediction_steps++;
 		return;
 	} else {
 		_prediction_steps = 0;
+		_prediction_last = hrt_absolute_time();
 	}
 
 	// perform a covariance prediction if the total delta angle has exceeded the limit
@@ -1120,7 +1122,7 @@ int AttitudePositionEstimatorEKF::start()
 	_estimator_task = px4_task_spawn_cmd("ekf_att_pos_estimator",
 					 SCHED_DEFAULT,
 					 SCHED_PRIORITY_MAX - 20,
-					 4200,
+					 3900,
 					 (px4_main_t)&AttitudePositionEstimatorEKF::task_main_trampoline,
 					 nullptr);
 
@@ -1161,6 +1163,7 @@ void AttitudePositionEstimatorEKF::print_status()
 	PX4_INFO("dang: %8.4f %8.4f %8.4f dang corr: %8.4f %8.4f %8.4f" , (double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.y,
 	       (double)_ekf->dAngIMU.z, (double)_ekf->correctedDelAng.x, (double)_ekf->correctedDelAng.y,
 	       (double)_ekf->correctedDelAng.z);
+
 	PX4_INFO("states (quat)        [0-3]: %8.4f, %8.4f, %8.4f, %8.4f", (double)_ekf->states[0], (double)_ekf->states[1],
 	       (double)_ekf->states[2], (double)_ekf->states[3]);
 	PX4_INFO("states (vel m/s)     [4-6]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[4], (double)_ekf->states[5],
@@ -1180,11 +1183,12 @@ void AttitudePositionEstimatorEKF::print_status()
 		PX4_INFO("states (terrain)      [22]: %8.4f", (double)_ekf->states[22]);
 
 	} else {
-		PX4_INFO("states (wind)      [13-14]: %8.4f, %8.4f", (double)_ekf->states[13], (double)_ekf->states[14]);
-		PX4_INFO("states (earth mag) [15-17]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[15], (double)_ekf->states[16],
-		       (double)_ekf->states[17]);
-		PX4_INFO("states (body mag)  [18-20]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[18], (double)_ekf->states[19],
-		       (double)_ekf->states[20]);
+		PX4_INFO("states (accel offs)   [13]: %8.4f", (double)_ekf->states[13]);
+		PX4_INFO("states (wind)      [14-15]: %8.4f, %8.4f", (double)_ekf->states[14], (double)_ekf->states[15]);
+		PX4_INFO("states (earth mag) [16-18]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[16], (double)_ekf->states[17],
+		       (double)_ekf->states[18]);
+		PX4_INFO("states (mag bias)  [19-21]: %8.4f, %8.4f, %8.4f", (double)_ekf->states[19], (double)_ekf->states[20],
+		       (double)_ekf->states[21]);
 	}
 
 	PX4_INFO("states: %s %s %s %s %s %s %s %s %s %s",
@@ -1263,7 +1267,7 @@ void AttitudePositionEstimatorEKF::pollData()
 		} else {
 			_ekf->dVelIMU.x = 0.5f * (_ekf->accel.x + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0]);
 			_ekf->dVelIMU.y = 0.5f * (_ekf->accel.y + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 1]);
-			_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]);
+			_ekf->dVelIMU.z = 0.5f * (_ekf->accel.z + _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 2]) + 9.80665f;
 		}
 
 		_ekf->accel.x = _sensor_combined.accelerometer_m_s2[_accel_main * 3 + 0];
@@ -1331,25 +1335,33 @@ void AttitudePositionEstimatorEKF::pollData()
 	// leave this in as long as larger improvements are still being made.
 	#if 0
 
-	float deltaTIntegral = (_sensor_combined.gyro_integral_dt) / 1e6f;
-	float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt) / 1e6f;
+	float deltaTIntegral = (_sensor_combined.gyro_integral_dt[0]) / 1e6f;
+	float deltaTIntAcc = (_sensor_combined.accelerometer_integral_dt[0]) / 1e6f;
 
 	static unsigned dtoverflow5 = 0;
 	static unsigned dtoverflow10 = 0;
 	static hrt_abstime lastprint = 0;
 
 	if (hrt_elapsed_time(&lastprint) > 1000000) {
-		warnx("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
+		PX4_WARN("dt: %8.6f, dtg: %8.6f, dta: %8.6f, dt > 5 ms: %u, dt > 10 ms: %u",
 			(double)deltaT, (double)deltaTIntegral, (double)deltaTIntAcc,
 			dtoverflow5, dtoverflow10);
 
-		warnx("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f",
+		PX4_WARN("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
 		(double)_ekf->dAngIMU.x, (double)_ekf->dAngIMU.z,
-		(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.z);
+		(double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y, (double)_ekf->dVelIMU.z);
 
-		warnx("EKF: dang: %8.4f %8.4f dvel: %8.4f %8.4f",
+		PX4_WARN("INT: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
+		(double)(_sensor_combined.gyro_integral_rad[0]), (double)(_sensor_combined.gyro_integral_rad[2]),
+		(double)(_sensor_combined.accelerometer_integral_m_s[0]),
+		(double)(_sensor_combined.accelerometer_integral_m_s[1]),
+		(double)(_sensor_combined.accelerometer_integral_m_s[2]));
+
+		PX4_WARN("DRV: dang: %8.4f %8.4f dvel: %8.4f %8.4f %8.4f",
 		(double)(_sensor_combined.gyro_rad_s[0] * deltaT), (double)(_sensor_combined.gyro_rad_s[2] * deltaT),
-		(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT), (double)(_sensor_combined.accelerometer_m_s2[2] * deltaT));
+		(double)(_sensor_combined.accelerometer_m_s2[0] * deltaT),
+		(double)(_sensor_combined.accelerometer_m_s2[1] * deltaT),
+		(double)((_sensor_combined.accelerometer_m_s2[2] + 9.8665f) * deltaT));
 
 		lastprint = hrt_absolute_time();
 	}
@@ -1655,8 +1667,6 @@ int ekf_att_pos_estimator_main(int argc, char *argv[])
 	}
 
 	if (!strcmp(argv[1], "status")) {
-		PX4_INFO("running");
-
 		estimator::g_estimator->print_status();
 
 		return 0;

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -81,6 +81,7 @@
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/parameter_update.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/tecs_status.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/pid/pid.h>

src/modules/fw_pos_control_l1/mtecs/mTecs.cpp

@@ -52,15 +52,17 @@ mTecs::mTecs() :
 	_mTecsEnabled(this, "ENABLED"),
 	_airspeedMin(this, "FW_AIRSPD_MIN", false),
 	/* Publications */
+#if defined(__PX4_NUTTX)
 	_status(ORB_ID(tecs_status), &getPublications()),
+#endif // defined(__PX4_NUTTX)
 	/* control blocks */
 	_controlTotalEnergy(this, "THR"),
 	_controlEnergyDistribution(this, "PIT", true),
 	_controlAltitude(this, "FPA", true),
 	_controlAirSpeed(this, "ACC"),
-	_flightPathAngleLowpass(this, "FPA_LP"),
-	_altitudeLowpass(this, "ALT_LP"),
-	_airspeedLowpass(this, "A_LP"),
+	_flightPathAngleLowpass(this, "FPA_LP", 50),
+	_altitudeLowpass(this, "ALT_LP", 50),
+	_airspeedLowpass(this, "A_LP", 50),
 	_airspeedDerivative(this, "AD"),
 	_throttleSp(0.0f),
 	_pitchSp(0.0f),
@@ -107,9 +109,11 @@ int mTecs::updateAltitudeSpeed(float flightPathAngle, float altitude, float alti
 		debug("updateAltitudeSpeed: altitudeSp %.4f, altitude %.4f, altitude filtered %.4f, flightPathAngleSp %.4f", (double)altitudeSp, (double)altitude, (double)altitudeFiltered, (double)flightPathAngleSp);
 	}
 
+#if defined(__PX4_NUTTX)
 	/* Write part of the status message */
 	_status.altitudeSp = altitudeSp;
 	_status.altitude_filtered = altitudeFiltered;
+#endif // defined(__PX4_NUTTX)
 
 
 	/* use flightpath angle setpoint for total energy control */
@@ -143,9 +147,11 @@ int mTecs::updateFlightPathAngleSpeed(float flightPathAngle, float flightPathAng
 				(double)airspeedFiltered, (double)accelerationLongitudinalSp);
 	}
 
+#if defined(__PX4_NUTTX)
 	/* Write part of the status message */
 	_status.airspeedSp = airspeedSp;
 	_status.airspeed_filtered = airspeedFiltered;
+#endif // defined(__PX4_NUTTX)
 
 
 	/* use longitudinal acceleration setpoint for total energy control */
@@ -200,23 +206,23 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	}
 
 	/* Check airspeed: if below safe value switch to underspeed mode (if not in land or takeoff mode) */
-	if (mode != tecs_status_s::TECS_MODE_LAND && mode != tecs_status_s::TECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) {
-		mode = tecs_status_s::TECS_MODE_UNDERSPEED;
+	if (mode != MTECS_MODE_LAND && mode != MTECS_MODE_TAKEOFF && airspeedFiltered < _airspeedMin.get()) {
+		mode = MTECS_MODE_UNDERSPEED;
 	}
 
-	/* Set special output limiters if we are not in TECS_MODE_NORMAL */
+	/* Set special output limiters if we are not in MTECS_MODE_NORMAL */
 	BlockOutputLimiter *outputLimiterThrottle = &_controlTotalEnergy.getOutputLimiter();
 	BlockOutputLimiter *outputLimiterPitch = &_controlEnergyDistribution.getOutputLimiter();
-	if (mode == tecs_status_s::TECS_MODE_TAKEOFF) {
+	if (mode == MTECS_MODE_TAKEOFF) {
 		outputLimiterThrottle = &_BlockOutputLimiterTakeoffThrottle;
 		outputLimiterPitch = &_BlockOutputLimiterTakeoffPitch;
-	} else if (mode == tecs_status_s::TECS_MODE_LAND) {
+	} else if (mode == MTECS_MODE_LAND) {
 		// only limit pitch but do not limit throttle
 		outputLimiterPitch = &_BlockOutputLimiterLandPitch;
-	} else if (mode == tecs_status_s::TECS_MODE_LAND_THROTTLELIM) {
+	} else if (mode == MTECS_MODE_LAND_THROTTLELIM) {
 		outputLimiterThrottle = &_BlockOutputLimiterLandThrottle;
 		outputLimiterPitch = &_BlockOutputLimiterLandPitch;
-	} else if (mode == tecs_status_s::TECS_MODE_UNDERSPEED) {
+	} else if (mode == MTECS_MODE_UNDERSPEED) {
 		outputLimiterThrottle = &_BlockOutputLimiterUnderspeedThrottle;
 		outputLimiterPitch = &_BlockOutputLimiterUnderspeedPitch;
 	}
@@ -226,6 +232,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	 * is running) */
 	limitOverride.applyOverride(*outputLimiterThrottle, *outputLimiterPitch);
 
+// #if defined(__PX4_NUTTX)
 	// /* Write part of the status message */
 	// _status.flightPathAngleSp = flightPathAngleSp;
 	// _status.flightPathAngle = flightPathAngle;
@@ -237,6 +244,7 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 	// _status.energyDistributionRateSp = energyDistributionRateSp;
 	// _status.energyDistributionRate = energyDistributionRate;
 	// _status.mode = mode;
+// #endif // defined(__PX4_NUTTX)
 
 	/** update control blocks **/
 	/* update total energy rate control block */
@@ -253,8 +261,10 @@ int mTecs::updateFlightPathAngleAcceleration(float flightPathAngle, float flight
 				(double)accelerationLongitudinalSp, (double)airspeedDerivative);
 	}
 
+#if defined(__PX4_NUTTX)
 	/* publish status message */
 	_status.update();
+#endif // defined(__PX4_NUTTX)
 
 	/* clean up */
 	_firstIterationAfterReset = false;

src/modules/fw_pos_control_l1/mtecs/mTecs.h

@@ -49,11 +49,25 @@
 #include <controllib/block/BlockParam.hpp>
 #include <drivers/drv_hrt.h>
 #include <uORB/Publication.hpp>
+
+#if defined(__PX4_NUTTX)
 #include <uORB/topics/tecs_status.h>
+#endif // defined(__PX4_NUTTX)
 
 namespace fwPosctrl
 {
 
+/* corresponds to TECS_MODE in tecs_status.msg */
+enum MTECS_MODE {
+	MTECS_MODE_NORMAL = 0,
+	MTECS_MODE_UNDERSPEED = 1,
+	MTECS_MODE_TAKEOFF = 2,
+	MTECS_MODE_LAND = 3,
+	MTECS_MODE_LAND_THROTTLELIM = 4,
+	MTECS_MODE_BAD_DESCENT = 5,
+	MTECS_MODE_CLIMBOUT = 6
+};
+
 /* Main class of the mTecs */
 class mTecs : public control::SuperBlock
 {
@@ -94,6 +108,10 @@ public:
 	float getThrottleSetpoint() { return _throttleSp; }
 	float getPitchSetpoint() { return _pitchSp; }
 	float airspeedLowpassUpdate(float input) { return _airspeedLowpass.update(input); }
+	float getFlightPathAngleLowpassState() { return _flightPathAngleLowpass.getState(); }
+	float getAltitudeLowpassState() { return _altitudeLowpass.getState(); }
+	float getAirspeedLowpassState() { return _airspeedLowpass.getState(); }
+	float getAirspeedDerivativeLowpassState() { return _airspeedDerivative.getO(); }
 
 protected:
 	/* parameters */
@@ -101,7 +119,9 @@ protected:
 	control::BlockParamFloat _airspeedMin;		/**< minimal airspeed */
 
 	/* Publications */
+#if defined(__PX4_NUTTX)
 	uORB::Publication<tecs_status_s> _status;	/**< publish internal values for logging */
+#endif // defined(__PX4_NUTTX)
 
 	/* control blocks */
 	BlockFFPILimitedCustom _controlTotalEnergy;		/**< FFPI controller for total energy control: output
@@ -114,9 +134,9 @@ protected:
 								  setpoint */
 
 	/* Other calculation Blocks */
-	control::BlockLowPass _flightPathAngleLowpass;	/**< low pass filter for the flight path angle */
-	control::BlockLowPass _altitudeLowpass;	/**< low pass filter for altitude */
-	control::BlockLowPass _airspeedLowpass;		/**< low pass filter for airspeed */
+	control::BlockLowPass2 _flightPathAngleLowpass;	/**< low pass filter for the flight path angle */
+	control::BlockLowPass2 _altitudeLowpass;	/**< low pass filter for altitude */
+	control::BlockLowPass2 _airspeedLowpass;		/**< low pass filter for airspeed */
 	control::BlockDerivative _airspeedDerivative;	/**< airspeed derivative calulation */
 
 	/* Output setpoints */

src/modules/mavlink/mavlink_mission.cpp

@@ -779,13 +779,12 @@ MavlinkMissionManager::parse_mavlink_mission_item(const mavlink_mission_item_t *
 		mission_item->altitude_is_relative = true;
 		break;
 
-	case MAV_FRAME_LOCAL_NED:
-	case MAV_FRAME_LOCAL_ENU:
-		return MAV_MISSION_UNSUPPORTED_FRAME;
-
 	case MAV_FRAME_MISSION:
+		// This is a mission item with no coordinate
+		break;
+			
 	default:
-		return MAV_MISSION_ERROR;
+		return MAV_MISSION_UNSUPPORTED_FRAME;
 	}
 
 	switch (mavlink_mission_item->command) {

src/modules/mavlink/mavlink_parameters.cpp

@@ -49,6 +49,7 @@
 
 ORB_DEFINE(uavcan_parameter_request, struct uavcan_parameter_request_s);
 ORB_DEFINE(uavcan_parameter_value, struct uavcan_parameter_value_s);
+#define HASH_PARAM "_HASH_CHECK"
 
 MavlinkParametersManager::MavlinkParametersManager(Mavlink *mavlink) : MavlinkStream(mavlink),
 	_send_all_index(-1),
@@ -163,13 +164,27 @@ MavlinkParametersManager::handle_message(const mavlink_message_t *msg)
 
 				/* when no index is given, loop through string ids and compare them */
 				if (req_read.param_index < 0) {
-					/* local name buffer to enforce null-terminated string */
-					char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
-					strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
-					/* enforce null termination */
-					name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
-					/* attempt to find parameter and send it */
-					send_param(param_find_no_notification(name));
+					if (strncmp(req_read.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN) == 0) {
+						/* return hash check for cached params */
+						uint32_t hash = param_hash_check();
+
+						/* build the one-off response message */
+						mavlink_param_value_t msg;
+						msg.param_count = param_count_used();
+						msg.param_index = -1;
+						strncpy(msg.param_id, HASH_PARAM, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+						msg.param_type = MAV_PARAM_TYPE_UINT32;
+						memcpy(&msg.param_value, &hash, sizeof(hash));
+						_mavlink->send_message(MAVLINK_MSG_ID_PARAM_VALUE, &msg);
+					} else {
+						/* local name buffer to enforce null-terminated string */
+						char name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN + 1];
+						strncpy(name, req_read.param_id, MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN);
+						/* enforce null termination */
+						name[MAVLINK_MSG_PARAM_VALUE_FIELD_PARAM_ID_LEN] = '\0';
+						/* attempt to find parameter and send it */
+						send_param(param_find_no_notification(name));
+					}
 
 				} else {
 					/* when index is >= 0, send this parameter again */

src/modules/navigator/mission_feasibility_checker.cpp

@@ -336,7 +336,7 @@ MissionFeasibilityChecker::check_dist_1wp(dm_item_t dm_current, size_t nMissionI
 
 					} else {
 						/* item is too far from home */
-						mavlink_log_critical(_mavlink_fd, "Waypoint too far: %d m,[MIS_DIST_1WP=%d]", (int)dist_to_1wp, (int)dist_first_wp);
+						mavlink_log_critical(_mavlink_fd, "First waypoint too far: %d m,refusing mission", (int)dist_to_1wp, (int)dist_first_wp);
 						warning_issued = true;
 						return false;
 					}

src/modules/sdlog2/logbuffer.c

@@ -50,8 +50,8 @@ int logbuffer_init(struct logbuffer_s *lb, int size)
 	lb->size  = size;
 	lb->write_ptr = 0;
 	lb->read_ptr = 0;
-	lb->data = malloc(lb->size);
-	return (lb->data == 0) ? PX4_ERROR : PX4_OK;
+	lb->data = NULL;
+	return PX4_OK;
 }
 
 int logbuffer_count(struct logbuffer_s *lb)
@@ -72,6 +72,16 @@ int logbuffer_is_empty(struct logbuffer_s *lb)
 
 bool logbuffer_write(struct logbuffer_s *lb, void *ptr, int size)
 {
+	// allocate buffer if not yet present
+	if (lb->data == NULL) {
+		lb->data = malloc(lb->size);
+	}
+
+	// allocation failed, bail out
+	if (lb->data == NULL) {
+		return false;
+	}
+
 	// bytes available to write
 	int available = lb->read_ptr - lb->write_ptr - 1;
 

src/modules/sensors/CMakeLists.txt

@@ -40,7 +40,6 @@ px4_add_module(
 		-O3
 	SRCS
 		sensors.cpp
-		sensor_params.c
 	DEPENDS
 		platforms__common
 	)

src/modules/sensors/sensor_params.c

@@ -585,6 +585,34 @@ PARAM_DEFINE_FLOAT(CAL_ACC2_YSCALE, 1.0f);
  */
 PARAM_DEFINE_FLOAT(CAL_ACC2_ZSCALE, 1.0f);
 
+/**
+ * Primary accel ID
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_ACC_PRIME, 0);
+
+/**
+ * Primary gyro ID
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_GYRO_PRIME, 0);
+
+/**
+ * Primary mag ID
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_MAG_PRIME, 0);
+
+/**
+ * Primary baro ID
+ *
+ * @group Sensor Calibration
+ */
+PARAM_DEFINE_INT32(CAL_BARO_PRIME, 0);
+
 /**
  * Differential pressure sensor offset
  *

src/modules/sensors/sensors.cpp

@@ -1412,6 +1412,8 @@ Sensors::parameter_update_poll(bool forced)
 				(void)sprintf(str, "CAL_MAG%u_ID", i);
 				int device_id;
 				failed = failed || (OK != param_get(param_find(str), &device_id));
+				(void)sprintf(str, "CAL_MAG%u_ROT", i);
+				(void)param_find(str);
 
 				if (failed) {
 					px4_close(fd);
@@ -2020,6 +2022,11 @@ Sensors::task_main()
 	 * do subscriptions
 	 */
 
+	int gcount_prev = _gyro_count;
+	int mcount_prev = _mag_count;
+	int acount_prev = _accel_count;
+	int bcount_prev = _baro_count;
+
 	_gyro_count = init_sensor_class(ORB_ID(sensor_gyro), &_gyro_sub[0],
 		&raw.gyro_priority[0], &raw.gyro_errcount[0]);
 
@@ -2032,6 +2039,15 @@ Sensors::task_main()
 	_baro_count = init_sensor_class(ORB_ID(sensor_baro), &_baro_sub[0],
 		&raw.baro_priority[0], &raw.baro_errcount[0]);
 
+	if (gcount_prev != _gyro_count ||
+	    mcount_prev != _mag_count ||
+	    acount_prev != _accel_count ||
+	    bcount_prev != _baro_count) {
+
+		/* reload calibration params */
+		parameter_update_poll(true);
+	}
+
 	_rc_sub = orb_subscribe(ORB_ID(input_rc));
 	_diff_pres_sub = orb_subscribe(ORB_ID(differential_pressure));
 	_vcontrol_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));

src/modules/systemlib/param/param.c

@@ -65,6 +65,8 @@
 #include "uORB/topics/parameter_update.h"
 #include "px4_parameters.h"
 
+#include <crc32.h>
+
 #if 0
 # define debug(fmt, args...)		do { warnx(fmt, ##args); } while(0)
 #else
@@ -1035,3 +1037,26 @@ param_foreach(void (*func)(void *arg, param_t param), void *arg, bool only_chang
 		func(arg, param);
 	}
 }
+
+uint32_t param_hash_check(void)
+{
+	uint32_t param_hash = 0;
+
+	param_lock();
+
+	/* compute the CRC32 over all string param names and 4 byte values */
+	for (param_t param = 0; handle_in_range(param); param++) {
+		if (!param_used(param)) {
+			continue;
+		}
+
+		const char *name = param_name(param);
+		const void *val = param_get_value_ptr(param);
+		param_hash = crc32part((const uint8_t *)name, strlen(name), param_hash);
+		param_hash = crc32part(val, sizeof(union param_value_u), param_hash);
+	}
+
+	param_unlock();
+
+	return param_hash;
+}

src/modules/systemlib/param/param.h

@@ -333,6 +333,13 @@ __EXPORT int 		param_save_default(void);
  */
 __EXPORT int 		param_load_default(void);
 
+/**
+ * Generate the hash of all parameters and their values
+ *
+ * @return		CRC32 hash of all param_ids and values
+ */
+__EXPORT uint32_t	param_hash_check(void);
+
 /*
  * Macros creating static parameter definitions.
  *

src/modules/uavcan/CMakeLists.txt

@@ -31,65 +31,30 @@
 #
 ############################################################################
 
-# uavcan project
-set(uavcan_c_flags ${c_flags})
-list(REMOVE_ITEM uavcan_c_flags -std=gnu++0x -D__CUSTOM_FILE_IO__)
-set(uavcan_cxx_flags ${cxx_flags})
-list(REMOVE_ITEM uavcan_cxx_flags -std=gnu++0x -std=c++11 -Wundef -Werror -D__CUSTOM_FILE_IO__)
-set(uavcan_deps git_uavcan)
-set(uavcan_platform generic)
-
-set(nuttx_export_dir ${CMAKE_BINARY_DIR}/${BOARD}/NuttX/nuttx-export)
+set(UAVCAN_USE_CPP03 ON CACHE BOOL "uavcan cpp03")
+set(UAVCAN_PLATFORM stm32 CACHE STRING "uavcan platform")
 
 string(TOUPPER "${OS}" OS_UPPER)
-
-set(uavcan_definitions
-	-DUAVCAN_NO_ASSERTIONS
-	-DUAVCAN_STM32_NUM_IFACES=2
-	-DUAVCAN_USE_EXTERNAL_SNPRINT
-	-DUAVCAN_MEM_POOL_BLOCK_SIZE=48
-	-DUAVCAN_MAX_NETWORK_SIZE_HINT=16
-	-DUAVCAN_STM32_TIMER_NUMBER=5
-	-DUAVCAN_STM32_${OS_UPPER}=1
-	-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
-	)
-set(uavcan_extra_flags ${uavcan_definitions})
-
-if (${OS} STREQUAL "nuttx")
-	set(uavcan_platform stm32)
-	list(APPEND uavcan_extra_flags
-		-I${nuttx_export_dir}/include
-		-I${nuttx_export_dir}/include/cxx
-		-I${nuttx_export_dir}/arch/chip
-		-I${nuttx_export_dir}/arch/common
-		)
-	list(APPEND uavcan_deps nuttx_export_${BOARD})
-endif()
-
-list(APPEND uavcan_c_flags ${uavcan_extra_flags})
-list(APPEND uavcan_cxx_flags ${uavcan_extra_flags})
-
-px4_join(OUT uavcan_c_flags LIST "${uavcan_c_flags}" GLUE " ")
-px4_join(OUT uavcan_cxx_flags LIST "${uavcan_cxx_flags}" GLUE " ")
-
-externalproject_add(libuavcan
-	DEPENDS ${uavcan_deps}
-	DOWNLOAD_COMMAND ""
-	UPDATE_COMMAND git submodule update --init
-	LOG_INSTALL 1
-	SOURCE_DIR ${CMAKE_SOURCE_DIR}/src/lib/uavcan
-	CMAKE_ARGS -DCMAKE_TOOLCHAIN_FILE=${CMAKE_TOOLCHAIN_FILE}
+add_definitions(
+		-DUAVCAN_CPP_VERSION=UAVCAN_CPP03
+		-DUAVCAN_MAX_NETWORK_SIZE_HINT=16
+		-DUAVCAN_MEM_POOL_BLOCK_SIZE=48
+		-DUAVCAN_NO_ASSERTIONS
+		-DUAVCAN_PLATFORM=stm32
+		-DUAVCAN_STM32_${OS_UPPER}=1
+		-DUAVCAN_STM32_NUM_IFACES=2
+		-DUAVCAN_STM32_TIMER_NUMBER=5
 		-DUAVCAN_USE_CPP03=ON
-		-DUAVCAN_PLATFORM=${uavcan_platform}
-		-DUAVCAN_OS=${OS}
-		-DCMAKE_CXX_FLAGS=${uavcan_cxx_flags}
-		-DCMAKE_C_FLAGS=${uavcan_c_flags}
-		-DCMAKE_INSTALL_PREFIX=${ep_base}/Install
-)
+		-DUAVCAN_USE_EXTERNAL_SNPRINT
+		)
 
-string(TOUPPER ${OS} OS_UPPER)
+add_subdirectory(libuavcan EXCLUDE_FROM_ALL)
+add_dependencies(uavcan platforms__nuttx)
 
-add_definitions(${uavcan_definitions})
+include_directories(libuavcan/libuavcan/include)
+include_directories(libuavcan/libuavcan/include/dsdlc_generated)
+include_directories(libuavcan/libuavcan_drivers/posix/include)
+include_directories(libuavcan/libuavcan_drivers/stm32/driver/include)
 
 px4_add_module(
 	MODULE modules__uavcan
@@ -98,9 +63,8 @@ px4_add_module(
 	COMPILE_FLAGS
 		-Wframe-larger-than=1500
 		-Wno-deprecated-declarations
-		-O3
+		-Os
 	SRCS
-
 		# Main
 		uavcan_main.cpp
 		uavcan_servers.cpp
@@ -117,7 +81,7 @@ px4_add_module(
 
 	DEPENDS
 		platforms__common
-		libuavcan	
+		uavcan
 	)
 
 ## vim: set noet ft=cmake fenc=utf-8 ff=unix : 

src/modules/uavcan/uavcan_main.hpp

@@ -94,7 +94,7 @@ class UavcanNode : public device::CDev
 	 */
 
 	static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At
-	static constexpr unsigned StackSize          = 1600;
+	static constexpr unsigned StackSize          = 1800;
 
 public:
 	typedef uavcan::Node<MemPoolSize> Node;