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

Conflicts:
	src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

.gitignore

@@ -35,3 +35,4 @@ mavlink/include/mavlink/v0.9/
 /Documentation/doxygen*objdb*tmp
 .tags
 .tags_sorted_by_file
+.pydevproject

ROMFS/px4fmu_common/init.d/3031_phantom

@@ -5,4 +5,6 @@
 # Simon Wilks <sjwilks@gmail.com>
 #
 
+sh /etc/init.d/rc.fw_defaults
+
 set MIXER FMU_Q

ROMFS/px4fmu_common/init.d/4008_ardrone

@@ -0,0 +1,35 @@
+#!nsh
+#
+# ARDrone
+#
+
+echo "[init] 4008_ardrone: PX4FMU on PX4IOAR carrier board"
+
+# Just use the default multicopter settings.
+sh /etc/init.d/rc.mc_defaults
+
+#
+# Load default params for this platform
+#
+if [ $DO_AUTOCONFIG == yes ]
+then
+    # Set all params here, then disable autoconfig
+    param set MC_ROLL_P 5.0
+    param set MC_ROLLRATE_P 0.13
+    param set MC_ROLLRATE_I 0.0
+    param set MC_ROLLRATE_D 0.0
+    param set MC_PITCH_P 5.0
+    param set MC_PITCHRATE_P 0.13
+    param set MC_PITCHRATE_I 0.0
+    param set MC_PITCHRATE_D 0.0
+    param set MC_YAW_P 1.0
+    param set MC_YAW_D 0.1
+    param set MC_YAWRATE_P 0.15
+    param set MC_YAWRATE_I 0.0
+    param set MC_YAWRATE_D 0.0
+    param set MC_YAW_FF 0.15
+fi
+
+set OUTPUT_MODE ardrone
+set USE_IO no
+set MIXER skip

ROMFS/px4fmu_common/init.d/rc.autostart

@@ -106,6 +106,15 @@ then
 	sh /etc/init.d/4001_quad_x
 fi
 
+#
+# ARDrone
+#
+
+if param compare SYS_AUTOSTART 4008 8
+then
+    sh /etc/init.d/4008_ardrone
+fi
+
 if param compare SYS_AUTOSTART 4010 10
 then
 	sh /etc/init.d/4010_dji_f330

ROMFS/px4fmu_common/init.d/rc.interface

@@ -3,7 +3,7 @@
 # Script to configure control interface
 #
 
-if [ $MIXER != none ]
+if [ $MIXER != none -a $MIXER != skip]
 then
 	#
 	# Load mixer
@@ -33,8 +33,11 @@ then
 		tone_alarm $TUNE_OUT_ERROR
 	fi
 else
-	echo "[init] Mixer not defined"
+	if [ $MIXER != skip ]
-	tone_alarm $TUNE_OUT_ERROR
+	then
+		echo "[init] Mixer not defined"
+		tone_alarm $TUNE_OUT_ERROR
+	fi
 fi
 
 if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE == io ]

ROMFS/px4fmu_common/init.d/rcS

@@ -240,6 +240,11 @@ then
 		fi
 	fi
 
+	if [ $OUTPUT_MODE == ardrone ]
+	then
+		set FMU_MODE gpio_serial
+	fi
+
 	if [ $HIL == yes ]
 	then
 		set OUTPUT_MODE hil
@@ -277,9 +282,9 @@ then
 				tone_alarm $TUNE_OUT_ERROR
 			fi
 		fi
-		if [ $OUTPUT_MODE == fmu ]
+		if [ $OUTPUT_MODE == fmu -o $OUTPUT_MODE = ardrone ]
 		then
-			echo "[init] Use FMU PWM as primary output"
+			echo "[init] Use FMU as primary output"
 			if fmu mode_$FMU_MODE
 			then
 				echo "[init] FMU mode_$FMU_MODE started"
@@ -294,7 +299,7 @@ then
 				then
 					set TTYS1_BUSY yes
 				fi
-				if [ $FMU_MODE == pwm_gpio ]
+				if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ]
 				then
 					set TTYS1_BUSY yes
 				fi
@@ -351,7 +356,7 @@ then
 				fi
 			fi
 		else
-			if [ $OUTPUT_MODE != fmu ]
+			if [ $OUTPUT_MODE != fmu -a $OUTPUT_MODE != ardrone ]
 			then
 				if fmu mode_$FMU_MODE
 				then
@@ -367,7 +372,7 @@ then
 					then
 						set TTYS1_BUSY yes
 					fi
-					if [ $FMU_MODE == pwm_gpio ]
+					if [ $FMU_MODE == pwm_gpio -o $OUTPUT_MODE == ardrone ]
 					then
 						set TTYS1_BUSY yes
 					fi
@@ -427,6 +432,14 @@ then
 		gps start
 	fi
 	
+	#
+	# Start up ARDrone Motor interface
+	#
+	if [ $OUTPUT_MODE == ardrone ]
+	then
+		ardrone_interface start -d /dev/ttyS1
+	fi
+
 	#
 	# Fixed wing setup
 	#

ROMFS/px4fmu_logging/init.d/rcS

@@ -1,88 +0,0 @@
-#!nsh
-#
-# PX4FMU startup script for logging purposes
-#
-
-#
-# Try to mount the microSD card.
-#
-echo "[init] looking for microSD..."
-if mount -t vfat /dev/mmcsd0 /fs/microsd
-then
-	echo "[init] card mounted at /fs/microsd"
-	# Start playing the startup tune
-	tone_alarm start
-else
-	echo "[init] no microSD card found"
-	# Play SOS
-	tone_alarm error
-fi
-
-uorb start
-
-#
-# Start sensor drivers here.
-#
-
-ms5611 start
-adc start
-
-# mag might be external
-if hmc5883 start
-then
-	echo "using HMC5883"
-fi
-
-if mpu6000 start
-then
-	echo "using MPU6000"
-fi
-
-if l3gd20 start
-then
-	echo "using L3GD20(H)"
-fi
-
-if lsm303d start
-then
-	set BOARD fmuv2
-else
-	set BOARD fmuv1
-fi
-
-# Start airspeed sensors
-if meas_airspeed start
-then
-	echo "using MEAS airspeed sensor"
-else
-	if ets_airspeed start
-	then
-		echo "using ETS airspeed sensor (bus 3)"
-	else
-		if ets_airspeed start -b 1
-		then
-			echo "Using ETS airspeed sensor (bus 1)"
-		fi
-	fi
-fi
-
-#
-# Start the sensor collection task.
-# IMPORTANT: this also loads param offsets
-# ALWAYS start this task before the
-# preflight_check.
-#
-if sensors start
-then
-	echo "SENSORS STARTED"
-fi
-
-sdlog2 start -r 250 -e -b 16
-
-if sercon
-then
-	echo "[init] USB interface connected"
-
-	# Try to get an USB console
-	nshterm /dev/ttyACM0 &
-fi

Tools/fix_code_style.sh

@@ -16,4 +16,5 @@ astyle \
     --ignore-exclude-errors-x	\
     --lineend=linux		\
     --exclude=EASTL		\
+    --add-brackets \
     $*

Tools/fix_code_style_ubuntu.sh

@@ -1,19 +0,0 @@
-#!/bin/sh
-astyle \
-    --style=linux		\
-    --indent=force-tab=8	\
-    --indent-cases		\
-    --indent-preprocessor	\
-    --break-blocks=all		\
-    --pad-oper			\
-    --pad-header		\
-    --unpad-paren		\
-    --keep-one-line-blocks	\
-    --keep-one-line-statements	\
-    --align-pointer=name	\
-    --suffix=none		\
-    --lineend=linux		\
-    $*
-    #--ignore-exclude-errors-x	\
-    #--exclude=EASTL		\
-    #--align-reference=name	\

nuttx-configs/px4fmu-v2/nsh/defconfig

@@ -306,7 +306,7 @@ CONFIG_UART5_RXDMA=y
 # CONFIG_USART6_RS485 is not set
 CONFIG_USART6_RXDMA=y
 # CONFIG_UART7_RS485 is not set
-# CONFIG_UART7_RXDMA is not set
+CONFIG_UART7_RXDMA=y
 # CONFIG_UART8_RS485 is not set
 CONFIG_UART8_RXDMA=y
 CONFIG_SERIAL_DISABLE_REORDERING=y
@@ -539,8 +539,8 @@ CONFIG_SERIAL_NPOLLWAITERS=2
 # CONFIG_USART3_SERIAL_CONSOLE is not set
 # CONFIG_UART4_SERIAL_CONSOLE is not set
 # CONFIG_USART6_SERIAL_CONSOLE is not set
-# CONFIG_UART7_SERIAL_CONSOLE is not set
+CONFIG_UART7_SERIAL_CONSOLE=y
-CONFIG_UART8_SERIAL_CONSOLE=y
+# CONFIG_UART8_SERIAL_CONSOLE is not set
 # CONFIG_NO_SERIAL_CONSOLE is not set
 
 #

src/drivers/drv_pwm_output.h

@@ -160,7 +160,7 @@ ORB_DECLARE(output_pwm);
 
 #define DSM2_BIND_PULSES 3	/* DSM_BIND_START ioctl parameter, pulses required to start dsm2 pairing */
 #define DSMX_BIND_PULSES 7	/* DSM_BIND_START ioctl parameter, pulses required to start dsmx pairing */
-#define DSMX8_BIND_PULSES 10	/* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
+#define DSMX8_BIND_PULSES 10 	/* DSM_BIND_START ioctl parameter, pulses required to start 8 or more channel dsmx pairing */
 
 /** power up DSM receiver */
 #define DSM_BIND_POWER_UP _IOC(_PWM_SERVO_BASE, 11)

src/drivers/mkblctrl/mkblctrl.cpp

@@ -705,7 +705,7 @@ MK::mk_check_for_blctrl(unsigned int count, bool showOutput, bool initI2C)
 			Motor[i].State |= MOTOR_STATE_PRESENT_MASK; // set present bit;
 			foundMotorCount++;
 
-			if (Motor[i].MaxPWM == 250) {
+			if ((Motor[i].MaxPWM & 252) == 248) {
 				Motor[i].Version = BLCTRL_NEW;
 
 			} else {

src/drivers/px4fmu/fmu.cpp

@@ -1714,7 +1714,7 @@ fmu_main(int argc, char *argv[])
 	}
 
 
-	fprintf(stderr, "FMU: unrecognised command, try:\n");
+	fprintf(stderr, "FMU: unrecognised command %s, try:\n", verb);
 #if defined(CONFIG_ARCH_BOARD_PX4FMU_V1)
 	fprintf(stderr, "  mode_gpio, mode_serial, mode_pwm, mode_gpio_serial, mode_pwm_serial, mode_pwm_gpio, test\n");
 #elif defined(CONFIG_ARCH_BOARD_PX4FMU_V2)

src/drivers/px4io/px4io.cpp

@@ -244,8 +244,7 @@ private:
 	volatile int		_task;			///< worker task id
 	volatile bool		_task_should_exit;	///< worker terminate flag
 
-	int			_mavlink_fd;		///< mavlink file descriptor. This is opened by class instantiation and Doesn't appear to be usable in main thread.
+	int			_mavlink_fd;		///< mavlink file descriptor.
-	int			_thread_mavlink_fd;	///< mavlink file descriptor for thread.
 
 	perf_counter_t		_perf_update;		///<local performance counter for status updates
 	perf_counter_t		_perf_write;		///<local performance counter for PWM control writes
@@ -474,7 +473,6 @@ PX4IO::PX4IO(device::Device *interface) :
 	_task(-1),
 	_task_should_exit(false),
 	_mavlink_fd(-1),
-	_thread_mavlink_fd(-1),
 	_perf_update(perf_alloc(PC_ELAPSED, "io update")),
 	_perf_write(perf_alloc(PC_ELAPSED, "io write")),
 	_perf_chan_count(perf_alloc(PC_COUNT, "io rc #")),
@@ -507,9 +505,6 @@ PX4IO::PX4IO(device::Device *interface) :
 	/* we need this potentially before it could be set in task_main */
 	g_dev = this;
 
-	/* open MAVLink text channel */
-	_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
-
 	_debug_enabled = false;
 	_servorail_status.rssi_v = 0;
 }
@@ -785,7 +780,7 @@ PX4IO::task_main()
 	hrt_abstime poll_last = 0;
 	hrt_abstime orb_check_last = 0;
 
-	_thread_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
+	_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
 
 	/*
 	 * Subscribe to the appropriate PWM output topic based on whether we are the
@@ -880,6 +875,10 @@ PX4IO::task_main()
 			/* run at 5Hz */
 			orb_check_last = now;
 
+			/* try to claim the MAVLink log FD */
+			if (_mavlink_fd < 0)
+				_mavlink_fd = ::open(MAVLINK_LOG_DEVICE, 0);
+
 			/* check updates on uORB topics and handle it */
 			bool updated = false;
 
@@ -1275,16 +1274,14 @@ void
 PX4IO::dsm_bind_ioctl(int dsmMode)
 {
 	if (!(_status & PX4IO_P_STATUS_FLAGS_SAFETY_OFF)) {
-		/* 0: dsm2, 1:dsmx */
+		mavlink_log_info(_mavlink_fd, "[IO] binding DSM%s RX", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "-X" : "-X8"));
-		if ((dsmMode == 0) || (dsmMode == 1)) {
+		int ret = ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES));
-			mavlink_log_info(_thread_mavlink_fd, "[IO] binding dsm%s rx", (dsmMode == 0) ? "2" : ((dsmMode == 1) ? "x" : "x8"));
+
-			ioctl(nullptr, DSM_BIND_START, (dsmMode == 0) ? DSM2_BIND_PULSES : ((dsmMode == 1) ? DSMX_BIND_PULSES : DSMX8_BIND_PULSES));
+		if (ret)
-		} else {
+			mavlink_log_critical(_mavlink_fd, "binding failed.");
-			mavlink_log_info(_thread_mavlink_fd, "[IO] invalid dsm bind mode, bind request rejected");
-		}
 
 	} else {
-		mavlink_log_info(_thread_mavlink_fd, "[IO] system armed, bind request rejected");
+		mavlink_log_info(_mavlink_fd, "[IO] system armed, bind request rejected");
 	}
 }
 
@@ -2115,14 +2112,24 @@ PX4IO::ioctl(file * /*filep*/, int cmd, unsigned long arg)
 		break;
 
 	case DSM_BIND_START:
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
+
-		usleep(500000);
+		/* only allow DSM2, DSM-X and DSM-X with more than 7 channels */
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
+		if (arg == DSM2_BIND_PULSES ||
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
+		    arg == DSMX_BIND_PULSES ||
-		usleep(72000);
+		    arg == DSMX8_BIND_PULSES) {
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4));
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_down);
-		usleep(50000);
+			usleep(500000);
-		io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_set_rx_out);
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_power_up);
+			usleep(72000);
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_send_pulses | (arg << 4));
+			usleep(50000);
+			io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_DSM, dsm_bind_reinit_uart);
+
+			ret = OK;
+		} else {
+			ret = -EINVAL;
+		}
 		break;
 
 	case DSM_BIND_POWER_UP:
@@ -2615,7 +2622,7 @@ bind(int argc, char *argv[])
 #endif
 
 	if (argc < 3)
-		errx(0, "needs argument, use dsm2 or dsmx");
+		errx(0, "needs argument, use dsm2, dsmx or dsmx8");
 
 	if (!strcmp(argv[2], "dsm2"))
 		pulses = DSM2_BIND_PULSES;
@@ -2624,7 +2631,7 @@ bind(int argc, char *argv[])
 	else if (!strcmp(argv[2], "dsmx8"))
 		pulses = DSMX8_BIND_PULSES;
 	else 
-		errx(1, "unknown parameter %s, use dsm2 or dsmx", argv[2]);
+		errx(1, "unknown parameter %s, use dsm2, dsmx or dsmx8", argv[2]);
 	// Test for custom pulse parameter
 	if (argc > 3)
 		pulses = atoi(argv[3]);

src/lib/ecl/ecl.h

@@ -38,7 +38,6 @@
  */
 
 #include <drivers/drv_hrt.h>
-#include <geo/geo.h>
 
 #define ecl_absolute_time hrt_absolute_time
-#define ecl_elapsed_time hrt_elapsed_time
+#define ecl_elapsed_time hrt_elapsed_time

src/lib/ecl/l1/ecl_l1_pos_controller.cpp

@@ -38,6 +38,8 @@
  *
  */
 
+#include <float.h>
+
 #include "ecl_l1_pos_controller.h"
 
 float ECL_L1_Pos_Controller::nav_roll()
@@ -231,8 +233,15 @@ void ECL_L1_Pos_Controller::navigate_loiter(const math::Vector<2> &vector_A, con
 	/* calculate the vector from waypoint A to current position */
 	math::Vector<2> vector_A_to_airplane = get_local_planar_vector(vector_A, vector_curr_position);
 
-	/* store the normalized vector from waypoint A to current position */
+	math::Vector<2> vector_A_to_airplane_unit;
-	math::Vector<2> vector_A_to_airplane_unit = (vector_A_to_airplane).normalized();
+
+	/* prevent NaN when normalizing */
+	if (vector_A_to_airplane.length() > FLT_EPSILON) {
+		/* store the normalized vector from waypoint A to current position */
+		vector_A_to_airplane_unit = vector_A_to_airplane.normalized();
+	} else {
+		vector_A_to_airplane_unit = vector_A_to_airplane;
+	}
 
 	/* calculate eta angle towards the loiter center */
 

src/lib/external_lgpl/tecs/tecs.cpp

@@ -3,13 +3,10 @@
 #include "tecs.h"
 #include <ecl/ecl.h>
 #include <systemlib/err.h>
+#include <geo/geo.h>
 
 using namespace math;
 
-#ifndef CONSTANTS_ONE_G
-#define CONSTANTS_ONE_G GRAVITY
-#endif
-
 /**
  * @file tecs.cpp
  *

src/lib/external_lgpl/tecs/tecs.h

@@ -28,16 +28,7 @@ class __EXPORT TECS
 {
 public:
 	TECS() :
-
+		_pitch_dem(0.0f),
-		_airspeed_enabled(false),
-		_throttle_slewrate(0.0f),
-		_climbOutDem(false),
-		_hgt_dem_prev(0.0f),
-		_hgt_dem_adj_last(0.0f),
-		_hgt_dem_in_old(0.0f),
-		_TAS_dem_last(0.0f),
-		_TAS_dem_adj(0.0f),
-		_TAS_dem(0.0f),
 		_integ1_state(0.0f),
 		_integ2_state(0.0f),
 		_integ3_state(0.0f),
@@ -45,8 +36,16 @@ public:
 		_integ5_state(0.0f),
 		_integ6_state(0.0f),
 		_integ7_state(0.0f),
-		_pitch_dem(0.0f),
 		_last_pitch_dem(0.0f),
+		_vel_dot(0.0f),
+		_TAS_dem(0.0f),
+		_TAS_dem_last(0.0f),
+		_hgt_dem_in_old(0.0f),
+		_hgt_dem_adj_last(0.0f),
+		_hgt_dem_prev(0.0f),
+		_TAS_dem_adj(0.0f),
+		_STEdotErrLast(0.0f),
+		_climbOutDem(false),
 		_SPE_dem(0.0f),
 		_SKE_dem(0.0f),
 		_SPEdot_dem(0.0f),
@@ -55,9 +54,9 @@ public:
 		_SKE_est(0.0f),
 		_SPEdot(0.0f),
 		_SKEdot(0.0f),
-		_vel_dot(0.0f),
+		_airspeed_enabled(false),
-		_STEdotErrLast(0.0f) {
+		_throttle_slewrate(0.0f)
-
+	{
 	}
 
 	bool airspeed_sensor_enabled() {

src/modules/commander/accelerometer_calibration.cpp

@@ -311,7 +311,7 @@ int do_accel_calibration_measurements(int mavlink_fd, float accel_offs[3], float
 				 (double)accel_ref[orient][2]);
 
 		data_collected[orient] = true;
-		tune_neutral();
+		tune_neutral(true);
 	}
 
 	close(sensor_combined_sub);

src/modules/commander/airspeed_calibration.cpp

@@ -142,7 +142,7 @@ int do_airspeed_calibration(int mavlink_fd)
 		}
 
 		mavlink_log_info(mavlink_fd, CAL_DONE_MSG, sensor_name);
-		tune_neutral();
+		tune_neutral(true);
 		close(diff_pres_sub);
 		return OK;
 

src/modules/commander/commander.cpp

@@ -610,7 +610,6 @@ int commander_thread_main(int argc, char *argv[])
 	/* not yet initialized */
 	commander_initialized = false;
 
-	bool battery_tune_played = false;
 	bool arm_tune_played = false;
 
 	/* set parameters */
@@ -902,11 +901,13 @@ 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 */
-			// /* disarm if safety is now on and still armed */
+			if (status.hil_state == HIL_STATE_OFF && safety.safety_switch_available && !safety.safety_off && armed.armed) {
-			// if (safety.safety_switch_available && !safety.safety_off) {
+				arming_state_t new_arming_state = (status.arming_state == ARMING_STATE_ARMED ? ARMING_STATE_STANDBY : ARMING_STATE_STANDBY_ERROR);
-			// 	(void)arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
+				if (TRANSITION_CHANGED == arming_state_transition(&status, &safety, new_arming_state, &armed)) {
-			// }
+					mavlink_log_info(mavlink_fd, "[cmd] DISARMED by safety switch");
+				}
+			}
 		}
 
 		/* update global position estimate */
@@ -961,7 +962,7 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(battery_status), battery_sub, &battery);
 
 			/* only consider battery voltage if system has been running 2s and battery voltage is valid */
-			if (hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
+			if (status.hil_state == HIL_STATE_OFF && hrt_absolute_time() > start_time + 2000000 && battery.voltage_filtered_v > 0.0f) {
 				status.battery_voltage = battery.voltage_filtered_v;
 				status.battery_current = battery.current_a;
 				status.condition_battery_voltage_valid = true;
@@ -1024,14 +1025,12 @@ int commander_thread_main(int argc, char *argv[])
 			mavlink_log_critical(mavlink_fd, "#audio: WARNING: LOW BATTERY");
 			status.battery_warning = VEHICLE_BATTERY_WARNING_LOW;
 			status_changed = true;
-			battery_tune_played = false;
 
 		} else if (status.condition_battery_voltage_valid && status.battery_remaining < 0.1f && !critical_battery_voltage_actions_done && low_battery_voltage_actions_done) {
 			/* critical battery voltage, this is rather an emergency, change state machine */
 			critical_battery_voltage_actions_done = true;
 			mavlink_log_critical(mavlink_fd, "#audio: EMERGENCY: CRITICAL BATTERY");
 			status.battery_warning = VEHICLE_BATTERY_WARNING_CRITICAL;
-			battery_tune_played = false;
 
 			if (armed.armed) {
 				arming_state_transition(&status, &safety, ARMING_STATE_ARMED_ERROR, &armed);
@@ -1105,7 +1104,7 @@ int commander_thread_main(int argc, char *argv[])
 
 				/* mark home position as set */
 				status.condition_home_position_valid = true;
-				tune_positive();
+				tune_positive(true);
 			}
 		}
 
@@ -1200,8 +1199,9 @@ int commander_thread_main(int argc, char *argv[])
 			/* evaluate the main state machine according to mode switches */
 			res = set_main_state_rc(&status);
 
+			/* play tune on mode change only if armed, blink LED always */
 			if (res == TRANSITION_CHANGED) {
-				tune_positive();
+				tune_positive(armed.armed);
 
 			} else if (res == TRANSITION_DENIED) {
 				/* DENIED here indicates bug in the commander */
@@ -1257,7 +1257,7 @@ int commander_thread_main(int argc, char *argv[])
 		/* flight termination in manual mode if assisted switch is on easy position */
 		if (!status.is_rotary_wing && parachute_enabled && armed.armed && status.main_state == MAIN_STATE_MANUAL && sp_man.assisted_switch > STICK_ON_OFF_LIMIT) {
 			if (TRANSITION_CHANGED == failsafe_state_transition(&status, FAILSAFE_STATE_TERMINATION)) {
-				tune_positive();
+				tune_positive(armed.armed);
 			}
 		}
 
@@ -1312,26 +1312,23 @@ int commander_thread_main(int argc, char *argv[])
 		/* play arming and battery warning tunes */
 		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)
+			set_tune(TONE_ARMING_WARNING_TUNE);
-				arm_tune_played = true;
+			arm_tune_played = true;
-
-		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
-			/* play tune on battery warning */
-			if (tune_low_bat() == OK)
-				battery_tune_played = true;
 
 		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_CRITICAL) {
 			/* play tune on battery critical */
-			if (tune_critical_bat() == OK)
+			set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
-				battery_tune_played = true;
 
-		} else if (battery_tune_played) {
+		} else if (status.battery_warning == VEHICLE_BATTERY_WARNING_LOW || status.failsafe_state != FAILSAFE_STATE_NORMAL) {
-			tune_stop();
+			/* play tune on battery warning or failsafe */
-			battery_tune_played = false;
+			set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
+
+		} else {
+			set_tune(TONE_STOP_TUNE);
 		}
 
 		/* reset arm_tune_played when disarmed */
-		if (status.arming_state != ARMING_STATE_ARMED || (safety.safety_switch_available && !safety.safety_off)) {
+		if (!armed.armed || (safety.safety_switch_available && !safety.safety_off)) {
 			arm_tune_played = false;
 		}
 
@@ -1426,11 +1423,8 @@ control_status_leds(vehicle_status_s *status, const actuator_armed_s *actuator_a
 
 		if (set_normal_color) {
 			/* set color */
-			if (status->battery_warning != VEHICLE_BATTERY_WARNING_NONE) {
+			if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW || status->failsafe_state != FAILSAFE_STATE_NORMAL) {
-				if (status->battery_warning == VEHICLE_BATTERY_WARNING_LOW) {
+				rgbled_set_color(RGBLED_COLOR_AMBER);
-					rgbled_set_color(RGBLED_COLOR_AMBER);
-				}
-
 				/* VEHICLE_BATTERY_WARNING_CRITICAL handled as ARMING_STATE_ARMED_ERROR / ARMING_STATE_STANDBY_ERROR */
 
 			} else {
@@ -1697,15 +1691,9 @@ print_reject_mode(struct vehicle_status_s *status, const char *msg)
 		sprintf(s, "#audio: REJECT %s", msg);
 		mavlink_log_critical(mavlink_fd, s);
 
-		// only buzz if armed, because else we're driving people nuts indoors
+		/* only buzz if armed, because else we're driving people nuts indoors
-		// they really need to look at the leds as well.
+		they really need to look at the leds as well. */
-		if (status->arming_state == ARMING_STATE_ARMED) {
+		tune_negative(armed.armed);
-			tune_negative();
-		} else {
-
-			// Always show the led indication
-			led_negative();
-		}
 	}
 }
 
@@ -1719,7 +1707,7 @@ print_reject_arm(const char *msg)
 		char s[80];
 		sprintf(s, "#audio: %s", msg);
 		mavlink_log_critical(mavlink_fd, s);
-		tune_negative();
+		tune_negative(true);
 	}
 }
 
@@ -1727,27 +1715,27 @@ void answer_command(struct vehicle_command_s &cmd, enum VEHICLE_CMD_RESULT resul
 {
 	switch (result) {
 	case VEHICLE_CMD_RESULT_ACCEPTED:
-			tune_positive();
+			tune_positive(true);
 		break;
 
 	case VEHICLE_CMD_RESULT_DENIED:
 		mavlink_log_critical(mavlink_fd, "#audio: command denied: %u", cmd.command);
-		tune_negative();
+		tune_negative(true);
 		break;
 
 	case VEHICLE_CMD_RESULT_FAILED:
 		mavlink_log_critical(mavlink_fd, "#audio: command failed: %u", cmd.command);
-		tune_negative();
+		tune_negative(true);
 		break;
 
 	case VEHICLE_CMD_RESULT_TEMPORARILY_REJECTED:
 		mavlink_log_critical(mavlink_fd, "#audio: command temporarily rejected: %u", cmd.command);
-		tune_negative();
+		tune_negative(true);
 		break;
 
 	case VEHICLE_CMD_RESULT_UNSUPPORTED:
 		mavlink_log_critical(mavlink_fd, "#audio: command unsupported: %u", cmd.command);
-		tune_negative();
+		tune_negative(true);
 		break;
 
 	default:
@@ -1887,9 +1875,9 @@ void *commander_low_prio_loop(void *arg)
 				}
 
 				if (calib_ret == OK)
-					tune_positive();
+					tune_positive(true);
 				else
-					tune_negative();
+					tune_negative(true);
 
 				arming_state_transition(&status, &safety, ARMING_STATE_STANDBY, &armed);
 

src/modules/commander/commander_helper.cpp

@@ -45,6 +45,7 @@
 #include <stdbool.h>
 #include <fcntl.h>
 #include <math.h>
+#include <string.h>
 
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_status.h>
@@ -81,11 +82,22 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status)
 	       || (current_status->system_type == VEHICLE_TYPE_COAXIAL);
 }
 
-static int buzzer;
+static int buzzer = -1;
-static hrt_abstime blink_msg_end;
+static hrt_abstime blink_msg_end = 0;	// end time for currently blinking LED message, 0 if no blink message
+static hrt_abstime tune_end = 0;		// end time of currently played tune, 0 for repeating tunes or silence
+static int tune_current = TONE_STOP_TUNE;		// currently playing tune, can be interrupted after tune_end
+static unsigned int tune_durations[TONE_NUMBER_OF_TUNES];
 
 int buzzer_init()
 {
+	tune_end = 0;
+	tune_current = 0;
+	memset(tune_durations, 0, sizeof(tune_durations));
+	tune_durations[TONE_NOTIFY_POSITIVE_TUNE] = 800000;
+	tune_durations[TONE_NOTIFY_NEGATIVE_TUNE] = 900000;
+	tune_durations[TONE_NOTIFY_NEUTRAL_TUNE] = 500000;
+	tune_durations[TONE_ARMING_WARNING_TUNE] = 3000000;
+
 	buzzer = open(TONEALARM_DEVICE_PATH, O_WRONLY);
 
 	if (buzzer < 0) {
@@ -101,58 +113,60 @@ void buzzer_deinit()
 	close(buzzer);
 }
 
-void tune_error()
+void set_tune(int tune) {
-{
+	unsigned int new_tune_duration = tune_durations[tune];
-	ioctl(buzzer, TONE_SET_ALARM, TONE_ERROR_TUNE);
+	/* don't interrupt currently playing non-repeating tune by repeating */
+	if (tune_end == 0 || new_tune_duration != 0 || hrt_absolute_time() > tune_end) {
+		/* allow interrupting current non-repeating tune by the same tune */
+		if (tune != tune_current || new_tune_duration != 0) {
+			ioctl(buzzer, TONE_SET_ALARM, tune);
+		}
+		tune_current = tune;
+		if (new_tune_duration != 0) {
+			tune_end = hrt_absolute_time() + new_tune_duration;
+		} else {
+			tune_end = 0;
+		}
+	}
 }
 
-void tune_positive()
+/**
+ * Blink green LED and play positive tune (if use_buzzer == true).
+ */
+void tune_positive(bool use_buzzer)
 {
 	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
 	rgbled_set_color(RGBLED_COLOR_GREEN);
 	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
-	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_POSITIVE_TUNE);
+	if (use_buzzer) {
+		set_tune(TONE_NOTIFY_POSITIVE_TUNE);
+	}
 }
 
-void tune_neutral()
+/**
+ * Blink white LED and play neutral tune (if use_buzzer == true).
+ */
+void tune_neutral(bool use_buzzer)
 {
 	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
 	rgbled_set_color(RGBLED_COLOR_WHITE);
 	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
-	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEUTRAL_TUNE);
+	if (use_buzzer) {
+		set_tune(TONE_NOTIFY_NEUTRAL_TUNE);
+	}
 }
 
-void tune_negative()
+/**
-{
+ * Blink red LED and play negative tune (if use_buzzer == true).
-	led_negative();
+ */
-	ioctl(buzzer, TONE_SET_ALARM, TONE_NOTIFY_NEGATIVE_TUNE);
+void tune_negative(bool use_buzzer)
-}
-
-void led_negative()
 {
 	blink_msg_end = hrt_absolute_time() + BLINK_MSG_TIME;
 	rgbled_set_color(RGBLED_COLOR_RED);
 	rgbled_set_mode(RGBLED_MODE_BLINK_FAST);
-}
+	if (use_buzzer) {
-
+		set_tune(TONE_NOTIFY_NEGATIVE_TUNE);
-int tune_arm()
+	}
-{
-	return ioctl(buzzer, TONE_SET_ALARM, TONE_ARMING_WARNING_TUNE);
-}
-
-int tune_low_bat()
-{
-	return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_SLOW_TUNE);
-}
-
-int tune_critical_bat()
-{
-	return ioctl(buzzer, TONE_SET_ALARM, TONE_BATTERY_WARNING_FAST_TUNE);
-}
-
-void tune_stop()
-{
-	ioctl(buzzer, TONE_SET_ALARM, TONE_STOP_TUNE);
 }
 
 int blink_msg_state()
@@ -161,6 +175,7 @@ int blink_msg_state()
 		return 0;
 
 	} else if (hrt_absolute_time() > blink_msg_end) {
+		blink_msg_end = 0;
 		return 2;
 
 	} else {
@@ -168,8 +183,8 @@ int blink_msg_state()
 	}
 }
 
-static int leds;
+static int leds = -1;
-static int rgbleds;
+static int rgbleds = -1;
 
 int led_init()
 {

src/modules/commander/commander_helper.h

@@ -54,16 +54,10 @@ bool is_rotary_wing(const struct vehicle_status_s *current_status);
 int buzzer_init(void);
 void buzzer_deinit(void);
 
-void tune_error(void);
+void set_tune(int tune);
-void tune_positive(void);
+void tune_positive(bool use_buzzer);
-void tune_neutral(void);
+void tune_neutral(bool use_buzzer);
-void tune_negative(void);
+void tune_negative(bool use_buzzer);
-int tune_arm(void);
-int tune_low_bat(void);
-int tune_critical_bat(void);
-void tune_stop(void);
-
-void led_negative();
 
 int blink_msg_state();
 

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -130,23 +130,23 @@ private:
 	int		_att_sub;			/**< vehicle attitude subscription */
 	int		_attitude_sub;			/**< raw rc channels data subscription */
 	int		_airspeed_sub;			/**< airspeed subscription */
-	int		_control_mode_sub;			/**< vehicle status subscription */
+	int		_control_mode_sub;		/**< vehicle status subscription */
 	int 		_params_sub;			/**< notification of parameter updates */
 	int 		_manual_control_sub;		/**< notification of manual control updates */
-	int		_sensor_combined_sub;			/**< for body frame accelerations */
+	int		_sensor_combined_sub;		/**< for body frame accelerations */
 
 	orb_advert_t	_attitude_sp_pub;		/**< attitude setpoint */
 	orb_advert_t	_nav_capabilities_pub;		/**< navigation capabilities publication */
 
-	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
+	struct vehicle_attitude_s			_att;				/**< vehicle attitude */
-	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
+	struct vehicle_attitude_setpoint_s		_att_sp;			/**< vehicle attitude setpoint */
-	struct navigation_capabilities_s		_nav_capabilities;	/**< navigation capabilities */
+	struct navigation_capabilities_s		_nav_capabilities;		/**< navigation capabilities */
-	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
+	struct manual_control_setpoint_s		_manual;			/**< r/c channel data */
-	struct airspeed_s				_airspeed;		/**< airspeed */
+	struct airspeed_s				_airspeed;			/**< airspeed */
-	struct vehicle_control_mode_s				_control_mode;		/**< vehicle status */
+	struct vehicle_control_mode_s			_control_mode;			/**< vehicle status */
-	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
+	struct vehicle_global_position_s		_global_pos;			/**< global vehicle position */
-	struct position_setpoint_triplet_s			_pos_sp_triplet;		/**< triplet of mission items */
+	struct position_setpoint_triplet_s		_pos_sp_triplet;		/**< triplet of mission items */
-	struct sensor_combined_s				_sensor_combined;			/**< for body frame accelerations */
+	struct sensor_combined_s			_sensor_combined;		/**< for body frame accelerations */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -154,13 +154,13 @@ private:
 
 	/** manual control states */
 	float		_seatbelt_hold_heading;		/**< heading the system should hold in seatbelt mode */
-	float		_loiter_hold_lat;
+	double		_loiter_hold_lat;
-	float		_loiter_hold_lon;
+	double		_loiter_hold_lon;
 	float		_loiter_hold_alt;
 	bool		_loiter_hold;
 
-	float		_launch_lat;
+	double		_launch_lat;
-	float		_launch_lon;
+	double		_launch_lon;
 	float		_launch_alt;
 	bool		_launch_valid;
 
@@ -194,7 +194,7 @@ private:
 	uint64_t _airspeed_last_valid;			///< last time airspeed was valid. Used to detect sensor failures
 	float _groundspeed_undershoot;			///< ground speed error to min. speed in m/s
 	bool _global_pos_valid;				///< global position is valid
-	math::Matrix<3, 3> _R_nb;				///< current attitude
+	math::Matrix<3, 3> _R_nb;			///< current attitude
 
 	ECL_L1_Pos_Controller				_l1_control;
 	TECS						_tecs;
@@ -235,7 +235,6 @@ private:
 		float speedrate_p;
 
 		float land_slope_angle;
-		float land_slope_length;
 		float land_H1_virt;
 		float land_flare_alt_relative;
 		float land_thrust_lim_alt_relative;
@@ -280,7 +279,6 @@ private:
 		param_t speedrate_p;
 
 		param_t land_slope_angle;
-		param_t land_slope_length;
 		param_t land_H1_virt;
 		param_t land_flare_alt_relative;
 		param_t land_thrust_lim_alt_relative;
@@ -374,6 +372,7 @@ FixedwingPositionControl	*g_control;
 
 FixedwingPositionControl::FixedwingPositionControl() :
 
+	_mavlink_fd(-1),
 	_task_should_exit(false),
 	_control_task(-1),
 
@@ -392,39 +391,34 @@ FixedwingPositionControl::FixedwingPositionControl() :
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
+
 /* states */
 	_setpoint_valid(false),
 	_loiter_hold(false),
-	_airspeed_error(0.0f),
-	_airspeed_valid(false),
-	_groundspeed_undershoot(0.0f),
-	_global_pos_valid(false),
 	land_noreturn_horizontal(false),
 	land_noreturn_vertical(false),
 	land_stayonground(false),
 	land_motor_lim(false),
 	land_onslope(false),
-	flare_curve_alt_last(0.0f),
-	_mavlink_fd(-1),
-	launchDetector(),
 	launch_detected(false),
+	last_manual(false),
 	usePreTakeoffThrust(false),
-	last_manual(false)
+	flare_curve_alt_last(0.0f),
+	launchDetector(),
+	_airspeed_error(0.0f),
+	_airspeed_valid(false),
+	_groundspeed_undershoot(0.0f),
+	_global_pos_valid(false),
+	_att(),
+	_att_sp(),
+	_nav_capabilities(),
+	_manual(),
+	_airspeed(),
+	_control_mode(),
+	_global_pos(),
+	_pos_sp_triplet(),
+	_sensor_combined()
 {
-	/* safely initialize structs */
-	vehicle_attitude_s			_att = {0};
-	vehicle_attitude_setpoint_s		_att_sp = {0};
-	navigation_capabilities_s		_nav_capabilities = {0};
-	manual_control_setpoint_s		_manual = {0};
-	airspeed_s				_airspeed = {0};
-	vehicle_control_mode_s			_control_mode = {0};
-	vehicle_global_position_s		_global_pos = {0};
-	position_setpoint_triplet_s			_pos_sp_triplet = {0};
-	sensor_combined_s				_sensor_combined = {0};
-
-
-
-
 	_nav_capabilities.turn_distance = 0.0f;
 
 	_parameter_handles.l1_period = param_find("FW_L1_PERIOD");
@@ -444,7 +438,6 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_parameter_handles.throttle_land_max = param_find("FW_THR_LND_MAX");
 
 	_parameter_handles.land_slope_angle = param_find("FW_LND_ANG");
-	_parameter_handles.land_slope_length = param_find("FW_LND_SLLR");
 	_parameter_handles.land_H1_virt = param_find("FW_LND_HVIRT");
 	_parameter_handles.land_flare_alt_relative = param_find("FW_LND_FLALT");
 	_parameter_handles.land_thrust_lim_alt_relative = param_find("FW_LND_TLALT");
@@ -533,7 +526,6 @@ FixedwingPositionControl::parameters_update()
 	param_get(_parameter_handles.speedrate_p, &(_parameters.speedrate_p));
 
 	param_get(_parameter_handles.land_slope_angle, &(_parameters.land_slope_angle));
-	param_get(_parameter_handles.land_slope_length, &(_parameters.land_slope_length));
 	param_get(_parameter_handles.land_H1_virt, &(_parameters.land_H1_virt));
 	param_get(_parameter_handles.land_flare_alt_relative, &(_parameters.land_flare_alt_relative));
 	param_get(_parameter_handles.land_thrust_lim_alt_relative, &(_parameters.land_thrust_lim_alt_relative));
@@ -600,8 +592,8 @@ FixedwingPositionControl::vehicle_control_mode_poll()
 		orb_copy(ORB_ID(vehicle_control_mode), _control_mode_sub, &_control_mode);
 
 		if (!was_armed && _control_mode.flag_armed) {
-			_launch_lat = _global_pos.lat / 1e7f;
+			_launch_lat = _global_pos.lat;
-			_launch_lon = _global_pos.lon / 1e7f;
+			_launch_lon = _global_pos.lon;
 			_launch_alt = _global_pos.alt;
 			_launch_valid = true;
 		}
@@ -716,7 +708,7 @@ void
 FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &current_position, const math::Vector<2> &ground_speed, const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
 
-	if (_global_pos_valid) {
+	if (_global_pos_valid && !(pos_sp_triplet.current.type == SETPOINT_TYPE_LOITER)) {
 
 		/* rotate ground speed vector with current attitude */
 		math::Vector<2> yaw_vector(_R_nb(0, 0), _R_nb(1, 0));
@@ -902,8 +894,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			float airspeed_land = 1.3f * _parameters.airspeed_min;
 			float airspeed_approach = 1.3f * _parameters.airspeed_min;
 
-			float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1)) * _parameters.land_slope_length;
+			/* Calculate distance (to landing waypoint) and altitude of last ordinary waypoint L */
+			float L_wp_distance = get_distance_to_next_waypoint(prev_wp(0), prev_wp(1), curr_wp(0), curr_wp(1));
 			float L_altitude = landingslope.getLandingSlopeAbsoluteAltitude(L_wp_distance, _pos_sp_triplet.current.alt);
+
 			float bearing_airplane_currwp = get_bearing_to_next_waypoint(current_position(0), current_position(1), curr_wp(0), curr_wp(1));
 			float landing_slope_alt_desired = landingslope.getLandingSlopeAbsoluteAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
 
@@ -929,7 +923,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				float flare_curve_alt = landingslope.getFlareCurveAltitudeSave(wp_distance, bearing_lastwp_currwp, bearing_airplane_currwp, _pos_sp_triplet.current.alt);
 
 				/* avoid climbout */
-				if (flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical || land_stayonground)
+				if ((flare_curve_alt_last < flare_curve_alt && land_noreturn_vertical) || land_stayonground)
 				{
 					flare_curve_alt = pos_sp_triplet.current.alt;
 					land_stayonground = true;
@@ -948,38 +942,24 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				//warnx("Landing:  flare, _global_pos.alt  %.1f, flare_curve_alt %.1f, flare_curve_alt_last %.1f, flare_length %.1f, wp_distance %.1f", _global_pos.alt, flare_curve_alt, flare_curve_alt_last, flare_length, wp_distance);
 
 				flare_curve_alt_last = flare_curve_alt;
-
-			} else if (wp_distance < L_wp_distance) {
-
-				/* minimize speed to approach speed, stay on landing slope */
-				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, landing_slope_alt_desired, calculate_target_airspeed(airspeed_approach),
-							    _airspeed.indicated_airspeed_m_s, eas2tas,
-							    false, flare_pitch_angle_rad,
-							    _parameters.throttle_min, _parameters.throttle_max, _parameters.throttle_cruise,
-							    math::radians(_parameters.pitch_limit_min), math::radians(_parameters.pitch_limit_max));
-				//warnx("Landing: stay on slope, alt_desired: %.1f (wp_distance: %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f, d1 %.1f, flare_length %.1f", landing_slope_alt_desired, wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement, d1, flare_length);
-
-				if (!land_onslope) {
-
-					mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
-					land_onslope = true;
-				}
-
 			} else {
 
 				 /* intersect glide slope:
-				 * if current position is higher or within 10m of slope follow the glide slope
+				  * minimize speed to approach speed
-				 * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope
+				  * if current position is higher or within 10m of slope follow the glide slope
-				 * */
+				  * if current position is below slope -10m continue on maximum of previous wp altitude or L_altitude until the intersection with the slope
+				  * */
 				float altitude_desired = _global_pos.alt;
 				if (_global_pos.alt > landing_slope_alt_desired - 10.0f) {
 					/* stay on slope */
 					altitude_desired = landing_slope_alt_desired;
-					//warnx("Landing: before L, stay on landing slope, alt_desired: %.1f (wp_distance: %.1f, L_wp_distance %.1f), calculate_target_airspeed(airspeed_land) %.1f, horizontal_slope_displacement %.1f", altitude_desired, wp_distance, L_wp_distance, calculate_target_airspeed(airspeed_land), horizontal_slope_displacement);
+					if (!land_onslope) {
+						mavlink_log_info(_mavlink_fd, "#audio: Landing, on slope");
+						land_onslope = true;
+					}
 				} else {
 					/* continue horizontally */
 					altitude_desired =  math::max(_global_pos.alt, L_altitude);
-					//warnx("Landing: before L,continue at: %.4f, (landing_slope_alt_desired %.4f, wp_distance: %.4f, L_altitude: %.4f L_wp_distance: %.4f)", altitude_desired, landing_slope_alt_desired, wp_distance, L_altitude, L_wp_distance);
 				}
 
 				_tecs.update_pitch_throttle(_R_nb, _att.pitch, _global_pos.alt, altitude_desired, calculate_target_airspeed(airspeed_approach),
@@ -1082,13 +1062,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_seatbelt_hold_heading = _att.yaw + _manual.yaw;
 		}
 
-		/* climb out control */
+		//XXX not used
-		bool climb_out = false;
 
-		/* user wants to climb out */
+		/* climb out control */
-		if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
+//		bool climb_out = false;
-			climb_out = true;
+//
-		}
+//		/* user wants to climb out */
+//		if (_manual.pitch > 0.3f && _manual.throttle > 0.8f) {
+//			climb_out = true;
+//		}
 
 		/* if in seatbelt mode, set airspeed based on manual control */
 
@@ -1307,7 +1289,7 @@ FixedwingPositionControl::task_main()
 				float turn_distance = _l1_control.switch_distance(100.0f);
 
 				/* lazily publish navigation capabilities */
-				if (turn_distance != _nav_capabilities.turn_distance && turn_distance > 0) {
+				if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) {
 
 					/* set new turn distance */
 					_nav_capabilities.turn_distance = turn_distance;

src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c

@@ -348,13 +348,6 @@ PARAM_DEFINE_FLOAT(FW_T_SRATE_P, 0.05f);
  */
 PARAM_DEFINE_FLOAT(FW_LND_ANG, 5.0f);
 
-/**
- * Landing slope length
- *
- * @group L1 Control
- */
-PARAM_DEFINE_FLOAT(FW_LND_SLLR, 0.9f);
-
 /**
  *
  *

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -53,11 +53,9 @@
 #include <stdlib.h>
 #include <string.h>
 #include <unistd.h>
-#include <fcntl.h>
 #include <errno.h>
 #include <math.h>
 #include <poll.h>
-#include <time.h>
 #include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
 #include <uORB/uORB.h>
@@ -71,7 +69,6 @@
 #include <uORB/topics/parameter_update.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
-#include <systemlib/pid/pid.h>
 #include <systemlib/perf_counter.h>
 #include <systemlib/systemlib.h>
 #include <mathlib/mathlib.h>
@@ -84,8 +81,8 @@
  */
 extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
 
-#define MIN_TAKEOFF_THROTTLE	0.3f
 #define YAW_DEADZONE	0.05f
+#define MIN_TAKEOFF_THRUST    0.2f
 #define RATES_I_LIMIT	0.3f
 
 class MulticopterAttitudeControl
@@ -135,15 +132,13 @@ private:
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
-	math::Matrix<3, 3>	_R_sp;			/**< attitude setpoint rotation matrix */
-	math::Matrix<3, 3>	_R;				/**< rotation matrix for current state */
 	math::Vector<3>		_rates_prev;	/**< angular rates on previous step */
 	math::Vector<3>		_rates_sp;		/**< angular rates setpoint */
 	math::Vector<3>		_rates_int;		/**< angular rates integral error */
 	float				_thrust_sp;		/**< thrust setpoint */
 	math::Vector<3>		_att_control;	/**< attitude control vector */
 
-	math::Matrix<3, 3>	I;				/**< identity matrix */
+	math::Matrix<3, 3>  _I;				/**< identity matrix */
 
 	bool	_reset_yaw_sp;			/**< reset yaw setpoint flag */
 
@@ -262,13 +257,15 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_actuators_0_pub(-1),
 
 /* performance counters */
-	_loop_perf(perf_alloc(PC_ELAPSED, "mc att control"))
+	_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
 
 {
 	memset(&_v_att, 0, sizeof(_v_att));
 	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
+	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
 	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
 	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
+	memset(&_actuators, 0, sizeof(_actuators));
 	memset(&_armed, 0, sizeof(_armed));
 
 	_params.att_p.zero();
@@ -276,15 +273,13 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params.rate_i.zero();
 	_params.rate_d.zero();
 
-	_R_sp.identity();
-	_R.identity();
 	_rates_prev.zero();
 	_rates_sp.zero();
 	_rates_int.zero();
 	_thrust_sp = 0.0f;
 	_att_control.zero();
 
-	I.identity();
+	_I.identity();
 
 	_params_handles.roll_p			= 	param_find("MC_ROLL_P");
 	_params_handles.roll_rate_p		= 	param_find("MC_ROLLRATE_P");
@@ -535,16 +530,18 @@ MulticopterAttitudeControl::control_attitude(float dt)
 	_thrust_sp = _v_att_sp.thrust;
 
 	/* construct attitude setpoint rotation matrix */
+	math::Matrix<3, 3> R_sp;
+
 	if (_v_att_sp.R_valid) {
 		/* rotation matrix in _att_sp is valid, use it */
-		_R_sp.set(&_v_att_sp.R_body[0][0]);
+		R_sp.set(&_v_att_sp.R_body[0][0]);
 
 	} else {
 		/* rotation matrix in _att_sp is not valid, use euler angles instead */
-		_R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
+		R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body, _v_att_sp.yaw_body);
 
 		/* copy rotation matrix back to setpoint struct */
-		memcpy(&_v_att_sp.R_body[0][0], &_R_sp.data[0][0], sizeof(_v_att_sp.R_body));
+		memcpy(&_v_att_sp.R_body[0][0], &R_sp.data[0][0], sizeof(_v_att_sp.R_body));
 		_v_att_sp.R_valid = true;
 	}
 
@@ -561,23 +558,24 @@ MulticopterAttitudeControl::control_attitude(float dt)
 	}
 
 	/* rotation matrix for current state */
-	_R.set(_v_att.R);
+	math::Matrix<3, 3> R;
+	R.set(_v_att.R);
 
 	/* all input data is ready, run controller itself */
 
 	/* try to move thrust vector shortest way, because yaw response is slower than roll/pitch */
-	math::Vector<3> R_z(_R(0, 2), _R(1, 2), _R(2, 2));
+	math::Vector<3> R_z(R(0, 2), R(1, 2), R(2, 2));
-	math::Vector<3> R_sp_z(_R_sp(0, 2), _R_sp(1, 2), _R_sp(2, 2));
+	math::Vector<3> R_sp_z(R_sp(0, 2), R_sp(1, 2), R_sp(2, 2));
 
 	/* axis and sin(angle) of desired rotation */
-	math::Vector<3> e_R = _R.transposed() * (R_z % R_sp_z);
+	math::Vector<3> e_R = R.transposed() * (R_z % R_sp_z);
 
 	/* calculate angle error */
 	float e_R_z_sin = e_R.length();
 	float e_R_z_cos = R_z * R_sp_z;
 
 	/* calculate weight for yaw control */
-	float yaw_w = _R_sp(2, 2) * _R_sp(2, 2);
+	float yaw_w = R_sp(2, 2) * R_sp(2, 2);
 
 	/* calculate rotation matrix after roll/pitch only rotation */
 	math::Matrix<3, 3> R_rp;
@@ -600,15 +598,15 @@ MulticopterAttitudeControl::control_attitude(float dt)
 		e_R_cp(2, 1) = e_R_z_axis(0);
 
 		/* rotation matrix for roll/pitch only rotation */
-		R_rp = _R * (I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
+		R_rp = R * (_I + e_R_cp * e_R_z_sin + e_R_cp * e_R_cp * (1.0f - e_R_z_cos));
 
 	} else {
 		/* zero roll/pitch rotation */
-		R_rp = _R;
+		R_rp = R;
 	}
 
-	/* R_rp and _R_sp has the same Z axis, calculate yaw error */
+	/* R_rp and R_sp has the same Z axis, calculate yaw error */
-	math::Vector<3> R_sp_x(_R_sp(0, 0), _R_sp(1, 0), _R_sp(2, 0));
+	math::Vector<3> R_sp_x(R_sp(0, 0), R_sp(1, 0), R_sp(2, 0));
 	math::Vector<3> R_rp_x(R_rp(0, 0), R_rp(1, 0), R_rp(2, 0));
 	e_R(2) = atan2f((R_rp_x % R_sp_x) * R_sp_z, R_rp_x * R_sp_x) * yaw_w;
 
@@ -616,7 +614,7 @@ MulticopterAttitudeControl::control_attitude(float dt)
 		/* for large thrust vector rotations use another rotation method:
 		 * calculate angle and axis for R -> R_sp rotation directly */
 		math::Quaternion q;
-		q.from_dcm(_R.transposed() * _R_sp);
+		q.from_dcm(R.transposed() * R_sp);
 		math::Vector<3> e_R_d = q.imag();
 		e_R_d.normalize();
 		e_R_d *= 2.0f * atan2f(e_R_d.length(), q(0));
@@ -658,7 +656,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
 	_rates_prev = rates;
 
 	/* update integral only if not saturated on low limit */
-	if (_thrust_sp > 0.2f) {
+	if (_thrust_sp > MIN_TAKEOFF_THRUST) {
 		for (int i = 0; i < 3; i++) {
 			if (fabsf(_att_control(i)) < _thrust_sp) {
 				float rate_i = _rates_int(i) + _params.rate_i(i) * rates_err(i) * dt;
@@ -695,9 +693,6 @@ MulticopterAttitudeControl::task_main()
 	_manual_control_sp_sub = orb_subscribe(ORB_ID(manual_control_setpoint));
 	_armed_sub = orb_subscribe(ORB_ID(actuator_armed));
 
-	/* rate limit attitude updates to 200Hz, failsafe against spam, normally runs at the same rate as attitude estimator */
-	orb_set_interval(_v_att_sub, 5);
-
 	/* initialize parameters cache */
 	parameters_update();
 
@@ -767,10 +762,12 @@ MulticopterAttitudeControl::task_main()
 				}
 
 			} else {
-				/* attitude controller disabled */
+				/* attitude controller disabled, poll rates setpoint topic */
-				// TODO poll 'attitude_rates_setpoint' topic
+				vehicle_rates_setpoint_poll();
-				_rates_sp.zero();
+				_rates_sp(0) = _v_rates_sp.roll;
-				_thrust_sp = 0.0f;
+				_rates_sp(1) = _v_rates_sp.pitch;
+				_rates_sp(2) = _v_rates_sp.yaw;
+				_thrust_sp = _v_rates_sp.thrust;
 			}
 
 			if (_v_control_mode.flag_control_rates_enabled) {

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -51,7 +51,6 @@
 #include <errno.h>
 #include <math.h>
 #include <poll.h>
-#include <time.h>
 #include <drivers/drv_hrt.h>
 #include <arch/board/board.h>
 #include <uORB/uORB.h>
@@ -68,7 +67,6 @@
 #include <uORB/topics/vehicle_global_velocity_setpoint.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
-#include <systemlib/pid/pid.h>
 #include <systemlib/systemlib.h>
 #include <mathlib/mathlib.h>
 #include <lib/geo/geo.h>
@@ -732,7 +730,6 @@ MulticopterPositionControl::task_main()
 			} else {
 				/* run position & altitude controllers, calculate velocity setpoint */
 				math::Vector<3> pos_err;
-				float err_x, err_y;
 				get_vector_to_next_waypoint_fast(_global_pos.lat, _global_pos.lon, _lat_sp, _lon_sp, &pos_err.data[0], &pos_err.data[1]);
 				pos_err(2) = -(_alt_sp - alt);
 
@@ -794,7 +791,6 @@ MulticopterPositionControl::task_main()
 							}
 
 							thrust_int(2) = -i;
-							mavlink_log_info(_mavlink_fd, "[mpc] reset hovering thrust: %.2f", (double)i);
 						}
 
 					} else {
@@ -806,7 +802,6 @@ MulticopterPositionControl::task_main()
 							reset_int_xy = false;
 							thrust_int(0) = 0.0f;
 							thrust_int(1) = 0.0f;
-							mavlink_log_info(_mavlink_fd, "[mpc] reset xy vel integral");
 						}
 
 					} else {

src/modules/navigator/navigator_main.cpp

@@ -154,17 +154,16 @@ private:
 	int		_capabilities_sub;		/**< notification of vehicle capabilities updates */
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
 
-	orb_advert_t	_pos_sp_triplet_pub;			/**< publish position setpoint triplet */
+	orb_advert_t	_pos_sp_triplet_pub;		/**< publish position setpoint triplet */
 	orb_advert_t	_mission_result_pub;		/**< publish mission result topic */
 
 	struct vehicle_status_s				_vstatus;		/**< vehicle status */
-	struct vehicle_control_mode_s		_control_mode;		/**< vehicle control mode */
+	struct vehicle_control_mode_s			_control_mode;		/**< vehicle control mode */
 	struct vehicle_global_position_s		_global_pos;		/**< global vehicle position */
 	struct home_position_s				_home_pos;		/**< home position for RTL */
-	struct position_setpoint_triplet_s			_pos_sp_triplet;	/**< triplet of position setpoints */
+	struct position_setpoint_triplet_s		_pos_sp_triplet;	/**< triplet of position setpoints */
 	struct mission_result_s				_mission_result;	/**< mission result for commander/mavlink */
-	struct mission_item_s				_mission_item;	/**< current mission item */
+	struct mission_item_s				_mission_item;		/**< current mission item */
-	bool		_mission_item_valid;	/**< current mission item valid */
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
@@ -178,21 +177,22 @@ private:
 
 	class 		Mission				_mission;
 
-	bool		_global_pos_valid;				/**< track changes of global_position.global_valid flag */
+	bool		_mission_item_valid;		/**< current mission item valid */
-	bool		_reset_loiter_pos;				/**< if true then loiter position should be set to current position */
+	bool		_global_pos_valid;		/**< track changes of global_position.global_valid flag */
+	bool		_reset_loiter_pos;		/**< if true then loiter position should be set to current position */
 	bool		_waypoint_position_reached;
 	bool		_waypoint_yaw_reached;
 	uint64_t	_time_first_inside_orbit;
-	bool		_need_takeoff;		/**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */
+	bool		_need_takeoff;			/**< if need to perform vertical takeoff before going to waypoint (only for MISSION mode and VTOL vehicles) */
-	bool		_do_takeoff;		/**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */
+	bool		_do_takeoff;			/**< vertical takeoff state, current mission item is generated by navigator (only for MISSION mode and VTOL vehicles) */
 
 	MissionFeasibilityChecker missionFeasiblityChecker;
 
-	uint64_t	_set_nav_state_timestamp;		/**< timestamp of last handled navigation state request */
+	uint64_t	_set_nav_state_timestamp;	/**< timestamp of last handled navigation state request */
 
 	bool		_pos_sp_triplet_updated;
 
-	char *nav_states_str[NAV_STATE_MAX];
+	const char *nav_states_str[NAV_STATE_MAX];
 
 	struct {
 		float min_altitude;
@@ -321,6 +321,11 @@ private:
 	 */
 	bool		onboard_mission_available(unsigned relative_index);
 
+	/**
+	 * Reset all "reached" flags.
+	 */
+	void		reset_reached();
+
 	/**
 	 * Check if current mission item has been reached.
 	 */
@@ -382,11 +387,11 @@ Navigator::Navigator() :
 	_global_pos_sub(-1),
 	_home_pos_sub(-1),
 	_vstatus_sub(-1),
-	_control_mode_sub(-1),
 	_params_sub(-1),
 	_offboard_mission_sub(-1),
 	_onboard_mission_sub(-1),
 	_capabilities_sub(-1),
+	_control_mode_sub(-1),
 
 /* publications */
 	_pos_sp_triplet_pub(-1),
@@ -395,22 +400,22 @@ Navigator::Navigator() :
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "navigator")),
 
-/* states */
+	_geofence_violation_warning_sent(false),
-	_rtl_state(RTL_STATE_NONE),
 	_fence_valid(false),
 	_inside_fence(true),
 	_mission(),
+	_mission_item_valid(false),
 	_global_pos_valid(false),
 	_reset_loiter_pos(true),
 	_waypoint_position_reached(false),
 	_waypoint_yaw_reached(false),
 	_time_first_inside_orbit(0),
-	_set_nav_state_timestamp(0),
-	_mission_item_valid(false),
 	_need_takeoff(true),
 	_do_takeoff(false),
+	_set_nav_state_timestamp(0),
 	_pos_sp_triplet_updated(false),
-	_geofence_violation_warning_sent(false)
+/* states */
+	_rtl_state(RTL_STATE_NONE)
 {
 	_parameter_handles.min_altitude = param_find("NAV_MIN_ALT");
 	_parameter_handles.acceptance_radius = param_find("NAV_ACCEPT_RAD");
@@ -695,7 +700,7 @@ Navigator::task_main()
 					if (_vstatus.return_switch == RETURN_SWITCH_RETURN) {
 						/* switch to RTL if not already landed after RTL and home position set */
 						if (!(_rtl_state == RTL_STATE_DESCEND &&
-						      (myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
+						      (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
 						    _vstatus.condition_home_position_valid) {
 							dispatch(EVENT_RTL_REQUESTED);
 						}
@@ -741,7 +746,7 @@ Navigator::task_main()
 
 						case NAV_STATE_RTL:
 							if (!(_rtl_state == RTL_STATE_DESCEND &&
-							      (myState == NAV_STATE_READY || myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
+							      (myState == NAV_STATE_LAND || myState == NAV_STATE_LOITER)) &&
 							    _vstatus.condition_home_position_valid) {
 								dispatch(EVENT_RTL_REQUESTED);
 							}
@@ -749,9 +754,7 @@ Navigator::task_main()
 							break;
 
 						case NAV_STATE_LAND:
-							if (myState != NAV_STATE_READY) {
+							dispatch(EVENT_LAND_REQUESTED);
-								dispatch(EVENT_LAND_REQUESTED);
-							}
 
 							break;
 
@@ -853,11 +856,8 @@ Navigator::task_main()
 
 		/* notify user about state changes */
 		if (myState != prevState) {
-			mavlink_log_info(_mavlink_fd, "[navigator] nav state: %s", nav_states_str[myState]);
+			mavlink_log_info(_mavlink_fd, "#audio: navigation state: %s", nav_states_str[myState]);
 			prevState = myState;
-
-			/* reset time counter on state changes */
-			_time_first_inside_orbit = 0;
 		}
 
 		perf_end(_loop_perf);
@@ -955,7 +955,7 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 		/* EVENT_READY_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
 		/* EVENT_LOITER_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
 		/* EVENT_MISSION_REQUESTED */		{ACTION(&Navigator::start_mission), NAV_STATE_MISSION},
-		/* EVENT_RTL_REQUESTED */		{ACTION(&Navigator::start_rtl), NAV_STATE_RTL},
+		/* EVENT_RTL_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
 		/* EVENT_LAND_REQUESTED */		{NO_ACTION, NAV_STATE_READY},
 		/* EVENT_MISSION_CHANGED */		{NO_ACTION, NAV_STATE_READY},
 		/* EVENT_HOME_POSITION_CHANGED */	{NO_ACTION, NAV_STATE_READY},
@@ -1009,6 +1009,8 @@ StateTable::Tran const Navigator::myTable[NAV_STATE_MAX][MAX_EVENT] = {
 void
 Navigator::start_none()
 {
+	reset_reached();
+
 	_pos_sp_triplet.previous.valid = false;
 	_pos_sp_triplet.current.valid = false;
 	_pos_sp_triplet.next.valid = false;
@@ -1024,6 +1026,8 @@ Navigator::start_none()
 void
 Navigator::start_ready()
 {
+	reset_reached();
+
 	_pos_sp_triplet.previous.valid = false;
 	_pos_sp_triplet.current.valid = true;
 	_pos_sp_triplet.next.valid = false;
@@ -1046,6 +1050,8 @@ Navigator::start_ready()
 void
 Navigator::start_loiter()
 {
+	reset_reached();
+
 	_do_takeoff = false;
 
 	/* set loiter position if needed */
@@ -1061,11 +1067,11 @@ Navigator::start_loiter()
 		/* use current altitude if above min altitude set by parameter */
 		if (_global_pos.alt < min_alt_amsl && !_vstatus.is_rotary_wing) {
 			_pos_sp_triplet.current.alt = min_alt_amsl;
-			mavlink_log_info(_mavlink_fd, "[navigator] loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
+			mavlink_log_info(_mavlink_fd, "#audio: loiter %.1fm higher", (double)(min_alt_amsl - _global_pos.alt));
 
 		} else {
 			_pos_sp_triplet.current.alt = _global_pos.alt;
-			mavlink_log_info(_mavlink_fd, "[navigator] loiter at current altitude");
+			mavlink_log_info(_mavlink_fd, "#audio: loiter at current altitude");
 		}
 
 	}
@@ -1091,6 +1097,8 @@ Navigator::start_mission()
 void
 Navigator::set_mission_item()
 {
+	reset_reached();
+
 	/* copy current mission to previous item */
 	memcpy(&_pos_sp_triplet.previous, &_pos_sp_triplet.current, sizeof(position_setpoint_s));
 
@@ -1104,9 +1112,6 @@ Navigator::set_mission_item()
 	ret = _mission.get_current_mission_item(&_mission_item, &onboard, &index);
 
 	if (ret == OK) {
-		/* reset time counter for new item */
-		_time_first_inside_orbit = 0;
-
 		_mission_item_valid = true;
 		position_setpoint_from_mission_item(&_pos_sp_triplet.current, &_mission_item);
 
@@ -1162,14 +1167,14 @@ Navigator::set_mission_item()
 		}
 
 		if (_do_takeoff) {
-			mavlink_log_info(_mavlink_fd, "[navigator] takeoff to %.1fm above home", _pos_sp_triplet.current.alt - _home_pos.alt);
+			mavlink_log_info(_mavlink_fd, "#audio: takeoff to %.1fm above home", (double)(_pos_sp_triplet.current.alt - _home_pos.alt));
 
 		} else {
 			if (onboard) {
-				mavlink_log_info(_mavlink_fd, "[navigator] heading to onboard WP %d", index);
+				mavlink_log_info(_mavlink_fd, "#audio: heading to onboard WP %d", index);
 
 			} else {
-				mavlink_log_info(_mavlink_fd, "[navigator] heading to offboard WP %d", index);
+				mavlink_log_info(_mavlink_fd, "#audio: heading to offboard WP %d", index);
 			}
 		}
 
@@ -1224,6 +1229,8 @@ Navigator::start_rtl()
 void
 Navigator::start_land()
 {
+	reset_reached();
+
 	/* this state can be requested by commander even if no global position available,
 	 * in his case controller must perform landing without position control */
 	_do_takeoff = false;
@@ -1255,6 +1262,8 @@ Navigator::start_land()
 void
 Navigator::start_land_home()
 {
+	reset_reached();
+
 	/* land to home position, should be called when hovering above home, from RTL state */
 	_do_takeoff = false;
 	_reset_loiter_pos = true;
@@ -1285,8 +1294,7 @@ Navigator::start_land_home()
 void
 Navigator::set_rtl_item()
 {
-	/*reset time counter for new RTL item */
+	reset_reached();
-	_time_first_inside_orbit = 0;
 
 	switch (_rtl_state) {
 	case RTL_STATE_CLIMB: {
@@ -1318,7 +1326,7 @@ Navigator::set_rtl_item()
 
 			_pos_sp_triplet.next.valid = false;
 
-			mavlink_log_info(_mavlink_fd, "[navigator] RTL: climb to %.1fm above home", climb_alt - _home_pos.alt);
+			mavlink_log_info(_mavlink_fd, "#audio: RTL: climb to %.1fm above home", (double)(climb_alt - _home_pos.alt));
 			break;
 		}
 
@@ -1330,7 +1338,14 @@ Navigator::set_rtl_item()
 			_mission_item.lat = _home_pos.lat;
 			_mission_item.lon = _home_pos.lon;
 			// don't change altitude
-			_mission_item.yaw = NAN;	// TODO set heading to home
+			if (_pos_sp_triplet.previous.valid) {
+				/* if previous setpoint is valid then use it to calculate heading to home */
+				_mission_item.yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, _mission_item.lat, _mission_item.lon);
+
+			} else {
+				/* else use current position */
+				_mission_item.yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, _mission_item.lat, _mission_item.lon);
+			}
 			_mission_item.loiter_radius = _parameters.loiter_radius;
 			_mission_item.loiter_direction = 1;
 			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
@@ -1344,7 +1359,7 @@ Navigator::set_rtl_item()
 
 			_pos_sp_triplet.next.valid = false;
 
-			mavlink_log_info(_mavlink_fd, "[navigator] RTL: return at %.1fm above home", _mission_item.altitude - _home_pos.alt);
+			mavlink_log_info(_mavlink_fd, "#audio: RTL: return at %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
 			break;
 		}
 
@@ -1362,7 +1377,7 @@ Navigator::set_rtl_item()
 			_mission_item.loiter_direction = 1;
 			_mission_item.nav_cmd = NAV_CMD_WAYPOINT;
 			_mission_item.acceptance_radius = _parameters.acceptance_radius;
-			_mission_item.time_inside = _parameters.rtl_land_delay < 0.0 ? 0.0f : _parameters.rtl_land_delay;
+			_mission_item.time_inside = _parameters.rtl_land_delay < 0.0f ? 0.0f : _parameters.rtl_land_delay;
 			_mission_item.pitch_min = 0.0f;
 			_mission_item.autocontinue = _parameters.rtl_land_delay > -0.001f;
 			_mission_item.origin = ORIGIN_ONBOARD;
@@ -1371,12 +1386,12 @@ Navigator::set_rtl_item()
 
 			_pos_sp_triplet.next.valid = false;
 
-			mavlink_log_info(_mavlink_fd, "[navigator] RTL: descend to %.1fm above home", _mission_item.altitude - _home_pos.alt);
+			mavlink_log_info(_mavlink_fd, "#audio: RTL: descend to %.1fm above home", (double)(_mission_item.altitude - _home_pos.alt));
 			break;
 		}
 
 	default: {
-			mavlink_log_critical(_mavlink_fd, "[navigator] error: unknown RTL state: %d", _rtl_state);
+			mavlink_log_critical(_mavlink_fd, "#audio: [navigator] error: unknown RTL state: %d", _rtl_state);
 			start_loiter();
 			break;
 		}
@@ -1388,7 +1403,8 @@ Navigator::set_rtl_item()
 void
 Navigator::request_loiter_or_ready()
 {
-	if (_vstatus.condition_landed) {
+	/* XXX workaround: no landing detector for fixedwing yet */
+	if (_vstatus.condition_landed && _vstatus.is_rotary_wing) {
 		dispatch(EVENT_READY_REQUESTED);
 
 	} else {
@@ -1418,17 +1434,28 @@ Navigator::position_setpoint_from_mission_item(position_setpoint_s *sp, mission_
 		sp->lon = _home_pos.lon;
 		sp->alt = _home_pos.alt + _parameters.rtl_alt;
 
+		if (_pos_sp_triplet.previous.valid) {
+			/* if previous setpoint is valid then use it to calculate heading to home */
+			sp->yaw = get_bearing_to_next_waypoint(_pos_sp_triplet.previous.lat, _pos_sp_triplet.previous.lon, sp->lat, sp->lon);
+
+		} else {
+			/* else use current position */
+			sp->yaw = get_bearing_to_next_waypoint(_global_pos.lat, _global_pos.lon, sp->lat, sp->lon);
+		}
+		sp->loiter_radius = _parameters.loiter_radius;
+		sp->loiter_direction = 1;
+		sp->pitch_min = 0.0f;
+
 	} else {
 		sp->lat = item->lat;
 		sp->lon = item->lon;
 		sp->alt = item->altitude_is_relative ? item->altitude + _home_pos.alt : item->altitude;
+		sp->yaw = item->yaw;
+		sp->loiter_radius = item->loiter_radius;
+		sp->loiter_direction = item->loiter_direction;
+		sp->pitch_min = item->pitch_min;
 	}
 
-	sp->yaw = item->yaw;
-	sp->loiter_radius = item->loiter_radius;
-	sp->loiter_direction = item->loiter_direction;
-	sp->pitch_min = item->pitch_min;
-
 	if (item->nav_cmd == NAV_CMD_TAKEOFF) {
 		sp->type = SETPOINT_TYPE_TAKEOFF;
 
@@ -1505,7 +1532,7 @@ Navigator::check_mission_item_reached()
 		}
 	}
 
-	if (!_waypoint_yaw_reached) {
+	if (_waypoint_position_reached && !_waypoint_yaw_reached) {
 		if (_vstatus.is_rotary_wing && !_do_takeoff && isfinite(_mission_item.yaw)) {
 			/* check yaw if defined only for rotary wing except takeoff */
 			float yaw_err = _wrap_pi(_mission_item.yaw - _global_pos.yaw);
@@ -1525,16 +1552,14 @@ Navigator::check_mission_item_reached()
 			_time_first_inside_orbit = now;
 
 			if (_mission_item.time_inside > 0.01f) {
-				mavlink_log_info(_mavlink_fd, "[navigator] waypoint reached, wait for %.1fs", _mission_item.time_inside);
+				mavlink_log_info(_mavlink_fd, "#audio: waypoint reached, wait for %.1fs", (double)_mission_item.time_inside);
 			}
 		}
 
 		/* check if the MAV was long enough inside the waypoint orbit */
 		if ((now - _time_first_inside_orbit >= (uint64_t)_mission_item.time_inside * 1e6)
 		    || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
-			_time_first_inside_orbit = 0;
+			reset_reached();
-			_waypoint_yaw_reached = false;
-			_waypoint_position_reached = false;
 			return true;
 		}
 	}
@@ -1543,6 +1568,15 @@ Navigator::check_mission_item_reached()
 
 }
 
+void
+Navigator::reset_reached()
+{
+	_time_first_inside_orbit = 0;
+	_waypoint_position_reached = false;
+	_waypoint_yaw_reached = false;
+
+}
+
 void
 Navigator::on_mission_item_reached()
 {
@@ -1550,7 +1584,7 @@ Navigator::on_mission_item_reached()
 		if (_do_takeoff) {
 			/* takeoff completed */
 			_do_takeoff = false;
-			mavlink_log_info(_mavlink_fd, "[navigator] takeoff completed");
+			mavlink_log_info(_mavlink_fd, "#audio: takeoff completed");
 
 		} else {
 			/* advance by one mission item */

src/modules/position_estimator_inav/position_estimator_inav_main.c

@@ -42,14 +42,11 @@
 #include <stdio.h>
 #include <stdbool.h>
 #include <fcntl.h>
-#include <float.h>
 #include <string.h>
 #include <nuttx/config.h>
 #include <nuttx/sched.h>
 #include <sys/prctl.h>
 #include <termios.h>
-#include <errno.h>
-#include <limits.h>
 #include <math.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/parameter_update.h>
@@ -170,12 +167,13 @@ void write_debug_log(const char *msg, float dt, float x_est[3], float y_est[3],
 	FILE *f = fopen("/fs/microsd/inav.log", "a");
 	if (f) {
 		char *s = malloc(256);
-		snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]);
+		unsigned n = snprintf(s, 256, "%llu %s\n\tdt=%.5f x_est=[%.5f %.5f %.5f] y_est=[%.5f %.5f %.5f] z_est=[%.5f %.5f %.5f]\n", hrt_absolute_time(), msg, dt, x_est[0], x_est[1], x_est[2], y_est[0], y_est[1], y_est[2], z_est[0], z_est[1], z_est[2]);
-		fputs(f, s);
+		fwrite(s, 1, n, f);
-		snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v);
+		n = snprintf(s, 256, "\tacc_corr=[%.5f %.5f %.5f] gps_pos_corr=[%.5f %.5f %.5f] gps_vel_corr=[%.5f %.5f %.5f] w_xy_gps_p=%.5f w_xy_gps_v=%.5f\n", corr_acc[0], corr_acc[1], corr_acc[2], corr_gps[0][0], corr_gps[1][0], corr_gps[2][0], corr_gps[0][1], corr_gps[1][1], corr_gps[2][1], w_xy_gps_p, w_xy_gps_v);
-		fputs(f, s);
+		fwrite(s, 1, n, f);
 		free(s);
 	}
+	fsync(fileno(f));
 	fclose(f);
 }
 
@@ -711,6 +709,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 		inertial_filter_correct(corr_gps[2][0], dt, z_est, 0, w_z_gps_p);
 		inertial_filter_correct(corr_acc[2], dt, z_est, 2, params.w_z_acc);
 
+		float x_est_prev[3], y_est_prev[3];
+
+		memcpy(x_est_prev, x_est, sizeof(x_est));
+		memcpy(y_est_prev, y_est, sizeof(y_est));
+
 		if (can_estimate_xy) {
 			/* inertial filter prediction for position */
 			inertial_filter_predict(dt, x_est);
@@ -718,7 +721,8 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 			if (!isfinite(x_est[0]) || !isfinite(y_est[0])) {
 				write_debug_log("BAD ESTIMATE AFTER PREDICTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
-				thread_should_exit = true;
+				memcpy(x_est, x_est_prev, sizeof(x_est));
+				memcpy(y_est, y_est_prev, sizeof(y_est));
 			}
 
 			/* inertial filter correction for position */
@@ -742,7 +746,11 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 
 			if (!isfinite(x_est[0]) || !isfinite(y_est[0])) {
 				write_debug_log("BAD ESTIMATE AFTER CORRECTION", dt, x_est, y_est, z_est, corr_acc, corr_gps, w_xy_gps_p, w_xy_gps_v);
-				thread_should_exit = true;
+				memcpy(x_est, x_est_prev, sizeof(x_est));
+				memcpy(y_est, y_est_prev, sizeof(y_est));
+				memset(corr_acc, 0, sizeof(corr_acc));
+				memset(corr_gps, 0, sizeof(corr_gps));
+				memset(corr_flow, 0, sizeof(corr_flow));
 			}
 		}
 

src/modules/px4iofirmware/dsm.c

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -162,6 +162,7 @@ dsm_guess_format(bool reset)
 		0xff,	/* 8 channels (DX8) */
 		0x1ff,	/* 9 channels (DX9, etc.) */
 		0x3ff,	/* 10 channels (DX10) */
+		0x1fff,	/* 13 channels (DX10t) */
 		0x3fff	/* 18 channels (DX10) */
 	};
 	unsigned votes10 = 0;
@@ -368,11 +369,25 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 		if (channel >= *num_values)
 			*num_values = channel + 1;
 
-		/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding in a very sloppy fashion */
+		/* convert 0-1024 / 0-2048 values to 1000-2000 ppm encoding. */
-		if (dsm_channel_shift == 11)
+		if (dsm_channel_shift == 10)
-			value /= 2;
+			value *= 2;
 
-		value += 998;
+		/*
+		 * Spektrum scaling is special. There are these basic considerations
+		 *
+		 *   * Midpoint is 1520 us
+		 *   * 100% travel channels are +- 400 us
+		 *
+		 * We obey the original Spektrum scaling (so a default setup will scale from
+		 * 1100 - 1900 us), but we do not obey the weird 1520 us center point
+		 * and instead (correctly) center the center around 1500 us. This is in order
+		 * to get something useful without requiring the user to calibrate on a digital
+		 * link for no reason.
+		 */
+
+		/* scaled integer for decent accuracy while staying efficient */
+		value = ((((int)value - 1024) * 1000) / 1700) + 1500;
 
 		/*
 		 * Store the decoded channel into the R/C input buffer, taking into
@@ -400,6 +415,15 @@ dsm_decode(hrt_abstime frame_time, uint16_t *values, uint16_t *num_values)
 		values[channel] = value;
 	}
 
+	/*
+	 * Spektrum likes to send junk in higher channel numbers to fill
+	 * their packets. We don't know about a 13 channel model in their TX
+	 * lines, so if we get a channel count of 13, we'll return 12 (the last
+	 * data index that is stable).
+	 */
+	if (*num_values == 13)
+		*num_values = 12;
+
 	if (dsm_channel_shift == 11) {
 		/* Set the 11-bit data indicator */
 		*num_values |= 0x8000;

src/modules/px4iofirmware/mixer.cpp

@@ -179,7 +179,7 @@ mixer_tick(void)
 							    ((r_setup_arming & PX4IO_P_SETUP_ARMING_FMU_ARMED)
 		/* and there is valid input via or mixer */         &&   (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK) )
 		/* or direct PWM is set */               || (r_status_flags & PX4IO_P_STATUS_FLAGS_RAW_PWM)
-		/* or failsafe was set manually */	 || (r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM)
+		/* or failsafe was set manually */	 || ((r_setup_arming & PX4IO_P_SETUP_ARMING_FAILSAFE_CUSTOM) && !(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK))
 						     )
 	);
 

src/modules/px4iofirmware/registers.c

@@ -566,7 +566,7 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			break;
 
 		case PX4IO_P_SETUP_DSM:
-			dsm_bind(value & 0x0f, (value >> 4) & 7);
+			dsm_bind(value & 0x0f, (value >> 4) & 0xF);
 			break;
 
 		default:

src/modules/sdlog2/logbuffer.c

@@ -1,7 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2013, 2014 PX4 Development Team. All rights reserved.
- *   Author: Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions

src/modules/sdlog2/sdlog2.c

@@ -1,8 +1,6 @@
 /****************************************************************************
  *
  *   Copyright (c) 2012-2014 PX4 Development Team. All rights reserved.
- *   Author: Lorenz Meier <lm@inf.ethz.ch>
- *           Anton Babushkin <anton.babushkin@me.com>
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -451,6 +449,7 @@ static void *logwriter_thread(void *arg)
 				n = available;
 			}
 
+			lseek(log_fd, 0, SEEK_CUR);
 			n = write(log_fd, read_ptr, n);
 
 			should_wait = (n == available) && !is_part;

src/modules/systemlib/perf_counter.h

@@ -39,6 +39,8 @@
 #ifndef _SYSTEMLIB_PERF_COUNTER_H
 #define _SYSTEMLIB_PERF_COUNTER_H value
 
+#include <stdint.h>
+
 /**
  * Counter types.
  */

src/systemcmds/tests/module.mk

@@ -26,6 +26,7 @@ SRCS			 = test_adc.c \
 			   test_mixer.cpp \
 			   test_mathlib.cpp \
 			   test_file.c \
+			   test_file2.c \
 			   tests_main.c \
 			   test_param.c \
 			   test_ppm_loopback.c \

src/systemcmds/tests/test_file2.c

@@ -0,0 +1,196 @@
+/****************************************************************************
+ *
+ *   Copyright (C) 2012 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file test_file2.c
+ *
+ * File write test.
+ */
+
+#include <sys/stat.h>
+#include <dirent.h>
+#include <stdio.h>
+#include <stddef.h>
+#include <unistd.h>
+#include <fcntl.h>
+#include <systemlib/err.h>
+#include <systemlib/perf_counter.h>
+#include <string.h>
+#include <stdlib.h>
+#include <getopt.h>
+
+#define FLAG_FSYNC 1
+#define FLAG_LSEEK 2
+
+/*
+  return a predictable value for any file offset to allow detection of corruption
+ */
+static uint8_t get_value(uint32_t ofs)
+{
+	union {
+		uint32_t ofs;
+		uint8_t buf[4];
+	} u;
+	u.ofs = ofs;
+	return u.buf[ofs % 4];
+}
+
+static void test_corruption(const char *filename, uint32_t write_chunk, uint32_t write_size, uint16_t flags)
+{
+	printf("Testing on %s with write_chunk=%u write_size=%u\n", 
+	       filename, (unsigned)write_chunk, (unsigned)write_size);
+	
+	uint32_t ofs = 0;
+	int fd = open(filename, O_CREAT | O_RDWR | O_TRUNC);
+	if (fd == -1) {
+		perror(filename);
+		exit(1);
+	}
+	
+	// create a file of size write_size, in write_chunk blocks
+	uint8_t counter = 0;
+	while (ofs < write_size) {
+        uint8_t buffer[write_chunk];
+        for (uint16_t j=0; j<write_chunk; j++) {
+		buffer[j] = get_value(ofs);
+		ofs++;
+        }
+        if (write(fd, buffer, sizeof(buffer)) != sizeof(buffer)) {
+		printf("write failed at offset %u\n", ofs);
+		exit(1);
+        }
+	if (flags & FLAG_FSYNC) {
+		fsync(fd);
+	}
+        if (counter % 100 == 0) {
+		printf("write ofs=%u\r", ofs);
+        }
+        counter++;
+	}
+	close(fd);
+	
+	printf("write ofs=%u\n", ofs);
+	
+	// read and check
+	fd = open(filename, O_RDONLY);
+	if (fd == -1) {
+		perror(filename);
+		exit(1);
+	}
+
+	counter = 0;
+	ofs = 0;
+	while (ofs < write_size) {
+		uint8_t buffer[write_chunk];
+		if (counter % 100 == 0) {
+			printf("read ofs=%u\r", ofs);
+		}
+		counter++;
+		if (read(fd, buffer, sizeof(buffer)) != sizeof(buffer)) {
+			printf("read failed at offset %u\n", ofs);
+			exit(1);
+		}
+		for (uint16_t j=0; j<write_chunk; j++) {
+			if (buffer[j] != get_value(ofs)) {
+				printf("corruption at ofs=%u got %u\n", ofs, buffer[j]);
+				exit(1);
+			}
+			ofs++;
+		}
+		if (flags & FLAG_LSEEK) {
+			lseek(fd, 0, SEEK_CUR);
+		}
+	}
+	printf("read ofs=%u\n", ofs);
+	close(fd);
+	unlink(filename);
+	printf("All OK\n");    
+}
+
+static void usage(void)
+{
+	printf("test file2 [options] [filename]\n");
+	printf("\toptions:\n");
+	printf("\t-s SIZE     set file size\n");
+	printf("\t-c CHUNK    set IO chunk size\n");
+	printf("\t-F          fsync on every write\n");
+	printf("\t-L          lseek on every read\n");
+}
+
+int test_file2(int argc, char *argv[])
+{
+	int opt;
+	uint16_t flags = 0;
+	const char *filename = "/fs/microsd/testfile2.dat";
+	uint32_t write_chunk = 64;
+	uint32_t write_size = 5*1024;
+
+	while ((opt = getopt(argc, argv, "c:s:FLh")) != EOF) {
+		switch (opt) {
+		case 'F':
+			flags |= FLAG_FSYNC;
+			break;
+		case 'L':
+			flags |= FLAG_LSEEK;
+			break;
+		case 's':
+			write_size = strtoul(optarg, NULL, 0);
+			break;
+		case 'c':
+			write_chunk = strtoul(optarg, NULL, 0);
+			break;
+		case 'h':
+		default:
+			usage();
+			exit(1);
+		}
+	}
+
+	argc -= optind;
+	argv += optind;
+
+	if (argc > 0) {
+		filename = argv[0];
+	}
+
+	/* check if microSD card is mounted */
+	struct stat buffer;
+	if (stat("/fs/microsd/", &buffer)) {
+		warnx("no microSD card mounted, aborting file test");
+		return 1;
+	}
+
+        test_corruption(filename, write_chunk, write_size, flags);
+        return 0;
+}
+

src/systemcmds/tests/test_mount.c

@@ -141,8 +141,8 @@ test_mount(int argc, char *argv[])
 			/* announce mode switch */
 			if (it_left_fsync_prev != it_left_fsync && it_left_fsync == 0) {
 				warnx("\n SUCCESSFULLY PASSED FSYNC'ED WRITES, CONTINUTING WITHOUT FSYNC");
-				fsync(stdout);
+				fsync(fileno(stdout));
-				fsync(stderr);
+				fsync(fileno(stderr));
 				usleep(20000);
 			}
 
@@ -162,7 +162,7 @@ test_mount(int argc, char *argv[])
 	}
 
 	char buf[64];
-	int wret = sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort);
+	(void)sprintf(buf, "TEST: %d %d ", it_left_fsync, it_left_abort);
 	lseek(cmd_fd, 0, SEEK_SET);
 	write(cmd_fd, buf, strlen(buf) + 1);
 	fsync(cmd_fd);
@@ -174,8 +174,8 @@ test_mount(int argc, char *argv[])
 
 		printf("\n\n====== FILE TEST: %u bytes chunks (%s) ======\n", chunk_sizes[c], (it_left_fsync > 0) ? "FSYNC" : "NO FSYNC");
 		printf("unpower the system immediately (within 0.5s) when the hash (#) sign appears\n");
-		fsync(stdout);
+		fsync(fileno(stdout));
-		fsync(stderr);
+		fsync(fileno(stderr));
 		usleep(50000);
 
 		for (unsigned a = 0; a < alignments; a++) {
@@ -185,22 +185,20 @@ test_mount(int argc, char *argv[])
 			uint8_t write_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
 
 			/* fill write buffer with known values */
-			for (int i = 0; i < sizeof(write_buf); i++) {
+			for (unsigned i = 0; i < sizeof(write_buf); i++) {
 				/* this will wrap, but we just need a known value with spacing */
 				write_buf[i] = i+11;
 			}
 
 			uint8_t read_buf[chunk_sizes[c] + alignments] __attribute__((aligned(64)));
-			hrt_abstime start, end;
 
 			int fd = open("/fs/microsd/testfile", O_TRUNC | O_WRONLY | O_CREAT);
 
-			start = hrt_absolute_time();
 			for (unsigned i = 0; i < iterations; i++) {
 
 				int wret = write(fd, write_buf + a, chunk_sizes[c]);
 
-				if (wret != chunk_sizes[c]) {
+				if (wret != (int)chunk_sizes[c]) {
 					warn("WRITE ERROR!");
 
 					if ((0x3 & (uintptr_t)(write_buf + a)))
@@ -214,8 +212,8 @@ test_mount(int argc, char *argv[])
 					fsync(fd);
 				} else {
 					printf("#");
-					fsync(stdout);
+					fsync(fileno(stdout));
-					fsync(stderr);
+					fsync(fileno(stderr));
 				}
 			}
 
@@ -224,12 +222,10 @@ test_mount(int argc, char *argv[])
 			}
 
 			printf(".");
-			fsync(stdout);
+			fsync(fileno(stdout));
-			fsync(stderr);
+			fsync(fileno(stderr));
 			usleep(200000);
 
-			end = hrt_absolute_time();
-
 			close(fd);
 			fd = open("/fs/microsd/testfile", O_RDONLY);
 
@@ -237,7 +233,7 @@ test_mount(int argc, char *argv[])
 			for (unsigned i = 0; i < iterations; i++) {
 				int rret = read(fd, read_buf, chunk_sizes[c]);
 
-				if (rret != chunk_sizes[c]) {
+				if (rret != (int)chunk_sizes[c]) {
 					warnx("READ ERROR!");
 					return 1;
 				}
@@ -245,7 +241,7 @@ test_mount(int argc, char *argv[])
 				/* compare value */
 				bool compare_ok = true;
 
-				for (int j = 0; j < chunk_sizes[c]; j++) {
+				for (unsigned j = 0; j < chunk_sizes[c]; j++) {
 					if (read_buf[j] != write_buf[j + a]) {
 						warnx("COMPARISON ERROR: byte %d, align shift: %d", j, a);
 						compare_ok = false;
@@ -271,16 +267,16 @@ test_mount(int argc, char *argv[])
 		}
 	}
 
-	fsync(stdout);
+	fsync(fileno(stdout));
-	fsync(stderr);
+	fsync(fileno(stderr));
 	usleep(20000);
 
 
 
 	/* we always reboot for the next test if we get here */
 	warnx("Iteration done, rebooting..");
-	fsync(stdout);
+	fsync(fileno(stdout));
-	fsync(stderr);
+	fsync(fileno(stderr));
 	usleep(50000);
 	systemreset(false);
 

src/systemcmds/tests/tests.h

@@ -107,6 +107,7 @@ extern int	test_jig_voltages(int argc, char *argv[]);
 extern int	test_param(int argc, char *argv[]);
 extern int	test_bson(int argc, char *argv[]);
 extern int	test_file(int argc, char *argv[]);
+extern int	test_file2(int argc, char *argv[]);
 extern int	test_mixer(int argc, char *argv[]);
 extern int	test_rc(int argc, char *argv[]);
 extern int	test_conv(int argc, char *argv[]);

src/systemcmds/tests/tests_main.c

@@ -104,6 +104,7 @@ const struct {
 	{"param",		test_param,	0},
 	{"bson",		test_bson,	0},
 	{"file",		test_file,	0},
+	{"file2",		test_file2,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"mixer",		test_mixer,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"rc",			test_rc,	OPT_NOJIGTEST | OPT_NOALLTEST},
 	{"conv",		test_conv,	OPT_NOJIGTEST | OPT_NOALLTEST},

src/systemcmds/top/top.c

@@ -233,8 +233,8 @@ top_main(void)
 					   system_load.tasks[i].tcb->pid,
 					   CONFIG_TASK_NAME_SIZE, system_load.tasks[i].tcb->name,
 					   (system_load.tasks[i].total_runtime / 1000),
-					   (int)(curr_loads[i] * 100),
+					   (int)(curr_loads[i] * 100.0f),
-					   (int)(curr_loads[i] * 100000.0f - (int)(curr_loads[i] * 1000.0f) * 100),
+					   (int)((curr_loads[i] * 100.0f - (int)(curr_loads[i] * 100.0f)) * 1000),
 					   stack_size - stack_free,
 					   stack_size,
 					   system_load.tasks[i].tcb->sched_priority,