Merge branch 'master' into fw_autoland

nored, and an empty message aborts

20 files changed, 249 insertions, 194 deletions

diff --git a/src/drivers/drv_pwm_output.h b/src/drivers/drv_pwm_output.h
index efd6afb4b..51f916f37 100644
--- a/src/drivers/drv_pwm_output.h
+++ b/src/drivers/drv_pwm_output.h
@@ -65,9 +65,14 @@ __BEGIN_DECLS
 #define PWM_OUTPUT_MAX_CHANNELS	16
 
 /**
- * Minimum PWM in us
+ * Lowest minimum PWM in us
  */
-#define PWM_MIN 900
+#define PWM_LOWEST_MIN 900
+
+/**
+ * Default minimum PWM in us
+ */
+#define PWM_DEFAULT_MIN 1000
 
 /**
  * Highest PWM allowed as the minimum PWM
@@ -75,9 +80,14 @@ __BEGIN_DECLS
 #define PWM_HIGHEST_MIN 1300
 
 /**
- * Maximum PWM in us
+ * Highest maximum PWM in us
+ */
+#define PWM_HIGHEST_MAX 2100
+
+/**
+ * Default maximum PWM in us
  */
-#define PWM_MAX 2100
+#define PWM_DEFAULT_MAX 2000
 
 /**
  * Lowest PWM allowed as the maximum PWM
diff --git a/src/drivers/px4fmu/fmu.cpp b/src/drivers/px4fmu/fmu.cpp
index 4bd27f907..0441566e9 100644
--- a/src/drivers/px4fmu/fmu.cpp
+++ b/src/drivers/px4fmu/fmu.cpp
@@ -232,8 +232,8 @@ PX4FMU::PX4FMU() :
 	_num_disarmed_set(0)
 {
 	for (unsigned i = 0; i < _max_actuators; i++) {
-		_min_pwm[i] = PWM_MIN;
-		_max_pwm[i] = PWM_MAX;
+		_min_pwm[i] = PWM_DEFAULT_MIN;
+		_max_pwm[i] = PWM_DEFAULT_MAX;
 	}
 
 	_debug_enabled = true;
@@ -762,10 +762,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		for (unsigned i = 0; i < pwm->channel_count; i++) {
 			if (pwm->values[i] == 0) {
 				/* ignore 0 */
-			} else if (pwm->values[i] > PWM_MAX) {
-				_failsafe_pwm[i] = PWM_MAX;
-			} else if (pwm->values[i] < PWM_MIN) {
-				_failsafe_pwm[i] = PWM_MIN;
+			} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+				_failsafe_pwm[i] = PWM_HIGHEST_MAX;
+			} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+				_failsafe_pwm[i] = PWM_LOWEST_MIN;
 			} else {
 				_failsafe_pwm[i] = pwm->values[i];
 			}
@@ -801,10 +801,10 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 		for (unsigned i = 0; i < pwm->channel_count; i++) {
 			if (pwm->values[i] == 0) {
 				/* ignore 0 */
-			} else if (pwm->values[i] > PWM_MAX) {
-				_disarmed_pwm[i] = PWM_MAX;
-			} else if (pwm->values[i] < PWM_MIN) {
-				_disarmed_pwm[i] = PWM_MIN;
+			} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+				_disarmed_pwm[i] = PWM_HIGHEST_MAX;
+			} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+				_disarmed_pwm[i] = PWM_LOWEST_MIN;
 			} else {
 				_disarmed_pwm[i] = pwm->values[i];
 			}
@@ -842,8 +842,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 				/* ignore 0 */
 			} else if (pwm->values[i] > PWM_HIGHEST_MIN) {
 				_min_pwm[i] = PWM_HIGHEST_MIN;
-			} else if (pwm->values[i] < PWM_MIN) {
-				_min_pwm[i] = PWM_MIN;
+			} else if (pwm->values[i] < PWM_LOWEST_MIN) {
+				_min_pwm[i] = PWM_LOWEST_MIN;
 			} else {
 				_min_pwm[i] = pwm->values[i];
 			}
@@ -872,8 +872,8 @@ PX4FMU::pwm_ioctl(file *filp, int cmd, unsigned long arg)
 				/* ignore 0 */
 			} else if (pwm->values[i] < PWM_LOWEST_MAX) {
 				_max_pwm[i] = PWM_LOWEST_MAX;
-			} else if (pwm->values[i] > PWM_MAX) {
-				_max_pwm[i] = PWM_MAX;
+			} else if (pwm->values[i] > PWM_HIGHEST_MAX) {
+				_max_pwm[i] = PWM_HIGHEST_MAX;
 			} else {
 				_max_pwm[i] = pwm->values[i];
 			}
diff --git a/src/drivers/px4io/px4io.cpp b/src/drivers/px4io/px4io.cpp
index 63e775857..08e697b9f 100644
--- a/src/drivers/px4io/px4io.cpp
+++ b/src/drivers/px4io/px4io.cpp
@@ -1985,7 +1985,7 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 			/* TODO: we could go lower for e.g. TurboPWM */
 			unsigned channel = cmd - PWM_SERVO_SET(0);
 
-			if ((channel >= _max_actuators) || (arg < PWM_MIN) || (arg > PWM_MAX)) {
+			if ((channel >= _max_actuators) || (arg < PWM_LOWEST_MIN) || (arg > PWM_HIGHEST_MAX)) {
 				ret = -EINVAL;
 
 			} else {
diff --git a/src/examples/px4_simple_app/px4_simple_app.c b/src/examples/px4_simple_app/px4_simple_app.c
index 7f655964d..44e6dc7f3 100644
--- a/src/examples/px4_simple_app/px4_simple_app.c
+++ b/src/examples/px4_simple_app/px4_simple_app.c
@@ -71,7 +71,7 @@ int px4_simple_app_main(int argc, char *argv[])
 
 	int error_counter = 0;
 
-	while (true) {
+	for (int i = 0; i < 5; i++) {
 		/* wait for sensor update of 1 file descriptor for 1000 ms (1 second) */
 		int poll_ret = poll(fds, 1, 1000);
 	 
diff --git a/src/lib/external_lgpl/tecs/tecs.h b/src/lib/external_lgpl/tecs/tecs.h
index 4a98c8e97..f8f832ed7 100644
--- a/src/lib/external_lgpl/tecs/tecs.h
+++ b/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) {
 
 	}
 
diff --git a/src/lib/mathlib/math/arm/Matrix.hpp b/src/lib/mathlib/math/arm/Matrix.hpp
index 715fd3a5e..1945bb02d 100644
--- a/src/lib/mathlib/math/arm/Matrix.hpp
+++ b/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");
diff --git a/src/lib/mathlib/math/arm/Vector.hpp b/src/lib/mathlib/math/arm/Vector.hpp
index 4155800e8..52220fc15 100644
--- a/src/lib/mathlib/math/arm/Vector.hpp
+++ b/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");
diff --git a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp b/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
index 33879892e..18fb6dcbc 100644
--- a/src/modules/att_pos_estimator_ekf/KalmanNav.cpp
+++ b/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
diff --git a/src/modules/commander/commander.cpp b/src/modules/commander/commander.cpp
index 44a144296..ed090271c 100644
--- a/src/modules/commander/commander.cpp
+++ b/src/modules/commander/commander.cpp
@@ -199,7 +199,7 @@ void handle_command(struct vehicle_status_s *status, struct vehicle_control_mode
  */
 int commander_thread_main(int argc, char *argv[]);
 
-void control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed);
+void control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed);
 
 void check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *valid_out, bool *changed);
 
@@ -843,6 +843,12 @@ int commander_thread_main(int argc, char *argv[])
 
 		if (updated) {
 			orb_copy(ORB_ID(safety), safety_sub, &safety);
+
+			// XXX this would be the right approach to do it, but do we *WANT* this?
+			// /* disarm if safety is now on and still armed */
+			// if (safety.safety_switch_available && !safety.safety_off) {
+			// 	(void)arming_state_transition(&status, &safety, &control_mode, ARMING_STATE_STANDBY, &armed);
+			// }
 		}
 
 		/* update global position estimate */
@@ -1219,7 +1225,7 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* play arming and battery warning tunes */
-		if (!arm_tune_played && armed.armed) {
+		if (!arm_tune_played && armed.armed && (!safety.safety_switch_available || (safety.safety_switch_available && safety.safety_off))) {
 			/* play tune when armed */
 			if (tune_arm() == OK)
 				arm_tune_played = true;
@@ -1240,7 +1246,7 @@ int commander_thread_main(int argc, char *argv[])
 		}
 
 		/* reset arm_tune_played when disarmed */
-		if (!(armed.armed && (!safety.safety_switch_available || (safety.safety_off && safety.safety_switch_available)))) {
+		if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) {
 			arm_tune_played = false;
 		}
 
@@ -1309,7 +1315,7 @@ check_valid(hrt_abstime timestamp, hrt_abstime timeout, bool valid_in, bool *val
 }
 
 void
-control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool changed)
+control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_armed, bool changed)
 {
 	/* driving rgbled */
 	if (changed) {
@@ -1356,11 +1362,11 @@ control_status_leds(vehicle_status_s *status, actuator_armed_s *armed, bool chan
 #ifdef CONFIG_ARCH_BOARD_PX4FMU_V1
 
 	/* this runs at around 20Hz, full cycle is 16 ticks = 10/16Hz */
-	if (armed->armed) {
+	if (actuator_armed->armed) {
 		/* armed, solid */
 		led_on(LED_BLUE);
 
-	} else if (armed->ready_to_arm) {
+	} else if (actuator_armed->ready_to_arm) {
 		/* ready to arm, blink at 1Hz */
 		if (leds_counter % 20 == 0)
 			led_toggle(LED_BLUE);
diff --git a/src/modules/commander/mag_calibration.cpp b/src/modules/commander/mag_calibration.cpp
index 09f4104f8..4ebf266f4 100644
--- a/src/modules/commander/mag_calibration.cpp
+++ b/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;
 }
diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index eb67874db..00a0dcd61 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/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 {
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 1a263b741..536380129 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -470,7 +470,7 @@ FixedwingPositionControl::parameters_update()
 	_tecs.set_speed_weight(_parameters.speed_weight);
 	_tecs.set_pitch_damping(_parameters.pitch_damping);
 	_tecs.set_indicated_airspeed_min(_parameters.airspeed_min);
-	_tecs.set_indicated_airspeed_max(_parameters.airspeed_min);
+	_tecs.set_indicated_airspeed_max(_parameters.airspeed_max);
 	_tecs.set_max_climb_rate(_parameters.max_climb_rate);
 
 	/* sanity check parameters */
@@ -520,7 +520,6 @@ FixedwingPositionControl::vehicle_airspeed_poll()
 		orb_copy(ORB_ID(airspeed), _airspeed_sub, &_airspeed);
 		_airspeed_valid = true;
 		_airspeed_last_valid = hrt_absolute_time();
-		return true;
 
 	} else {
 
@@ -533,7 +532,7 @@ FixedwingPositionControl::vehicle_airspeed_poll()
 	/* update TECS state */
 	_tecs.enable_airspeed(_airspeed_valid);
 
-	return false;
+	return airspeed_updated;
 }
 
 void
diff --git a/src/modules/px4iofirmware/registers.c b/src/modules/px4iofirmware/registers.c
index 40597adf1..86a40bc22 100644
--- a/src/modules/px4iofirmware/registers.c
+++ b/src/modules/px4iofirmware/registers.c
@@ -207,7 +207,7 @@ uint16_t		r_page_servo_failsafe[PX4IO_SERVO_COUNT] = { 0, 0, 0, 0, 0, 0, 0, 0 };
  * minimum PWM values when armed
  *
  */
-uint16_t		r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN, PWM_MIN };
+uint16_t		r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN, PWM_DEFAULT_MIN };
 
 /**
  * PAGE 107
@@ -215,7 +215,7 @@ uint16_t		r_page_servo_control_min[PX4IO_SERVO_COUNT] = { PWM_MIN, PWM_MIN, PWM_
  * maximum PWM values when armed
  *
  */
-uint16_t		r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX, PWM_MAX };
+uint16_t		r_page_servo_control_max[PX4IO_SERVO_COUNT] = { PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX, PWM_DEFAULT_MAX };
 
 /**
  * PAGE 108
@@ -278,10 +278,10 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 
 			if (*values == 0) {
 				/* ignore 0 */
-			} else if (*values < PWM_MIN) {
-				r_page_servo_failsafe[offset] = PWM_MIN;
-			} else if (*values > PWM_MAX) {
-				r_page_servo_failsafe[offset] = PWM_MAX;
+			} else if (*values < PWM_LOWEST_MIN) {
+				r_page_servo_failsafe[offset] = PWM_LOWEST_MIN;
+			} else if (*values > PWM_HIGHEST_MAX) {
+				r_page_servo_failsafe[offset] = PWM_HIGHEST_MAX;
 			} else {
 				r_page_servo_failsafe[offset] = *values;
 			}
@@ -304,8 +304,8 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 				/* ignore 0 */
 			} else if (*values > PWM_HIGHEST_MIN) {
 				r_page_servo_control_min[offset] = PWM_HIGHEST_MIN;
-			} else if (*values < PWM_MIN) {
-				r_page_servo_control_min[offset] = PWM_MIN;
+			} else if (*values < PWM_LOWEST_MIN) {
+				r_page_servo_control_min[offset] = PWM_LOWEST_MIN;
 			} else {
 				r_page_servo_control_min[offset] = *values;
 			}
@@ -323,8 +323,8 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 
 			if (*values == 0) {
 				/* ignore 0 */
-			} else if (*values > PWM_MAX) {
-				r_page_servo_control_max[offset] = PWM_MAX;
+			} else if (*values > PWM_HIGHEST_MAX) {
+				r_page_servo_control_max[offset] = PWM_HIGHEST_MAX;
 			} else if (*values < PWM_LOWEST_MAX) {
 				r_page_servo_control_max[offset] = PWM_LOWEST_MAX;
 			} else {
@@ -348,11 +348,11 @@ registers_set(uint8_t page, uint8_t offset, const uint16_t *values, unsigned num
 				if (*values == 0) {
 					/* 0 means disabling always PWM */
 					r_page_servo_disarmed[offset] = 0;
-				} else if (*values < PWM_MIN) {
-					r_page_servo_disarmed[offset] = PWM_MIN;
+				} else if (*values < PWM_LOWEST_MIN) {
+					r_page_servo_disarmed[offset] = PWM_LOWEST_MIN;
 					all_disarmed_off = false;
-				} else if (*values > PWM_MAX) {
-					r_page_servo_disarmed[offset] = PWM_MAX;
+				} else if (*values > PWM_HIGHEST_MAX) {
+					r_page_servo_disarmed[offset] = PWM_HIGHEST_MAX;
 					all_disarmed_off = false;
 				} else {
 					r_page_servo_disarmed[offset] = *values;
diff --git a/src/modules/systemlib/mixer/mixer.h b/src/modules/systemlib/mixer/mixer.h
index 723bf9f3b..1c889a811 100644
--- a/src/modules/systemlib/mixer/mixer.h
+++ b/src/modules/systemlib/mixer/mixer.h
@@ -449,6 +449,7 @@ public:
 		HEX_PLUS,	/**< hex in + configuration */
 		OCTA_X,
 		OCTA_PLUS,
+		OCTA_COX,
 
 		MAX_GEOMETRY
 	};
diff --git a/src/modules/systemlib/mixer/mixer_multirotor.cpp b/src/modules/systemlib/mixer/mixer_multirotor.cpp
index b89f341b6..bf77795d5 100644
--- a/src/modules/systemlib/mixer/mixer_multirotor.cpp
+++ b/src/modules/systemlib/mixer/mixer_multirotor.cpp
@@ -130,6 +130,16 @@ const MultirotorMixer::Rotor _config_octa_plus[] = {
 	{  1.000000,  0.000000, -1.00 },
 	{ -1.000000,  0.000000, -1.00 },
 };
+const MultirotorMixer::Rotor _config_octa_cox[] = {
+	{ -0.707107,  0.707107,  1.00 },
+	{  0.707107,  0.707107, -1.00 },
+	{  0.707107, -0.707107,  1.00 },
+	{ -0.707107, -0.707107, -1.00 },
+	{  0.707107,  0.707107,  1.00 },
+	{ -0.707107,  0.707107, -1.00 },
+	{ -0.707107, -0.707107,  1.00 },
+	{  0.707107, -0.707107, -1.00 },
+};
 const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
 	&_config_quad_x[0],
 	&_config_quad_plus[0],
@@ -139,6 +149,7 @@ const MultirotorMixer::Rotor *_config_index[MultirotorMixer::MAX_GEOMETRY] = {
 	&_config_hex_plus[0],
 	&_config_octa_x[0],
 	&_config_octa_plus[0],
+	&_config_octa_cox[0],
 };
 const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
 	4, /* quad_x */
@@ -149,6 +160,7 @@ const unsigned _config_rotor_count[MultirotorMixer::MAX_GEOMETRY] = {
 	6, /* hex_plus */
 	8, /* octa_x */
 	8, /* octa_plus */
+	8, /* octa_cox */
 };
 
 }
@@ -240,6 +252,9 @@ MultirotorMixer::from_text(Mixer::ControlCallback control_cb, uintptr_t cb_handl
 
 	} else if (!strcmp(geomname, "8x")) {
 		geometry = MultirotorMixer::OCTA_X;
+		
+	} else if (!strcmp(geomname, "8c")) {
+		geometry = MultirotorMixer::OCTA_COX;
 
 	} else {
 		debug("unrecognised geometry '%s'", geomname);
diff --git a/src/modules/systemlib/mixer/multi_tables b/src/modules/systemlib/mixer/multi_tables
index 683c63040..050bf2f47 100755
--- a/src/modules/systemlib/mixer/multi_tables
+++ b/src/modules/systemlib/mixer/multi_tables
@@ -74,7 +74,18 @@ set octa_plus {
 	  90 CW
 }
 
-set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus}
+set octa_cox {
+	  45 CCW
+	 -45 CW
+	-135 CCW
+	 135 CW
+	 -45 CCW
+	  45 CW
+	 135 CCW
+	-135 CW
+}
+
+set tables {quad_x quad_plus quad_v quad_wide hex_x hex_plus octa_x octa_plus octa_cox}
 
 proc factors {a d} { puts [format "\t{ %9.6f, %9.6f, %5.2f }," [rcos [expr $a + 90]] [rcos $a] [expr -$d]]}
 
diff --git a/src/modules/uORB/uORB.cpp b/src/modules/uORB/uORB.cpp
index 7abbf42ae..149b8f6d2 100644
--- a/src/modules/uORB/uORB.cpp
+++ b/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);
diff --git a/src/systemcmds/config/config.c b/src/systemcmds/config/config.c
index 188dafa4e..80689f20c 100644
--- a/src/systemcmds/config/config.c
+++ b/src/systemcmds/config/config.c
@@ -272,7 +272,7 @@ do_accel(int argc, char *argv[])
 			}
 
 		} else {
-			errx(1,"no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 2' to set measurement range to 4 G\n\t");
+			errx(1,"no arguments given. Try: \n\n\t'sampling 500' to set sampling to 500 Hz\n\t'rate 500' to set publication rate to 500 Hz\n\t'range 4' to set measurement range to 4 G\n\t");
 		}
 
 		int srate = ioctl(fd, ACCELIOCGSAMPLERATE, 0);
diff --git a/src/systemcmds/esc_calib/esc_calib.c b/src/systemcmds/esc_calib/esc_calib.c
index d524ab23b..b237b31be 100644
--- a/src/systemcmds/esc_calib/esc_calib.c
+++ b/src/systemcmds/esc_calib/esc_calib.c
@@ -67,8 +67,6 @@
 static void	usage(const char *reason);
 __EXPORT int	esc_calib_main(int argc, char *argv[]);
 
-#define MAX_CHANNELS 14
-
 static void
 usage(const char *reason)
 {
@@ -76,12 +74,15 @@ usage(const char *reason)
 		warnx("%s", reason);
 
 	errx(1,
-	     "usage:\n"
-	     "esc_calib [-l <low pwm>] [-h <high pwm>] [-d <device>] <channels>\n"
-	     "  <device>           PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
-	     "  <channels>         Provide channels (e.g.: 1 2 3 4)\n"
-	    );
-
+		"usage:\n"
+		"esc_calib\n"
+		"    [-d <device>        PWM output device (defaults to " PWM_OUTPUT_DEVICE_PATH ")\n"
+		"    [-l <pwm>           Low PWM value in us (default: %dus)\n"
+		"    [-h <pwm>           High PWM value in us (default: %dus)\n"
+		"    [-c <channels>]     Supply channels (e.g. 1234)\n"
+		"    [-m <chanmask> ]    Directly supply channel mask (e.g. 0xF)\n"
+		"    [-a]                Use all outputs\n"
+		, PWM_DEFAULT_MIN, PWM_DEFAULT_MAX);
 }
 
 int
@@ -89,13 +90,18 @@ esc_calib_main(int argc, char *argv[])
 {
 	char *dev = PWM_OUTPUT_DEVICE_PATH;
 	char *ep;
-	bool channels_selected[MAX_CHANNELS] = {false};
 	int ch;
 	int ret;
 	char c;
 
-	int low = -1;
-	int high = -1;
+	unsigned max_channels = 0;
+
+	uint32_t set_mask = 0;
+	unsigned long channels;
+	unsigned single_ch = 0;
+
+	uint16_t pwm_high = PWM_DEFAULT_MAX;
+	uint16_t pwm_low = PWM_DEFAULT_MIN;
 
 	struct pollfd fds;
 	fds.fd = 0; /* stdin */
@@ -107,7 +113,7 @@ esc_calib_main(int argc, char *argv[])
 
 	int arg_consumed = 0;
 
-	while ((ch = getopt(argc, &argv[0], "l:h:d:")) != -1) {
+	while ((ch = getopt(argc, argv, "d:c:m:al:h:")) != EOF) {
 		switch (ch) {
 
 		case 'd':
@@ -115,41 +121,49 @@ esc_calib_main(int argc, char *argv[])
 			arg_consumed += 2;
 			break;
 
-		case 'l':
-			low = strtoul(optarg, &ep, 0);
-			if (*ep != '\0')
-				usage("bad low pwm value");
-			arg_consumed += 2;
+		case 'c':
+			/* Read in channels supplied as one int and convert to mask: 1234 -> 0xF */
+			channels = strtoul(optarg, &ep, 0);
+
+			while ((single_ch = channels % 10)) {
+
+				set_mask |= 1<<(single_ch-1);
+				channels /= 10;
+			}
 			break;
 
-		case 'h':
-			high = strtoul(optarg, &ep, 0);
+		case 'm':
+			/* Read in mask directly */
+			set_mask = strtoul(optarg, &ep, 0);
 			if (*ep != '\0')
-				usage("bad high pwm value");
-			arg_consumed += 2;
+				usage("bad set_mask value");
+			break;
+
+		case 'a':
+			/* Choose all channels */
+			for (unsigned i = 0; i<PWM_OUTPUT_MAX_CHANNELS; i++) {
+				set_mask |= 1<<i;
+			}
 			break;
 
+		case 'l':
+			/* Read in custom low value */
+			if (*ep != '\0' || pwm_low < PWM_LOWEST_MIN || pwm_low > PWM_HIGHEST_MIN)
+				usage("low PWM invalid");
+			break;
+		case 'h':
+			/* Read in custom high value */
+			pwm_high = strtoul(optarg, &ep, 0);
+			if (*ep != '\0' || pwm_high > PWM_HIGHEST_MAX || pwm_high < PWM_LOWEST_MAX)
+				usage("high PWM invalid");
+			break;
 		default:
 			usage(NULL);
 		}
 	}
 
-	while ((--argc - arg_consumed) > 0) {
-		const char *arg = argv[argc];
-		unsigned channel_number = strtol(arg, &ep, 0);
-
-		warnx("adding channel #%d", channel_number);
-
-		if (*ep == '\0') {
-			if (channel_number > MAX_CHANNELS || channel_number <= 0) {
-				err(1, "invalid channel number: %d", channel_number);
-
-			} else {
-				channels_selected[channel_number - 1] = true;
-
-			}
-		}
-	}
+	if (set_mask == 0)
+		usage("no channels chosen");
 
 	/* make sure no other source is publishing control values now */
 	struct actuator_controls_s actuators;
@@ -217,50 +231,10 @@ esc_calib_main(int argc, char *argv[])
 	if (fd < 0)
 		err(1, "can't open %s", dev);
 
-	/* get max PWM value setting */
-	uint16_t pwm_max = 0;
-
-	if (high > 0 && high > low && high < 2200) {
-		pwm_max = high;
-	} else {
-		ret = ioctl(fd, PWM_SERVO_GET_MAX_PWM, &pwm_max);
-
-		if (ret != OK)
-			err(1, "PWM_SERVO_GET_MAX_PWM");
-	}
-
-	/* bound to sane values */
-	if (pwm_max > 2200)
-		pwm_max = 2200;
-
-	if (pwm_max < 1700)
-		pwm_max = 1700;
-
-	/* get disarmed PWM value setting */
-	uint16_t pwm_disarmed = 0;
-
-	if (low > 0 && low < high && low > 800) {
-		pwm_disarmed = low;
-	} else {
-		ret = ioctl(fd, PWM_SERVO_GET_DISARMED_PWM, &pwm_disarmed);
-
-		if (ret != OK)
-			err(1, "PWM_SERVO_GET_DISARMED_PWM");
-
-		if (pwm_disarmed == 0) {
-			ret = ioctl(fd, PWM_SERVO_GET_MIN_PWM, &pwm_disarmed);
-
-			if (ret != OK)
-				err(1, "PWM_SERVO_GET_MIN_PWM");
-		}
-	}
-
-	/* bound to sane values */
-	if (pwm_disarmed > 1300)
-		pwm_disarmed = 1300;
-
-	if (pwm_disarmed < 800)
-		pwm_disarmed = 800;
+	/* get number of channels available on the device */
+	ret = ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&max_channels);
+	if (ret != OK)
+		err(1, "PWM_SERVO_GET_COUNT");
 
 	/* tell IO/FMU that its ok to disable its safety with the switch */
 	ret = ioctl(fd, PWM_SERVO_SET_ARM_OK, 0);
@@ -273,21 +247,23 @@ esc_calib_main(int argc, char *argv[])
 
 	warnx("Outputs armed");
 
+
 	/* wait for user confirmation */
 	printf("\nHigh PWM set: %d\n"
 	       "\n"
 	       "Connect battery now and hit ENTER after the ESCs confirm the first calibration step\n"
-	       "\n", pwm_max);
+	       "\n", pwm_high);
 	fflush(stdout);
 
 	while (1) {
 		/* set max PWM */
-		for (unsigned i = 0; i < MAX_CHANNELS; i++) {
-			if (channels_selected[i]) {
-				ret = ioctl(fd, PWM_SERVO_SET(i), pwm_max);
+		for (unsigned i = 0; i < max_channels; i++) {
+
+			if (set_mask & 1<<i) {
+				ret = ioctl(fd, PWM_SERVO_SET(i), pwm_high);
 
 				if (ret != OK)
-					err(1, "PWM_SERVO_SET(%d), value: %d", i, pwm_max);
+					err(1, "PWM_SERVO_SET(%d), value: %d", i, pwm_high);
 			}
 		}
 
@@ -314,17 +290,17 @@ esc_calib_main(int argc, char *argv[])
 	printf("Low PWM set: %d\n"
 	       "\n"
 	       "Hit ENTER when finished\n"
-	       "\n", pwm_disarmed);
+	       "\n", pwm_low);
 
 	while (1) {
 
 		/* set disarmed PWM */
-		for (unsigned i = 0; i < MAX_CHANNELS; i++) {
-			if (channels_selected[i]) {
-				ret = ioctl(fd, PWM_SERVO_SET(i), pwm_disarmed);
+		for (unsigned i = 0; i < max_channels; i++) {
+			if (set_mask & 1<<i) {
+				ret = ioctl(fd, PWM_SERVO_SET(i), pwm_low);
 
 				if (ret != OK)
-					err(1, "PWM_SERVO_SET(%d), value: %d", i, pwm_disarmed);
+					err(1, "PWM_SERVO_SET(%d), value: %d", i, pwm_low);
 			}
 		}
 
diff --git a/src/systemcmds/preflight_check/preflight_check.c b/src/systemcmds/preflight_check/preflight_check.c
index 1c58a2db6..07581459b 100644
--- a/src/systemcmds/preflight_check/preflight_check.c
+++ b/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 */