Merged release into master

ROMFS/px4fmu_common/init.d/1000_rc_fw_easystar.hil

@@ -6,5 +6,30 @@
 
 sh /etc/init.d/rc.fw_defaults
 
+if [ $AUTOCNF == yes ]
+then
+	param set BAT_N_CELLS 3
+	param set FW_AIRSPD_MAX 20
+	param set FW_AIRSPD_MIN 12
+	param set FW_AIRSPD_TRIM 14
+	param set FW_ATT_TC 0.3
+	param set FW_L1_DAMPING 0.74
+	param set FW_L1_PERIOD 16
+	param set FW_LND_ANG 15
+	param set FW_LND_FLALT 5
+	param set FW_LND_HHDIST 15
+	param set FW_LND_HVIRT 13
+	param set FW_LND_TLALT 5
+	param set FW_THR_LND_MAX 0
+	param set FW_PR_FF 0.35
+	param set FW_PR_I 0.1
+	param set FW_PR_IMAX 0.4
+	param set FW_PR_P 0.2
+	param set FW_RR_FF 0.6
+	param set FW_RR_I 0.1
+	param set FW_RR_IMAX 0.2
+	param set FW_RR_P 0.3
+fi
+
 set HIL yes
 set MIXER AERT

ROMFS/px4fmu_common/init.d/3034_fx79

@@ -7,4 +7,12 @@
 
 sh /etc/init.d/rc.fw_defaults
 
+if [ $AUTOCNF == yes ]
+then
+	param set NAV_LOITER_RAD 150
+	param set FW_AIRSPD_MAX 30
+	param set FW_AIRSPD_MIN 13
+	param set FW_AIRSPD_TRIM 15
+fi
+
 set MIXER FX79

ROMFS/px4fmu_common/init.d/3100_tbs_caipirinha

@@ -2,33 +2,39 @@
 #
 # TBS Caipirinha
 #
-# Thomas Gubler <thomas@px4.io>
+# Lorenz Meier <lorenz@px4.io>
 #
 
 sh /etc/init.d/rc.fw_defaults
 
 if [ $AUTOCNF == yes ]
 then
-
+	param set FW_AIRSPD_MAX 20
-	# TODO: these are the X5 default parameters, update them to the caipi
+	param set FW_AIRSPD_MIN 12
-
+	param set FW_AIRSPD_TRIM 16
-	param set FW_AIRSPD_MIN 15
-	param set FW_AIRSPD_TRIM 20
-	param set FW_AIRSPD_MAX 40
 	param set FW_ATT_TC 0.3
 	param set FW_L1_DAMPING 0.74
-	param set FW_L1_PERIOD 15
+	param set FW_L1_PERIOD 16
-	param set FW_PR_FF 0.3
+	param set FW_LND_ANG 15
-	param set FW_PR_I 0
+	param set FW_LND_FLALT 5
-	param set FW_PR_IMAX 0.2
+	param set FW_LND_HHDIST 15
-	param set FW_PR_P 0.03
+	param set FW_LND_HVIRT 13
-	param set FW_P_ROLLFF 0
+	param set FW_LND_TLALT 5
-	param set FW_RR_FF 0.3
+	param set FW_THR_LND_MAX 0
-	param set FW_RR_I 0
+	param set FW_PR_FF 0.35
+	param set FW_PR_I 0.01
+	param set FW_PR_IMAX 0.4
+	param set FW_PR_P 0.08
+	param set FW_RR_FF 0.6
+	param set FW_RR_I 0.01
 	param set FW_RR_IMAX 0.2
-	param set FW_RR_P 0.03
+	param set FW_RR_P 0.04
-	param set FW_R_LIM 60
+	param set SYS_COMPANION 157600
-	param set FW_R_RMAX 0
+	param set PWM_MAIN_REV0 1
+	param set PWM_MAIN_REV1 1
 fi
 
-set MIXER Q
+set MIXER caipi
+# Provide ESC a constant 1000 us pulse
+set PWM_OUT 4
+set PWM_DISARMED 1000

ROMFS/px4fmu_common/init.d/rc.logging

@@ -11,7 +11,7 @@ then
 		then
 		fi
 	else
-		if sdlog2 start -r 100 -a -b 22 -t
+		if sdlog2 start -r 100 -a -b 12 -t
 		then
 		fi
 	fi

ROMFS/px4fmu_common/init.d/rcS

@@ -485,6 +485,10 @@ then
 		then
 			mavlink start -d /dev/ttyS2 -b 57600 -m onboard -r 1000
 		fi
+		if param compare SYS_COMPANION 157600
+		then
+			mavlink start -d /dev/ttyS2 -b 57600 -m osd -r 1000
+		fi
 	fi
 
 	# UAVCAN

ROMFS/px4fmu_common/mixers/FX79.main.mix

@@ -27,13 +27,13 @@ for the elevons.
 
 M: 2
 O:      10000  10000      0 -10000  10000
-S: 0 0   7500   7500      0 -10000  10000
+S: 0 0   8500   8500      0 -10000  10000
-S: 0 1   8000   8000      0 -10000  10000
+S: 0 1   9500   9500      0 -10000  10000
 
 M: 2
 O:      10000  10000      0 -10000  10000
-S: 0 0   7500   7500      0 -10000  10000
+S: 0 0   8500   8500      0 -10000  10000
-S: 0 1  -8000  -8000      0 -10000  10000
+S: 0 1  -9500  -9500      0 -10000  10000
 
 Output 2
 --------
@@ -51,19 +51,3 @@ range.  Inputs below zero are treated as zero.
 M: 1
 O:      10000  10000      0 -10000  10000
 S: 0 3      0  20000 -10000 -10000  10000
-
-Inputs to the mixer come from channel group 2 (payload), channels 0
-(bay servo 1), 1 (bay servo 2) and 3 (drop release).
------------------------------------------------------
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 2 0  10000  10000      0 -10000  10000
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 2 1  10000  10000      0 -10000  10000
-
-M: 1
-O:      10000  10000      0 -10000  10000
-S: 2 2 -10000 -10000      0 -10000  10000

ROMFS/px4fmu_common/mixers/caipi.main.mix

@@ -0,0 +1,51 @@
+Delta-wing mixer
+===========================
+
+Designed for TBS Caipirinha
+
+This file defines mixers suitable for controlling a delta wing aircraft using
+PX4FMU. The configuration assumes the elevon servos are connected to PX4FMU
+servo outputs 0 and 1 and the motor speed control to output 3. Output 2 is
+assumed to be unused.
+
+Inputs to the mixer come from channel group 0 (vehicle attitude), channels 0
+(roll), 1 (pitch) and 3 (thrust).
+
+See the README for more information on the scaler format.
+
+Elevon mixers
+-------------
+Three scalers total (output, roll, pitch).
+
+On the assumption that the two elevon servos are physically reversed, the pitch
+input is inverted between the two servos.
+
+The scaling factor for roll inputs is adjusted to implement differential travel
+for the elevons.
+
+M: 2
+O:      10000  10000      3000 -10000  10000
+S: 0 0   8000   8000      0 -10000  10000
+S: 0 1   9000   9000      0 -10000  10000
+
+M: 2
+O:      10000  10000      -3000 -10000  10000
+S: 0 0   8000   8000      0 -10000  10000
+S: 0 1  -9000  -9000      0 -10000  10000
+
+Output 2
+--------
+This mixer is empty.
+
+Z:
+
+Motor speed mixer
+-----------------
+Two scalers total (output, thrust).
+
+This mixer generates a full-range output (-1 to 1) from an input in the (0 - 1)
+range.  Inputs below zero are treated as zero.
+
+M: 1
+O:      10000  10000      0 -10000  10000
+S: 0 3      0  20000 -10000 -10000  10000

makefiles/nuttx/toolchain_gnu-arm-eabi.mk

@@ -132,6 +132,12 @@ LIBC	 := $(shell ${CC} ${ARCHCPUFLAGS} -print-file-name=libc.a)
 ARCHCFLAGS		 = -std=gnu99
 ARCHCXXFLAGS		 = -fno-exceptions -fno-rtti -std=gnu++0x -fno-threadsafe-statics -D__CUSTOM_FILE_IO__
 
+#
+# Provide defaults, but allow for module override
+WFRAME_LARGER_THAN ?= 1024
+
+
+
 # Generic warnings
 #
 ARCHWARNINGS		 = -Wall \

msg/navigation_capabilities.msg

@@ -0,0 +1,5 @@
+uint64 timestamp		# timestamp this topic was emitted
+float32 turn_distance		# the optimal distance to a waypoint to switch to the next
+float32 landing_horizontal_slope_displacement
+float32 landing_slope_angle_rad
+float32 landing_flare_length

msg/vehicle_status.msg

@@ -7,7 +7,8 @@ uint8 MAIN_STATE_AUTO_LOITER = 4
 uint8 MAIN_STATE_AUTO_RTL = 5
 uint8 MAIN_STATE_ACRO = 6
 uint8 MAIN_STATE_OFFBOARD = 7
-uint8 MAIN_STATE_MAX = 8
+uint8 MAIN_STATE_STAB = 8
+uint8 MAIN_STATE_MAX = 9
 
 # If you change the order, add or remove arming_state_t states make sure to update the arrays
 # in state_machine_helper.cpp as well.
@@ -39,7 +40,8 @@ uint8 NAVIGATION_STATE_LAND = 11		# Land mode
 uint8 NAVIGATION_STATE_DESCEND = 12		# Descend mode (no position control)
 uint8 NAVIGATION_STATE_TERMINATION = 13		# Termination mode
 uint8 NAVIGATION_STATE_OFFBOARD = 14
-uint8 NAVIGATION_STATE_MAX = 15
+uint8 NAVIGATION_STATE_STAB = 15		# Stabilized mode
+uint8 NAVIGATION_STATE_MAX = 16
 
 # VEHICLE_MODE_FLAG, same as MAV_MODE_FLAG of MAVLink 1.0 protocol
 uint8 VEHICLE_MODE_FLAG_SAFETY_ARMED = 128

src/drivers/px4io/px4io.cpp

@@ -1114,6 +1114,27 @@ PX4IO::task_main()
 				}
 
 				(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_PWM_REVERSE, pwm_invert_mask);
+
+				float trim_val;
+				param_t trim_parm;
+
+				trim_parm = param_find("TRIM_ROLL");
+				if (trim_parm != PARAM_INVALID) {
+					param_get(trim_parm, &trim_val);
+					(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL, FLOAT_TO_REG(trim_val));
+				}
+
+				trim_parm = param_find("TRIM_PITCH");
+				if (trim_parm != PARAM_INVALID) {
+					param_get(trim_parm, &trim_val);
+					(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH, FLOAT_TO_REG(trim_val));
+				}
+
+				trim_parm = param_find("TRIM_YAW");
+				if (trim_parm != PARAM_INVALID) {
+					param_get(trim_parm, &trim_val);
+					(void)io_reg_set(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW, FLOAT_TO_REG(trim_val));
+				}
 			}
 
 		}
@@ -1240,7 +1261,7 @@ PX4IO::io_set_arming_state()
 	uint16_t set = 0;
 	uint16_t clear = 0;
 
-    if (have_armed == OK) {
+	if (have_armed == OK) {
 		_in_esc_calibration_mode = armed.in_esc_calibration_mode;
 		if (armed.armed || _in_esc_calibration_mode) {
 			set |= PX4IO_P_SETUP_ARMING_FMU_ARMED;
@@ -2127,7 +2148,14 @@ PX4IO::print_status(bool extended_status)
 	for (unsigned i = 0; i < _max_actuators; i++) {
 		printf("%s", (pwm_invert_mask & (1 << i)) ? "x" : "_");
 	}
-	printf("]\n");
+	printf("]");
+
+	float trim_roll = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_ROLL));
+	float trim_pitch = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_PITCH));
+	float trim_yaw = REG_TO_FLOAT(io_reg_get(PX4IO_PAGE_SETUP, PX4IO_P_SETUP_TRIM_YAW));
+
+	printf(" trims: r: %8.4f p: %8.4f y: %8.4f\n",
+		(double)trim_roll, (double)trim_pitch, (double)trim_yaw);
 
 	uint16_t raw_inputs = io_reg_get(PX4IO_PAGE_RAW_RC_INPUT, PX4IO_P_RAW_RC_COUNT);
 	printf("%d raw R/C inputs", raw_inputs);

src/modules/commander/commander.cpp

@@ -37,9 +37,9 @@
  *
  * @author Petri Tanskanen <petri.tanskanen@inf.ethz.ch>
  * @author Lorenz Meier <lorenz@px4.io>
- * @author Thomas Gubler <thomasgubler@student.ethz.ch>
+ * @author Thomas Gubler <thomas@px4.io>
- * @author Julian Oes <julian@oes.ch>
+ * @author Julian Oes <julian@px4.io>
- * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Anton Babushkin <anton@px4.io>
  */
 
 #include <px4_config.h>
@@ -93,7 +93,7 @@
 #include <uORB/topics/geofence_result.h>
 #include <uORB/topics/telemetry_status.h>
 #include <uORB/topics/vtol_vehicle_status.h>
- #include <uORB/topics/vehicle_land_detected.h>
+#include <uORB/topics/vehicle_land_detected.h>
 
 #include <drivers/drv_led.h>
 #include <drivers/drv_hrt.h>
@@ -148,7 +148,12 @@ static constexpr uint8_t COMMANDER_MAX_GPS_NOISE = 60;		/**< Maximum percentage
 #define DIFFPRESS_TIMEOUT		2000000
 
 #define PRINT_INTERVAL	5000000
-#define PRINT_MODE_REJECT_INTERVAL	2000000
+#define PRINT_MODE_REJECT_INTERVAL	10000000
+
+#define INAIR_RESTART_HOLDOFF_INTERVAL	2000000
+
+#define HIL_ID_MIN 1000
+#define HIL_ID_MAX 1999
 
 enum MAV_MODE_FLAG {
 	MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
@@ -174,6 +179,7 @@ static volatile bool thread_should_exit = false;	/**< daemon exit flag */
 static volatile bool thread_running = false;		/**< daemon status flag */
 static int daemon_task;					/**< Handle of daemon task / thread */
 static bool need_param_autosave = false;		/**< Flag set to true if parameters should be autosaved in next iteration (happens on param update and if functionality is enabled) */
+static hrt_abstime commander_boot_timestamp = 0;
 
 static unsigned int leds_counter;
 /* To remember when last notification was sent */
@@ -189,6 +195,7 @@ static struct actuator_armed_s armed;
 static struct safety_s safety;
 static struct vehicle_control_mode_s control_mode;
 static struct offboard_control_mode_s offboard_control_mode;
+static struct home_position_s _home;
 
 /**
  * The daemon app only briefly exists to start
@@ -212,7 +219,7 @@ void usage(const char *reason);
  */
 bool handle_command(struct vehicle_status_s *status, const struct safety_s *safety, struct vehicle_command_s *cmd,
 		    struct actuator_armed_s *armed, struct home_position_s *home, struct vehicle_global_position_s *global_pos,
-		    orb_advert_t *home_pub);
+		    struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub);
 
 /**
  * Mainloop of commander.
@@ -254,6 +261,15 @@ void *commander_low_prio_loop(void *arg);
 
 void answer_command(struct vehicle_command_s &cmd, unsigned result);
 
+/**
+ * check whether autostart ID is in the reserved range for HIL setups
+ */
+bool is_hil_setup(int id);
+
+bool is_hil_setup(int id) {
+	return (id >= HIL_ID_MIN) && (id <= HIL_ID_MAX);
+}
+
 
 int commander_main(int argc, char *argv[])
 {
@@ -354,6 +370,7 @@ int commander_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "arm")) {
 		int mavlink_fd_local = open(MAVLINK_LOG_DEVICE, 0);
 		arm_disarm(true, mavlink_fd_local, "command line");
+		warnx("note: not updating home position on commandline arming!");
 		close(mavlink_fd_local);
 		return 0;
 	}
@@ -383,6 +400,8 @@ void print_status()
 	warnx("type: %s", (status.is_rotary_wing) ? "symmetric motion" : "forward motion");
 	warnx("usb powered: %s", (status.usb_connected) ? "yes" : "no");
 	warnx("avionics rail: %6.2f V", (double)status.avionics_power_rail_voltage);
+	warnx("home: lat = %.7f, lon = %.7f, alt = %.2f ", _home.lat, _home.lon, (double)_home.alt);
+	warnx("home: x = %.7f, y = %.7f, z = %.2f ", (double)_home.x, (double)_home.y, (double)_home.z);
 
 	/* read all relevant states */
 	int state_sub = orb_subscribe(ORB_ID(vehicle_status));
@@ -437,6 +456,12 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
 {
 	transition_result_t arming_res = TRANSITION_NOT_CHANGED;
 
+	// For HIL platforms, require that simulated sensors are connected
+	if (is_hil_setup(autostart_id) && status.hil_state != vehicle_status_s::HIL_STATE_ON) {
+		mavlink_and_console_log_critical(mavlink_fd_local, "HIL platform: Connect to simulator before arming");
+		return TRANSITION_DENIED;
+	}
+
 	// Transition the armed state. By passing mavlink_fd to arming_state_transition it will
 	// output appropriate error messages if the state cannot transition.
 	arming_res = arming_state_transition(&status, &safety, arm ? vehicle_status_s::ARMING_STATE_ARMED : vehicle_status_s::ARMING_STATE_STANDBY, &armed,
@@ -454,7 +479,8 @@ transition_result_t arm_disarm(bool arm, const int mavlink_fd_local, const char
 
 bool handle_command(struct vehicle_status_s *status_local, const struct safety_s *safety_local,
 		    struct vehicle_command_s *cmd, struct actuator_armed_s *armed_local,
-		    struct home_position_s *home, struct vehicle_global_position_s *global_pos, orb_advert_t *home_pub)
+		    struct home_position_s *home, struct vehicle_global_position_s *global_pos,
+		    struct vehicle_local_position_s *local_pos, orb_advert_t *home_pub)
 {
 	/* only handle commands that are meant to be handled by this system and component */
 	if (cmd->target_system != status_local->system_id || ((cmd->target_component != status_local->component_id)
@@ -480,7 +506,16 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 			transition_result_t hil_ret = hil_state_transition(new_hil_state, status_pub, status_local, mavlink_fd);
 
 			// Transition the arming state
-			arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, mavlink_fd, "set mode command");
+			bool cmd_arm = base_mode & MAV_MODE_FLAG_SAFETY_ARMED;
+
+			arming_ret = arm_disarm(cmd_arm, mavlink_fd, "set mode command");
+
+			/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
+			if (cmd_arm && (arming_ret == TRANSITION_CHANGED) &&
+				(hrt_absolute_time() > (commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL))) {
+
+				commander_set_home_position(*home_pub, *home, *local_pos, *global_pos);
+			}
 
 			if (base_mode & MAV_MODE_FLAG_CUSTOM_MODE_ENABLED) {
 				/* use autopilot-specific mode */
@@ -504,6 +539,10 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 					/* ACRO */
 					main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_ACRO);
 
+				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_STABILIZED) {
+					/* STABILIZED */
+					main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB);
+
 				} else if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_OFFBOARD) {
 					/* OFFBOARD */
 					main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_OFFBOARD);
@@ -521,6 +560,9 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 						main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_POSCTL);
 
 					} else if (base_mode & MAV_MODE_FLAG_STABILIZE_ENABLED) {
+						/* STABILIZED */
+						main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_STAB);
+					} else {
 						/* MANUAL */
 						main_ret = main_state_transition(status_local, vehicle_status_s::MAIN_STATE_MANUAL);
 					}
@@ -845,6 +887,7 @@ int commander_thread_main(int argc, char *argv[])
 	main_states_str[vehicle_status_s::MAIN_STATE_AUTO_LOITER]			= "AUTO_LOITER";
 	main_states_str[vehicle_status_s::MAIN_STATE_AUTO_RTL]			= "AUTO_RTL";
 	main_states_str[vehicle_status_s::MAIN_STATE_ACRO]			= "ACRO";
+	main_states_str[vehicle_status_s::MAIN_STATE_STAB]			= "STAB";
 	main_states_str[vehicle_status_s::MAIN_STATE_OFFBOARD]			= "OFFBOARD";
 
 	const char *arming_states_str[vehicle_status_s::ARMING_STATE_MAX];
@@ -858,6 +901,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	const char *nav_states_str[vehicle_status_s::NAVIGATION_STATE_MAX];
 	nav_states_str[vehicle_status_s::NAVIGATION_STATE_MANUAL]			= "MANUAL";
+	nav_states_str[vehicle_status_s::NAVIGATION_STATE_STAB]				= "STAB";
 	nav_states_str[vehicle_status_s::NAVIGATION_STATE_ALTCTL]			= "ALTCTL";
 	nav_states_str[vehicle_status_s::NAVIGATION_STATE_POSCTL]			= "POSCTL";
 	nav_states_str[vehicle_status_s::NAVIGATION_STATE_AUTO_MISSION]		= "AUTO_MISSION";
@@ -955,8 +999,7 @@ int commander_thread_main(int argc, char *argv[])
 
 	/* home position */
 	orb_advert_t home_pub = nullptr;
-	struct home_position_s home;
+	memset(&_home, 0, sizeof(_home));
-	memset(&home, 0, sizeof(home));
 
 	/* init mission state, do it here to allow navigator to use stored mission even if mavlink failed to start */
 	orb_advert_t mission_pub = nullptr;
@@ -1141,15 +1184,22 @@ int commander_thread_main(int argc, char *argv[])
 	}
 
 	// Run preflight check
-	status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check);
+	param_get(_param_autostart_id, &autostart_id);
-	if (!status.condition_system_sensors_initialized) {
+	if (is_hil_setup(autostart_id)) {
-		set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
+		// HIL configuration selected: real sensors will be disabled
-	}
+		status.condition_system_sensors_initialized = false;
-	else {
 		set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
+	} else {
+		status.condition_system_sensors_initialized = Commander::preflightCheck(mavlink_fd, true, true, true, true, checkAirspeed, !status.rc_input_mode, !status.circuit_breaker_engaged_gpsfailure_check);
+		if (!status.condition_system_sensors_initialized) {
+			set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
+		}
+		else {
+			set_tune_override(TONE_STARTUP_TUNE); //normal boot tune
+		}
 	}
 
-	const hrt_abstime commander_boot_timestamp = hrt_absolute_time();
+	commander_boot_timestamp = hrt_absolute_time();
 
 	transition_result_t arming_ret;
 
@@ -1317,8 +1367,15 @@ int commander_thread_main(int argc, char *argv[])
 					}
 
 					/* provide RC and sensor status feedback to the user */
-					(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed,
+					if (is_hil_setup(autostart_id)) {
-						!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
+						/* HIL configuration: check only RC input */
+						(void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false,
+								!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false);
+					} else {
+						/* check sensors also */
+						(void)Commander::preflightCheck(mavlink_fd, true, true, true, true, chAirspeed,
+								!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
+					}
 				}
 
 				telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
@@ -1589,6 +1646,8 @@ int commander_thread_main(int argc, char *argv[])
 			low_battery_voltage_actions_done = true;
 			if (armed.armed) {
 				mavlink_log_critical(mavlink_fd, "LOW BATTERY, RETURN TO LAND ADVISED");
+			} else {
+				mavlink_log_critical(mavlink_fd, "LOW BATTERY, TAKEOFF DISCOURAGED");
 			}
 			status.battery_warning = vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW;
 			status_changed = true;
@@ -1676,7 +1735,7 @@ int commander_thread_main(int argc, char *argv[])
 				if (status.gps_failure && !gpsIsNoisy) {
 					status.gps_failure = false;
 					status_changed = true;
-					mavlink_log_critical(mavlink_fd, "gps regained");
+					mavlink_log_critical(mavlink_fd, "gps fix regained");
 				}
 
 			} else if (!status.gps_failure) {
@@ -1710,12 +1769,12 @@ int commander_thread_main(int argc, char *argv[])
 
 			if (!flight_termination_printed) {
 				warnx("Flight termination because of navigator request or geofence");
-				mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
+				mavlink_log_critical(mavlink_fd, "Geofence violation: flight termination");
 				flight_termination_printed = true;
 			}
 
 			if (counter % (1000000 / COMMANDER_MONITORING_INTERVAL) == 0) {
-				mavlink_log_critical(mavlink_fd, "GF violation: flight termination");
+				mavlink_log_critical(mavlink_fd, "Flight termination active");
 			}
 		} // no reset is done here on purpose, on geofence violation we want to stay in flighttermination
 
@@ -1725,7 +1784,7 @@ int commander_thread_main(int argc, char *argv[])
 			/* handle the case where RC signal was regained */
 			if (!status.rc_signal_found_once) {
 				status.rc_signal_found_once = true;
-				mavlink_log_info(mavlink_fd, "Detected RC signal first time");
+				mavlink_log_info(mavlink_fd, "Detected radio control");
 				status_changed = true;
 
 			} else {
@@ -1742,7 +1801,10 @@ int commander_thread_main(int argc, char *argv[])
 			 * do it only for rotary wings */
 			if (status.is_rotary_wing &&
 			    (status.arming_state == vehicle_status_s::ARMING_STATE_ARMED || status.arming_state == vehicle_status_s::ARMING_STATE_ARMED_ERROR) &&
-			    (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL || status.main_state == vehicle_status_s::MAIN_STATE_ACRO || status.condition_landed) &&
+			    (status.main_state == vehicle_status_s::MAIN_STATE_MANUAL ||
+			    	status.main_state == vehicle_status_s::MAIN_STATE_ACRO ||
+			    	status.main_state == vehicle_status_s::MAIN_STATE_STAB ||
+			    	status.condition_landed) &&
 			    sp_man.r < -STICK_ON_OFF_LIMIT && sp_man.z < 0.1f) {
 
 				if (stick_off_counter > STICK_ON_OFF_COUNTER_LIMIT) {
@@ -1775,7 +1837,8 @@ int commander_thread_main(int argc, char *argv[])
 					 * for being in manual mode only applies to manual arming actions.
 					 * the system can be armed in auto if armed via the GCS.
 					 */
-					if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL) {
+					if ((status.main_state != vehicle_status_s::MAIN_STATE_MANUAL) &&
+						(status.main_state != vehicle_status_s::MAIN_STATE_STAB)) {
 						print_reject_arm("NOT ARMING: Switch to MANUAL mode first.");
 
 					} else {
@@ -1932,7 +1995,7 @@ int commander_thread_main(int argc, char *argv[])
 			orb_copy(ORB_ID(vehicle_command), cmd_sub, &cmd);
 
 			/* handle it */
-			if (handle_command(&status, &safety, &cmd, &armed, &home, &global_position, &home_pub)) {
+			if (handle_command(&status, &safety, &cmd, &armed, &_home, &global_position, &local_position, &home_pub)) {
 				status_changed = true;
 			}
 		}
@@ -1944,6 +2007,7 @@ int commander_thread_main(int argc, char *argv[])
 			 * and both failed we want to terminate the flight */
 			if (status.main_state !=vehicle_status_s::MAIN_STATE_MANUAL &&
 			    status.main_state !=vehicle_status_s::MAIN_STATE_ACRO &&
+			    status.main_state !=vehicle_status_s::MAIN_STATE_STAB &&
 			    status.main_state !=vehicle_status_s::MAIN_STATE_ALTCTL &&
 			    status.main_state !=vehicle_status_s::MAIN_STATE_POSCTL &&
 			    ((status.data_link_lost && status.gps_failure) ||
@@ -1968,6 +2032,7 @@ int commander_thread_main(int argc, char *argv[])
 			 * if both failed we want to terminate the flight */
 			if ((status.main_state ==vehicle_status_s::MAIN_STATE_ACRO ||
 			     status.main_state ==vehicle_status_s::MAIN_STATE_MANUAL ||
+			     status.main_state !=vehicle_status_s::MAIN_STATE_STAB ||
 			     status.main_state ==vehicle_status_s::MAIN_STATE_ALTCTL ||
 			     status.main_state ==vehicle_status_s::MAIN_STATE_POSCTL) &&
 			    ((status.rc_signal_lost && status.gps_failure) ||
@@ -1987,17 +2052,17 @@ int commander_thread_main(int argc, char *argv[])
 			}
 		}
 
-		//Get current timestamp
+		/* Get current timestamp */
 		const hrt_abstime now = hrt_absolute_time();
 
-		//First time home position update
+		/* First time home position update - but only if disarmed */
-		if (!status.condition_home_position_valid) {
+		if (!status.condition_home_position_valid && !armed.armed) {
-			commander_set_home_position(home_pub, home, local_position, global_position);
+			commander_set_home_position(home_pub, _home, local_position, global_position);
 		}
 
 		/* update home position on arming if at least 2s from commander start spent to avoid setting home on in-air restart */
-		else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + 2000000) {
+		else if (arming_state_changed && armed.armed && !was_armed && now > commander_boot_timestamp + INAIR_RESTART_HOLDOFF_INTERVAL) {
-			commander_set_home_position(home_pub, home, local_position, global_position);
+			commander_set_home_position(home_pub, _home, local_position, global_position);
 		}
 
 		/* print new state */
@@ -2014,14 +2079,6 @@ int commander_thread_main(int argc, char *argv[])
 						       mission_result.finished,
 						       mission_result.stay_in_failsafe);
 
-		// TODO handle mode changes by commands
-		if (main_state_changed) {
-			status_changed = true;
-			warnx("main state: %s", main_states_str[status.main_state]);
-			mavlink_log_info(mavlink_fd, "[cmd] main state: %s", main_states_str[status.main_state]);
-			main_state_changed = false;
-		}
-
 		if (status.failsafe != failsafe_old) {
 			status_changed = true;
 
@@ -2035,10 +2092,12 @@ int commander_thread_main(int argc, char *argv[])
 			failsafe_old = status.failsafe;
 		}
 
-		if (nav_state_changed) {
+		// TODO handle mode changes by commands
+		if (main_state_changed || nav_state_changed) {
 			status_changed = true;
-			warnx("nav state: %s", nav_states_str[status.nav_state]);
+			warnx("main state: %s nav state: %s", main_states_str[status.main_state], nav_states_str[status.nav_state]);
-			mavlink_log_info(mavlink_fd, "[cmd] nav state: %s", nav_states_str[status.nav_state]);
+			mavlink_log_info(mavlink_fd, "Flight mode: %s", nav_states_str[status.nav_state]);
+			main_state_changed = false;
 		}
 
 		/* publish states (armed, control mode, vehicle status) at least with 5 Hz */
@@ -2061,11 +2120,13 @@ int commander_thread_main(int argc, char *argv[])
 			set_tune(TONE_ARMING_WARNING_TUNE);
 			arm_tune_played = true;
 
-		} else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
+		} else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
+				status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_CRITICAL) {
 			/* play tune on battery critical */
 			set_tune(TONE_BATTERY_WARNING_FAST_TUNE);
 
-		} else if (status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe) {
+		} else if ((status.hil_state != vehicle_status_s::HIL_STATE_ON) &&
+				(status.battery_warning == vehicle_status_s::VEHICLE_BATTERY_WARNING_LOW || status.failsafe)) {
 			/* play tune on battery warning or failsafe */
 			set_tune(TONE_BATTERY_WARNING_SLOW_TUNE);
 
@@ -2292,7 +2353,15 @@ set_main_state_rc(struct vehicle_status_s *status_local, struct manual_control_s
 
 	case manual_control_setpoint_s::SWITCH_POS_OFF:		// MANUAL
 		if (sp_man->acro_switch == manual_control_setpoint_s::SWITCH_POS_ON) {
-			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO);
+
+			/* manual mode is stabilized already for multirotors, so switch to acro
+			 * for any non-manual mode
+			 */
+			if (status.is_rotary_wing) {
+				res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_ACRO);
+			} else {
+				res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_STAB);
+			}
 
 		} else {
 			res = main_state_transition(status_local,vehicle_status_s::MAIN_STATE_MANUAL);
@@ -2403,6 +2472,20 @@ set_control_mode()
 		control_mode.flag_control_termination_enabled = false;
 		break;
 
+	case vehicle_status_s::NAVIGATION_STATE_STAB:
+		control_mode.flag_control_manual_enabled = true;
+		control_mode.flag_control_auto_enabled = false;
+		control_mode.flag_control_rates_enabled = true;
+		control_mode.flag_control_attitude_enabled = true;
+		control_mode.flag_control_altitude_enabled = false;
+		control_mode.flag_control_climb_rate_enabled = false;
+		control_mode.flag_control_position_enabled = false;
+		control_mode.flag_control_velocity_enabled = false;
+		control_mode.flag_control_termination_enabled = false;
+		/* override is not ok in stabilized mode */
+		control_mode.flag_external_manual_override_ok = false;
+		break;
+
 	case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
 		control_mode.flag_control_manual_enabled = true;
 		control_mode.flag_control_auto_enabled = false;

src/modules/commander/px4_custom_mode.h

@@ -1,8 +1,41 @@
-/*
+/****************************************************************************
- * px4_custom_mode.h
  *
- *  Created on: 09.08.2013
+ *   Copyright (c) 2013-2015 PX4 Development Team. All rights reserved.
- *      Author: ton
+ *
+ * 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 px4_custom_mode.h
+ * PX4 custom flight modes
+ *
+ * @author Anton Babushkin <anton@px4.io>
  */
 
 #ifndef PX4_CUSTOM_MODE_H_
@@ -17,6 +50,7 @@ enum PX4_CUSTOM_MAIN_MODE {
 	PX4_CUSTOM_MAIN_MODE_AUTO,
 	PX4_CUSTOM_MAIN_MODE_ACRO,
 	PX4_CUSTOM_MAIN_MODE_OFFBOARD,
+	PX4_CUSTOM_MAIN_MODE_STABILIZED
 };
 
 enum PX4_CUSTOM_SUB_MODE_AUTO {

src/modules/commander/rc_calibration.cpp

@@ -59,7 +59,7 @@ static const int ERROR = -1;
 int do_trim_calibration(int mavlink_fd)
 {
 	int sub_man = orb_subscribe(ORB_ID(manual_control_setpoint));
-	usleep(200000);
+	usleep(400000);
 	struct manual_control_setpoint_s sp;
 	bool changed;
 	orb_check(sub_man, &changed);
@@ -71,20 +71,34 @@ int do_trim_calibration(int mavlink_fd)
 
 	orb_copy(ORB_ID(manual_control_setpoint), sub_man, &sp);
 
-	/* set parameters */
+	/* load trim values which are active */
-	float p = sp.y;
+	float roll_trim_active;
-	param_set(param_find("TRIM_ROLL"), &p);
+	param_get(param_find("TRIM_ROLL"), &roll_trim_active);
-	p = sp.x;
+	float pitch_trim_active;
-	param_set(param_find("TRIM_PITCH"), &p);
+	param_get(param_find("TRIM_PITCH"), &pitch_trim_active);
-	p = sp.r;
+	float yaw_trim_active;
-	param_set(param_find("TRIM_YAW"), &p);
+	param_get(param_find("TRIM_YAW"), &yaw_trim_active);
+
+	/* set parameters: the new trim values are the combination of active trim values
+	   and the values coming from the remote control of the user
+	*/
+	float p = sp.y + roll_trim_active;
+	int p1r = param_set(param_find("TRIM_ROLL"), &p);
+	/*
+	 we explicitly swap sign here because the trim is added to the actuator controls
+	 which are moving in an inverse sense to manual pitch inputs
+	*/
+	p = -sp.x + pitch_trim_active;
+	int p2r = param_set(param_find("TRIM_PITCH"), &p);
+	p = sp.r + yaw_trim_active;
+	int p3r = param_set(param_find("TRIM_YAW"), &p);
 
 	/* store to permanent storage */
 	/* auto-save */
 	int save_ret = param_save_default();
 
-	if (save_ret != 0) {
+	if (save_ret != 0 || p1r != 0 || p2r != 0 || p3r != 0) {
-		mavlink_log_critical(mavlink_fd, "TRIM: SAVE FAIL");
+		mavlink_log_critical(mavlink_fd, "TRIM: PARAM SET FAIL");
 		px4_close(sub_man);
 		return ERROR;
 	}

src/modules/commander/state_machine_helper.cpp

@@ -126,7 +126,8 @@ arming_state_transition(struct vehicle_status_s *status,		///< current vehicle s
 		int prearm_ret = OK;
 
 		/* only perform the check if we have to */
-		if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED) {
+		if (fRunPreArmChecks && new_arming_state == vehicle_status_s::ARMING_STATE_ARMED
+				&& status->hil_state == vehicle_status_s::HIL_STATE_OFF) {
 			prearm_ret = prearm_check(status, mavlink_fd);
 		}
 
@@ -304,6 +305,7 @@ main_state_transition(struct vehicle_status_s *status, main_state_t new_main_sta
 	switch (new_main_state) {
 	case vehicle_status_s::MAIN_STATE_MANUAL:
 	case vehicle_status_s::MAIN_STATE_ACRO:
+	case vehicle_status_s::MAIN_STATE_STAB:
 		ret = TRANSITION_CHANGED;
 		break;
 
@@ -538,10 +540,11 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 	switch (status->main_state) {
 	case vehicle_status_s::MAIN_STATE_ACRO:
 	case vehicle_status_s::MAIN_STATE_MANUAL:
+	case vehicle_status_s::MAIN_STATE_STAB:
 	case vehicle_status_s::MAIN_STATE_ALTCTL:
 	case vehicle_status_s::MAIN_STATE_POSCTL:
 		/* require RC for all manual modes */
-		if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed) {
+		if ((status->rc_signal_lost || status->rc_signal_lost_cmd) && armed && !status->condition_landed) {
 			status->failsafe = true;
 
 			if (status->condition_global_position_valid && status->condition_home_position_valid) {
@@ -564,6 +567,10 @@ bool set_nav_state(struct vehicle_status_s *status, const bool data_link_loss_en
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_MANUAL;
 				break;
 
+			case vehicle_status_s::MAIN_STATE_STAB:
+				status->nav_state = vehicle_status_s::NAVIGATION_STATE_STAB;
+				break;
+
 			case vehicle_status_s::MAIN_STATE_ALTCTL:
 				status->nav_state = vehicle_status_s::NAVIGATION_STATE_ALTCTL;
 				break;

src/modules/ekf_att_pos_estimator/AttitudePositionEstimatorEKF.h

@@ -174,7 +174,7 @@ private:
 
     struct map_projection_reference_s   _pos_ref;
 
-    float                       _baro_ref_offset;   /**< offset between initial baro reference and GPS init baro altitude */
+    float                       _filter_ref_offset;   /**< offset between initial baro reference and GPS init baro altitude */
     float                       _baro_gps_offset;   /**< offset between baro altitude (at GPS init time) and GPS altitude */
     hrt_abstime                 _last_debug_print = 0;
 
@@ -193,7 +193,6 @@ private:
     bool            _gpsIsGood;               ///< True if the current GPS fix is good enough for us to use
     uint64_t        _previousGPSTimestamp;    ///< Timestamp of last good GPS fix we have received
     bool            _baro_init;
-    float           _baroAltRef;
     bool            _gps_initialized;
     hrt_abstime     _filter_start_time;
     hrt_abstime     _last_sensor_timestamp;
@@ -333,6 +332,12 @@ private:
     **/
     void initializeGPS();
 
+    /**
+     * Initialize the reference position for the local coordinate frame
+     */
+    void initReferencePosition(hrt_abstime timestamp,
+            double lat, double lon, float gps_alt, float baro_alt);
+
     /**
     * @brief
     *   Polls all uORB subscriptions if any new sensor data has been publish and sets the appropriate

src/modules/ekf_att_pos_estimator/ekf_att_pos_estimator_main.cpp

@@ -75,6 +75,9 @@ static uint64_t IMUusec = 0;
 static constexpr float rc = 10.0f;	// RC time constant of 1st order LPF in seconds
 static constexpr uint64_t FILTER_INIT_DELAY = 1 * 1000 * 1000; 	///< units: microseconds
 static constexpr float POS_RESET_THRESHOLD = 5.0f; 				///< Seconds before we signal a total GPS failure
+static constexpr unsigned MAG_SWITCH_HYSTERESIS = 10;	///< Ignore the first few mag failures (which amounts to a few milliseconds)
+static constexpr unsigned GYRO_SWITCH_HYSTERESIS = 5;	///< Ignore the first few gyro failures (which amounts to a few milliseconds)
+static constexpr unsigned ACCEL_SWITCH_HYSTERESIS = 5;	///< Ignore the first few accel failures (which amounts to a few milliseconds)
 
 /**
  * estimator app start / stop handling function
@@ -157,7 +160,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
     _sensor_combined {},
 
     _pos_ref{},
-    _baro_ref_offset(0.0f),
+    _filter_ref_offset(0.0f),
     _baro_gps_offset(0.0f),
 
     /* performance counters */
@@ -177,7 +180,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
     _gpsIsGood(false),
     _previousGPSTimestamp(0),
     _baro_init(false),
-    _baroAltRef(0.0f),
     _gps_initialized(false),
     _filter_start_time(0),
     _last_sensor_timestamp(0),
@@ -408,6 +410,15 @@ int AttitudePositionEstimatorEKF::check_filter_state()
 
 		// Count the reset condition
 		perf_count(_perf_reset);
+		// GPS is in scaled integers, convert
+		double lat = _gps.lat / 1.0e7;
+		double lon = _gps.lon / 1.0e7;
+		float gps_alt = _gps.alt / 1e3f;
+
+		// Set up height correctly
+		orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
+
+		initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude);
 
 	} else if (_ekf_logging) {
 		_ekf->GetFilterState(&ekf_report);
@@ -590,6 +601,7 @@ void AttitudePositionEstimatorEKF::task_main()
 
 				_baro_init = false;
 				_gps_initialized = false;
+
 				_last_sensor_timestamp = hrt_absolute_time();
 				_last_run = _last_sensor_timestamp;
 
@@ -626,7 +638,10 @@ void AttitudePositionEstimatorEKF::task_main()
 				// gps sample rate is 5Hz and altitude is assumed accurate when averaged over 30 seconds
 				// maintain heavily filtered values for both baro and gps altitude
 				// Assume the filtered output should be identical for both sensors
-				_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
+
+				if (_gpsIsGood) {
+					_baro_gps_offset = _baro_alt_filt - _gps_alt_filt;
+				}
 //				if (hrt_elapsed_time(&_last_debug_print) >= 5e6) {
 //					_last_debug_print = hrt_absolute_time();
 //					perf_print_counter(_perf_baro);
@@ -648,12 +663,15 @@ void AttitudePositionEstimatorEKF::task_main()
 					_ekf->posNE[1] = 0.0f;
 
 					_local_pos.ref_alt = 0.0f;
-					_baro_ref_offset = 0.0f;
 					_baro_gps_offset = 0.0f;
 					_baro_alt_filt = _baro.altitude;
 
 					_ekf->InitialiseFilter(initVelNED, 0.0, 0.0, 0.0f, 0.0f);
 
+					_filter_ref_offset = -_baro.altitude;
+
+					warnx("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset);
+
 				} else {
 
 					if (!_gps_initialized && _gpsIsGood) {
@@ -705,6 +723,32 @@ void AttitudePositionEstimatorEKF::task_main()
 	return;
 }
 
+void AttitudePositionEstimatorEKF::initReferencePosition(hrt_abstime timestamp,
+	double lat, double lon, float gps_alt, float baro_alt)
+{
+	// Reference altitude
+	if (isfinite(_ekf->states[9])) {
+		_filter_ref_offset = _ekf->states[9];
+	} else if (isfinite(-_ekf->hgtMea)) {
+		_filter_ref_offset = -_ekf->hgtMea;
+	} else {
+		_filter_ref_offset = -_baro.altitude;
+	}
+
+	// init filtered gps and baro altitudes
+	_gps_alt_filt = gps_alt;
+	_baro_alt_filt = baro_alt;
+
+	// Initialize projection
+	_local_pos.ref_lat = lat;
+	_local_pos.ref_lon = lon;
+	_local_pos.ref_alt = gps_alt;
+	_local_pos.ref_timestamp = timestamp;
+
+	map_projection_init(&_pos_ref, lat, lon);
+	mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
+}
+
 void AttitudePositionEstimatorEKF::initializeGPS()
 {
 	// GPS is in scaled integers, convert
@@ -714,11 +758,6 @@ void AttitudePositionEstimatorEKF::initializeGPS()
 
 	// Set up height correctly
 	orb_copy(ORB_ID(sensor_baro), _baro_sub, &_baro);
-	_baro_ref_offset = _ekf->states[9]; // this should become zero in the local frame
-
-	// init filtered gps and baro altitudes
-	_gps_alt_filt = gps_alt;
-	_baro_alt_filt = _baro.altitude;
 
 	_ekf->baroHgt = _baro.altitude;
 	_ekf->hgtMea = _ekf->baroHgt;
@@ -740,20 +779,13 @@ void AttitudePositionEstimatorEKF::initializeGPS()
 
 	_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);
 
-	// Initialize projection
+	initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude);
-	_local_pos.ref_lat = lat;
-	_local_pos.ref_lon = lon;
-	_local_pos.ref_alt = gps_alt;
-	_local_pos.ref_timestamp = _gps.timestamp_position;
-
-	map_projection_init(&_pos_ref, lat, lon);
-	mavlink_log_info(_mavlink_fd, "[ekf] ref: LA %.4f,LO %.4f,ALT %.2f", lat, lon, (double)gps_alt);
 
 #if 0
 	warnx("HOME/REF: LA %8.4f,LO %8.4f,ALT %8.2f V: %8.4f %8.4f %8.4f", lat, lon, (double)gps_alt,
 	      (double)_ekf->velNED[0], (double)_ekf->velNED[1], (double)_ekf->velNED[2]);
 	warnx("BARO: %8.4f m / ref: %8.4f m / gps offs: %8.4f m", (double)_ekf->baroHgt, (double)_baro_ref,
-	      (double)_baro_ref_offset);
+	      (double)_filter_ref_offset);
 	warnx("GPS: eph: %8.4f, epv: %8.4f, declination: %8.4f", (double)_gps.eph, (double)_gps.epv,
 	      (double)math::degrees(declination));
 #endif
@@ -814,7 +846,8 @@ void AttitudePositionEstimatorEKF::publishLocalPosition()
 	_local_pos.y = _ekf->states[8];
 
 	// XXX need to announce change of Z reference somehow elegantly
-	_local_pos.z = _ekf->states[9] - _baro_ref_offset - _baroAltRef;
+	_local_pos.z = _ekf->states[9] - _filter_ref_offset;
+	//_local_pos.z_stable = _ekf->states[9];
 
 	_local_pos.vx = _ekf->states[4];
 	_local_pos.vy = _ekf->states[5];
@@ -829,6 +862,17 @@ void AttitudePositionEstimatorEKF::publishLocalPosition()
 	_local_pos.z_global = false;
 	_local_pos.yaw = _att.yaw;
 
+	if (!isfinite(_local_pos.x) ||
+		!isfinite(_local_pos.y) ||
+		!isfinite(_local_pos.z) ||
+		!isfinite(_local_pos.vx) ||
+		!isfinite(_local_pos.vy) ||
+		!isfinite(_local_pos.vz))
+	{
+		// bad data, abort publication
+		return;
+	}
+
 	/* lazily publish the local position only once available */
 	if (_local_pos_pub != nullptr) {
 		/* publish the attitude setpoint */
@@ -862,7 +906,7 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
 	}
 
 	/* local pos alt is negative, change sign and add alt offsets */
-	_global_pos.alt = (-_local_pos.z) - _baro_gps_offset;
+	_global_pos.alt = (-_local_pos.z) - _filter_ref_offset -  _baro_gps_offset;
 
 	if (_local_pos.v_z_valid) {
 		_global_pos.vel_d = _local_pos.vz;
@@ -877,6 +921,17 @@ void AttitudePositionEstimatorEKF::publishGlobalPosition()
 	_global_pos.eph = _gps.eph;
 	_global_pos.epv = _gps.epv;
 
+	if (!isfinite(_global_pos.lat) ||
+		!isfinite(_global_pos.lon) ||
+		!isfinite(_global_pos.alt) ||
+		!isfinite(_global_pos.vel_n) ||
+		!isfinite(_global_pos.vel_e) ||
+		!isfinite(_global_pos.vel_d))
+	{
+		// bad data, abort publication
+		return;
+	}
+
 	/* lazily publish the global position only once available */
 	if (_global_pos_pub != nullptr) {
 		/* publish the global position */
@@ -1073,8 +1128,9 @@ void AttitudePositionEstimatorEKF::print_status()
 	// 19-21: Body Magnetic Field Vector - gauss (X,Y,Z)
 
 	printf("dtIMU: %8.6f filt: %8.6f IMUmsec: %d\n", (double)_ekf->dtIMU, (double)_ekf->dtIMUfilt, (int)IMUmsec);
-	printf("baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)(_gps.alt / 1e3f));
+	printf("alt RAW: baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)_ekf->gpsHgt);
-	printf("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset,
+	printf("alt EST: local alt: %8.4f (NED), AMSL alt: %8.4f (ENU)\n", (double)(_local_pos.z), (double)_global_pos.alt);
+	printf("filter ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_filter_ref_offset,
 	       (double)_baro_gps_offset);
 	printf("dvel: %8.6f %8.6f %8.6f accel: %8.6f %8.6f %8.6f\n", (double)_ekf->dVelIMU.x, (double)_ekf->dVelIMU.y,
 	       (double)_ekf->dVelIMU.z, (double)_ekf->accel.x, (double)_ekf->accel.y, (double)_ekf->accel.z);
@@ -1173,7 +1229,7 @@ void AttitudePositionEstimatorEKF::pollData()
 	if (PX4_ISFINITE(_sensor_combined.gyro_rad_s[0]) &&
 	    PX4_ISFINITE(_sensor_combined.gyro_rad_s[1]) &&
 	    PX4_ISFINITE(_sensor_combined.gyro_rad_s[2]) &&
-	    (_sensor_combined.gyro_errcount <= _sensor_combined.gyro1_errcount)) {
+	    (_sensor_combined.gyro_errcount <= (_sensor_combined.gyro1_errcount + GYRO_SWITCH_HYSTERESIS))) {
 
 		_ekf->angRate.x = _sensor_combined.gyro_rad_s[0];
 		_ekf->angRate.y = _sensor_combined.gyro_rad_s[1];
@@ -1212,7 +1268,7 @@ void AttitudePositionEstimatorEKF::pollData()
 		int last_accel_main = _accel_main;
 
 		/* fail over to the 2nd accel if we know the first is down */
-		if (_sensor_combined.accelerometer_errcount <= _sensor_combined.accelerometer1_errcount) {
+		if (_sensor_combined.accelerometer_errcount <= (_sensor_combined.accelerometer1_errcount + ACCEL_SWITCH_HYSTERESIS)) {
 			_ekf->accel.x = _sensor_combined.accelerometer_m_s2[0];
 			_ekf->accel.y = _sensor_combined.accelerometer_m_s2[1];
 			_ekf->accel.z = _sensor_combined.accelerometer_m_s2[2];
@@ -1320,7 +1376,11 @@ void AttitudePositionEstimatorEKF::pollData()
 				_ekf->updateDtGpsFilt(math::constrain(dtLastGoodGPS, 0.01f, POS_RESET_THRESHOLD));
 
 				// update LPF
-				_gps_alt_filt += (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
+				float filter_step = (dtLastGoodGPS / (rc + dtLastGoodGPS)) * (_ekf->gpsHgt - _gps_alt_filt);
+
+				if (isfinite(filter_step)) {
+					_gps_alt_filt += filter_step;
+				}
 			}
 
 			//check if we had a GPS outage for a long time
@@ -1403,15 +1463,19 @@ void AttitudePositionEstimatorEKF::pollData()
 		}
 
 		baro_last = _baro.timestamp;
-		if(!_baro_init) {
+		if (!_baro_init) {
 			_baro_init = true;
-			_baroAltRef = _baro.altitude;
+			_baro_alt_filt = _baro.altitude;
 		}
 
 		_ekf->updateDtHgtFilt(math::constrain(baro_elapsed, 0.001f, 0.1f));
 
 		_ekf->baroHgt = _baro.altitude;
-		_baro_alt_filt += (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
+		float filter_step = (baro_elapsed / (rc + baro_elapsed)) * (_baro.altitude - _baro_alt_filt);
+
+		if (isfinite(filter_step)) {
+			_baro_alt_filt += filter_step;
+		}
 
 		perf_count(_perf_baro);
 	}
@@ -1435,7 +1499,7 @@ void AttitudePositionEstimatorEKF::pollData()
 
 		/* fail over to the 2nd mag if we know the first is down */
 		if (hrt_elapsed_time(&_sensor_combined.magnetometer_timestamp) < mag_timeout_us &&
-			_sensor_combined.magnetometer_errcount <= _sensor_combined.magnetometer1_errcount &&
+			_sensor_combined.magnetometer_errcount <= (_sensor_combined.magnetometer1_errcount + MAG_SWITCH_HYSTERESIS) &&
 			mag0.length() > 0.1f) {
 			_ekf->magData.x = mag0.x;
 			_ekf->magBias.x = 0.000001f; // _mag_offsets.x_offset
@@ -1463,7 +1527,7 @@ void AttitudePositionEstimatorEKF::pollData()
 		}
 
 		if (last_mag_main != _mag_main) {
-			mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Switched from #%d to %d", last_mag_main, _mag_main);
+			mavlink_and_console_log_emergency(_mavlink_fd, "MAG FAILED! Failover from unit %d to unit %d", last_mag_main, _mag_main);
 		}
 	}
 
@@ -1497,30 +1561,34 @@ int AttitudePositionEstimatorEKF::trip_nan()
 
 		float nan_val = 0.0f / 0.0f;
 
-		warnx("system not armed, tripping state vector with NaN values");
+		warnx("system not armed, tripping state vector with NaN");
 		_ekf->states[5] = nan_val;
 		usleep(100000);
 
-		warnx("tripping covariance #1 with NaN values");
+		warnx("tripping covariance #1 with NaN");
 		_ekf->KH[2][2] = nan_val; //  intermediate result used for covariance updates
 		usleep(100000);
 
-		warnx("tripping covariance #2 with NaN values");
+		warnx("tripping covariance #2 with NaN");
 		_ekf->KHP[5][5] = nan_val; // intermediate result used for covariance updates
 		usleep(100000);
 
-		warnx("tripping covariance #3 with NaN values");
+		warnx("tripping covariance #3 with NaN");
 		_ekf->P[3][3] = nan_val; // covariance matrix
 		usleep(100000);
 
-		warnx("tripping Kalman gains with NaN values");
+		warnx("tripping Kalman gains with NaN");
 		_ekf->Kfusion[0] = nan_val; // Kalman gains
 		usleep(100000);
 
-		warnx("tripping stored states[0] with NaN values");
+		warnx("tripping stored states[0] with NaN");
 		_ekf->storedStates[0][0] = nan_val;
 		usleep(100000);
 
+		warnx("tripping states[9] with NaN");
+		_ekf->states[9] = nan_val;
+		usleep(100000);
+
 		warnx("\nDONE - FILTER STATE:");
 		print_status();
 	}

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -859,11 +859,36 @@ FixedwingAttitudeControl::task_main()
 						_yaw_ctrl.reset_integrator();
 					}
 				} else if (_vcontrol_mode.flag_control_velocity_enabled) {
+
+					/* the pilot does not want to change direction,
+					 * take straight attitude setpoint from position controller
+					 */
+					if (fabsf(_manual.y) < 0.01f && fabsf(_att.roll) < 0.2f) {
+						roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
+					} else {
+						roll_sp = (_manual.y * _parameters.man_roll_max)
+								+ _parameters.rollsp_offset_rad;
+					}
+
+					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
+					throttle_sp = _att_sp.thrust;
+
+					/* reset integrals where needed */
+					if (_att_sp.roll_reset_integral) {
+						_roll_ctrl.reset_integrator();
+					}
+					if (_att_sp.pitch_reset_integral) {
+						_pitch_ctrl.reset_integrator();
+					}
+					if (_att_sp.yaw_reset_integral) {
+						_yaw_ctrl.reset_integrator();
+					}
+
+				} else if (_vcontrol_mode.flag_control_altitude_enabled) {
  					/*
 					 * Velocity should be controlled and manual is enabled.
 					*/
-					roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+					roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
-											+ _parameters.rollsp_offset_rad;
 					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
 					throttle_sp = _att_sp.thrust;
 
@@ -890,10 +915,8 @@ FixedwingAttitudeControl::task_main()
 					 * the intended attitude setpoint. Later, after the rate control step the
 					 * trim is added again to the control signal.
 					 */
-					roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+					roll_sp = (_manual.y * _parameters.man_roll_max) + _parameters.rollsp_offset_rad;
-						+ _parameters.rollsp_offset_rad;
+					pitch_sp = -(_manual.x * _parameters.man_pitch_max) + _parameters.pitchsp_offset_rad;
-					pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
-						+ _parameters.pitchsp_offset_rad;
 					/* allow manual control of rudder deflection */
 					yaw_manual = _manual.r;
 					throttle_sp = _manual.z;
@@ -1060,9 +1083,9 @@ FixedwingAttitudeControl::task_main()
 
 			} else {
 				/* manual/direct control */
-				_actuators.control[0] = _manual.y;
+				_actuators.control[0] = _manual.y + _parameters.trim_roll;
-				_actuators.control[1] = -_manual.x;
+				_actuators.control[1] = -_manual.x + _parameters.trim_pitch;
-				_actuators.control[2] = _manual.r;
+				_actuators.control[2] = _manual.r + _parameters.trim_yaw;
 				_actuators.control[3] = _manual.z;
 				_actuators.control[4] = _manual.flaps;
 			}
@@ -1079,8 +1102,8 @@ FixedwingAttitudeControl::task_main()
 
 			/* Only publish if any of the proper modes are enabled */
 			if(_vcontrol_mode.flag_control_rates_enabled ||
-               _vcontrol_mode.flag_control_attitude_enabled ||
+			   _vcontrol_mode.flag_control_attitude_enabled ||
-               _vcontrol_mode.flag_control_manual_enabled)
+			   _vcontrol_mode.flag_control_manual_enabled)
 			{
 				/* publish the actuator controls */
 				if (_actuators_0_pub != nullptr) {

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -94,6 +94,13 @@
 #include <platforms/px4_defines.h>
 
 static int	_control_task = -1;			/**< task handle for sensor task */
+#define HDG_HOLD_DIST_NEXT 			3000.0f 	// initial distance of waypoint in front of plane in heading hold mode
+#define HDG_HOLD_REACHED_DIST 		1000.0f 	// distance (plane to waypoint in front) at which waypoints are reset in heading hold mode
+#define HDG_HOLD_SET_BACK_DIST 		100.0f 		// distance by which previous waypoint is set behind the plane
+#define HDG_HOLD_YAWRATE_THRESH 	0.1f 		// max yawrate at which plane locks yaw for heading hold mode
+#define HDG_HOLD_MAN_INPUT_THRESH 	0.01f 	// max manual roll input from user which does not change the locked heading
+
+#define THROTTLE_THRESH 0.05f 	// max throttle from user which will not lead to motors spinning up in altitude controlled modes
 
 
 /**
@@ -165,7 +172,13 @@ private:
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
-	float	_hold_alt;				/**< hold altitude for velocity mode */
+	float	_hold_alt;				/**< hold altitude for altitude mode */
+	float 	_ground_alt;			/**< ground altitude at which plane was launched */
+	float	_hdg_hold_yaw;				/**< hold heading for velocity mode */
+	bool	_hdg_hold_enabled;			/**< heading hold enabled */
+	bool 	_yaw_lock_engaged;			/**< yaw is locked for heading hold */
+	struct position_setpoint_s _hdg_hold_prev_wp;	/**< position where heading hold started */
+	struct position_setpoint_s _hdg_hold_curr_wp;	/**< position to which heading hold flies */
 	hrt_abstime _control_position_last_called; /**<last call of control_position  */
 
 	/* land states */
@@ -176,6 +189,9 @@ private:
 	bool land_onslope;
 	bool land_useterrain;
 
+	bool _was_in_air;	/**< indicated wether the plane was in the air in the previous interation*/
+	hrt_abstime _time_went_in_air;	/**< time at which the plane went in the air */
+
 	/* takeoff/launch states */
 	LaunchDetectionResult launch_detection_state;
 
@@ -205,6 +221,7 @@ private:
 	enum FW_POSCTRL_MODE {
 		FW_POSCTRL_MODE_AUTO,
 		FW_POSCTRL_MODE_POSITION,
+		FW_POSCTRL_MODE_ALTITUDE,
 		FW_POSCTRL_MODE_OTHER
 	} _control_mode_current;			///< used to check the mode in the last control loop iteration. Use to check if the last iteration was in the same mode.
 
@@ -355,6 +372,17 @@ private:
 	 */
 	void navigation_capabilities_publish();
 
+	/**
+	 * Get a new waypoint based on heading and distance from current position
+	 *
+	 * @param heading the heading to fly to
+	 * @param distance the distance of the generated waypoint
+	 * @param waypoint_prev the waypoint at the current position
+	 * @param waypoint_next the waypoint in the heading direction
+	 */
+	void		get_waypoint_heading_distance(float heading, float distance,
+					struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init);
+
 	/**
 	 * Return the terrain estimate during landing: uses the wp altitude value or the terrain estimate if available
 	 */
@@ -363,6 +391,17 @@ private:
 	/**
 	 * Control position.
 	 */
+
+	/**
+	 *	Do takeoff help when in altitude controlled modes
+	 */
+	 void do_takeoff_help();
+
+	/**
+	 *	Update desired altitude base on user pitch stick input
+	 */
+	 void update_desired_altitude(float dt);
+
 	bool		control_position(const math::Vector<2> &global_pos, const math::Vector<3> &ground_speed,
 					 const struct position_setpoint_triplet_s &_pos_sp_triplet);
 
@@ -454,6 +493,12 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
 
 	_hold_alt(0.0f),
+	_ground_alt(0.0f),
+	_hdg_hold_yaw(0.0f),
+	_hdg_hold_enabled(false),
+	_yaw_lock_engaged(false),
+	_hdg_hold_prev_wp{},
+	_hdg_hold_curr_wp{},
 	_control_position_last_called(0),
 
 	land_noreturn_horizontal(false),
@@ -462,6 +507,8 @@ FixedwingPositionControl::FixedwingPositionControl() :
 	land_motor_lim(false),
 	land_onslope(false),
 	land_useterrain(false),
+	_was_in_air(false),
+	_time_went_in_air(0),
 	launch_detection_state(LAUNCHDETECTION_RES_NONE),
 	last_manual(false),
 	landingslope(),
@@ -855,6 +902,8 @@ FixedwingPositionControl::calculate_gndspeed_undershoot(const math::Vector<2> &c
 
 void FixedwingPositionControl::navigation_capabilities_publish()
 {
+	_nav_capabilities.timestamp = hrt_absolute_time();
+
 	if (_nav_capabilities_pub != nullptr) {
 		orb_publish(ORB_ID(navigation_capabilities), _nav_capabilities_pub, &_nav_capabilities);
 	} else {
@@ -862,6 +911,31 @@ void FixedwingPositionControl::navigation_capabilities_publish()
 	}
 }
 
+void FixedwingPositionControl::get_waypoint_heading_distance(float heading, float distance,
+					struct position_setpoint_s &waypoint_prev, struct position_setpoint_s &waypoint_next, bool flag_init)
+{
+	waypoint_prev.valid = true;
+	waypoint_prev.alt = _hold_alt;
+
+	if (flag_init) {
+		// on init set first waypoint to momentary position
+		waypoint_prev.lat = _global_pos.lat - cos(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6;
+		waypoint_prev.lon = _global_pos.lon - sin(heading) * (double)(HDG_HOLD_SET_BACK_DIST) / 1e6;
+	} else {
+		/*
+		for previous waypoint use the one still in front of us but shift it such that it is
+		located on the desired flight path but HDG_HOLD_SET_BACK_DIST behind us
+		*/
+		waypoint_prev.lat = waypoint_next.lat - cos(heading) * (double)(HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST) / 1e6;
+		waypoint_prev.lon = waypoint_next.lon - sin(heading) * (double)(HDG_HOLD_REACHED_DIST + HDG_HOLD_SET_BACK_DIST) / 1e6;
+	}
+
+	waypoint_next.valid = true;
+	waypoint_next.lat = waypoint_prev.lat + cos(heading) * (double)(distance + HDG_HOLD_SET_BACK_DIST) / 1e6;
+	waypoint_next.lon = waypoint_prev.lon + sin(heading) * (double)(distance + HDG_HOLD_SET_BACK_DIST) / 1e6;
+	waypoint_next.alt = _hold_alt;
+}
+
 float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint_alt, const struct vehicle_global_position_s &global_pos)
 {
 	if (!isfinite(global_pos.terrain_alt)) {
@@ -881,6 +955,41 @@ float FixedwingPositionControl::get_terrain_altitude_landing(float land_setpoint
 	}
 }
 
+void FixedwingPositionControl::update_desired_altitude(float dt)
+{
+	const float deadBand = (60.0f/1000.0f);
+	const float factor = 1.0f - deadBand;
+	static bool was_in_deadband = false;
+
+	if (_manual.x > deadBand) {
+		float pitch = (_manual.x - deadBand) / factor;
+		_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
+		was_in_deadband = false;
+	} else if (_manual.x < - deadBand) {
+		float pitch = (_manual.x + deadBand) / factor;
+		_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
+		was_in_deadband = false;
+	} else if (!was_in_deadband) {
+		 /* store altitude at which manual.x was inside deadBand
+		  * The aircraft should immediately try to fly at this altitude
+		  * as this is what the pilot expects when he moves the stick to the center */
+		_hold_alt = _global_pos.alt;
+		was_in_deadband = true;
+	}
+}
+
+void FixedwingPositionControl::do_takeoff_help()
+{
+	const hrt_abstime delta_takeoff = 10000000;
+	const float throttle_threshold = 0.3f;
+	const float delta_alt_takeoff = 30.0f;
+
+	/* demand 30 m above ground if user switched into this mode during takeoff */
+	if (hrt_elapsed_time(&_time_went_in_air) < delta_takeoff && _manual.z > throttle_threshold && _global_pos.alt <= _ground_alt + delta_alt_takeoff) {
+		_hold_alt = _ground_alt + delta_alt_takeoff;
+	}
+}
+
 bool
 FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<3> &ground_speed,
 		const struct position_setpoint_triplet_s &pos_sp_triplet)
@@ -912,6 +1021,17 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 	/* no throttle limit as default */
 	float throttle_max = 1.0f;
 
+	/* save time when airplane is in air */
+	if (!_was_in_air && !_vehicle_status.condition_landed) {
+		_was_in_air = true;
+		_time_went_in_air = hrt_absolute_time();
+		_ground_alt = _global_pos.alt;
+	}
+	/* reset flag when airplane landed */
+	if (_vehicle_status.condition_landed) {
+		_was_in_air = false;
+	}
+
 	if (_control_mode.flag_control_auto_enabled &&
 			pos_sp_triplet.current.valid) {
 		/* AUTONOMOUS FLIGHT */
@@ -928,6 +1048,8 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 		/* reset hold altitude */
 		_hold_alt = _global_pos.alt;
+		/* reset hold yaw */
+		_hdg_hold_yaw = _att.yaw;
 
 		/* get circle mode */
 		bool was_circle_mode = _l1_control.circle_mode();
@@ -1251,13 +1373,15 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	} else if (_control_mode.flag_control_velocity_enabled &&
 			_control_mode.flag_control_altitude_enabled) {
-		/* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
+		/* POSITION CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed,
+		   heading is set to a distant waypoint */
 
-		const float deadBand = (60.0f/1000.0f);
-		const float factor = 1.0f - deadBand;
 		if (_control_mode_current != FW_POSCTRL_MODE_POSITION) {
 			/* Need to init because last loop iteration was in a different mode */
 			_hold_alt = _global_pos.alt;
+			_hdg_hold_yaw = _att.yaw;
+			_hdg_hold_enabled = false; // this makes sure the waypoints are reset below
+			_yaw_lock_engaged = false;
 		}
 		/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
 		if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
@@ -1274,30 +1398,118 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 					  (_parameters.airspeed_max - _parameters.airspeed_min) *
 					  _manual.z;
 
-		/* Get demanded vertical velocity from pitch control */
+		/* update desired altitude based on user pitch stick input */
-		static bool was_in_deadband = false;
+		update_desired_altitude(dt);
-		if (_manual.x > deadBand) {
+
-			float pitch = (_manual.x - deadBand) / factor;
+		/* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/
-			_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
+		do_takeoff_help();
-			was_in_deadband = false;
+
-		} else if (_manual.x < - deadBand) {
+		/* throttle limiting */
-			float pitch = (_manual.x + deadBand) / factor;
+		throttle_max = _parameters.throttle_max;
-			_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
+		if (fabsf(_manual.z) < THROTTLE_THRESH) {
-			was_in_deadband = false;
+			throttle_max = 0.0f;
-		} else if (!was_in_deadband) {
-			 /* store altitude at which manual.x was inside deadBand
-			  * The aircraft should immediately try to fly at this altitude
-			  * as this is what the pilot expects when he moves the stick to the center */
-			_hold_alt = _global_pos.alt;
-			was_in_deadband = true;
 		}
+
 		tecs_update_pitch_throttle(_hold_alt,
 				altctrl_airspeed,
 				eas2tas,
 				math::radians(_parameters.pitch_limit_min),
 				math::radians(_parameters.pitch_limit_max),
 				_parameters.throttle_min,
-				_parameters.throttle_max,
+				throttle_max,
+				_parameters.throttle_cruise,
+				false,
+				math::radians(_parameters.pitch_limit_min),
+				_global_pos.alt,
+				ground_speed,
+				tecs_status_s::TECS_MODE_NORMAL);
+
+		/* heading control */
+
+		if (fabsf(_manual.y) < HDG_HOLD_MAN_INPUT_THRESH) {
+			/* heading / roll is zero, lock onto current heading */
+			if (fabsf(_att.yawspeed) < HDG_HOLD_YAWRATE_THRESH && !_yaw_lock_engaged) {
+				// little yaw movement, lock to current heading
+				_yaw_lock_engaged = true;
+
+			}
+
+			if (_yaw_lock_engaged) {
+
+				/* just switched back from non heading-hold to heading hold */
+				if (!_hdg_hold_enabled) {
+					_hdg_hold_enabled = true;
+					_hdg_hold_yaw = _att.yaw;
+
+					get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, true);
+				}
+
+				/* we have a valid heading hold position, are we too close? */
+				if (get_distance_to_next_waypoint(_global_pos.lat, _global_pos.lon,
+					_hdg_hold_curr_wp.lat, _hdg_hold_curr_wp.lon) < HDG_HOLD_REACHED_DIST) {
+					get_waypoint_heading_distance(_hdg_hold_yaw, HDG_HOLD_DIST_NEXT, _hdg_hold_prev_wp, _hdg_hold_curr_wp, false);
+				}
+
+				math::Vector<2> prev_wp;
+				prev_wp(0) = (float)_hdg_hold_prev_wp.lat;
+				prev_wp(1) = (float)_hdg_hold_prev_wp.lon;
+
+				math::Vector<2> curr_wp;
+				curr_wp(0) = (float)_hdg_hold_curr_wp.lat;
+				curr_wp(1) = (float)_hdg_hold_curr_wp.lon;
+
+				/* populate l1 control setpoint */
+				_l1_control.navigate_waypoints(prev_wp, curr_wp, current_position, ground_speed_2d);
+
+				_att_sp.roll_body = _l1_control.nav_roll();
+				_att_sp.yaw_body = _l1_control.nav_bearing();
+			}
+		} else {
+			_hdg_hold_enabled = false;
+			_yaw_lock_engaged = false;
+		}
+
+	} else if (_control_mode.flag_control_altitude_enabled) {
+		/* ALTITUDE CONTROL: pitch stick moves altitude setpoint, throttle stick sets airspeed */
+
+		if (_control_mode_current != FW_POSCTRL_MODE_POSITION && _control_mode_current != FW_POSCTRL_MODE_ALTITUDE) {
+			/* Need to init because last loop iteration was in a different mode */
+			_hold_alt = _global_pos.alt;
+		}
+		/* Reset integrators if switching to this mode from a other mode in which posctl was not active */
+		if (_control_mode_current == FW_POSCTRL_MODE_OTHER) {
+			/* reset integrators */
+			if (_mTecs.getEnabled()) {
+				_mTecs.resetIntegrators();
+				_mTecs.resetDerivatives(_airspeed.true_airspeed_m_s);
+			}
+		}
+		_control_mode_current = FW_POSCTRL_MODE_ALTITUDE;
+
+		/* Get demanded airspeed */
+		float altctrl_airspeed = _parameters.airspeed_min +
+					  (_parameters.airspeed_max - _parameters.airspeed_min) *
+					  _manual.z;
+
+		/* update desired altitude based on user pitch stick input */
+		update_desired_altitude(dt);
+
+		/* if we assume that user is taking off then help by demanding altitude setpoint well above ground*/
+		do_takeoff_help();
+
+		/* throttle limiting */
+		throttle_max = _parameters.throttle_max;
+		if (fabsf(_manual.z) < THROTTLE_THRESH) {
+			throttle_max = 0.0f;
+		}
+
+		tecs_update_pitch_throttle(_hold_alt,
+				altctrl_airspeed,
+				eas2tas,
+				math::radians(_parameters.pitch_limit_min),
+				math::radians(_parameters.pitch_limit_max),
+				_parameters.throttle_min,
+				throttle_max,
 				_parameters.throttle_cruise,
 				false,
 				math::radians(_parameters.pitch_limit_min),
@@ -1476,7 +1688,8 @@ FixedwingPositionControl::task_main()
 				float turn_distance = _l1_control.switch_distance(100.0f);
 
 				/* lazily publish navigation capabilities */
-				if (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON && turn_distance > 0) {
+				if ((hrt_elapsed_time(&_nav_capabilities.timestamp) > 1000000) || (fabsf(turn_distance - _nav_capabilities.turn_distance) > FLT_EPSILON
+					&& turn_distance > 0)) {
 
 					/* set new turn distance */
 					_nav_capabilities.turn_distance = turn_distance;

src/modules/land_detector/FixedwingLandDetector.cpp

@@ -84,11 +84,22 @@ bool FixedwingLandDetector::update()
 	const uint64_t now = hrt_absolute_time();
 	bool landDetected = false;
 
-	// TODO: reset filtered values on arming?
+	if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) {
-	_velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx *
+		float val = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx *
-				_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
+					_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
-	_velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz);
+		if (isfinite(val)) {
-	_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
+			_velocity_xy_filtered = val;
+		}
+		val = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz);
+
+		if (isfinite(val)) {
+			_velocity_z_filtered = val;
+		}
+	}
+
+	if (hrt_elapsed_time(&_airspeed.timestamp) < 500 * 1000) {
+		_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
+	}
 
 	// crude land detector for fixedwing
 	if (_velocity_xy_filtered < _params.maxVelocity

src/modules/land_detector/LandDetector.h

@@ -87,7 +87,7 @@ protected:
 	virtual void initialize() = 0;
 
 	/**
-	* @brief Convinience function for polling uORB subscriptions
+	* @brief Convenience function for polling uORB subscriptions
 	* @return true if there was new data and it was successfully copied
 	**/
 	bool orb_update(const struct orb_metadata *meta, int handle, void *buffer);

src/modules/land_detector/land_detector_params.c

@@ -45,6 +45,8 @@
  *
  * Maximum vertical velocity allowed to trigger a land (m/s up and down)
  *
+ * @unit m/s
+ *
  * @group Land Detector
  */
 PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f);
@@ -54,6 +56,8 @@ PARAM_DEFINE_FLOAT(LNDMC_Z_VEL_MAX, 0.30f);
  *
  * Maximum horizontal velocity allowed to trigger a land (m/s)
  *
+ * @unit m/s
+ *
  * @group Land Detector
  */
 PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f);
@@ -63,6 +67,8 @@ PARAM_DEFINE_FLOAT(LNDMC_XY_VEL_MAX, 1.00f);
  *
  * Maximum allowed around each axis to trigger a land (degrees per second)
  *
+ * @unit deg/s
+ *
  * @group Land Detector
  */
 PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f);
@@ -72,6 +78,9 @@ PARAM_DEFINE_FLOAT(LNDMC_ROT_MAX, 20.0f);
  *
  * Maximum actuator output on throttle before triggering a land
  *
+ * @min 0.1
+ * @max 0.5
+ *
  * @group Land Detector
  */
 PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f);
@@ -81,24 +90,36 @@ PARAM_DEFINE_FLOAT(LNDMC_THR_MAX, 0.20f);
  *
  * Maximum horizontal velocity allowed to trigger a land (m/s)
  *
+ * @min 0.5
+ * @max 10
+ * @unit m/s
+ *
  * @group Land Detector
  */
-PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 0.40f);
+PARAM_DEFINE_FLOAT(LNDFW_VEL_XY_MAX, 4.0f);
 
 /**
  * Fixedwing max climb rate
  *
  * Maximum vertical velocity allowed to trigger a land (m/s up and down)
  *
+ * @min 5
+ * @max 20
+ * @unit m/s
+ *
  * @group Land Detector
  */
-PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.00f);
+PARAM_DEFINE_FLOAT(LNDFW_VEL_Z_MAX, 10.0f);
 
 /**
  * Airspeed max
  *
  * Maximum airspeed allowed to trigger a land (m/s)
  *
+ * @min 4
+ * @max 20
+ * @unit m/s
+ *
  * @group Land Detector
  */
-PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 10.00f);
+PARAM_DEFINE_FLOAT(LNDFW_AIRSPD_MAX, 8.00f);

src/modules/mavlink/mavlink_ftp.cpp

@@ -299,7 +299,7 @@ MavlinkFTP::_reply(mavlink_file_transfer_protocol_t* ftp_req)
 
 /// @brief Responds to a List command
 MavlinkFTP::ErrorCode
-MavlinkFTP::_workList(PayloadHeader* payload)
+MavlinkFTP::_workList(PayloadHeader* payload, bool list_hidden)
 {
 	char dirPath[kMaxDataLength];
 	strncpy(dirPath, _data_as_cstring(payload), kMaxDataLength);
@@ -383,7 +383,8 @@ MavlinkFTP::_workList(PayloadHeader* payload)
 #else
 		case DT_DIR:
 #endif
-			if (strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) {
+			if ((!list_hidden && (strncmp(entry.d_name, ".", 1) == 0)) ||
+				strcmp(entry.d_name, ".") == 0 || strcmp(entry.d_name, "..") == 0) {
 				// Don't bother sending these back
 				direntType = kDirentSkip;
 			} else {

src/modules/mavlink/mavlink_ftp.h

@@ -130,7 +130,7 @@ private:
 	void		_reply(mavlink_file_transfer_protocol_t* ftp_req);
 	int		_copy_file(const char *src_path, const char *dst_path, size_t length);
 
-	ErrorCode	_workList(PayloadHeader *payload);
+	ErrorCode	_workList(PayloadHeader *payload, bool list_hidden = false);
 	ErrorCode	_workOpen(PayloadHeader *payload, int oflag);
 	ErrorCode	_workRead(PayloadHeader *payload);
 	ErrorCode	_workBurst(PayloadHeader* payload, uint8_t target_system_id);

src/modules/mavlink/mavlink_main.cpp

@@ -1421,6 +1421,8 @@ Mavlink::task_main(int argc, char *argv[])
 
 			} else if (strcmp(myoptarg, "onboard") == 0) {
 				_mode = MAVLINK_MODE_ONBOARD;
+			} else if (strcmp(optarg, "osd") == 0) {
+				_mode = MAVLINK_MODE_OSD;
 			}
 
 			break;
@@ -1593,7 +1595,6 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("SYS_STATUS", 1.0f);
 		configure_stream("ATTITUDE", 50.0f);
 		configure_stream("HIGHRES_IMU", 50.0f);
-		configure_stream("VFR_HUD", 5.0f);
 		configure_stream("GPS_RAW_INT", 5.0f);
 		configure_stream("GLOBAL_POSITION_INT", 50.0f);
 		configure_stream("LOCAL_POSITION_NED", 30.0f);
@@ -1610,6 +1611,18 @@ Mavlink::task_main(int argc, char *argv[])
 		configure_stream("ACTUATOR_CONTROL_TARGET0", 10.0f);
 		break;
 
+	case MAVLINK_MODE_OSD:
+		configure_stream("SYS_STATUS", 5.0f);
+		configure_stream("ATTITUDE", 25.0f);
+		configure_stream("VFR_HUD", 5.0f);
+		configure_stream("GPS_RAW_INT", 1.0f);
+		configure_stream("GLOBAL_POSITION_INT", 10.0f);
+		configure_stream("ATTITUDE_TARGET", 10.0f);
+		configure_stream("BATTERY_STATUS", 1.0f);
+		configure_stream("SYSTEM_TIME", 1.0f);
+		configure_stream("RC_CHANNELS_RAW", 5.0f);
+		break;
+
 	default:
 		break;
 	}

src/modules/mavlink/mavlink_main.h

@@ -148,7 +148,8 @@ public:
 	enum MAVLINK_MODE {
 		MAVLINK_MODE_NORMAL = 0,
 		MAVLINK_MODE_CUSTOM,
-		MAVLINK_MODE_ONBOARD
+		MAVLINK_MODE_ONBOARD,
+		MAVLINK_MODE_OSD
 	};
 
 	void			set_mode(enum MAVLINK_MODE);

src/modules/mavlink/mavlink_messages.cpp

@@ -131,6 +131,12 @@ void get_mavlink_mode_state(struct vehicle_status_s *status, struct position_set
 			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_ACRO;
 			break;
 
+		case vehicle_status_s::NAVIGATION_STATE_STAB:
+			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
+								  | MAV_MODE_FLAG_STABILIZE_ENABLED;
+			custom_mode.main_mode = PX4_CUSTOM_MAIN_MODE_STABILIZED;
+			break;
+
 		case vehicle_status_s::NAVIGATION_STATE_ALTCTL:
 			*mavlink_base_mode |= MAV_MODE_FLAG_MANUAL_INPUT_ENABLED
 			                      | MAV_MODE_FLAG_STABILIZE_ENABLED;

src/modules/mavlink/mavlink_receiver.cpp

@@ -1007,7 +1007,7 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 		 * which makes the corner positions unreachable.
 		 * scale yaw up and clip it to overcome this.
 		 */
-		rc.values[2] = man.r / 1.2f + 1500;
+		rc.values[2] = man.r / 1.1f + 1500;
 		if (rc.values[2] > 2000) {
 			rc.values[2] = 2000;
 		} else if (rc.values[2] < 1000) {
@@ -1015,7 +1015,12 @@ MavlinkReceiver::handle_message_manual_control(mavlink_message_t *msg)
 		}
 
 		/* throttle */
-		rc.values[3] = man.z + 1000;
+		rc.values[3] = man.z / 0.9f + 1000;
+		if (rc.values[3] > 2000) {
+			rc.values[3] = 2000;
+		} else if (rc.values[3] < 1000) {
+			rc.values[3] = 1000;
+		}
 
 		rc.values[4] = decode_switch_pos_n(man.buttons, 0) * 1000 + 1000;
 		rc.values[5] = decode_switch_pos_n(man.buttons, 1) * 1000 + 1000;
@@ -1346,6 +1351,7 @@ MavlinkReceiver::handle_message_hil_sensor(mavlink_message_t *msg)
 		hil_sensors.baro_timestamp = timestamp;
 
 		hil_sensors.differential_pressure_pa = imu.diff_pressure * 1e2f; //from hPa to Pa
+		hil_sensors.differential_pressure_filtered_pa = hil_sensors.differential_pressure_pa;
 		hil_sensors.differential_pressure_timestamp = timestamp;
 
 		/* publish combined sensor topic */

src/modules/mc_att_control/mc_att_control_main.cpp

@@ -100,6 +100,7 @@ extern "C" __EXPORT int mc_att_control_main(int argc, char *argv[]);
 #define YAW_DEADZONE	0.05f
 #define MIN_TAKEOFF_THRUST    0.2f
 #define RATES_I_LIMIT	0.3f
+#define MANUAL_THROTTLE_MAX_MULTICOPTER	0.9f
 
 class MulticopterAttitudeControl
 {
@@ -834,7 +835,7 @@ MulticopterAttitudeControl::task_main()
 				if (_v_control_mode.flag_control_manual_enabled) {
 					/* manual rates control - ACRO mode */
 					_rates_sp = math::Vector<3>(_manual_control_sp.y, -_manual_control_sp.x, _manual_control_sp.r).emult(_params.acro_rate_max);
-					_thrust_sp = _manual_control_sp.z;
+					_thrust_sp = math::min(_manual_control_sp.z, MANUAL_THROTTLE_MAX_MULTICOPTER);
 
 					/* publish attitude rates setpoint */
 					_v_rates_sp.roll = _rates_sp(0);

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -86,6 +86,7 @@
 #define TILT_COS_MAX	0.7f
 #define SIGMA			0.000001f
 #define MIN_DIST		0.01f
+#define MANUAL_THROTTLE_MAX_MULTICOPTER	0.9f
 
 /**
  * Multicopter position control app start / stop handling function
@@ -1390,7 +1391,7 @@ MulticopterPositionControl::task_main()
 
 			//Control climb rate directly if no aiding altitude controller is active
 			if(!_control_mode.flag_control_climb_rate_enabled) {
-				_att_sp.thrust = _manual.z;
+				_att_sp.thrust = math::min(_manual.z, MANUAL_THROTTLE_MAX_MULTICOPTER);
 			}
 
 			//Construct attitude setpoint rotation matrix

src/modules/mc_pos_control/mc_pos_control_params.c

@@ -55,13 +55,16 @@ PARAM_DEFINE_FLOAT(MPC_THR_MIN, 0.1f);
 /**
  * Maximum thrust
  *
- * Limit max allowed thrust.
+ * Limit max allowed thrust. Setting a value of one can put
+ * the system into actuator saturation as no spread between
+ * the motors is possible any more. A value of 0.8 - 0.9
+ * is recommended.
  *
  * @min 0.0
  * @max 1.0
  * @group Multicopter Position Control
  */
-PARAM_DEFINE_FLOAT(MPC_THR_MAX, 1.0f);
+PARAM_DEFINE_FLOAT(MPC_THR_MAX, 0.9f);
 
 /**
  * Proportional gain for vertical position error

src/modules/navigator/mission.cpp

@@ -230,9 +230,9 @@ Mission::update_offboard_mission()
 		 * however warnings are issued to the gcs via mavlink from inside the MissionFeasiblityChecker */
 		dm_item_t dm_current = DM_KEY_WAYPOINTS_OFFBOARD(_offboard_mission.dataman_id);
 
-		failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_vstatus()->is_rotary_wing,
+		failed = !_missionFeasiblityChecker.checkMissionFeasible(_navigator->get_mavlink_fd(), _navigator->get_vstatus()->is_rotary_wing,
 				dm_current, (size_t) _offboard_mission.count, _navigator->get_geofence(),
-				_navigator->get_home_position()->alt);
+				_navigator->get_home_position()->alt, _navigator->home_position_valid());
 
 	} else {
 		warnx("offboard mission update failed");
@@ -271,6 +271,16 @@ Mission::advance_mission()
 	}
 }
 
+int
+Mission::get_absolute_altitude_for_item(struct mission_item_s &mission_item)
+{
+	if (_mission_item.altitude_is_relative) {
+		return _mission_item.altitude + _navigator->get_home_position()->alt;
+	} else {
+		return _mission_item.altitude;
+	}
+}
+
 bool
 Mission::check_dist_1wp()
 {
@@ -297,10 +307,10 @@ Mission::check_dist_1wp()
 							mission_item.nav_cmd == NAV_CMD_TAKEOFF ||
 							mission_item.nav_cmd == NAV_CMD_PATHPLANNING) {
 
-						/* check distance from home to item */
+						/* check distance from current position to item */
 						float dist_to_1wp = get_distance_to_next_waypoint(
 								mission_item.lat, mission_item.lon,
-								_navigator->get_home_position()->lat, _navigator->get_home_position()->lon);
+								_navigator->get_global_position()->lat, _navigator->get_global_position()->lon);
 
 						if (dist_to_1wp < _param_dist_1wp.get()) {
 							_dist_1wp_ok = true;
@@ -354,7 +364,7 @@ Mission::set_mission_items()
 
 	/* Copy previous mission item altitude (can be extended to a copy of the full mission item if needed) */
 	if (pos_sp_triplet->previous.valid) {
-		_mission_item_previous_alt = _mission_item.altitude;
+		_mission_item_previous_alt = get_absolute_altitude_for_item(_mission_item);
 	}
 
 	/* get home distance state */
@@ -374,13 +384,14 @@ Mission::set_mission_items()
 	} else if (home_dist_ok && read_mission_item(false, true, &_mission_item)) {
 		/* if mission type changed, notify */
 		if (_mission_type != MISSION_TYPE_OFFBOARD) {
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "offboard mission now running");
+			mavlink_log_info(_navigator->get_mavlink_fd(), "offboard mission now running");
 		}
 		_mission_type = MISSION_TYPE_OFFBOARD;
 	} else {
 		/* no mission available or mission finished, switch to loiter */
 		if (_mission_type != MISSION_TYPE_NONE) {
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished");
+			/* https://en.wikipedia.org/wiki/Loiter_(aeronautics) */
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "mission finished, loitering");
 
 			/* use last setpoint for loiter */
 			_navigator->set_can_loiter_at_sp(true);
@@ -388,8 +399,9 @@ Mission::set_mission_items()
 		} else if (!user_feedback_done) {
 			/* only tell users that we got no mission if there has not been any
 			 * better, more specific feedback yet
+			 * https://en.wikipedia.org/wiki/Loiter_(aeronautics)
 			 */
-			mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available");
+			mavlink_log_critical(_navigator->get_mavlink_fd(), "no valid mission available, loitering");
 		}
 		_mission_type = MISSION_TYPE_NONE;
 
@@ -440,10 +452,7 @@ Mission::set_mission_items()
 		mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->next);
 
 		/* calculate takeoff altitude */
-		float takeoff_alt = _mission_item.altitude;
+		float takeoff_alt = get_absolute_altitude_for_item(_mission_item);
-		if (_mission_item.altitude_is_relative) {
-			takeoff_alt += _navigator->get_home_position()->alt;
-		}
 
 		/* takeoff to at least NAV_TAKEOFF_ALT above home/ground, even if first waypoint is lower */
 		if (_navigator->get_vstatus()->condition_landed) {
@@ -586,7 +595,7 @@ Mission::altitude_sp_foh_update()
 	}
 
 	/* Do not try to find a solution if the last waypoint is inside the acceptance radius of the current one */
-	if (_distance_current_previous - _mission_item.acceptance_radius < 0.0f) {
+	if (_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius) < 0.0f) {
 		return;
 	}
 
@@ -608,16 +617,16 @@ Mission::altitude_sp_foh_update()
 
 	/* if the minimal distance is smaller then the acceptance radius, we should be at waypoint alt
 	 * navigator will soon switch to the next waypoint item (if there is one) as soon as we reach this altitude */
-	if (_min_current_sp_distance_xy < _mission_item.acceptance_radius) {
+	if (_min_current_sp_distance_xy < _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) {
-		pos_sp_triplet->current.alt = _mission_item.altitude;
+		pos_sp_triplet->current.alt = get_absolute_altitude_for_item(_mission_item);
 	} else {
 		/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
 		* of the mission item as it is used to check if the mission item is reached
 		* The setpoint is set linearly and such that the system reaches the current altitude at the acceptance
 		* radius around the current waypoint
 		**/
-		float delta_alt = (_mission_item.altitude - _mission_item_previous_alt);
+		float delta_alt = (get_absolute_altitude_for_item(_mission_item) - _mission_item_previous_alt);
-		float grad = -delta_alt/(_distance_current_previous - _mission_item.acceptance_radius);
+		float grad = -delta_alt/(_distance_current_previous - _navigator->get_acceptance_radius(_mission_item.acceptance_radius));
 		float a = _mission_item_previous_alt - grad * _distance_current_previous;
 		pos_sp_triplet->current.alt = a + grad * _min_current_sp_distance_xy;
 

src/modules/navigator/mission.h

@@ -132,6 +132,8 @@ private:
 	 */
 	void altitude_sp_foh_reset();
 
+	int get_absolute_altitude_for_item(struct mission_item_s &mission_item);
+
 	/**
 	 * Read current or next mission item from the dataman and watch out for DO_JUMPS
 	 * @return true if successful

src/modules/navigator/mission_block.cpp

@@ -123,12 +123,12 @@ MissionBlock::is_mission_item_reached()
 			 * Therefore the item is marked as reached once the system reaches the loiter
 			 * radius (+ some margin). Time inside and turn count is handled elsewhere.
 			 */
-			if (dist >= 0.0f && dist <= _mission_item.loiter_radius * 1.2f) {
+			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.loiter_radius * 1.2f)) {
 				_waypoint_position_reached = true;
 			}
 		} else {
 			/* for normal mission items used their acceptance radius */
-			if (dist >= 0.0f && dist <= _mission_item.acceptance_radius) {
+			if (dist >= 0.0f && dist <= _navigator->get_acceptance_radius(_mission_item.acceptance_radius)) {
 				_waypoint_position_reached = true;
 			}
 		}

src/modules/navigator/mission_feasibility_checker.cpp

@@ -63,42 +63,43 @@ MissionFeasibilityChecker::MissionFeasibilityChecker() : _mavlink_fd(-1), _capab
 }
 
 
-bool MissionFeasibilityChecker::checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
+bool MissionFeasibilityChecker::checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
 {
 	bool failed = false;
 	/* Init if not done yet */
 	init();
 
-	/* Open mavlink fd */
+	_mavlink_fd = mavlink_fd;
-	if (_mavlink_fd < 0) {
-		/* try to open the mavlink log device every once in a while */
-		_mavlink_fd = open(MAVLINK_LOG_DEVICE, 0);
-	}
 
 	// check if all mission item commands are supported
 	failed |= !checkMissionItemValidity(dm_current, nMissionItems);
 
 
-	if (isRotarywing)
+	if (isRotarywing) {
-		failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt);
+		failed |= !checkMissionFeasibleRotarywing(dm_current, nMissionItems, geofence, home_alt, home_valid);
-	else
+	} else {
-		failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt);
+		failed |= !checkMissionFeasibleFixedwing(dm_current, nMissionItems, geofence, home_alt, home_valid);
+	}
+
+	if (!failed) {
+		mavlink_log_info(_mavlink_fd, "Mission checked and ready.");
+	}
 
 	return !failed;
 }
 
-bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
+bool MissionFeasibilityChecker::checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
 {
 
 	/* Perform checks and issue feedback to the user for all checks */
 	bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
-	bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
+	bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid);
 
 	/* Mission is only marked as feasible if all checks return true */
 	return (resGeofence && resHomeAltitude);
 }
 
-bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt)
+bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid)
 {
 	/* Update fixed wing navigation capabilites */
 	updateNavigationCapabilities();
@@ -107,7 +108,7 @@ bool MissionFeasibilityChecker::checkMissionFeasibleFixedwing(dm_item_t dm_curre
 	/* Perform checks and issue feedback to the user for all checks */
 	bool resLanding = checkFixedWingLanding(dm_current, nMissionItems);
 	bool resGeofence = checkGeofence(dm_current, nMissionItems, geofence);
-	bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt);
+	bool resHomeAltitude = checkHomePositionAltitude(dm_current, nMissionItems, home_alt, home_valid);
 
 	/* Mission is only marked as feasible if all checks return true */
 	return (resLanding && resGeofence && resHomeAltitude);
@@ -136,7 +137,7 @@ bool MissionFeasibilityChecker::checkGeofence(dm_item_t dm_current, size_t nMiss
 	return true;
 }
 
-bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error)
+bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error)
 {
 	/* Check if all all waypoints are above the home altitude, only return false if bool throw_error = true */
 	for (size_t i = 0; i < nMissionItems; i++) {
@@ -152,6 +153,12 @@ bool MissionFeasibilityChecker::checkHomePositionAltitude(dm_item_t dm_current,
 			}
 		}
 
+		/* always reject relative alt without home set */
+		if (missionitem.altitude_is_relative && !home_valid) {
+			mavlink_log_critical(_mavlink_fd, "Rejecting Mission: No home pos, WP %d uses rel alt", i);
+			return false;
+		}
+
 		/* calculate the global waypoint altitude */
 		float wp_alt = (missionitem.altitude_is_relative) ? missionitem.altitude + home_alt : missionitem.altitude;
 
@@ -197,7 +204,6 @@ bool MissionFeasibilityChecker::checkMissionItemValidity(dm_item_t dm_current, s
 			return false;
 		}
 	}
-	mavlink_log_info(_mavlink_fd, "Mission ready.");
 	return true;
 }
 

src/modules/navigator/mission_feasibility_checker.h

@@ -61,16 +61,16 @@ private:
 
 	/* Checks for all airframes */
 	bool checkGeofence(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence);
-	bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool throw_error = false);
+	bool checkHomePositionAltitude(dm_item_t dm_current, size_t nMissionItems, float home_alt, bool home_valid, bool throw_error = false);
 	bool checkMissionItemValidity(dm_item_t dm_current, size_t nMissionItems);
 
 	/* Checks specific to fixedwing airframes */
-	bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
+	bool checkMissionFeasibleFixedwing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
 	bool checkFixedWingLanding(dm_item_t dm_current, size_t nMissionItems);
 	void updateNavigationCapabilities();
 
 	/* Checks specific to rotarywing airframes */
-	bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt);
+	bool checkMissionFeasibleRotarywing(dm_item_t dm_current, size_t nMissionItems, Geofence &geofence, float home_alt, bool home_valid);
 public:
 
 	MissionFeasibilityChecker();
@@ -79,8 +79,8 @@ public:
 	/*
 	 * Returns true if mission is feasible and false otherwise
 	 */
-	bool checkMissionFeasible(bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
+	bool checkMissionFeasible(int mavlink_fd, bool isRotarywing, dm_item_t dm_current, size_t nMissionItems, Geofence &geofence,
-				  float home_alt);
+				  float home_alt, bool home_valid);
 
 };
 

src/modules/navigator/mission_params.c

@@ -81,7 +81,7 @@ PARAM_DEFINE_INT32(MIS_ONBOARD_EN, 1);
  * @max 1000
  * @group Mission
  */
-PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500);
+PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 900);
 
 /**
  * Altitude setpoint mode
@@ -94,7 +94,7 @@ PARAM_DEFINE_FLOAT(MIS_DIST_1WP, 500);
  * @max 1
  * @group Mission
  */
-PARAM_DEFINE_INT32(MIS_ALTMODE, 0);
+PARAM_DEFINE_INT32(MIS_ALTMODE, 1);
 
 /**
  * Multirotor only. Yaw setpoint mode.

src/modules/navigator/navigator.h

@@ -134,6 +134,7 @@ public:
 	struct vehicle_gps_position_s*	    get_gps_position() { return &_gps_pos; }
 	struct sensor_combined_s*	    get_sensor_combined() { return &_sensor_combined; }
 	struct home_position_s*		    get_home_position() { return &_home_pos; }
+	bool				    home_position_valid() { return _home_position_set; }
 	struct position_setpoint_triplet_s* get_position_setpoint_triplet() { return &_pos_sp_triplet; }
 	struct mission_result_s*	    get_mission_result() { return &_mission_result; }
 	struct geofence_result_s*		    get_geofence_result() { return &_geofence_result; }
@@ -144,7 +145,22 @@ public:
 	Geofence&	get_geofence() { return _geofence; }
 	bool		get_can_loiter_at_sp() { return _can_loiter_at_sp; }
 	float		get_loiter_radius() { return _param_loiter_radius.get(); }
-	float		get_acceptance_radius() { return _param_acceptance_radius.get(); }
+
+	/**
+	 * Get the acceptance radius
+	 *
+	 * @return the distance at which the next waypoint should be used
+	 */
+	float		get_acceptance_radius();
+
+	/**
+	 * Get the acceptance radius given the mission item preset radius
+	 *
+	 * @param mission_item_radius the radius to use in case the controller-derived radius is smaller
+	 *
+	 * @return the distance at which the next waypoint should be used
+	 */
+	float		get_acceptance_radius(float mission_item_radius);
 	int		get_mavlink_fd() { return _mavlink_fd; }
 
 private:

src/modules/navigator/navigator_main.cpp

@@ -212,7 +212,10 @@ Navigator::home_position_update()
 
 	if (updated) {
 		orb_copy(ORB_ID(home_position), _home_pos_sub, &_home_pos);
-		_home_position_set = true;
+
+		if (_home_pos.timestamp > 0) {
+			_home_position_set = true;
+		}
 	}
 }
 
@@ -574,6 +577,26 @@ Navigator::publish_position_setpoint_triplet()
 	}
 }
 
+float
+Navigator::get_acceptance_radius()
+{
+	return get_acceptance_radius(_param_acceptance_radius.get());
+}
+
+float
+Navigator::get_acceptance_radius(float mission_item_radius)
+{
+	float radius = mission_item_radius;
+
+	if (hrt_elapsed_time(&_nav_caps.timestamp) < 5000000) {
+		if (_nav_caps.turn_distance > radius) {
+			radius = _nav_caps.turn_distance;
+		}
+	}
+
+	return radius;
+}
+
 void Navigator::add_fence_point(int argc, char *argv[])
 {
 	_geofence.addPoint(argc, argv);

src/modules/px4iofirmware/controls.c

@@ -64,6 +64,10 @@ static int _dsm_fd;
 
 static uint16_t rc_value_override = 0;
 
+#ifdef ADC_RSSI
+static unsigned _rssi_adc_counts = 0;
+#endif
+
 bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool *sumd_updated)
 {
 	perf_begin(c_gather_dsm);
@@ -71,17 +75,22 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
 	uint8_t n_bytes = 0;
 	uint8_t *bytes;
 	*dsm_updated = dsm_input(r_raw_rc_values, &temp_count, &n_bytes, &bytes);
+
 	if (*dsm_updated) {
 		r_raw_rc_count = temp_count & 0x7fff;
-		if (temp_count & 0x8000)
+
+		if (temp_count & 0x8000) {
 			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
-		else
+
+		} else {
 			r_raw_rc_flags &= ~PX4IO_P_RAW_RC_FLAGS_RC_DSM11;
+		}
 
 		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
 		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 
 	}
+
 	perf_end(c_gather_dsm);
 
 	/* get data from FD and attempt to parse with DSM and ST24 libs */
@@ -94,7 +103,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
 		/* set updated flag if one complete packet was parsed */
 		st24_rssi = RC_INPUT_RSSI_MAX;
 		*st24_updated |= (OK == st24_decode(bytes[i], &st24_rssi, &rx_count,
-					&st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
+						    &st24_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
 	}
 
 	if (*st24_updated) {
@@ -121,7 +130,7 @@ bool dsm_port_input(uint16_t *rssi, bool *dsm_updated, bool *st24_updated, bool
 		/* set updated flag if one complete packet was parsed */
 		sumd_rssi = RC_INPUT_RSSI_MAX;
 		*sumd_updated |= (OK == sumd_decode(bytes[i], &sumd_rssi, &sumd_rx_count,
-					&sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
+						    &sumd_channel_count, r_raw_rc_values, PX4IO_RC_INPUT_CHANNELS));
 	}
 
 	if (*sumd_updated) {
@@ -170,7 +179,8 @@ controls_init(void)
 }
 
 void
-controls_tick() {
+controls_tick()
+{
 
 	/*
 	 * Gather R/C control inputs from supported sources.
@@ -184,19 +194,24 @@ controls_tick() {
 	uint16_t rssi = 0;
 
 #ifdef ADC_RSSI
+
 	if (r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI) {
 		unsigned counts = adc_measure(ADC_RSSI);
-		if (counts != 0xffff) {
-			/* use 1:1 scaling on 3.3V ADC input */
-			unsigned mV = counts * 3300 / 4096;
 
-			/* scale to 0..253 and lowpass */
+		if (counts != 0xffff) {
-			rssi = (rssi * 0.99f) + ((mV / (3300 / RC_INPUT_RSSI_MAX)) * 0.01f);
+			/* low pass*/
+			_rssi_adc_counts = (_rssi_adc_counts * 0.998f) + (counts * 0.002f);
+			/* use 1:1 scaling on 3.3V, 12-Bit ADC input */
+			unsigned mV = _rssi_adc_counts * 3300 / 4095;
+			/* scale to 0..100 (RC_INPUT_RSSI_MAX == 100) */
+			rssi = (mV * RC_INPUT_RSSI_MAX / 3300);
+
 			if (rssi > RC_INPUT_RSSI_MAX) {
 				rssi = RC_INPUT_RSSI_MAX;
 			}
 		}
 	}
+
 #endif
 
 	/* zero RSSI if signal is lost */
@@ -207,21 +222,26 @@ controls_tick() {
 	perf_begin(c_gather_dsm);
 	bool dsm_updated, st24_updated, sumd_updated;
 	(void)dsm_port_input(&rssi, &dsm_updated, &st24_updated, &sumd_updated);
+
 	if (dsm_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_DSM;
 	}
+
 	if (st24_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_ST24;
 	}
+
 	if (sumd_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SUMD;
 	}
+
 	perf_end(c_gather_dsm);
 
 	perf_begin(c_gather_sbus);
 
 	bool sbus_failsafe, sbus_frame_drop;
-	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop, PX4IO_RC_INPUT_CHANNELS);
+	bool sbus_updated = sbus_input(r_raw_rc_values, &r_raw_rc_count, &sbus_failsafe, &sbus_frame_drop,
+				       PX4IO_RC_INPUT_CHANNELS);
 
 	if (sbus_updated) {
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_SBUS;
@@ -231,17 +251,19 @@ controls_tick() {
 		if (sbus_frame_drop) {
 			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FRAME_DROP;
 			sbus_rssi = RC_INPUT_RSSI_MAX / 2;
+
 		} else {
 			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
 		}
 
 		if (sbus_failsafe) {
 			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
+
 		} else {
 			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 		}
 
-		/* set RSSI to an emulated value if ADC RSSI is off */ 
+		/* set RSSI to an emulated value if ADC RSSI is off */
 		if (!(r_setup_features & PX4IO_P_SETUP_FEATURES_ADC_RSSI)) {
 			rssi = sbus_rssi;
 		}
@@ -257,17 +279,20 @@ controls_tick() {
 	 */
 	perf_begin(c_gather_ppm);
 	bool ppm_updated = ppm_input(r_raw_rc_values, &r_raw_rc_count, &r_page_raw_rc_input[PX4IO_P_RAW_RC_DATA]);
+
 	if (ppm_updated) {
 
 		r_status_flags |= PX4IO_P_STATUS_FLAGS_RC_PPM;
 		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FRAME_DROP);
 		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 	}
+
 	perf_end(c_gather_ppm);
 
 	/* limit number of channels to allowable data size */
-	if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS)
+	if (r_raw_rc_count > PX4IO_RC_INPUT_CHANNELS) {
 		r_raw_rc_count = PX4IO_RC_INPUT_CHANNELS;
+	}
 
 	/* store RSSI */
 	r_page_raw_rc_input[PX4IO_P_RAW_RC_NRSSI] = rssi;
@@ -308,10 +333,13 @@ controls_tick() {
 				/*
 				 * 1) Constrain to min/max values, as later processing depends on bounds.
 				 */
-				if (raw < conf[PX4IO_P_RC_CONFIG_MIN])
+				if (raw < conf[PX4IO_P_RC_CONFIG_MIN]) {
 					raw = conf[PX4IO_P_RC_CONFIG_MIN];
-				if (raw > conf[PX4IO_P_RC_CONFIG_MAX])
+				}
+
+				if (raw > conf[PX4IO_P_RC_CONFIG_MAX]) {
 					raw = conf[PX4IO_P_RC_CONFIG_MAX];
+				}
 
 				/*
 				 * 2) Scale around the mid point differently for lower and upper range.
@@ -330,10 +358,12 @@ controls_tick() {
 				 * DO NOT REMOVE OR ALTER STEP 1!
 				 */
 				if (raw > (conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
+					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(
+								     conf[PX4IO_P_RC_CONFIG_MAX] - conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE]));
 
 				} else if (raw < (conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE])) {
-					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
+					scaled = 10000.0f * ((raw - conf[PX4IO_P_RC_CONFIG_CENTER] + conf[PX4IO_P_RC_CONFIG_DEADZONE]) / (float)(
+								     conf[PX4IO_P_RC_CONFIG_CENTER] - conf[PX4IO_P_RC_CONFIG_DEADZONE] - conf[PX4IO_P_RC_CONFIG_MIN]));
 
 				} else {
 					/* in the configured dead zone, output zero */
@@ -347,6 +377,7 @@ controls_tick() {
 
 				/* and update the scaled/mapped version */
 				unsigned mapped = conf[PX4IO_P_RC_CONFIG_ASSIGNMENT];
+
 				if (mapped < PX4IO_CONTROL_CHANNELS) {
 
 					/* invert channel if pitch - pulling the lever down means pitching up by convention */
@@ -360,6 +391,7 @@ controls_tick() {
 						if (((raw < conf[PX4IO_P_RC_CONFIG_MIN]) && (raw < r_setup_rc_thr_failsafe)) ||
 						    ((raw > conf[PX4IO_P_RC_CONFIG_MAX]) && (raw > r_setup_rc_thr_failsafe))) {
 							r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_FAILSAFE;
+
 						} else {
 							r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_FAILSAFE);
 						}
@@ -389,6 +421,7 @@ controls_tick() {
 		/* if we have enough channels (5) to control the vehicle, the mapping is ok */
 		if (assigned_channels > 4) {
 			r_raw_rc_flags |= PX4IO_P_RAW_RC_FLAGS_MAPPING_OK;
+
 		} else {
 			r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_MAPPING_OK);
 		}
@@ -408,9 +441,9 @@ controls_tick() {
 
 		/* clear the input-kind flags here */
 		r_status_flags &= ~(
-			PX4IO_P_STATUS_FLAGS_RC_PPM |
+					  PX4IO_P_STATUS_FLAGS_RC_PPM |
-			PX4IO_P_STATUS_FLAGS_RC_DSM |
+					  PX4IO_P_STATUS_FLAGS_RC_DSM |
-			PX4IO_P_STATUS_FLAGS_RC_SBUS);
+					  PX4IO_P_STATUS_FLAGS_RC_SBUS);
 
 	}
 
@@ -427,8 +460,8 @@ controls_tick() {
 	if (rc_input_lost) {
 		/* Clear the RC input status flag, clear manual override flag */
 		r_status_flags &= ~(
-			PX4IO_P_STATUS_FLAGS_OVERRIDE |
+					  PX4IO_P_STATUS_FLAGS_OVERRIDE |
-			PX4IO_P_STATUS_FLAGS_RC_OK);
+					  PX4IO_P_STATUS_FLAGS_RC_OK);
 
 		/* flag raw RC as lost */
 		r_raw_rc_flags &= ~(PX4IO_P_RAW_RC_FLAGS_RC_OK);
@@ -451,9 +484,9 @@ controls_tick() {
 	 * Override is enabled if either the hardcoded channel / value combination
 	 * is selected, or the AP has requested it.
 	 */
-	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) && 
+	if ((r_setup_arming & PX4IO_P_SETUP_ARMING_MANUAL_OVERRIDE_OK) &&
-		(r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
+	    (r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) &&
-		!(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
+	    !(r_raw_rc_flags & PX4IO_P_RAW_RC_FLAGS_FAILSAFE)) {
 
 		bool override = false;
 
@@ -464,8 +497,9 @@ controls_tick() {
 		 * requested override.
 		 *
 		 */
-		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH))
+		if ((r_status_flags & PX4IO_P_STATUS_FLAGS_RC_OK) && (REG_TO_SIGNED(rc_value_override) < RC_CHANNEL_LOW_THRESH)) {
 			override = true;
+		}
 
 		/*
 		  if the FMU is dead then enable override if we have a
@@ -473,20 +507,23 @@ controls_tick() {
 		 */
 		if (!(r_status_flags & PX4IO_P_STATUS_FLAGS_FMU_OK) &&
 		    (r_setup_arming & PX4IO_P_SETUP_ARMING_OVERRIDE_IMMEDIATE) &&
-		    (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK))
+		    (r_status_flags & PX4IO_P_STATUS_FLAGS_MIXER_OK)) {
-			override = true;                
+			override = true;
+		}
 
 		if (override) {
 
 			r_status_flags |= PX4IO_P_STATUS_FLAGS_OVERRIDE;
 
 			/* mix new RC input control values to servos */
-			if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated)
+			if (dsm_updated || sbus_updated || ppm_updated || st24_updated || sumd_updated) {
 				mixer_tick();
+			}
 
 		} else {
 			r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
 		}
+
 	} else {
 		r_status_flags &= ~(PX4IO_P_STATUS_FLAGS_OVERRIDE);
 	}
@@ -512,8 +549,10 @@ ppm_input(uint16_t *values, uint16_t *num_values, uint16_t *frame_len)
 
 		/* PPM data exists, copy it */
 		*num_values = ppm_decoded_channels;
-		if (*num_values > PX4IO_RC_INPUT_CHANNELS)
+
+		if (*num_values > PX4IO_RC_INPUT_CHANNELS) {
 			*num_values = PX4IO_RC_INPUT_CHANNELS;
+		}
 
 		for (unsigned i = 0; ((i < *num_values) && (i < PPM_MAX_CHANNELS)); i++) {
 			values[i] = ppm_buffer[i];

src/modules/px4iofirmware/mixer.cpp

@@ -318,6 +318,13 @@ mixer_callback(uintptr_t handle,
 	case MIX_OVERRIDE:
 		if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
 			control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
+			if (control_group == 0 && control_index == 0) {
+				control += REG_TO_FLOAT(r_setup_trim_roll);
+			} else if (control_group == 0 && control_index == 1) {
+				control += REG_TO_FLOAT(r_setup_trim_pitch);
+			} else if (control_group == 0 && control_index == 2) {
+				control += REG_TO_FLOAT(r_setup_trim_yaw);
+			}
 			break;
 		}
 		return -1;
@@ -326,6 +333,13 @@ mixer_callback(uintptr_t handle,
 		/* FMU is ok but we are in override mode, use direct rc control for the available rc channels. The remaining channels are still controlled by the fmu */
 		if (r_page_rc_input[PX4IO_P_RC_VALID] & (1 << CONTROL_PAGE_INDEX(control_group, control_index))) {
 			control = REG_TO_FLOAT(r_page_rc_input[PX4IO_P_RC_BASE + control_index]);
+			if (control_group == 0 && control_index == 0) {
+				control += REG_TO_FLOAT(r_setup_trim_roll);
+			} else if (control_group == 0 && control_index == 1) {
+				control += REG_TO_FLOAT(r_setup_trim_pitch);
+			} else if (control_group == 0 && control_index == 2) {
+				control += REG_TO_FLOAT(r_setup_trim_yaw);
+			}
 			break;
 		} else if (control_index < PX4IO_CONTROL_CHANNELS && control_group < PX4IO_CONTROL_GROUPS) {
 			control = REG_TO_FLOAT(r_page_controls[CONTROL_PAGE_INDEX(control_group, control_index)]);
@@ -339,6 +353,13 @@ mixer_callback(uintptr_t handle,
 		return -1;
 	}
 
+	/* limit output */
+	if (control > 1.0f) {
+		control = 1.0f;
+	} else if (control < -1.0f) {
+		control = -1.0f;
+	}
+
 	return 0;
 }
 

src/modules/px4iofirmware/protocol.h

@@ -234,6 +234,9 @@ enum {							/* DSM bind states */
 #define PX4IO_FORCE_SAFETY_MAGIC		22027	/* required argument for force safety (random) */
 
 #define PX4IO_P_SETUP_PWM_REVERSE		15	/**< Bitmask to reverse PWM channels 1-8 */
+#define PX4IO_P_SETUP_TRIM_ROLL			16	/**< Roll trim, in actuator units */
+#define PX4IO_P_SETUP_TRIM_PITCH		17	/**< Pitch trim, in actuator units */
+#define PX4IO_P_SETUP_TRIM_YAW			18	/**< Yaw trim, in actuator units */
 
 /* autopilot control values, -10000..10000 */
 #define PX4IO_PAGE_CONTROLS			51	/**< actuator control groups, one after the other, 8 wide */

src/modules/px4iofirmware/px4io.h

@@ -113,6 +113,10 @@ extern uint16_t			r_page_servo_disarmed[];	/* PX4IO_PAGE_DISARMED_PWM */
 
 #define r_setup_pwm_reverse	r_page_setup[PX4IO_P_SETUP_PWM_REVERSE]
 
+#define r_setup_trim_roll	r_page_setup[PX4IO_P_SETUP_TRIM_ROLL]
+#define r_setup_trim_pitch	r_page_setup[PX4IO_P_SETUP_TRIM_PITCH]
+#define r_setup_trim_yaw	r_page_setup[PX4IO_P_SETUP_TRIM_YAW]
+
 #define r_control_values	(&r_page_controls[0])
 
 /*

src/modules/px4iofirmware/registers.c

@@ -174,6 +174,9 @@ volatile uint16_t	r_page_setup[] =
 	[PX4IO_P_SETUP_CRC ... (PX4IO_P_SETUP_CRC+1)] = 0,
 	[PX4IO_P_SETUP_RC_THR_FAILSAFE_US] = 0,
 	[PX4IO_P_SETUP_PWM_REVERSE] = 0,
+	[PX4IO_P_SETUP_TRIM_ROLL] = 0,
+	[PX4IO_P_SETUP_TRIM_PITCH] = 0,
+	[PX4IO_P_SETUP_TRIM_YAW] = 0
 };
 
 #ifdef CONFIG_ARCH_BOARD_PX4IO_V2
@@ -627,6 +630,12 @@ registers_set_one(uint8_t page, uint8_t offset, uint16_t value)
 			r_page_setup[PX4IO_P_SETUP_PWM_REVERSE] = value;
 			break;
 
+		case PX4IO_P_SETUP_TRIM_ROLL:
+		case PX4IO_P_SETUP_TRIM_PITCH:
+		case PX4IO_P_SETUP_TRIM_YAW:
+			r_page_setup[offset] = value;
+			break;
+
 		default:
 			return -1;
 		}

src/modules/systemlib/system_params.c

@@ -88,9 +88,9 @@ PARAM_DEFINE_INT32(SYS_RESTART_TYPE, 2);
 * Companion computer interface
 *
 * Configures the baud rate of the companion computer interface.
-* Set to zero to disable, set to 921600 to enable.
+* Set to zero to disable, set to these values to enable (NO OTHER VALUES SUPPORTED!)
-* CURRENTLY ONLY SUPPORTS 921600 BAUD! Use extras.txt for
+* 921600: enables onboard mode at 921600 baud, 8N1. 57600: enables onboard mode at 57600 baud, 8N1.
-* other baud rates.
+* 157600: enables OSD mode at 57600 baud, 8N1.
 *
 * @min 0
 * @max 921600

src/modules/uORB/topics/navigation_capabilities.h

@@ -1,70 +0,0 @@
-/****************************************************************************
- *
- *   Copyright (C) 2012-2013 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 navigation_capabilities.h
- *
- * Definition of navigation capabilities uORB topic.
- */
-
-#ifndef TOPIC_NAVIGATION_CAPABILITIES_H_
-#define TOPIC_NAVIGATION_CAPABILITIES_H_
-
-#include "../uORB.h"
-#include <stdint.h>
-
-/**
- * @addtogroup topics
- * @{
- */
-
-/**
- * Airspeed
- */
-struct navigation_capabilities_s {
-	float turn_distance;		/**< the optimal distance to a waypoint to switch to the next */
-
-	/* Landing parameters: see fw_pos_control_l1/landingslope.h */
-	float landing_horizontal_slope_displacement;
-	float landing_slope_angle_rad;
-	float landing_flare_length;
-};
-
-/**
- * @}
- */
-
-/* register this as object request broker structure */
-ORB_DECLARE(navigation_capabilities);
-
-#endif

src/modules/uavcan/sensors/baro.hpp

@@ -58,7 +58,7 @@ public:
 	int init() override;
 
 private:
-	ssize_t	read(struct file *filp, char *buffer, size_t buflen);
+	ssize_t read(struct file *filp, char *buffer, size_t buflen);
 	int ioctl(struct file *filp, int cmd, unsigned long arg) override;
 
 	void air_pressure_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg);