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

Conflicts:
	src/lib/mathlib/math/Matrix.hpp
	src/modules/mc_att_control/mc_att_control_main.cpp
	src/modules/uORB/topics/vehicle_status.h
	src/platforms/px4_includes.h

2 files changed, 27 insertions, 15 deletions

diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index a0f74517c..00e8393bf 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -336,8 +336,8 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_actuators_0_pub(-1),
 	_actuators_2_pub(-1),
 
-	_rates_sp_id(ORB_ID(vehicle_rates_setpoint)),
-	_actuators_id(ORB_ID(actuator_controls_0)),
+	_rates_sp_id(0),
+	_actuators_id(0),
 
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw att control")),
@@ -589,12 +589,14 @@ FixedwingAttitudeControl::vehicle_status_poll()
 	if (vehicle_status_updated) {
 		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
 		/* set correct uORB ID, depending on if vehicle is VTOL or not */
-		if (_vehicle_status.is_vtol) {
-			_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
-			_actuators_id = ORB_ID(actuator_controls_virtual_fw);
-		} else {
-			_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
-			_actuators_id = ORB_ID(actuator_controls_0);
+		if (!_rates_sp_id) {
+			if (_vehicle_status.is_vtol) {
+				_rates_sp_id = ORB_ID(fw_virtual_rates_setpoint);
+				_actuators_id = ORB_ID(actuator_controls_virtual_fw);
+			} else {
+				_rates_sp_id = ORB_ID(vehicle_rates_setpoint);
+				_actuators_id = ORB_ID(actuator_controls_0);
+			}
 		}
 	}
 }
@@ -720,22 +722,24 @@ FixedwingAttitudeControl::task_main()
 				R.set(_att.R);
 				R_adapted.set(_att.R);
 
-				//move z to x
+				/* move z to x */
 				R_adapted(0, 0) = R(0, 2);
 				R_adapted(1, 0) = R(1, 2);
 				R_adapted(2, 0) = R(2, 2);
-				//move x to z
+
+				/* move x to z */
 				R_adapted(0, 2) = R(0, 0);
 				R_adapted(1, 2) = R(1, 0);
 				R_adapted(2, 2) = R(2, 0);
 
-				//change direction of pitch (convert to right handed system)
+				/* change direction of pitch (convert to right handed system) */
 				R_adapted(0, 0) = -R_adapted(0, 0);
 				R_adapted(1, 0) = -R_adapted(1, 0);
 				R_adapted(2, 0) = -R_adapted(2, 0);
 				math::Vector<3> euler_angles;		//adapted euler angles for fixed wing operation
 				euler_angles = R_adapted.to_euler();
-				//fill in new attitude data
+
+				/* fill in new attitude data */
 				_att.roll    = euler_angles(0);
 				_att.pitch   = euler_angles(1);
 				_att.yaw     = euler_angles(2);
@@ -749,7 +753,7 @@ FixedwingAttitudeControl::task_main()
 				PX4_R(_att.R, 2, 1) = R_adapted(2, 1);
 				PX4_R(_att.R, 2, 2) = R_adapted(2, 2);
 
-				// lastly, roll- and yawspeed have to be swaped
+				/* lastly, roll- and yawspeed have to be swaped */
 				float helper = _att.rollspeed;
 				_att.rollspeed = -_att.yawspeed;
 				_att.yawspeed = helper;
@@ -820,6 +824,7 @@ FixedwingAttitudeControl::task_main()
 
 				float roll_sp = _parameters.rollsp_offset_rad;
 				float pitch_sp = _parameters.pitchsp_offset_rad;
+				float yaw_manual = 0.0f;
 				float throttle_sp = 0.0f;
 
 				/* Read attitude setpoint from uorb if
@@ -880,6 +885,8 @@ FixedwingAttitudeControl::task_main()
 						+ _parameters.rollsp_offset_rad;
 					pitch_sp = -(_manual.x * _parameters.man_pitch_max - _parameters.trim_pitch)
 						+ _parameters.pitchsp_offset_rad;
+					/* allow manual control of rudder deflection */
+					yaw_manual = _manual.r;
 					throttle_sp = _manual.z;
 					_actuators.control[4] = _manual.flaps;
 
@@ -979,6 +986,9 @@ FixedwingAttitudeControl::task_main()
 							_pitch_ctrl.get_desired_rate(),
 							_parameters.airspeed_min, _parameters.airspeed_max, airspeed, airspeed_scaling, lock_integrator);
 					_actuators.control[2] = (isfinite(yaw_u)) ? yaw_u + _parameters.trim_yaw : _parameters.trim_yaw;
+
+					/* add in manual rudder control */
+					_actuators.control[2] += yaw_manual;
 					if (!isfinite(yaw_u)) {
 						_yaw_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
@@ -1018,7 +1028,7 @@ FixedwingAttitudeControl::task_main()
 				if (_rate_sp_pub > 0) {
 					/* publish the attitude rates setpoint */
 					orb_publish(_rates_sp_id, _rate_sp_pub, &_rates_sp);
-				} else {
+				} else if (_rates_sp_id) {
 					/* advertise the attitude rates setpoint */
 					_rate_sp_pub = orb_advertise(_rates_sp_id, &_rates_sp);
 				}
@@ -1043,7 +1053,7 @@ FixedwingAttitudeControl::task_main()
 			/* publish the actuator controls */
 			if (_actuators_0_pub > 0) {
 				orb_publish(_actuators_id, _actuators_0_pub, &_actuators);
-			} else {
+			} else if (_actuators_id) {
 				_actuators_0_pub= orb_advertise(_actuators_id, &_actuators);
 			}
 
diff --git a/src/modules/fw_att_control/module.mk b/src/modules/fw_att_control/module.mk
index 89c6494c5..3661a171f 100644
--- a/src/modules/fw_att_control/module.mk
+++ b/src/modules/fw_att_control/module.mk
@@ -41,3 +41,5 @@ SRCS		= fw_att_control_main.cpp \
 		  fw_att_control_params.c
 
 MODULE_STACKSIZE = 1200
+
+MAXOPTIMIZATION = -Os