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

Conflicts:
	src/examples/subscriber/subscriber_params.c
	src/modules/mc_att_control/mc_att_control_main.cpp
	src/modules/uORB/topics/vehicle_attitude.h
	src/modules/uORB/topics/vehicle_attitude_setpoint.h
	src/platforms/px4_middleware.h

4 files changed, 39 insertions, 9 deletions

diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp
index 89da8438b..90a6c90fe 100644
--- a/src/modules/mc_att_control/mc_att_control.cpp
+++ b/src/modules/mc_att_control/mc_att_control.cpp
@@ -97,9 +97,17 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_params_handles.acro_roll_max		= 	PX4_PARAM_INIT(MC_ACRO_R_MAX);
 	_params_handles.acro_pitch_max		= 	PX4_PARAM_INIT(MC_ACRO_P_MAX);
 	_params_handles.acro_yaw_max		= 	PX4_PARAM_INIT(MC_ACRO_Y_MAX);
+	_params_handles.autostart_id		= 	PX4_PARAM_INIT(SYS_AUTOSTART);
 
 	/* fetch initial parameter values */
 	parameters_update();
+	/* set correct uORB ID, depending on if vehicle is VTOL or not */
+	if (_params.autostart_id >= 13000 && _params.autostart_id <= 13999) { /* VTOL airframe?*/
+		_is_vtol = true;
+	}
+	else {
+		_is_vtol = false;
+	}
 
 	/*
 	 * do subscriptions
@@ -108,9 +116,11 @@ MulticopterAttitudeControl::MulticopterAttitudeControl() :
 	_v_att_sp = PX4_SUBSCRIBE(_n, vehicle_attitude_setpoint, 0);
 	_v_rates_sp = PX4_SUBSCRIBE(_n, vehicle_rates_setpoint, 0);
 	_v_control_mode = PX4_SUBSCRIBE(_n, vehicle_control_mode, 0);
-	PX4_SUBSCRIBE(_n, parameter_update, MulticopterAttitudeControl::handle_parameter_update, this, 1000);
+	_parameter_update = PX4_SUBSCRIBE(_n, parameter_update,
+			MulticopterAttitudeControl::handle_parameter_update, this, 1000);
 	_manual_control_sp = PX4_SUBSCRIBE(_n, manual_control_setpoint, 0);
 	_armed = PX4_SUBSCRIBE(_n, actuator_armed, 0);
+	_v_status = PX4_SUBSCRIBE(_n, vehicle_status, 0);
 
 }
 
@@ -204,7 +214,7 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 		control_attitude(dt);
 
 		/* publish the attitude setpoint if needed */
-		if (_publish_att_sp) {
+		if (_publish_att_sp && _v_status->get().is_rotary_wing) {
 			_v_att_sp_mod.timestamp = px4::get_time_micros();
 
 			if (_att_sp_pub != nullptr) {
@@ -225,7 +235,11 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 		if (_v_rates_sp_pub != nullptr) {
 			_v_rates_sp_pub->publish(_v_rates_sp_mod);
 		} else {
-			_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
+			if (_is_vtol) {
+				_v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint);
+			} else {
+				_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
+			}
 		}
 
 	} else {
@@ -250,7 +264,11 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 				_v_rates_sp_pub->publish(_v_rates_sp_mod);
 
 			} else {
-				_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
+				if (_is_vtol) {
+					_v_rates_sp_pub = PX4_ADVERTISE(_n, mc_virtual_rates_setpoint);
+				} else {
+					_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
+				}
 			}
 
 		} else {
@@ -277,7 +295,11 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 				_actuators_0_pub->publish(_actuators);
 
 			} else {
-				_actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0);
+				if (_is_vtol) {
+					_actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_virtual_mc);
+				} else {
+					_actuators_0_pub = PX4_ADVERTISE(_n, actuator_controls_0);
+				}
 			}
 		}
 	}
diff --git a/src/modules/mc_att_control/mc_att_control.h b/src/modules/mc_att_control/mc_att_control.h
index 76c095cc1..33552c269 100644
--- a/src/modules/mc_att_control/mc_att_control.h
+++ b/src/modules/mc_att_control/mc_att_control.h
@@ -93,6 +93,8 @@ private:
 	px4::Publisher *	_v_rates_sp_pub;		/**< rate setpoint publication */
 	px4::Publisher *	_actuators_0_pub;		/**< attitude actuator controls publication */
 
+	bool _is_vtol;						/**< true if vehicle is vtol, to be replaced with global API */
+
 	px4::NodeHandle _n;
 
 	struct {
@@ -117,6 +119,8 @@ private:
 		px4_param_t acro_roll_max;
 		px4_param_t acro_pitch_max;
 		px4_param_t acro_yaw_max;
+
+		px4_param_t autostart_id;
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	perf_counter_t _loop_perf; /**< loop performance counter */
diff --git a/src/modules/mc_att_control/mc_att_control_base.cpp b/src/modules/mc_att_control/mc_att_control_base.cpp
index 871f93ab8..d7fbf3acd 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -266,7 +266,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
 {
 	/* reset integral if disarmed */
-	if (!_armed->get().armed) {
+	if (!_armed->get().armed || !_v_status->get().is_rotary_wing) {
 		_rates_int.zero();
 	}
 
diff --git a/src/modules/mc_att_control/mc_att_control_base.h b/src/modules/mc_att_control/mc_att_control_base.h
index 17c779796..cf4c726a7 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -87,11 +87,13 @@ public:
 
 protected:
 	px4::PX4_SUBSCRIBER(vehicle_attitude) *_v_att;			    /**< vehicle attitude */
-	px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp;  /**< manual control setpoint */
+	px4::PX4_SUBSCRIBER(vehicle_attitude_setpoint) *_v_att_sp;	    /**< vehicle attitude setpoint */
+	px4::PX4_SUBSCRIBER(vehicle_rates_setpoint) *_v_rates_sp;	    /**< vehicle rates setpoint */
 	px4::PX4_SUBSCRIBER(vehicle_control_mode) *_v_control_mode;	    /**< vehicle control mode */
+	px4::PX4_SUBSCRIBER(parameter_update) *_parameter_update;	    /**< parameter update */
+	px4::PX4_SUBSCRIBER(manual_control_setpoint) *_manual_control_sp;   /**< manual control setpoint */
 	px4::PX4_SUBSCRIBER(actuator_armed) *_armed;			    /**< actuator arming status */
-	px4::PX4_SUBSCRIBER(vehicle_attitude_setpoint) *_v_att_sp;	/**< vehicle attitude setpoint */
-	px4::PX4_SUBSCRIBER(vehicle_rates_setpoint) *_v_rates_sp;	/**< vehicle rates setpoint */
+	px4::PX4_SUBSCRIBER(vehicle_status) *_v_status;			    /**< vehicle status */
 
 	PX4_TOPIC_T(vehicle_attitude_setpoint)	_v_att_sp_mod;	/**< modified vehicle attitude setpoint
 								  that gets published eventually */
@@ -121,6 +123,8 @@ protected:
 		float man_pitch_max;
 		float man_yaw_max;
 		math::Vector<3> acro_rate_max; /**< max attitude rates in acro mode */
+
+		int32_t autostart_id;
 	} _params;
 
 	bool _publish_att_sp;