Merge remote-tracking branch 'PX4/master' into logHandler

* PX4/master: (45 commits)
  don't use default source address for onboard udp link, wait on remote
  Configure Easystar HIL setup to do Runway takeoff
  ROMFS: Set 3DR quad tuning to more realistic default values
  Fix incomplete boot on new EKF config
  Fix px4fmu-v2_ekf2 config
  Updated MAVLink protocol version
  MAVLink: Start slightly differently on USB
  Start shell only if SD card not present
  Update ECL
  NuttX configs: added px4fmu-v2_ekf2 target for EKF2 development on Pixhawk
  Get QuRT drivers out of the way, as we are using our own
  Fix POSIX eagle config
  Remove unmaintained NuttX config
  VDev: fix code style
  Add new posix_eagle_default and qurt_eagle_default targets
  Fix QuRT build error
  Fix FMUv4 USB PID
  Speed up Vagrant
  VTOL: Fix motor index use in VT_FW_MOT_OFF. Create new param to re-default all deployed vehicles to not shut down motors.
  VTOL: Fix MOT_OFF bug
  ...

src/modules/commander/mag_calibration.cpp

@@ -73,7 +73,7 @@ static constexpr unsigned int calibration_sides = 6;			///< The total number of
 static constexpr unsigned int calibration_total_points = 240;		///< The total points per magnetometer
 static constexpr unsigned int calibraton_duration_seconds = 42; 	///< The total duration the routine is allowed to take
 
-static constexpr float MAG_MAX_OFFSET_LEN = 0.6f;	///< The maximum measurement range is ~1.4 Ga, the earth field is ~0.6 Ga, so an offset larger than ~0.8-0.6 Ga means the mag will saturate in some directions.
+static constexpr float MAG_MAX_OFFSET_LEN = 0.9f;	///< The maximum measurement range is ~1.4 Ga, the earth field is ~0.6 Ga, so an offset larger than ~0.8-0.6 Ga means the mag will saturate in some directions.
 
 int32_t	device_ids[max_mags];
 int device_prio_max = 0;

src/modules/ekf2/ekf2_main.cpp

@@ -70,6 +70,7 @@
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/control_state.h>
+#include <uORB/topics/vehicle_global_position.h>
 
 #include <ecl/EKF/ekf.h>
 
@@ -111,6 +112,8 @@ public:
 
 	void print();
 
+	void print_status();
+
 private:
 	static constexpr float _dt_max = 0.02;
 	bool		_task_should_exit = false;		/**< if true, task should exit */
@@ -123,6 +126,7 @@ private:
 	orb_advert_t _att_pub;
 	orb_advert_t _lpos_pub;
 	orb_advert_t _control_state_pub;
+	orb_advert_t _vehicle_global_position_pub;
 
 	/* Low pass filter for attitude rates */
 	math::LowPassFilter2p _lp_roll_rate;
@@ -162,6 +166,11 @@ void Ekf2::print()
 	_ekf->printStoredIMU();
 }
 
+void Ekf2::print_status()
+{
+	warnx("position OK %s", (_ekf->position_is_valid()) ? "[YES]" : "[NO]");
+}
+
 void Ekf2::task_main()
 {
 	// subscribe to relevant topics
@@ -244,12 +253,13 @@ void Ekf2::task_main()
 			_ekf->setAirspeedData(airspeed.timestamp, &airspeed.indicated_airspeed_m_s);
 		}
 
-		struct vehicle_attitude_s att;
-		struct vehicle_local_position_s lpos;
-		att.timestamp = hrt_absolute_time();
-		lpos.timestamp = hrt_absolute_time();
+		// run the EKF update
 		_ekf->update();
 
+		// generate vehicle attitude data
+		struct vehicle_attitude_s att;
+		att.timestamp = hrt_absolute_time();
+
 		_ekf->copy_quaternion(att.q);
 		matrix::Quaternion<float> q(att.q[0], att.q[1], att.q[2], att.q[3]);
 		matrix::Euler<float> euler(q);
@@ -257,25 +267,64 @@ void Ekf2::task_main()
 		att.pitch = euler(1);
 		att.yaw = euler(2);
 
+		// generate vehicle local position data
+		struct vehicle_local_position_s lpos;
 		float pos[3] = {};
 		float vel[3] = {};
 
+		lpos.timestamp = hrt_absolute_time();
+
+		// Position in local NED frame
 		_ekf->copy_position(pos);
 		lpos.x = pos[0];
 		lpos.y = pos[1];
 		lpos.z = pos[2];
 
+		// Velocity in NED frame (m/s)
 		_ekf->copy_velocity(vel);
 		lpos.vx = vel[0];
 		lpos.vy = vel[1];
 		lpos.vz = vel[2];
 
+		// TODO: better status reporting
+		lpos.xy_valid = _ekf->position_is_valid();
+		lpos.z_valid = true;
+		lpos.v_xy_valid = _ekf->position_is_valid();
+		lpos.v_z_valid = true;
+
+		// Position of local NED origin in GPS / WGS84 frame
+		lpos.ref_timestamp = _ekf->_last_gps_origin_time_us; // Time when reference position was set
+ 		lpos.xy_global = _ekf->position_is_valid();// true if position (x, y) is valid and has valid global reference (ref_lat, ref_lon)
+ 		lpos.z_global = true;// true if z is valid and has valid global reference (ref_alt)
+		lpos.ref_lat = _ekf->_posRef.lat_rad * (double)180.0 * M_PI; // Reference point latitude in degrees
+		lpos.ref_lon = _ekf->_posRef.lon_rad * (double)180.0 * M_PI; // Reference point longitude in degrees
+		lpos.ref_alt = _ekf->_gps_alt_ref; // Reference altitude AMSL in meters, MUST be set to current (not at reference point!) ground level		
+
+		// The rotation of the tangent plane vs. geographical north
+		lpos.yaw = 0.0f;
+
+		lpos.dist_bottom = 0.0f; // Distance to bottom surface (ground) in meters
+		lpos.dist_bottom_rate = 0.0f; // Distance to bottom surface (ground) change rate
+		lpos.surface_bottom_timestamp	= 0; // Time when new bottom surface found
+		lpos.dist_bottom_valid = false; // true if distance to bottom surface is valid
+
+ 		// TODO: uORB definition does not define what thes variables are. We have assumed them to be horizontal and vertical 1-std dev accuracy in metres 
+		// TODO: Should use sqrt of filter position variances
+		lpos.eph = gps.eph;
+		lpos.epv = gps.epv;
+
+		// publish vehicle local position data
+		if (_lpos_pub == nullptr) {
+			_lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos);
+		} else {
+			orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos);
+		}
+
+		// generate control state data
 		control_state_s ctrl_state = {};
 		ctrl_state.timestamp = hrt_absolute_time();
 		ctrl_state.roll_rate = _lp_roll_rate.apply(sensors.gyro_rad_s[0]);
-
 		ctrl_state.pitch_rate = _lp_pitch_rate.apply(sensors.gyro_rad_s[1]);
-
 		ctrl_state.yaw_rate = _lp_yaw_rate.apply(sensors.gyro_rad_s[2]);
 
 		ctrl_state.q[0] = q(0);
@@ -283,6 +332,14 @@ void Ekf2::task_main()
 		ctrl_state.q[2] = q(2);
 		ctrl_state.q[3] = q(3);
 
+		// publish control state data
+		if (_control_state_pub == nullptr) {
+			_control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
+		} else {
+			orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
+		}
+
+		// generate vehicle attitude data
 		att.q[0] = q(0);
 		att.q[1] = q(1);
 		att.q[2] = q(2);
@@ -293,25 +350,51 @@ void Ekf2::task_main()
 		att.pitchspeed = sensors.gyro_rad_s[1];
 		att.yawspeed = sensors.gyro_rad_s[2];
 
-		if (_control_state_pub == nullptr) {
-			_control_state_pub = orb_advertise(ORB_ID(control_state), &ctrl_state);
-		} else {
-			orb_publish(ORB_ID(control_state), _control_state_pub, &ctrl_state);
-		}
-
+		// publish vehicle attitude data
 		if (_att_pub == nullptr) {
 			_att_pub = orb_advertise(ORB_ID(vehicle_attitude), &att);
 		} else {
 			orb_publish(ORB_ID(vehicle_attitude), _att_pub, &att);
 		}
 
-		if (_lpos_pub == nullptr) {
-			_lpos_pub = orb_advertise(ORB_ID(vehicle_local_position), &lpos);
-		} else {
-			orb_publish(ORB_ID(vehicle_local_position), _lpos_pub, &lpos);
+		// generate and publish global position data
+		struct vehicle_global_position_s global_pos;
+		if (_ekf->position_is_valid()) {
+			// TODO: local origin is currenlty at GPS height origin - this is different to ekf_att_pos_estimator
+
+			global_pos.timestamp = hrt_absolute_time(); // Time of this estimate, in microseconds since system start
+			global_pos.time_utc_usec = gps.time_utc_usec; // GPS UTC timestamp in microseconds
+
+			double est_lat, est_lon;
+			map_projection_reproject(&_ekf->_posRef, lpos.x, lpos.y, &est_lat, &est_lon);
+			global_pos.lat = est_lat; // Latitude in degrees
+			global_pos.lon = est_lon; // Longitude in degrees
+
+			global_pos.alt = -pos[2]; // Altitude AMSL in meters
+
+			global_pos.vel_n = vel[0]; // Ground north velocity, m/s
+			global_pos.vel_e = vel[1]; // Ground east velocity, m/s
+			global_pos.vel_d = vel[2]; // Ground downside velocity, m/s
+
+			global_pos.yaw = euler(2); // Yaw in radians -PI..+PI.
+
+			global_pos.eph = gps.eph; // Standard deviation of position estimate horizontally
+			global_pos.epv = gps.epv; // Standard deviation of position vertically
+
+			// TODO: implement terrain estimator
+			global_pos.terrain_alt = 0.0f; // Terrain altitude in m, WGS84
+			global_pos.terrain_alt_valid = false; // Terrain altitude estimate is valid
+			// TODO use innovatun consistency check timouts to set this
+			global_pos.dead_reckoning = false; // True if this position is estimated through dead-reckoning
+
+			global_pos.pressure_alt = sensors.baro_alt_meter[0]; // Pressure altitude AMSL (m)
+
+			if (_vehicle_global_position_pub == nullptr) {
+				_vehicle_global_position_pub = orb_advertise(ORB_ID(vehicle_global_position), &global_pos);
+			} else {
+				orb_publish(ORB_ID(vehicle_global_position), _vehicle_global_position_pub, &global_pos);
+			}
 		}
-
-
 	}
 }
 
@@ -394,6 +477,7 @@ int ekf2_main(int argc, char *argv[])
 	if (!strcmp(argv[1], "status")) {
 		if (ekf2::instance) {
 			PX4_WARN("running");
+			ekf2::instance->print_status();
 			return 0;
 
 		} else {

src/modules/mavlink/mavlink_main.cpp

@@ -662,9 +662,48 @@ int Mavlink::mavlink_open_uart(int baud, const char *uart_name, struct termios *
 		return -EINVAL;
 	}
 
+	/* back off 1800 ms to avoid running into the USB setup timing */
+	while (_mode == MAVLINK_MODE_CONFIG &&
+		hrt_absolute_time() < 1800U * 1000U) {
+		usleep(50000);
+	}
+
 	/* open uart */
 	_uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
 
+	/* if this is a config link, stay here and wait for it to open */
+	if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) {
+
+		int armed_fd = orb_subscribe(ORB_ID(actuator_armed));
+		struct actuator_armed_s armed;
+
+		/* get the system arming state and abort on arming */
+		while (_uart_fd < 0) {
+
+			/* abort if an arming topic is published and system is armed */
+			bool updated = false;
+			orb_check(armed_fd, &updated);
+
+			if (updated) {
+				/* the system is now providing arming status feedback.
+				 * instead of timing out, we resort to abort bringing
+				 * up the terminal.
+				 */
+				orb_copy(ORB_ID(actuator_armed), armed_fd, &armed);
+
+				if (armed.armed) {
+					/* this is not an error, but we are done */
+					return -1;
+				}
+			}
+
+			usleep(100000);
+			_uart_fd = ::open(uart_name, O_RDWR | O_NOCTTY);
+		};
+
+		close(armed_fd);
+	}
+
 	if (_uart_fd < 0) {
 		return _uart_fd;
 	}
@@ -892,14 +931,17 @@ Mavlink::send_message(const uint8_t msgid, const void *msg, uint8_t component_ID
 	}
 #else
 	if (get_protocol() == UDP) {
-		ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
+		if (get_client_source_initialized()) {
+			ret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_src_addr, sizeof(_src_addr));
+		}
 
 		struct telemetry_status_s &tstatus = get_rx_status();
 
 		/* resend heartbeat via broadcast */
-		if (((hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000) ||
+		if ((_mode != MAVLINK_MODE_ONBOARD)
+			&& (((hrt_elapsed_time(&tstatus.heartbeat_time) > 3 * 1000 * 1000) ||
 			(tstatus.heartbeat_time == 0)) &&
-			msgid == MAVLINK_MSG_ID_HEARTBEAT) {
+			msgid == MAVLINK_MSG_ID_HEARTBEAT)) {
 
 			int bret = sendto(_socket_fd, buf, packet_len, 0, (struct sockaddr *)&_bcast_addr, sizeof(_bcast_addr));
 
@@ -1006,11 +1048,14 @@ Mavlink::init_udp()
 		return;
 	}
 
-	/* set default target address */
+	/* set default target address, but not for onboard mode (will be set on first recieved packet) */
 	memset((char *)&_src_addr, 0, sizeof(_src_addr));
-	_src_addr.sin_family = AF_INET;
-	inet_aton("127.0.0.1", &_src_addr.sin_addr);
-	_src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP);
+	if (_mode != MAVLINK_MODE_ONBOARD) {
+		set_client_source_initialized();
+		_src_addr.sin_family = AF_INET;
+		inet_aton("127.0.0.1", &_src_addr.sin_addr);
+		_src_addr.sin_port = htons(DEFAULT_REMOTE_PORT_UDP);
+	}
 
 	/* default broadcast address */
 	memset((char *)&_bcast_addr, 0, sizeof(_bcast_addr));
@@ -1573,14 +1618,20 @@ Mavlink::task_main(int argc, char *argv[])
 		_datarate = MAX_DATA_RATE;
 	}
 
-	if (Mavlink::instance_exists(_device_name, this)) {
-		warnx("%s already running", _device_name);
-		return ERROR;
-	}
-
 	if (get_protocol() == SERIAL) {
-	warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate);
+		if (Mavlink::instance_exists(_device_name, this)) {
+			warnx("%s already running", _device_name);
+			return ERROR;
+		}
+
+		warnx("mode: %u, data rate: %d B/s on %s @ %dB", _mode, _datarate, _device_name, _baudrate);
+
 	} else if (get_protocol() == UDP) {
+		if (Mavlink::get_instance_for_network_port(_network_port) != nullptr) {
+			warnx("port %d already occupied", _network_port);
+			return ERROR;
+		}
+
 		warnx("mode: %u, data rate: %d B/s on udp port %hu", _mode, _datarate, _network_port);
 	}
 	/* flush stdout in case MAVLink is about to take it over */
@@ -1592,9 +1643,12 @@ Mavlink::task_main(int argc, char *argv[])
 	/* default values for arguments */
 	_uart_fd = mavlink_open_uart(_baudrate, _device_name, &uart_config_original);
 
-	if (_uart_fd < 0) {
+	if (_uart_fd < 0 && _mode != MAVLINK_MODE_CONFIG) {
 		warn("could not open %s", _device_name);
 		return ERROR;
+	} else if (_uart_fd < 0 && _mode == MAVLINK_MODE_CONFIG) {
+		/* the config link is optional */
+		return OK;
 	}
 
 #endif

src/modules/mavlink/mavlink_main.h

@@ -340,9 +340,15 @@ public:
 	int 			get_socket_fd () { return _socket_fd; };
 #ifdef __PX4_POSIX
 	struct sockaddr_in * get_client_source_address() {return &_src_addr;};
+
+	void			set_client_source_initialized() { _src_addr_initialized = true; };
+
+	bool			get_client_source_initialized() { return _src_addr_initialized; };
 #endif
 	static bool		boot_complete() { return _boot_complete; }
 
+	bool			is_usb_uart() { return _is_usb_uart; }
+
 protected:
 	Mavlink			*next;
 
@@ -423,6 +429,7 @@ private:
 	struct sockaddr_in _myaddr;
 	struct sockaddr_in _src_addr;
 	struct sockaddr_in _bcast_addr;
+	bool _src_addr_initialized;
 
 #endif
 	int _socket_fd;

src/modules/mavlink/mavlink_receiver.cpp

@@ -1814,9 +1814,12 @@ MavlinkReceiver::receive_thread(void *arg)
 
 			struct sockaddr_in * srcaddr_last = _mavlink->get_client_source_address();
 			int localhost = (127 << 24) + 1;
-			if (srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost)) {
+			if ((srcaddr_last->sin_addr.s_addr == htonl(localhost) && srcaddr.sin_addr.s_addr != htonl(localhost))
+					|| (_mavlink->get_mode() == Mavlink::MAVLINK_MODE_ONBOARD && !_mavlink->get_client_source_initialized())) {
 				// if we were sending to localhost before but have a new host then accept him
 				memcpy(srcaddr_last, &srcaddr, sizeof(srcaddr));
+				PX4_WARN("UDP source addr changed: %s", inet_ntoa(srcaddr.sin_addr));
+				_mavlink->set_client_source_initialized();
 			}
 #endif
 			/* if read failed, this loop won't execute */

src/modules/navigator/mission.cpp

@@ -189,8 +189,9 @@ Mission::on_active()
 	}
 
 	/* see if we need to update the current yaw heading for rotary wing types */
-	if (_navigator->get_vstatus()->is_rotary_wing 
+	if (_navigator->get_vstatus()->is_rotary_wing
 			&& _param_yawmode.get() != MISSION_YAWMODE_NONE
+			&& _param_yawmode.get() < MISSION_YAWMODE_MAX
 			&& _mission_type != MISSION_TYPE_NONE) {
 		heading_sp_update();
 	}
@@ -525,36 +526,52 @@ Mission::heading_sp_update()
 		return;
 	}
 
-	/* Don't do FOH for landing and takeoff waypoints, the ground may be near
-	 * and the FW controller has a custom landing logic */
-	if (_mission_item.nav_cmd == NAV_CMD_LAND || _mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
+	/* Don't change heading for takeoff waypoints, the ground may be near */
+	if (_mission_item.nav_cmd == NAV_CMD_TAKEOFF) {
 		return;
 	}
 
 	/* set yaw angle for the waypoint iff a loiter time has been specified */
 	if (_waypoint_position_reached && _mission_item.time_inside > 0.0f) {
 		_mission_item.yaw = _on_arrival_yaw;
-	/* always keep the front of the rotary wing pointing to the next waypoint */
-	} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) {
-		_mission_item.yaw = get_bearing_to_next_waypoint(
-		        _navigator->get_global_position()->lat,
-		        _navigator->get_global_position()->lon,
-		        _mission_item.lat,
-		        _mission_item.lon);
-	/* always keep the back of the rotary wing pointing towards home */
-	} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) {
-		_mission_item.yaw = get_bearing_to_next_waypoint(
-		        _navigator->get_global_position()->lat,
-		        _navigator->get_global_position()->lon,
-		        _navigator->get_home_position()->lat,
-		        _navigator->get_home_position()->lon);
-	/* always keep the back of the rotary wing pointing towards home */
-	} else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
-		_mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint(
-		        _navigator->get_global_position()->lat,
-		        _navigator->get_global_position()->lon,
-		        _navigator->get_home_position()->lat,
-		        _navigator->get_home_position()->lon) + M_PI_F);
+	} else {
+
+		/* calculate direction the vehicle should point to:
+		 * normal waypoint: current position to {waypoint or home or home + 180deg}
+		 * landing waypoint: last waypoint to {waypoint or home or home + 180deg}
+		 * For landing the last waypoint (= constant) is used to avoid excessive yawing near the ground
+		 */
+		double point_from_latlon[2];
+		if (_mission_item.nav_cmd == NAV_CMD_LAND) {
+			point_from_latlon[0] = pos_sp_triplet->previous.lat;
+			point_from_latlon[1] = pos_sp_triplet->previous.lon;
+		} else {
+			point_from_latlon[0] = _navigator->get_global_position()->lat;
+			point_from_latlon[1] = _navigator->get_global_position()->lon;
+		}
+
+		/* always keep the front of the rotary wing pointing to the next waypoint */
+		if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_WAYPOINT) {
+			_mission_item.yaw = get_bearing_to_next_waypoint(
+				point_from_latlon[0],
+				point_from_latlon[1],
+				_mission_item.lat,
+				_mission_item.lon);
+		/* always keep the back of the rotary wing pointing towards home */
+		} else if (_param_yawmode.get() == MISSION_YAWMODE_FRONT_TO_HOME) {
+			_mission_item.yaw = get_bearing_to_next_waypoint(
+				point_from_latlon[0],
+				point_from_latlon[1],
+				_navigator->get_home_position()->lat,
+				_navigator->get_home_position()->lon);
+		/* always keep the back of the rotary wing pointing towards home */
+		} else if (_param_yawmode.get() == MISSION_YAWMODE_BACK_TO_HOME) {
+			_mission_item.yaw = _wrap_pi(get_bearing_to_next_waypoint(
+				point_from_latlon[0],
+				point_from_latlon[1],
+				_navigator->get_home_position()->lat,
+				_navigator->get_home_position()->lon) + M_PI_F);
+		}
 	}
 
 	mission_item_to_position_setpoint(&_mission_item, &pos_sp_triplet->current);

src/modules/navigator/mission.h

@@ -88,7 +88,8 @@ public:
 		MISSION_YAWMODE_NONE = 0,
 		MISSION_YAWMODE_FRONT_TO_WAYPOINT = 1,
 		MISSION_YAWMODE_FRONT_TO_HOME = 2,
-		MISSION_YAWMODE_BACK_TO_HOME = 3
+		MISSION_YAWMODE_BACK_TO_HOME = 3,
+		MISSION_YAWMODE_MAX = 4
 	};
 
 private:

src/modules/position_estimator_inav/position_estimator_inav_main.cpp

@@ -63,7 +63,6 @@
 #include <uORB/topics/vehicle_gps_position.h>
 #include <uORB/topics/vision_position_estimate.h>
 #include <uORB/topics/att_pos_mocap.h>
-#include <uORB/topics/home_position.h>
 #include <uORB/topics/optical_flow.h>
 #include <uORB/topics/distance_sensor.h>
 #include <mavlink/mavlink_log.h>
@@ -273,7 +272,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	const hrt_abstime ref_init_delay = 1000000;	// wait for 1s after 3D fix
 	struct map_projection_reference_s ref;
 	memset(&ref, 0, sizeof(ref));
-	hrt_abstime home_timestamp = 0;
 
 	uint16_t accel_updates = 0;
 	uint16_t baro_updates = 0;
@@ -346,8 +344,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	memset(&sensor, 0, sizeof(sensor));
 	struct vehicle_gps_position_s gps;
 	memset(&gps, 0, sizeof(gps));
-	struct home_position_s home;
-	memset(&home, 0, sizeof(home));
 	struct vehicle_attitude_s att;
 	memset(&att, 0, sizeof(att));
 	struct vehicle_local_position_s local_pos;
@@ -373,7 +369,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 	int vehicle_gps_position_sub = orb_subscribe(ORB_ID(vehicle_gps_position));
 	int vision_position_estimate_sub = orb_subscribe(ORB_ID(vision_position_estimate));
 	int att_pos_mocap_sub = orb_subscribe(ORB_ID(att_pos_mocap));
-	int home_position_sub = orb_subscribe(ORB_ID(home_position));
 	int distance_sensor_sub = orb_subscribe(ORB_ID(distance_sensor));
 
 	/* advertise */
@@ -677,46 +672,6 @@ int position_estimator_inav_thread_main(int argc, char *argv[])
 				flow_updates++;
 			}
 
-			/* home position */
-			orb_check(home_position_sub, &updated);
-
-			if (updated) {
-				orb_copy(ORB_ID(home_position), home_position_sub, &home);
-
-				if (home.timestamp != home_timestamp) {
-					home_timestamp = home.timestamp;
-
-					double est_lat, est_lon;
-					float est_alt;
-
-					if (ref_inited) {
-						/* calculate current estimated position in global frame */
-						est_alt = local_pos.ref_alt - local_pos.z;
-						map_projection_reproject(&ref, local_pos.x, local_pos.y, &est_lat, &est_lon);
-					}
-
-					/* update reference */
-					map_projection_init(&ref, home.lat, home.lon);
-
-					/* update baro offset */
-					baro_offset += home.alt - local_pos.ref_alt;
-
-					local_pos.ref_lat = home.lat;
-					local_pos.ref_lon = home.lon;
-					local_pos.ref_alt = home.alt;
-					local_pos.ref_timestamp = home.timestamp;
-
-					if (ref_inited) {
-						/* reproject position estimate with new reference */
-						map_projection_project(&ref, est_lat, est_lon, &x_est[0], &y_est[0]);
-						z_est[0] = -(est_alt - local_pos.ref_alt);
-					}
-
-					ref_inited = true;
-				}
-			}
-
-
 			/* check no vision circuit breaker is set */
 			if (params.no_vision != CBRK_NO_VISION_KEY) {
 				/* vehicle vision position */

src/modules/simulator/simulator.h

@@ -230,6 +230,7 @@ private:
 		_perf_gps(perf_alloc_once(PC_ELAPSED, "sim_gps_delay")),
 		_perf_airspeed(perf_alloc_once(PC_ELAPSED, "sim_airspeed_delay")),
 		_perf_sim_delay(perf_alloc_once(PC_ELAPSED, "sim_network_delay")),
+		_perf_sim_interval(perf_alloc(PC_INTERVAL, "sim_network_interval")),
 		_accel_pub(nullptr),
 		_baro_pub(nullptr),
 		_gyro_pub(nullptr),
@@ -270,6 +271,7 @@ private:
 	perf_counter_t _perf_gps;
 	perf_counter_t _perf_airspeed;
 	perf_counter_t _perf_sim_delay;
+	perf_counter_t _perf_sim_interval;
 
 	// uORB publisher handlers
 	orb_advert_t _accel_pub;

src/modules/simulator/simulator_mavlink.cpp

@@ -248,6 +248,7 @@ void Simulator::handle_message(mavlink_message_t *msg, bool publish)
 			uint64_t timestamp = ts.tv_sec * 1000 * 1000 + ts.tv_nsec / 1000;
 
 			perf_set(_perf_sim_delay, timestamp - sim_timestamp);
+			perf_count(_perf_sim_interval);
 
 			if (publish) {
 				publish_sensor_topics(&imu);
@@ -426,6 +427,13 @@ void Simulator::initializeSensorData()
 
 void Simulator::pollForMAVLinkMessages(bool publish)
 {
+	// set the threads name
+#ifdef __PX4_DARWIN
+	pthread_setname_np("sim_rcv");
+#else
+	pthread_setname_np(pthread_self(), "sim_rcv");
+#endif
+
 	// udp socket data
 	struct sockaddr_in _myaddr;
 	const int _port = UDP_PORT;
@@ -504,14 +512,13 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 		pret = ::poll(&fds[0], fd_count, 100);
 	}
 
-	PX4_INFO("Found initial message, pret = %d", pret);
 	_initialized = true;
 	// reset system time
 	(void)hrt_reset();
 
 	if (fds[0].revents & POLLIN) {
 		len = recvfrom(_fd, _buf, sizeof(_buf), 0, (struct sockaddr *)&_srcaddr, &_addrlen);
-		PX4_INFO("Sending initial controls message to jMAVSim.");
+		PX4_INFO("Sending initial controls message to simulator");
 		send_controls();
 	}
 
@@ -523,14 +530,6 @@ void Simulator::pollForMAVLinkMessages(bool publish)
 	pthread_create(&sender_thread, &sender_thread_attr, Simulator::sending_trampoline, NULL);
 	pthread_attr_destroy(&sender_thread_attr);
 
-
-	// set the threads name
-#ifdef __PX4_DARWIN
-	pthread_setname_np("sim_rcv");
-#else
-	pthread_setname_np(pthread_self(), "sim_rcv");
-#endif
-
 	// wait for new mavlink messages to arrive
 	while (true) {
 

src/modules/vtol_att_control/tiltrotor.cpp

@@ -67,7 +67,7 @@ Tiltrotor::Tiltrotor(VtolAttitudeControl *attc) :
 	_params_handles_tiltrotor.airspeed_blend_start = param_find("VT_ARSP_BLEND");
 	_params_handles_tiltrotor.elevons_mc_lock = param_find("VT_ELEV_MC_LOCK");
 	_params_handles_tiltrotor.front_trans_dur_p2 = param_find("VT_TRANS_P2_DUR");
-	_params_handles_tiltrotor.fw_motors_off = param_find("VT_FW_MOT_OFF");
+	_params_handles_tiltrotor.fw_motors_off = param_find("VT_FW_MOT_OFFID");
 }
 
 Tiltrotor::~Tiltrotor()
@@ -145,7 +145,7 @@ int Tiltrotor::get_motor_off_channels(int channels)
 			break;
 		}
 
-		channel_bitmap |= 1 << channel;
+		channel_bitmap |= 1 << (channel - 1);
 		channels = channels / 10;
 	}
 
@@ -428,7 +428,7 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state)
 		break;
 
 	case DISABLED:
-		pwm_value = PWM_LOWEST_MAX;
+		pwm_value = PWM_MOTOR_OFF;
 		_rear_motors = DISABLED;
 		break;
 
@@ -446,21 +446,21 @@ void Tiltrotor::set_rear_motor_state(rear_motor_state state)
 	if (fd < 0) {PX4_WARN("can't open %s", dev);}
 
 	ret = px4_ioctl(fd, PWM_SERVO_GET_COUNT, (unsigned long)&servo_count);
-	struct pwm_output_values pwm_values;
-	memset(&pwm_values, 0, sizeof(pwm_values));
+	struct pwm_output_values pwm_max_values;
+	memset(&pwm_max_values, 0, sizeof(pwm_max_values));
 
 	for (int i = 0; i < _params->vtol_motor_count; i++) {
 		if (is_motor_off_channel(i)) {
-			pwm_values.values[i] = pwm_value;
+			pwm_max_values.values[i] = pwm_value;
 
 		} else {
-			pwm_values.values[i] = PWM_DEFAULT_MAX;
+			pwm_max_values.values[i] = PWM_DEFAULT_MAX;
 		}
 
-		pwm_values.channel_count = _params->vtol_motor_count;
+		pwm_max_values.channel_count = _params->vtol_motor_count;
 	}
 
-	ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_values);
+	ret = px4_ioctl(fd, PWM_SERVO_SET_MAX_PWM, (long unsigned int)&pwm_max_values);
 
 	if (ret != OK) {PX4_WARN("failed setting max values");}
 

src/modules/vtol_att_control/tiltrotor_params.c

@@ -90,7 +90,7 @@ PARAM_DEFINE_FLOAT(VT_TRANS_P2_DUR, 0.5f);
  *
  *
  * @min 0
- * @max 123456
+ * @max 12345678
  * @group VTOL Attitude Control
  */
-PARAM_DEFINE_INT32(VT_FW_MOT_OFF, 0);
+PARAM_DEFINE_INT32(VT_FW_MOT_OFFID, 0);