Merge branch 'master' into inav_alt_gps

src/drivers/airspeed/airspeed.h

@@ -119,7 +119,7 @@ protected:
 	virtual int	collect() = 0;
 
 	work_s			_work;
-	uint16_t		_max_differential_pressure_pa;
+	float		_max_differential_pressure_pa;
 	bool			_sensor_ok;
 	int			_measure_ticks;
 	bool			_collect_phase;

src/drivers/boards/px4fmu-v1/px4fmu_init.c

@@ -222,14 +222,9 @@ __EXPORT int nsh_archinitialize(void)
 	 * If SPI2 is enabled in the defconfig, we loose some ADC pins as chip selects.
 	 * Keep the SPI2 init optional and conditionally initialize the ADC pins
 	 */
-	spi2 = up_spiinitialize(2);
 
-	if (!spi2) {
-		message("[boot] Enabling IN12/13 instead of SPI2\n");
-		/* no SPI2, use pins for ADC */
-		stm32_configgpio(GPIO_ADC1_IN12);
-		stm32_configgpio(GPIO_ADC1_IN13);	// jumperable to MPU6000 DRDY on some boards
-	} else {
+	#ifdef CONFIG_STM32_SPI2
+		spi2 = up_spiinitialize(2);
 		/* Default SPI2 to 1MHz and de-assert the known chip selects. */
 		SPI_SETFREQUENCY(spi2, 10000000);
 		SPI_SETBITS(spi2, 8);
@@ -238,7 +233,13 @@ __EXPORT int nsh_archinitialize(void)
 		SPI_SELECT(spi2, PX4_SPIDEV_ACCEL_MAG, false);
 
 		message("[boot] Initialized SPI port2 (ADC IN12/13 blocked)\n");
-	}
+	#else
+		spi2 = NULL;
+		message("[boot] Enabling IN12/13 instead of SPI2\n");
+		/* no SPI2, use pins for ADC */
+		stm32_configgpio(GPIO_ADC1_IN12);
+		stm32_configgpio(GPIO_ADC1_IN13);	// jumperable to MPU6000 DRDY on some boards
+	#endif
 
 	/* Get the SPI port for the microSD slot */
 

src/drivers/ets_airspeed/ets_airspeed.cpp

@@ -180,7 +180,7 @@ ETSAirspeed::collect()
 	differential_pressure_s report;
 	report.timestamp = hrt_absolute_time();
         report.error_count = perf_event_count(_comms_errors);
-	report.differential_pressure_pa = diff_pres_pa;
+	report.differential_pressure_pa = (float)diff_pres_pa;
 	report.voltage = 0;
 	report.max_differential_pressure_pa = _max_differential_pressure_pa;
 

src/drivers/meas_airspeed/meas_airspeed.cpp

@@ -36,6 +36,7 @@
  * @author Lorenz Meier
  * @author Sarthak Kaingade
  * @author Simon Wilks
+ * @author Thomas Gubler
  *
  * Driver for the MEAS Spec series connected via I2C.
  *
@@ -76,6 +77,7 @@
 #include <systemlib/err.h>
 #include <systemlib/param/param.h>
 #include <systemlib/perf_counter.h>
+#include <mathlib/mathlib.h>
 
 #include <drivers/drv_airspeed.h>
 #include <drivers/drv_hrt.h>
@@ -184,7 +186,7 @@ MEASAirspeed::collect()
 	//diff_pres_pa -= _diff_pres_offset;
 	int16_t dp_raw = 0, dT_raw = 0;
 	dp_raw = (val[0] << 8) + val[1];
-	dp_raw = 0x3FFF & dp_raw;
+	dp_raw = 0x3FFF & dp_raw; //mask the used bits
 	dT_raw = (val[2] << 8) + val[3];
 	dT_raw = (0xFFE0 & dT_raw) >> 5;
 	float temperature = ((200 * dT_raw) / 2047) - 50;
@@ -193,7 +195,11 @@ MEASAirspeed::collect()
 
 	// Calculate differential pressure. As its centered around 8000
 	// and can go positive or negative, enforce absolute value
-	uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
+//	uint16_t diff_press_pa = abs(dp_raw - (16384 / 2.0f));
+	const float P_min = -1.0f;
+	const float P_max = 1.0f;
+	float diff_press_pa = math::max(0.0f, fabsf( ( ((float)dp_raw - 0.1f*16383.0f) * (P_max-P_min)/(0.8f*16383.0f) + P_min) * 6894.8f)   - _diff_pres_offset);
+
 	struct differential_pressure_s report;
 
 	// Track maximum differential pressure measured (so we can work out top speed).

src/lib/ecl/attitude_fw/ecl_pitch_controller.cpp

@@ -62,8 +62,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
 	_last_run = ecl_absolute_time();
-
-	float dt = dt_micros / 1000000;
+	float dt = (float)dt_micros * 1e-6f;
 
 	/* lock integral for long intervals */
 	if (dt_micros > 500000)
@@ -115,7 +114,7 @@ float ECL_PitchController::control(float pitch_setpoint, float pitch, float pitc
 
 	_rate_error = _rate_setpoint - pitch_rate;
 
-	float ilimit_scaled = 0.0f;
+	float ilimit_scaled = _integrator_max * scaler;
 
 	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
 

src/lib/ecl/attitude_fw/ecl_roll_controller.cpp

@@ -64,8 +64,11 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 	/* get the usual dt estimate */
 	uint64_t dt_micros = ecl_elapsed_time(&_last_run);
 	_last_run = ecl_absolute_time();
+	float dt = (float)dt_micros * 1e-6f;
 
-	float dt = (dt_micros > 500000) ? 0.0f : dt_micros / 1000000;
+	/* lock integral for long intervals */
+	if (dt_micros > 500000)
+		lock_integrator = true;
 
 	float k_ff = math::max((_k_p - _k_i * _tc) * _tc - _k_d, 0.0f);
 	float k_i_rate = _k_i * _tc;
@@ -90,7 +93,7 @@ float ECL_RollController::control(float roll_setpoint, float roll, float roll_ra
 	_rate_error = _rate_setpoint - roll_rate;
 
 
-	float ilimit_scaled = 0.0f;
+	float ilimit_scaled = _integrator_max * scaler;
 
 	if (!lock_integrator && k_i_rate > 0.0f && airspeed > 0.5f * airspeed_min) {
 

src/lib/external_lgpl/tecs/tecs.h

@@ -54,7 +54,9 @@ public:
 		_SPE_est(0.0f),
 		_SKE_est(0.0f),
 		_SPEdot(0.0f),
-		_SKEdot(0.0f) {
+		_SKEdot(0.0f),
+		_vel_dot(0.0f),
+		_STEdotErrLast(0.0f) {
 
 	}
 

src/lib/mathlib/math/arm/Matrix.hpp

@@ -121,10 +121,10 @@ public:
 		for (size_t i = 0; i < getRows(); i++) {
 			for (size_t j = 0; j < getCols(); j++) {
 				float sig;
-				int exp;
+				int exponent;
 				float num = (*this)(i, j);
-				float2SigExp(num, sig, exp);
-				printf("%6.3fe%03.3d,", (double)sig, exp);
+				float2SigExp(num, sig, exponent);
+				printf("%6.3fe%03d ", (double)sig, exponent);
 			}
 
 			printf("\n");

src/lib/mathlib/math/arm/Vector.hpp

@@ -109,10 +109,10 @@ public:
 	inline void print() const {
 		for (size_t i = 0; i < getRows(); i++) {
 			float sig;
-			int exp;
+			int exponent;
 			float num = (*this)(i);
-			float2SigExp(num, sig, exp);
-			printf("%6.3fe%03.3d,", (double)sig, exp);
+			float2SigExp(num, sig, exponent);
+			printf("%6.3fe%03d ", (double)sig, exponent);
 		}
 
 		printf("\n");

src/modules/att_pos_estimator_ekf/KalmanNav.cpp

@@ -34,7 +34,7 @@
 /**
  * @file KalmanNav.cpp
  *
- * kalman filter navigation code
+ * Kalman filter navigation code
  */
 
 #include <poll.h>
@@ -228,10 +228,7 @@ void KalmanNav::update()
 	updateSubscriptions();
 
 	// initialize attitude when sensors online
-	if (!_attitudeInitialized && sensorsUpdate &&
-	    _sensors.accelerometer_counter > 10 &&
-	    _sensors.gyro_counter > 10 &&
-	    _sensors.magnetometer_counter > 10) {
+	if (!_attitudeInitialized && sensorsUpdate) {
 		if (correctAtt() == ret_ok) _attitudeInitCounter++;
 
 		if (_attitudeInitCounter > 100) {
@@ -643,7 +640,7 @@ int KalmanNav::correctAtt()
 
 	if (beta > _faultAtt.get()) {
 		warnx("fault in attitude: beta = %8.4f", (double)beta);
-		warnx("y:\n"); y.print();
+		warnx("y:"); y.print();
 	}
 
 	// update quaternions from euler

src/modules/commander/airspeed_calibration.cpp

@@ -37,12 +37,15 @@
  */
 
 #include "airspeed_calibration.h"
+#include "calibration_messages.h"
 #include "commander_helper.h"
 
 #include <stdio.h>
+#include <fcntl.h>
 #include <poll.h>
 #include <math.h>
 #include <drivers/drv_hrt.h>
+#include <drivers/drv_airspeed.h>
 #include <uORB/topics/sensor_combined.h>
 #include <uORB/topics/differential_pressure.h>
 #include <mavlink/mavlink_log.h>
@@ -55,10 +58,13 @@
 #endif
 static const int ERROR = -1;
 
+static const char *sensor_name = "dpress";
+
 int do_airspeed_calibration(int mavlink_fd)
 {
 	/* give directions */
-	mavlink_log_info(mavlink_fd, "airspeed calibration starting, keep it still");
+	mavlink_log_info(mavlink_fd, CAL_STARTED_MSG, sensor_name);
+	mavlink_log_info(mavlink_fd, "don't move system");
 
 	const int calibration_count = 2500;
 
@@ -68,6 +74,28 @@ int do_airspeed_calibration(int mavlink_fd)
 	int calibration_counter = 0;
 	float diff_pres_offset = 0.0f;
 
+	/* Reset sensor parameters */
+	struct airspeed_scale airscale = {
+		0.0f,
+		1.0f,
+	};
+
+	bool paramreset_successful = false;
+	int  fd = open(AIRSPEED_DEVICE_PATH, 0);
+	if (fd > 0) {
+		if (OK == ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale)) {
+			paramreset_successful = true;
+		}
+		close(fd);
+	}
+
+	if (!paramreset_successful) {
+		warn("WARNING: failed to set scale / offsets for airspeed sensor");
+		mavlink_log_critical(mavlink_fd, "could not reset dpress sensor");
+		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
+		return ERROR;
+	}
+
 	while (calibration_counter < calibration_count) {
 
 		/* wait blocking for new data */
@@ -82,9 +110,12 @@ int do_airspeed_calibration(int mavlink_fd)
 			diff_pres_offset += diff_pres.differential_pressure_pa;
 			calibration_counter++;
 
+			if (calibration_counter % (calibration_count / 20) == 0)
+				mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, (calibration_counter * 100) / calibration_count);
+
 		} else if (poll_ret == 0) {
 			/* any poll failure for 1s is a reason to abort */
-			mavlink_log_info(mavlink_fd, "airspeed calibration aborted");
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 			close(diff_pres_sub);
 			return ERROR;
 		}
@@ -95,7 +126,7 @@ int do_airspeed_calibration(int mavlink_fd)
 	if (isfinite(diff_pres_offset)) {
 
 		if (param_set(param_find("SENS_DPRES_OFF"), &(diff_pres_offset))) {
-			mavlink_log_critical(mavlink_fd, "Setting offs failed!");
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SET_PARAMS_MSG);
 			close(diff_pres_sub);
 			return ERROR;
 		}
@@ -105,17 +136,18 @@ int do_airspeed_calibration(int mavlink_fd)
 
 		if (save_ret != 0) {
 			warn("WARNING: auto-save of params to storage failed");
-			mavlink_log_info(mavlink_fd, "FAILED storing calibration");
+			mavlink_log_critical(mavlink_fd, CAL_FAILED_SAVE_PARAMS_MSG);
 			close(diff_pres_sub);
 			return ERROR;
 		}
 
-		mavlink_log_info(mavlink_fd, "airspeed calibration done");
+		mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
+		tune_neutral();
 		close(diff_pres_sub);
 		return OK;
 
 	} else {
-		mavlink_log_info(mavlink_fd, "airspeed calibration FAILED (NaN)");
+		mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 		close(diff_pres_sub);
 		return ERROR;
 	}

src/modules/commander/commander.cpp

@@ -116,6 +116,8 @@ extern struct system_load_s system_load;
 #define LOW_VOLTAGE_BATTERY_COUNTER_LIMIT (LOW_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 #define CRITICAL_VOLTAGE_BATTERY_COUNTER_LIMIT (CRITICAL_VOLTAGE_BATTERY_HYSTERESIS_TIME_MS*COMMANDER_MONITORING_LOOPSPERMSEC)
 
+#define MAVLINK_OPEN_INTERVAL 50000
+
 #define STICK_ON_OFF_LIMIT 0.75f
 #define STICK_THRUST_RANGE 1.0f
 #define STICK_ON_OFF_HYSTERESIS_TIME_MS 1000
@@ -582,16 +584,6 @@ int commander_thread_main(int argc, char *argv[])
 
 	mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
 
-	if (mavlink_fd < 0) {
-		/* try again later */
-		usleep(20000);
-		mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-
-		if (mavlink_fd < 0) {
-			warnx("ERROR: Failed to open MAVLink log stream again, start mavlink app first.");
-		}
-	}
-
 	/* Main state machine */
 	/* make sure we are in preflight state */
 	memset(&status, 0, sizeof(status));
@@ -770,6 +762,11 @@ int commander_thread_main(int argc, char *argv[])
 
 	while (!thread_should_exit) {
 
+		if (mavlink_fd < 0 && counter % (1000000 / MAVLINK_OPEN_INTERVAL) == 0) {
+			/* try to open the mavlink log device every once in a while */
+			mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
+		}
+
 		/* update parameters */
 		orb_check(param_changed_sub, &updated);
 

src/modules/commander/mag_calibration.cpp

@@ -73,7 +73,7 @@ int do_mag_calibration(int mavlink_fd)
 
 	/* maximum 500 values */
 	const unsigned int calibration_maxcount = 500;
-	unsigned int calibration_counter = 0;
+	unsigned int calibration_counter;
 
 	struct mag_scale mscale_null = {
 		0.0f,
@@ -99,28 +99,34 @@ int do_mag_calibration(int mavlink_fd)
 		res = ioctl(fd, MAGIOCCALIBRATE, fd);
 
 		if (res != OK) {
-			mavlink_log_critical(mavlink_fd, "ERROR: failed to calibrate scale");
+			mavlink_log_critical(mavlink_fd, "Skipped scale calibration");
+			/* this is non-fatal - mark it accordingly */
+			res = OK;
 		}
 	}
 
 	close(fd);
 
-	float *x;
-	float *y;
-	float *z;
+	float *x = NULL;
+	float *y = NULL;
+	float *z = NULL;
 
 	if (res == OK) {
 		/* allocate memory */
 		mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 20);
 
-		x = (float *)malloc(sizeof(float) * calibration_maxcount);
-		y = (float *)malloc(sizeof(float) * calibration_maxcount);
-		z = (float *)malloc(sizeof(float) * calibration_maxcount);
+		x = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
+		y = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
+		z = reinterpret_cast<float *>(malloc(sizeof(float) * calibration_maxcount));
 
 		if (x == NULL || y == NULL || z == NULL) {
 			mavlink_log_critical(mavlink_fd, "ERROR: out of memory");
 			res = ERROR;
+			return res;
 		}
+	} else {
+		/* exit */
+		return ERROR;
 	}
 
 	if (res == OK) {
@@ -136,6 +142,8 @@ int do_mag_calibration(int mavlink_fd)
 
 		mavlink_log_info(mavlink_fd, "rotate in a figure 8 around all axis");
 
+		calibration_counter = 0;
+
 		while (hrt_absolute_time() < calibration_deadline &&
 		       calibration_counter < calibration_maxcount) {
 
@@ -178,6 +186,7 @@ int do_mag_calibration(int mavlink_fd)
 	float sphere_radius;
 
 	if (res == OK) {
+
 		/* sphere fit */
 		mavlink_log_info(mavlink_fd, CAL_PROGRESS_MSG, sensor_name, 70);
 		sphere_fit_least_squares(x, y, z, calibration_counter, 100, 0.0f, &sphere_x, &sphere_y, &sphere_z, &sphere_radius);
@@ -270,7 +279,7 @@ int do_mag_calibration(int mavlink_fd)
 		} else {
 			mavlink_log_info(mavlink_fd, CAL_FAILED_MSG, sensor_name);
 		}
-
-		return res;
 	}
+
+	return res;
 }

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -279,20 +279,20 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_parameter_handles.p_i = param_find("FW_P_I");
 	_parameter_handles.p_rmax_pos = param_find("FW_P_RMAX_POS");
 	_parameter_handles.p_rmax_neg = param_find("FW_P_RMAX_NEG");
-	_parameter_handles.p_integrator_max = param_find("FW_P_integrator_max");
+	_parameter_handles.p_integrator_max = param_find("FW_P_IMAX");
 	_parameter_handles.p_roll_feedforward = param_find("FW_P_ROLLFF");
 
 	_parameter_handles.r_p = param_find("FW_R_P");
 	_parameter_handles.r_d = param_find("FW_R_D");
 	_parameter_handles.r_i = param_find("FW_R_I");
-	_parameter_handles.r_integrator_max = param_find("FW_R_integrator_max");
+	_parameter_handles.r_integrator_max = param_find("FW_R_IMAX");
 	_parameter_handles.r_rmax = param_find("FW_R_RMAX");
 
 	_parameter_handles.y_p = param_find("FW_Y_P");
 	_parameter_handles.y_i = param_find("FW_Y_I");
 	_parameter_handles.y_d = param_find("FW_Y_D");
 	_parameter_handles.y_roll_feedforward = param_find("FW_Y_ROLLFF");
-	_parameter_handles.y_integrator_max = param_find("FW_Y_integrator_max");
+	_parameter_handles.y_integrator_max = param_find("FW_Y_IMAX");
 
 	_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
 	_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
@@ -635,43 +635,46 @@ FixedwingAttitudeControl::task_main()
 					}
 				}
 
-				float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
-								    airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
-				_actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
+				if (isfinite(roll_sp) && isfinite(pitch_sp)) {
 
-				float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
-								      lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
-				_actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
+					float roll_rad = _roll_ctrl.control(roll_sp, _att.roll, _att.rollspeed,
+									    airspeed_scaling, lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
+					_actuators.control[0] = (isfinite(roll_rad)) ? roll_rad * actuator_scaling : 0.0f;
 
-				float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
-								  _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
-				_actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f;
+					float pitch_rad = _pitch_ctrl.control(pitch_sp, _att.pitch, _att.pitchspeed, _att.roll, airspeed_scaling,
+									      lock_integrator, _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
+					_actuators.control[1] = (isfinite(pitch_rad)) ? pitch_rad * actuator_scaling : 0.0f;
 
-				/* throttle passed through */
-				_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
+					float yaw_rad = _yaw_ctrl.control(_att.roll, _att.yawspeed, _accel.y, airspeed_scaling, lock_integrator,
+									  _parameters.airspeed_min, _parameters.airspeed_max, airspeed);
+					_actuators.control[2] = (isfinite(yaw_rad)) ? yaw_rad * actuator_scaling : 0.0f;
 
-				// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
-				// 			airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
-				// 			_actuators.control[2], _actuators.control[3]);
+					/* throttle passed through */
+					_actuators.control[3] = (isfinite(throttle_sp)) ? throttle_sp : 0.0f;
 
-				/*
-				 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
-				 * only once available
-				 */
-				vehicle_rates_setpoint_s rates_sp;
-				rates_sp.roll = _roll_ctrl.get_desired_rate();
-				rates_sp.pitch = _pitch_ctrl.get_desired_rate();
-				rates_sp.yaw = 0.0f; // XXX not yet implemented
+					// warnx("aspd: %s: %6.2f, aspd scaling: %6.2f, controls: %5.2f %5.2f %5.2f %5.2f", (_airspeed_valid) ? "valid" : "unknown",
+					// 			airspeed, airspeed_scaling, _actuators.control[0], _actuators.control[1],
+					// 			_actuators.control[2], _actuators.control[3]);
 
-				rates_sp.timestamp = hrt_absolute_time();
+					/*
+					 * Lazily publish the rate setpoint (for analysis, the actuators are published below)
+					 * only once available
+					 */
+					vehicle_rates_setpoint_s rates_sp;
+					rates_sp.roll = _roll_ctrl.get_desired_rate();
+					rates_sp.pitch = _pitch_ctrl.get_desired_rate();
+					rates_sp.yaw = 0.0f; // XXX not yet implemented
 
-				if (_rate_sp_pub > 0) {
-					/* publish the attitude setpoint */
-					orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
+					rates_sp.timestamp = hrt_absolute_time();
 
-				} else {
-					/* advertise and publish */
-					_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+					if (_rate_sp_pub > 0) {
+						/* publish the attitude setpoint */
+						orb_publish(ORB_ID(vehicle_rates_setpoint), _rate_sp_pub, &rates_sp);
+
+					} else {
+						/* advertise and publish */
+						_rate_sp_pub = orb_advertise(ORB_ID(vehicle_rates_setpoint), &rates_sp);
+					}
 				}
 
 			} else {

src/modules/mavlink/missionlib.c

@@ -64,6 +64,7 @@
 #include <systemlib/systemlib.h>
 #include <mavlink/mavlink_log.h>
 
+#include "geo/geo.h"
 #include "waypoints.h"
 #include "orb_topics.h"
 #include "missionlib.h"

src/modules/sensors/sensors.cpp

@@ -1129,6 +1129,7 @@ Sensors::parameter_update_poll(bool forced)
 
 			if (OK != ioctl(fd, AIRSPEEDIOCSSCALE, (long unsigned int)&airscale))
 				warn("WARNING: failed to set scale / offsets for airspeed sensor");
+			close(fd);
 		}
 
 #if 0

src/modules/systemlib/airspeed.c

@@ -59,7 +59,7 @@
 float calc_indicated_airspeed(float differential_pressure)
 {
 
-	if (differential_pressure > 0) {
+	if (differential_pressure > 0.0f) {
 		return sqrtf((2.0f*differential_pressure) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);
 	} else {
 		return -sqrtf((2.0f*fabsf(differential_pressure)) / CONSTANTS_AIR_DENSITY_SEA_LEVEL_15C);

src/modules/uORB/topics/differential_pressure.h

@@ -54,8 +54,8 @@
 struct differential_pressure_s {
 	uint64_t	timestamp;			/**< microseconds since system boot, needed to integrate */
 	uint64_t	error_count;
-	uint16_t	differential_pressure_pa;	/**< Differential pressure reading */
-	uint16_t	max_differential_pressure_pa;	/**< Maximum differential pressure reading */
+	float	differential_pressure_pa;	/**< Differential pressure reading */
+	float	max_differential_pressure_pa;	/**< Maximum differential pressure reading */
 	float		voltage;			/**< Voltage from analog airspeed sensors (voltage divider already compensated) */
 	float		temperature;			/**< Temperature provided by sensor */
 

src/modules/uORB/uORB.cpp

@@ -249,8 +249,10 @@ ORBDevNode::close(struct file *filp)
 	} else {
 		SubscriberData *sd = filp_to_sd(filp);
 
-		if (sd != nullptr)
+		if (sd != nullptr) {
+			hrt_cancel(&sd->update_call);
 			delete sd;
+		}
 	}
 
 	return CDev::close(filp);

src/systemcmds/preflight_check/preflight_check.c

@@ -109,7 +109,7 @@ int preflight_check_main(int argc, char *argv[])
 	/* ---- ACCEL ---- */
 
 	close(fd);
-	fd = open(ACCEL_DEVICE_PATH, 0);
+	fd = open(ACCEL_DEVICE_PATH, O_RDONLY);
 	ret = ioctl(fd, ACCELIOCSELFTEST, 0);
 	
 	if (ret != OK) {
@@ -119,6 +119,29 @@ int preflight_check_main(int argc, char *argv[])
 		goto system_eval;
 	}
 
+	/* check measurement result range */
+	struct accel_report acc;
+	ret = read(fd, &acc, sizeof(acc));
+
+	if (ret == sizeof(acc)) {
+		/* evaluate values */
+		if (sqrtf(acc.x * acc.x + acc.y * acc.y + acc.z * acc.z) > 30.0f /* m/s^2 */) {
+			warnx("accel with spurious values");
+			mavlink_log_critical(mavlink_fd, "SENSOR FAIL: |ACCEL| > 30 m/s^2");
+			/* this is frickin' fatal */
+			fail_on_error = true;
+			system_ok = false;
+			goto system_eval;
+		}
+	} else {
+		warnx("accel read failed");
+		mavlink_log_critical(mavlink_fd, "SENSOR FAIL: ACCEL READ");
+		/* this is frickin' fatal */
+		fail_on_error = true;
+		system_ok = false;
+		goto system_eval;
+	}
+
 	/* ---- GYRO ---- */
 
 	close(fd);
@@ -193,9 +216,10 @@ system_eval:
 		/* stop alarm */
 		ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
 
-		/* switch on leds */
+		/* switch off leds */
 		led_on(leds, LED_BLUE);
 		led_on(leds, LED_AMBER);
+		close(leds);
 
 		if (fail_on_error) {
 			/* exit with error message */