Merge branch 'release_v1.0.0' of github.com:PX4/Firmware into beta

2026-05-16 14:07:34 +08:00 · 2015-06-08 17:24:00 +02:00
parent e6b97321df 696e1fc9e2
commit cfb44b9192
38 changed files with 911 additions and 204 deletions
@@ -18,10 +18,11 @@ then
 	param set MC_PITCHRATE_P 0.13
 	param set MC_PITCHRATE_I 0.05
 	param set MC_PITCHRATE_D 0.004
-	param set MC_YAW_P 0.5
+	param set MC_YAW_P 2.8
 	param set MC_YAWRATE_P 0.2
-	param set MC_YAWRATE_I 0.0
+	param set MC_YAWRATE_I 0.1
 	param set MC_YAWRATE_D 0.0
+	param set MC_YAW_FF 0.5

 	param set BAT_N_CELLS 4
 fi
@@ -2,7 +2,7 @@
 #
 # TBS Caipirinha
 #
-# Thomas Gubler <thomas@px4.io>
+# Lorenz Meier <lorenz@px4.io>
 #

 sh /etc/init.d/rc.fw_defaults
@@ -29,6 +29,9 @@ then
 	param set FW_RR_I 0.01
 	param set FW_RR_IMAX 0.2
 	param set FW_RR_P 0.04
+	param set SYS_COMPANION 157600
+	param set PWM_MAIN_REV0 1
+	param set PWM_MAIN_REV1 1
 fi

 set MIXER caipi
@@ -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
@@ -484,6 +484,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
@@ -24,14 +24,14 @@ 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  -9000  -9000      0 -10000  10000
-S: 0 1   8000   8000      0 -10000  10000
+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  -9000  -9000      0 -10000  10000
-S: 0 1  -8000  -8000      0 -10000  10000
+O:      10000  10000      -3000 -10000  10000
+S: 0 0   8000   8000      0 -10000  10000
+S: 0 1  -9000  -9000      0 -10000  10000

 Output 2
 --------
@@ -130,6 +130,12 @@ INSTRUMENTATIONDEFINES	 = $(ARCHINSTRUMENTATIONDEFINES_$(CONFIG_ARCH))
 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 \
@@ -138,7 +144,7 @@ ARCHWARNINGS		 = -Wall \
 			   -Wdouble-promotion \
 			   -Wshadow \
 			   -Wfloat-equal \
-			   -Wframe-larger-than=1024 \
+			   -Wframe-larger-than=$(WFRAME_LARGER_THAN) \
 			   -Wpointer-arith \
 			   -Wlogical-op \
 			   -Wmissing-declarations \
@@ -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
@@ -1240,7 +1240,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;
@@ -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 Julian Oes <julian@oes.ch>
- * @author Anton Babushkin <anton.babushkin@me.com>
+ * @author Thomas Gubler <thomas@px4.io>
+ * @author Julian Oes <julian@px4.io>
+ * @author Anton Babushkin <anton@px4.io>
 */

 #include <nuttx/config.h>
@@ -88,7 +88,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>
@@ -143,7 +143,9 @@ 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

 enum MAV_MODE_FLAG {
 	MAV_MODE_FLAG_CUSTOM_MODE_ENABLED = 1, /* 0b00000001 Reserved for future use. | */
@@ -169,6 +171,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 */
@@ -184,6 +187,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
@@ -207,7 +211,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.
@@ -346,6 +350,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);
 		exit(0);
 	}
@@ -376,6 +381,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));
@@ -447,7 +454,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)
@@ -475,6 +483,11 @@ bool handle_command(struct vehicle_status_s *status_local, const struct safety_s
 			// Transition the arming state
 			arming_ret = arm_disarm(base_mode & MAV_MODE_FLAG_SAFETY_ARMED, 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 (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 */
 				if (custom_main_mode == PX4_CUSTOM_MAIN_MODE_MANUAL) {
@@ -497,6 +510,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);
@@ -514,6 +531,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);
 					}
@@ -838,6 +858,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];
@@ -851,6 +872,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";
@@ -948,8 +970,7 @@ int commander_thread_main(int argc, char *argv[])

 	/* home position */
 	orb_advert_t home_pub = -1;
-	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 = -1;
@@ -1134,15 +1155,23 @@ 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);
-	if (!status.condition_system_sensors_initialized) {
-		set_tune_override(TONE_GPS_WARNING_TUNE); //sensor fail tune
-	}
-	else {
+	param_get(_param_autostart_id, &autostart_id);
+	if (autostart_id > 1999) {
+		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
+		}
+	} else {
+		// HIL configuration selected: real sensors will be disabled
+		warnx("HIL configuration; autostart_id: %d, onboard IMU sensors disabled", autostart_id);
+		status.condition_system_sensors_initialized = false;
 		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;

@@ -1310,8 +1339,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,
-						!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), !status.circuit_breaker_engaged_gpsfailure_check);
+					if (autostart_id > 1999) {
+						(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);
+					} else {
+						/* HIL configuration: check only RC input */
+						warnx("new telemetry link connected: checking RC status");
+						(void)Commander::preflightCheck(mavlink_fd, false, false, false, false, false,
+								!(status.rc_input_mode == vehicle_status_s::RC_IN_MODE_OFF), false);
+					}
 				}

 				telemetry_last_heartbeat[i] = telemetry.heartbeat_time;
@@ -1671,7 +1707,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) {
@@ -1705,12 +1741,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

@@ -1720,7 +1756,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 {
@@ -1737,7 +1773,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) {
@@ -1770,7 +1809,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 {
@@ -1927,7 +1967,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;
 			}
 		}
@@ -1939,6 +1979,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) ||
@@ -1963,6 +2004,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,12 +2029,12 @@ int commander_thread_main(int argc, char *argv[])

 		//First time home position update
 		if (!status.condition_home_position_valid) {
-			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) {
-			commander_set_home_position(home_pub, home, local_position, global_position);
+		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);
 		}

 		/* print new state */
@@ -2009,14 +2051,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;

@@ -2030,10 +2064,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]);
-			mavlink_log_info(mavlink_fd, "[cmd] 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, "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 */
@@ -2290,7 +2326,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);
@@ -2401,6 +2445,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;
@@ -1,8 +1,41 @@
-/*
- * px4_custom_mode.h
+/****************************************************************************
 *
- *  Created on: 09.08.2013
- *      Author: ton
+ *   Copyright (c) 2013-2015 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 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 {
@@ -125,7 +125,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);
 		}

@@ -299,6 +300,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;

@@ -488,6 +490,7 @@ 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 */
@@ -514,6 +517,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;
@@ -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
@@ -153,7 +153,7 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
    _sensor_combined {},

    _pos_ref{},
-    _baro_ref_offset(0.0f),
+    _filter_ref_offset(0.0f),
    _baro_gps_offset(0.0f),

    /* performance counters */
@@ -173,7 +173,6 @@ AttitudePositionEstimatorEKF::AttitudePositionEstimatorEKF() :
    _gpsIsGood(false),
    _previousGPSTimestamp(0),
    _baro_init(false),
-    _baroAltRef(0.0f),
    _gps_initialized(false),
    _filter_start_time(0),
    _last_sensor_timestamp(0),
@@ -404,6 +403,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);
@@ -585,6 +593,7 @@ void AttitudePositionEstimatorEKF::task_main()

 				_baro_init = false;
 				_gps_initialized = false;
+
 				_last_sensor_timestamp = hrt_absolute_time();
 				_last_run = _last_sensor_timestamp;

@@ -643,12 +652,13 @@ 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);

+					warnx("filter ref off: baro_alt: %8.4f", (double)_filter_ref_offset);
+
 				} else {

 					if (!_gps_initialized && _gpsIsGood) {
@@ -702,6 +712,32 @@ void AttitudePositionEstimatorEKF::task_main()
 	_exit(0);
 }

+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
@@ -711,11 +747,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;
@@ -737,20 +768,13 @@ void AttitudePositionEstimatorEKF::initializeGPS()

 	_ekf->InitialiseFilter(initVelNED, math::radians(lat), math::radians(lon) - M_PI, gps_alt, declination);

-	// Initialize projection
-	_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);
+	initReferencePosition(_gps.timestamp_position, lat, lon, gps_alt, _baro.altitude);

 #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
@@ -811,7 +835,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];
@@ -826,6 +851,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 > 0) {
 		/* publish the attitude setpoint */
@@ -859,7 +895,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;
@@ -874,6 +910,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 > 0) {
 		/* publish the global position */
@@ -1070,8 +1117,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("baro ref offset: %8.4f baro GPS offset: %8.4f\n", (double)_baro_ref_offset,
+	printf("alt RAW: baro alt: %8.4f GPS alt: %8.4f\n", (double)_baro.altitude, (double)_ekf->gpsHgt);
+	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);
@@ -1317,7 +1365,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
@@ -1400,15 +1452,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);
 	}
@@ -1494,30 +1550,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();
 	}
@@ -854,6 +854,32 @@ 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.trim_roll)
+											+ _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.
 					*/
@@ -1074,8 +1100,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_manual_enabled)
+			   _vcontrol_mode.flag_control_attitude_enabled ||
+			   _vcontrol_mode.flag_control_manual_enabled)
 			{
 				/* publish the actuator controls */
 				if (_actuators_0_pub > 0) {
@@ -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 > 0) {
 		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 */
-		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;
+		/* 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,
-				_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;
@@ -84,11 +84,15 @@ bool FixedwingLandDetector::update()
 	const uint64_t now = hrt_absolute_time();
 	bool landDetected = false;

-	// TODO: reset filtered values on arming?
-	_velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx *
-				_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
-	_velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz);
-	_airspeed_filtered = 0.95f * _airspeed_filtered + 0.05f * _airspeed.true_airspeed_m_s;
+	if (hrt_elapsed_time(&_vehicleLocalPosition.timestamp) < 500 * 1000) {
+		_velocity_xy_filtered = 0.95f * _velocity_xy_filtered + 0.05f * sqrtf(_vehicleLocalPosition.vx *
+					_vehicleLocalPosition.vx + _vehicleLocalPosition.vy * _vehicleLocalPosition.vy);
+		_velocity_z_filtered = 0.95f * _velocity_z_filtered + 0.05f * fabsf(_vehicleLocalPosition.vz);
+	}
+
+	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
@@ -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);
@@ -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);
@@ -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);
@@ -375,7 +375,8 @@ MavlinkFTP::_workList(PayloadHeader* payload)
 			}
 			break;
 		case DTYPE_DIRECTORY:
-			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 {
@@ -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);
@@ -1295,6 +1295,8 @@ Mavlink::task_main(int argc, char *argv[])
 				_mode = MAVLINK_MODE_ONBOARD;
 			} else if (strcmp(optarg, "onboard") == 0) {
 				_mode = MAVLINK_MODE_ONBOARD;
+			} else if (strcmp(optarg, "osd") == 0) {
+				_mode = MAVLINK_MODE_OSD;
 			}

 			break;
@@ -1451,7 +1453,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);
@@ -1468,6 +1469,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;
 	}
@@ -128,7 +128,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);
@@ -130,6 +130,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;
@@ -1292,6 +1292,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 */
@@ -97,6 +97,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
 {
@@ -831,7 +832,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);
@@ -83,6 +83,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
@@ -1387,7 +1388,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
@@ -297,10 +297,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;
@@ -586,7 +586,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,7 +608,7 @@ 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;
 	} else {
 		/* update the altitude sp of the 'current' item in the sp triplet, but do not update the altitude sp
@@ -617,7 +617,7 @@ Mission::altitude_sp_foh_update()
 		* radius around the current waypoint
 		**/
 		float delta_alt = (_mission_item.altitude - _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;

@@ -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;
 			}
 		}
@@ -144,7 +144,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:
@@ -571,6 +571,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);
@@ -312,6 +312,9 @@ MultirotorMixer::mix(float *outputs, unsigned space, uint16_t *status_reg)
 			}
 		}
 		else if(out > 1.0f) {
+			// allow to reduce thrust to get some yaw response
+			float thrust_reduction = fminf(0.15f, out - 1.0f);
+			thrust -= thrust_reduction;
 			yaw = (1.0f - ((roll * _rotors[i].roll_scale + pitch * _rotors[i].pitch_scale) *
 				roll_pitch_scale + thrust + boost))/_rotors[i].yaw_scale;
 			if(status_reg != NULL) {
@@ -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.
-* CURRENTLY ONLY SUPPORTS 921600 BAUD! Use extras.txt for
-* other baud rates.
+* Set to zero to disable, set to these values to enable (NO OTHER VALUES SUPPORTED!)
+* 921600: enables onboard mode at 921600 baud, 8N1. 57600: enables onboard mode at 57600 baud, 8N1.
+* 157600: enables OSD mode at 57600 baud, 8N1.
 *
 * @min 0
 * @max 921600
@@ -39,6 +39,8 @@
 MODULE_COMMAND = uavcan

 MAXOPTIMIZATION = -O3
+MODULE_STACKSIZE = 3200
+WFRAME_LARGER_THAN = 1400

 # Main
 SRCS += uavcan_main.cpp              \
@@ -65,7 +67,6 @@ INCLUDE_DIRS += $(LIBUAVCAN_INC)
 # because this platform lacks most of the standard library and STL. Hence we need to force C++03 mode.
 override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_CPP_VERSION=UAVCAN_CPP03 -DUAVCAN_NO_ASSERTIONS

-
 #
 # libuavcan drivers for STM32
 #
@@ -75,6 +76,12 @@ SRCS += $(subst  $(PX4_MODULE_SRC),../../,$(LIBUAVCAN_STM32_SRC))
 INCLUDE_DIRS += $(LIBUAVCAN_STM32_INC)
 override EXTRADEFINES := $(EXTRADEFINES) -DUAVCAN_STM32_NUTTX -DUAVCAN_STM32_NUM_IFACES=2

+#
+# libuavcan drivers for posix
+#
+include $(PX4_LIB_DIR)uavcan/libuavcan_drivers/posix/include.mk
+INCLUDE_DIRS += $(LIBUAVCAN_POSIX_INC)
+
 #
 # Invoke DSDL compiler
 #
@@ -41,30 +41,44 @@

 const char *const UavcanBarometerBridge::NAME = "baro";

-UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode& node) :
-UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)),
-_sub_air_data(node),
-_reports(nullptr)
+UavcanBarometerBridge::UavcanBarometerBridge(uavcan::INode &node) :
+	UavcanCDevSensorBridgeBase("uavcan_baro", "/dev/uavcan/baro", BARO_BASE_DEVICE_PATH, ORB_ID(sensor_baro)),
+	_sub_air_pressure_data(node),
+	_sub_air_temperature_data(node),
+	_reports(nullptr)
 {
+	last_temperature = 0.0f;
 }

 int UavcanBarometerBridge::init()
 {
 	int res = device::CDev::init();
+
 	if (res < 0) {
 		return res;
 	}

 	/* allocate basic report buffers */
 	_reports = new RingBuffer(2, sizeof(baro_report));
-	if (_reports == nullptr)
-		return -1;

-	res = _sub_air_data.start(AirDataCbBinder(this, &UavcanBarometerBridge::air_data_sub_cb));
+	if (_reports == nullptr) {
+		return -1;
+	}
+
+	res = _sub_air_pressure_data.start(AirPressureCbBinder(this, &UavcanBarometerBridge::air_pressure_sub_cb));
+
 	if (res < 0) {
 		log("failed to start uavcan sub: %d", res);
 		return res;
 	}
+
+	res = _sub_air_temperature_data.start(AirTemperatureCbBinder(this, &UavcanBarometerBridge::air_temperature_sub_cb));
+
+	if (res < 0) {
+		log("failed to start uavcan sub: %d", res);
+		return res;
+	}
+
 	return 0;
 }

@@ -75,8 +89,9 @@ ssize_t UavcanBarometerBridge::read(struct file *filp, char *buffer, size_t bufl
 	int ret = 0;

 	/* buffer must be large enough */
-	if (count < 1)
+	if (count < 1) {
 		return -ENOSPC;
+	}

 	while (count--) {
 		if (_reports->get(baro_buf)) {
@@ -93,47 +108,62 @@ int UavcanBarometerBridge::ioctl(struct file *filp, int cmd, unsigned long arg)
 {
 	switch (cmd) {
 	case BAROIOCSMSLPRESSURE: {
-		if ((arg < 80000) || (arg > 120000)) {
-			return -EINVAL;
-		} else {
-			log("new msl pressure %u", _msl_pressure);
-			_msl_pressure = arg;
+			if ((arg < 80000) || (arg > 120000)) {
+				return -EINVAL;
+
+			} else {
+				log("new msl pressure %u", _msl_pressure);
+				_msl_pressure = arg;
+				return OK;
+			}
+		}
+
+	case BAROIOCGMSLPRESSURE: {
+			return _msl_pressure;
+		}
+
+	case SENSORIOCSPOLLRATE: {
+			// not supported yet, pretend that everything is ok
 			return OK;
 		}
-	}
-	case BAROIOCGMSLPRESSURE: {
-		return _msl_pressure;
-	}
-	case SENSORIOCSPOLLRATE: {
-		// not supported yet, pretend that everything is ok
-		return OK;
-	}
+
 	case SENSORIOCSQUEUEDEPTH: {
-		/* lower bound is mandatory, upper bound is a sanity check */
-		if ((arg < 1) || (arg > 100))
-			return -EINVAL;
+			/* lower bound is mandatory, upper bound is a sanity check */
+			if ((arg < 1) || (arg > 100)) {
+				return -EINVAL;
+			}
+
+			irqstate_t flags = irqsave();
+
+			if (!_reports->resize(arg)) {
+				irqrestore(flags);
+				return -ENOMEM;
+			}

-		irqstate_t flags = irqsave();
-		if (!_reports->resize(arg)) {
 			irqrestore(flags);
-			return -ENOMEM;
-		}
-		irqrestore(flags);

-		return OK;
-	}
+			return OK;
+		}
+
 	default: {
-		return CDev::ioctl(filp, cmd, arg);
-	}
+			return CDev::ioctl(filp, cmd, arg);
+		}
 	}
 }

-void UavcanBarometerBridge::air_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg)
+void UavcanBarometerBridge::air_temperature_sub_cb(const
+		uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg)
+{
+	last_temperature = msg.static_temperature;
+}
+
+void UavcanBarometerBridge::air_pressure_sub_cb(const
+		uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg)
 {
 	baro_report report;

 	report.timestamp   = msg.getMonotonicTimestamp().toUSec();
-	report.temperature = msg.static_temperature;
+	report.temperature = last_temperature;
 	report.pressure    = msg.static_pressure / 100.0F;  // Convert to millibar
 	report.error_count = 0;

@@ -40,7 +40,8 @@
 #include "sensor_bridge.hpp"
 #include <drivers/drv_baro.h>

-#include <uavcan/equipment/air_data/StaticAirData.hpp>
+#include <uavcan/equipment/air_data/StaticPressure.hpp>
+#include <uavcan/equipment/air_data/StaticTemperature.hpp>

 class RingBuffer;

@@ -56,17 +57,26 @@ 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_data_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &msg);
+	void air_pressure_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &msg);
+	void air_temperature_sub_cb(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &msg);

 	typedef uavcan::MethodBinder < UavcanBarometerBridge *,
 		void (UavcanBarometerBridge::*)
-		(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticAirData> &) >
-		AirDataCbBinder;
+		(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticPressure> &) >
+		AirPressureCbBinder;

-	uavcan::Subscriber<uavcan::equipment::air_data::StaticAirData, AirDataCbBinder> _sub_air_data;
+	typedef uavcan::MethodBinder < UavcanBarometerBridge *,
+		void (UavcanBarometerBridge::*)
+		(const uavcan::ReceivedDataStructure<uavcan::equipment::air_data::StaticTemperature> &) >
+		AirTemperatureCbBinder;
+
+	uavcan::Subscriber<uavcan::equipment::air_data::StaticPressure, AirPressureCbBinder> _sub_air_pressure_data;
+	uavcan::Subscriber<uavcan::equipment::air_data::StaticTemperature, AirTemperatureCbBinder> _sub_air_temperature_data;
 	unsigned _msl_pressure = 101325;
-	RingBuffer	*_reports;
+	RingBuffer  *_reports;
+	float last_temperature;
+
 };
@@ -53,25 +53,42 @@
 #include <drivers/drv_pwm_output.h>

 #include "uavcan_main.hpp"
+#include <uavcan_posix/dynamic_node_id_server/file_event_tracer.hpp>
+#include <uavcan_posix/dynamic_node_id_server/file_storage_backend.hpp>
+
+#include <uavcan_posix/firmware_version_checker.hpp>
+
+//todo:The Inclusion of file_server_backend is killing
+// #include <sys/types.h> and leaving OK undefined
+#define OK 0

 /**
 * @file uavcan_main.cpp
 *
- * Implements basic functinality of UAVCAN node.
+ * Implements basic functionality of UAVCAN node.
 *
 * @author Pavel Kirienko <pavel.kirienko@gmail.com>
+ *         David Sidrane <david_s5@nscdg.com>
 */

 /*
 * UavcanNode
 */
 UavcanNode *UavcanNode::_instance;
+uavcan::dynamic_node_id_server::CentralizedServer *UavcanNode::_server_instance;
+uavcan_posix::dynamic_node_id_server::FileEventTracer tracer;
+uavcan_posix::dynamic_node_id_server::FileStorageBackend storage_backend;
+uavcan_posix::FirmwareVersionChecker fw_version_checker;

 UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &system_clock) :
 	CDev("uavcan", UAVCAN_DEVICE_PATH),
 	_node(can_driver, system_clock),
 	_node_mutex(),
-	_esc_controller(_node)
+	_esc_controller(_node),
+	_fileserver_backend(_node),
+	_node_info_retriever(_node),
+	_fw_upgrade_trigger(_node, fw_version_checker),
+	_fw_server(_node, _fileserver_backend)
 {
 	_control_topics[0] = ORB_ID(actuator_controls_0);
 	_control_topics[1] = ORB_ID(actuator_controls_1);
@@ -79,6 +96,7 @@ UavcanNode::UavcanNode(uavcan::ICanDriver &can_driver, uavcan::ISystemClock &sys
 	_control_topics[3] = ORB_ID(actuator_controls_3);

 	const int res = pthread_mutex_init(&_node_mutex, nullptr);
+
 	if (res < 0) {
 		std::abort();
 	}
@@ -124,6 +142,7 @@ UavcanNode::~UavcanNode()

 	// Removing the sensor bridges
 	auto br = _sensor_bridges.getHead();
+
 	while (br != nullptr) {
 		auto next = br->getSibling();
 		delete br;
@@ -135,6 +154,8 @@ UavcanNode::~UavcanNode()
 	perf_free(_perfcnt_node_spin_elapsed);
 	perf_free(_perfcnt_esc_mixer_output_elapsed);
 	perf_free(_perfcnt_esc_mixer_total_elapsed);
+	delete(_server_instance);
+
 }

 int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
@@ -196,7 +217,7 @@ int UavcanNode::start(uavcan::NodeID node_id, uint32_t bitrate)
 	 */
 	static auto run_trampoline = [](int, char *[]) {return UavcanNode::_instance->run();};
 	_instance->_task = task_spawn_cmd("uavcan", SCHED_DEFAULT, SCHED_PRIORITY_ACTUATOR_OUTPUTS, StackSize,
-			      static_cast<main_t>(run_trampoline), nullptr);
+					  static_cast<main_t>(run_trampoline), nullptr);

 	if (_instance->_task < 0) {
 		warnx("start failed: %d", errno);
@@ -228,8 +249,10 @@ void UavcanNode::fill_node_info()

 	if (!std::strncmp(HW_ARCH, "PX4FMU_V1", 9)) {
 		hwver.major = 1;
+
 	} else if (!std::strncmp(HW_ARCH, "PX4FMU_V2", 9)) {
 		hwver.major = 2;
+
 	} else {
 		; // All other values of HW_ARCH resolve to zero
 	}
@@ -241,6 +264,7 @@ void UavcanNode::fill_node_info()
 	_node.setHardwareVersion(hwver);
 }

+
 int UavcanNode::init(uavcan::NodeID node_id)
 {
 	int ret = -1;
@@ -260,6 +284,7 @@ int UavcanNode::init(uavcan::NodeID node_id)

 	// Actuators
 	ret = _esc_controller.init();
+
 	if (ret < 0) {
 		return ret;
 	}
@@ -267,26 +292,101 @@ int UavcanNode::init(uavcan::NodeID node_id)
 	// Sensor bridges
 	IUavcanSensorBridge::make_all(_node, _sensor_bridges);
 	auto br = _sensor_bridges.getHead();
+
 	while (br != nullptr) {
 		ret = br->init();
+
 		if (ret < 0) {
 			warnx("cannot init sensor bridge '%s' (%d)", br->get_name(), ret);
 			return ret;
 		}
+
 		warnx("sensor bridge '%s' init ok", br->get_name());
 		br = br->getSibling();
 	}

+
+	/* Initialize the fw version checker.
+	* giving it it's path
+	*/
+
+	ret = fw_version_checker.createFwPaths(UAVCAN_FIRMWARE_PATH);
+
+	if (ret < 0) {
+		return ret;
+	}
+
+
+	/* Start fw file server back */
+
+	ret = _fw_server.start();
+
+	if (ret < 0) {
+		return ret;
+	}
+
+	/* Initialize storage back end for the node allocator using UAVCAN_NODE_DB_PATH directory */
+
+	ret = storage_backend.init(UAVCAN_NODE_DB_PATH);
+
+	if (ret < 0) {
+		return ret;
+	}
+
+	/* Initialize trace in the UAVCAN_NODE_DB_PATH directory */
+
+	ret = tracer.init(UAVCAN_LOG_FILE);
+
+	if (ret < 0) {
+		return ret;
+	}
+
+	/* Create dynamic node id server for the Firmware updates directory */
+
+	_server_instance = new uavcan::dynamic_node_id_server::CentralizedServer(_node, storage_backend, tracer);
+
+	if (_server_instance == 0) {
+		return -ENOMEM;
+	}
+
+	/* Initialize the dynamic node id server  */
+	ret = _server_instance->init(_node.getNodeStatusProvider().getHardwareVersion().unique_id);
+
+	if (ret < 0) {
+		return ret;
+	}
+
+	/* Start node info retriever to fetch node info from new nodes */
+
+	ret = _node_info_retriever.start();
+
+	if (ret < 0) {
+		return ret;
+	}
+
+
+	/* Start the fw version checker   */
+
+	ret = _fw_upgrade_trigger.start(_node_info_retriever, fw_version_checker.getFirmwarePath());
+
+	if (ret < 0) {
+		return ret;
+	}
+
+	/*  Start the Node   */
+
 	return _node.start();
 }

 void UavcanNode::node_spin_once()
 {
 	perf_begin(_perfcnt_node_spin_elapsed);
-	const int spin_res = _node.spin(uavcan::MonotonicTime());
+	const int spin_res = _node.spinOnce();
+
 	if (spin_res < 0) {
 		warnx("node spin error %i", spin_res);
 	}
+
 	perf_end(_perfcnt_node_spin_elapsed);
 }

@@ -297,9 +397,11 @@ void UavcanNode::node_spin_once()
 int UavcanNode::add_poll_fd(int fd)
 {
 	int ret = _poll_fds_num;
+
 	if (_poll_fds_num >= UAVCAN_NUM_POLL_FDS) {
 		errx(1, "uavcan: too many poll fds, exiting");
 	}
+
 	_poll_fds[_poll_fds_num] = ::pollfd();
 	_poll_fds[_poll_fds_num].fd = fd;
 	_poll_fds[_poll_fds_num].events = POLLIN;
@@ -312,8 +414,6 @@ int UavcanNode::run()
 {
 	(void)pthread_mutex_lock(&_node_mutex);

-	const unsigned PollTimeoutMs = 50;
-
 	// XXX figure out the output count
 	_output_count = 2;

@@ -324,8 +424,8 @@ int UavcanNode::run()
 	memset(&_outputs, 0, sizeof(_outputs));

 	const int busevent_fd = ::open(uavcan_stm32::BusEvent::DevName, 0);
-	if (busevent_fd < 0)
-	{
+
+	if (busevent_fd < 0) {
 		warnx("Failed to open %s", uavcan_stm32::BusEvent::DevName);
 		_task_should_exit = true;
 	}
@@ -354,6 +454,7 @@ int UavcanNode::run()
 	}

 	while (!_task_should_exit) {
+
 		// update actuator controls subscriptions if needed
 		if (_groups_subscribed != _groups_required) {
 			subscribe();
@@ -379,9 +480,11 @@ int UavcanNode::run()
 		if (poll_ret < 0) {
 			log("poll error %d", errno);
 			continue;
+
 		} else {
 			// get controls for required topics
 			bool controls_updated = false;
+
 			for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 				if (_control_subs[i] > 0) {
 					if (_poll_fds[_poll_ids[i]].revents & POLLIN) {
@@ -401,8 +504,9 @@ int UavcanNode::run()
 				if (_actuator_direct.nvalues > actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) {
 					_actuator_direct.nvalues = actuator_outputs_s::NUM_ACTUATOR_OUTPUTS;
 				}
+
 				memcpy(&_outputs.output[0], &_actuator_direct.values[0],
-				       _actuator_direct.nvalues*sizeof(float));
+				       _actuator_direct.nvalues * sizeof(float));
 				_outputs.noutputs = _actuator_direct.nvalues;
 				new_output = true;
 			}
@@ -410,11 +514,14 @@ int UavcanNode::run()
 			// can we mix?
 			if (_test_in_progress) {
 				memset(&_outputs, 0, sizeof(_outputs));
+
 				if (_test_motor.motor_number < actuator_outputs_s::NUM_ACTUATOR_OUTPUTS) {
-					_outputs.output[_test_motor.motor_number] = _test_motor.value*2.0f-1.0f;
-					_outputs.noutputs = _test_motor.motor_number+1;
+					_outputs.output[_test_motor.motor_number] = _test_motor.value * 2.0f - 1.0f;
+					_outputs.noutputs = _test_motor.motor_number + 1;
 				}
+
 				new_output = true;
+
 			} else if (controls_updated && (_mixers != nullptr)) {

 				// XXX one output group has 8 outputs max,
@@ -451,6 +558,7 @@ int UavcanNode::run()
 					_outputs.output[i] = 1.0f;
 				}
 			}
+
 			// Output to the bus
 			_outputs.timestamp = hrt_absolute_time();
 			perf_begin(_perfcnt_esc_mixer_output_elapsed);
@@ -509,6 +617,7 @@ UavcanNode::teardown()
 			_control_subs[i] = -1;
 		}
 	}
+
 	return (_armed_sub >= 0) ? ::close(_armed_sub) : 0;
 }

@@ -526,12 +635,14 @@ UavcanNode::subscribe()
 	// Subscribe/unsubscribe to required actuator control groups
 	uint32_t sub_groups = _groups_required & ~_groups_subscribed;
 	uint32_t unsub_groups = _groups_subscribed & ~_groups_required;
+
 	// the first fd used by CAN
 	for (unsigned i = 0; i < actuator_controls_s::NUM_ACTUATOR_CONTROL_GROUPS; i++) {
 		if (sub_groups & (1 << i)) {
 			warnx("subscribe to actuator_controls_%d", i);
 			_control_subs[i] = orb_subscribe(_control_topics[i]);
 		}
+
 		if (unsub_groups & (1 << i)) {
 			warnx("unsubscribe from actuator_controls_%d", i);
 			::close(_control_subs[i]);
@@ -583,8 +694,9 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
 			const char *buf = (const char *)arg;
 			unsigned buflen = strnlen(buf, 1024);

-			if (_mixers == nullptr)
+			if (_mixers == nullptr) {
 				_mixers = new MixerGroup(control_callback, (uintptr_t)_controls);
+			}

 			if (_mixers == nullptr) {
 				_groups_required = 0;
@@ -600,6 +712,7 @@ UavcanNode::ioctl(file *filp, int cmd, unsigned long arg)
 					_mixers = nullptr;
 					_groups_required = 0;
 					ret = -EINVAL;
+
 				} else {

 					_mixers->groups_required(_groups_required);
@@ -640,9 +753,10 @@ UavcanNode::print_info()
 	if (_outputs.noutputs != 0) {
 		printf("ESC output: ");

-		for (uint8_t i=0; i<_outputs.noutputs; i++) {
-			printf("%d ", (int)(_outputs.output[i]*1000));
+		for (uint8_t i = 0; i < _outputs.noutputs; i++) {
+			printf("%d ", (int)(_outputs.output[i] * 1000));
 		}
+
 		printf("\n");

 		// ESC status
@@ -653,7 +767,8 @@ UavcanNode::print_info()

 		printf("ESC Status:\n");
 		printf("Addr\tV\tA\tTemp\tSetpt\tRPM\tErr\n");
-		for (uint8_t i=0; i<_outputs.noutputs; i++) {
+
+		for (uint8_t i = 0; i < _outputs.noutputs; i++) {
 			printf("%d\t",    esc.esc[i].esc_address);
 			printf("%3.2f\t", (double)esc.esc[i].esc_voltage);
 			printf("%3.2f\t", (double)esc.esc[i].esc_current);
@@ -669,6 +784,7 @@ UavcanNode::print_info()

 	// Sensor bridges
 	auto br = _sensor_bridges.getHead();
+
 	while (br != nullptr) {
 		printf("Sensor '%s':\n", br->get_name());
 		br->print_status();
@@ -47,6 +47,14 @@
 #include "actuators/esc.hpp"
 #include "sensors/sensor_bridge.hpp"

+
+#include <uavcan/protocol/dynamic_node_id_server/centralized.hpp>
+#include <uavcan/protocol/node_info_retriever.hpp>
+#include <uavcan_posix/basic_file_server_backend.hpp>
+#include <uavcan/protocol/firmware_update_trigger.hpp>
+#include <uavcan/protocol/file_server.hpp>
+
+
 /**
 * @file uavcan_main.hpp
 *
@@ -57,18 +65,40 @@

 #define NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN	4
 #define UAVCAN_DEVICE_PATH	"/dev/uavcan/esc"
+#define UAVCAN_NODE_DB_PATH     "/fs/microsd/uavcan.db"
+#define UAVCAN_FIRMWARE_PATH    "/fs/microsd/fw"
+#define UAVCAN_LOG_FILE          UAVCAN_NODE_DB_PATH"/trace.log"

 // we add two to allow for actuator_direct and busevent
 #define UAVCAN_NUM_POLL_FDS (NUM_ACTUATOR_CONTROL_GROUPS_UAVCAN+2)
-
 /**
 * A UAVCAN node.
 */
 class UavcanNode : public device::CDev
 {
+	static constexpr unsigned MaxBitRatePerSec   = 1000000;
+	static constexpr unsigned bitPerFrame        = 148;
+	static constexpr unsigned FramePerSecond     = MaxBitRatePerSec / bitPerFrame;
+	static constexpr unsigned FramePerMSecond    = ((FramePerSecond / 1000) + 1);
+
+	static constexpr unsigned PollTimeoutMs      = 10;
+
 	static constexpr unsigned MemPoolSize        = 10752; ///< Refer to the libuavcan manual to learn why
-	static constexpr unsigned RxQueueLenPerIface = 64;
-	static constexpr unsigned StackSize          = 3000;
+
+	/*
+	 * This memory is reserved for uavcan to use for queuing CAN frames.
+	 * At 1Mbit there is approximately one CAN frame every 145 uS.
+	 * The number of buffers sets how long you can go without calling
+	 * node_spin_xxxx. Since our task is the only one running and the
+	 * driver will light the fd when there is a CAN frame we can nun with
+	 * a minimum number of buffers to conserver memory. Each buffer is
+	 * 32 bytes. So 5 buffers costs 160 bytes and gives us a poll rate
+	 * of ~1 mS
+	 *  1000000/200
+	 */
+
+	static constexpr unsigned RxQueueLenPerIface = FramePerMSecond * PollTimeoutMs; // At
+	static constexpr unsigned StackSize          = 3400;

 public:
 	typedef uavcan::Node<MemPoolSize> Node;
@@ -117,11 +147,19 @@ private:
 	unsigned		_output_count = 0;		///< number of actuators currently available

 	static UavcanNode	*_instance;			///< singleton pointer
+	static uavcan::dynamic_node_id_server::CentralizedServer *_server_instance;              ///< server singleton pointer
+
 	Node			_node;				///< library instance
 	pthread_mutex_t		_node_mutex;

 	UavcanEscController	_esc_controller;

+
+	uavcan_posix::BasicFileSeverBackend _fileserver_backend;
+	uavcan::NodeInfoRetriever  _node_info_retriever;
+	uavcan::FirmwareUpdateTrigger  _fw_upgrade_trigger;
+	uavcan::BasicFileServer        _fw_server;
+
 	List<IUavcanSensorBridge *> _sensor_bridges;		///< List of active sensor bridges

 	MixerGroup		*_mixers = nullptr;