ported control state attitude into mc_pos_ctrl

src/modules/mc_pos_control/mc_pos_control_main.cpp

@@ -68,7 +68,7 @@
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/vehicle_rates_setpoint.h>
-#include <uORB/topics/vehicle_attitude.h>
+#include <uORB/topics/control_state.h>
 #include <uORB/topics/vehicle_control_mode.h>
 #include <uORB/topics/actuator_armed.h>
 #include <uORB/topics/parameter_update.h>
@@ -126,7 +126,7 @@ private:
 	int		_mavlink_fd;			/**< mavlink fd */
 
 	int		_vehicle_status_sub;		/**< vehicle status subscription */
-	int		_att_sub;			/**< vehicle attitude subscription */
+	int		_ctrl_state_sub;			/**< control state subscription */
 	int		_att_sp_sub;			/**< vehicle attitude setpoint */
 	int		_control_mode_sub;		/**< vehicle control mode subscription */
 	int		_params_sub;			/**< notification of parameter updates */
@@ -142,7 +142,7 @@ private:
 	orb_advert_t	_global_vel_sp_pub;		/**< vehicle global velocity setpoint publication */
 
 	struct vehicle_status_s 			_vehicle_status; 	/**< vehicle status */
-	struct vehicle_attitude_s			_att;			/**< vehicle attitude */
+	struct control_state_s				_ctrl_state;			/**< vehicle attitude */
 	struct vehicle_attitude_setpoint_s		_att_sp;		/**< vehicle attitude setpoint */
 	struct manual_control_setpoint_s		_manual;		/**< r/c channel data */
 	struct vehicle_control_mode_s			_control_mode;		/**< vehicle control mode */
@@ -215,6 +215,9 @@ private:
 	math::Vector<3> _vel_ff;
 	math::Vector<3> _sp_move_rate;
 
+	math::Matrix<3, 3> _R;			/**< rotation matrix from attitude quaternions */
+	float _yaw;				/**< yaw angle (euler) */
+
 	/**
 	 * Update our local parameter cache.
 	 */
@@ -304,7 +307,7 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_mavlink_fd(-1),
 
 /* subscriptions */
-	_att_sub(-1),
+	_ctrl_state_sub(-1),
 	_att_sp_sub(-1),
 	_control_mode_sub(-1),
 	_params_sub(-1),
@@ -325,10 +328,11 @@ MulticopterPositionControl::MulticopterPositionControl() :
 
 	_reset_pos_sp(true),
 	_reset_alt_sp(true),
-	_mode_auto(false)
+	_mode_auto(false),
+	_yaw(0.0f)
 {
 	memset(&_vehicle_status, 0, sizeof(_vehicle_status));
-	memset(&_att, 0, sizeof(_att));
+	memset(&_ctrl_state, 0, sizeof(_ctrl_state));
 	memset(&_att_sp, 0, sizeof(_att_sp));
 	memset(&_manual, 0, sizeof(_manual));
 	memset(&_control_mode, 0, sizeof(_control_mode));
@@ -356,6 +360,8 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	_vel_ff.zero();
 	_sp_move_rate.zero();
 
+	_R.identity();
+
 	_params_handles.thr_min		= param_find("MPC_THR_MIN");
 	_params_handles.thr_max		= param_find("MPC_THR_MAX");
 	_params_handles.z_p			= param_find("MPC_Z_P");
@@ -493,10 +499,10 @@ MulticopterPositionControl::poll_subscriptions()
 		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub, &_vehicle_status);
 	}
 
-	orb_check(_att_sub, &updated);
+	orb_check(_ctrl_state_sub, &updated);
 
 	if (updated) {
-		orb_copy(ORB_ID(vehicle_attitude), _att_sub, &_att);
+		orb_copy(ORB_ID(control_state), _ctrl_state_sub, &_ctrl_state);
 	}
 
 	orb_check(_att_sp_sub, &updated);
@@ -923,7 +929,7 @@ MulticopterPositionControl::task_main()
 	 * do subscriptions
 	 */
 	_vehicle_status_sub = orb_subscribe(ORB_ID(vehicle_status));
-	_att_sub = orb_subscribe(ORB_ID(vehicle_attitude));
+	_ctrl_state_sub = orb_subscribe(ORB_ID(control_state));
 	_att_sp_sub = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
 	_control_mode_sub = orb_subscribe(ORB_ID(vehicle_control_mode));
 	_params_sub = orb_subscribe(ORB_ID(parameter_update));
@@ -978,6 +984,14 @@ MulticopterPositionControl::task_main()
 		}
 
 		poll_subscriptions();
+
+		/* get current rotation matrix and euler angles from control state quaternions */
+		math::Quaternion q_att(_ctrl_state.q[0], _ctrl_state.q[1], _ctrl_state.q[2], _ctrl_state.q[3]);
+		_R = q_att.to_dcm();
+		math::Vector<3> euler_angles;
+		euler_angles = _R.to_euler();
+		_yaw     = euler_angles(2);
+
 		parameters_update(false);
 
 		hrt_abstime t = hrt_absolute_time();
@@ -1046,7 +1060,7 @@ MulticopterPositionControl::task_main()
 
 				_att_sp.roll_body = 0.0f;
 				_att_sp.pitch_body = 0.0f;
-				_att_sp.yaw_body = _att.yaw;
+				_att_sp.yaw_body = _yaw;
 				_att_sp.thrust = 0.0f;
 
 				_att_sp.timestamp = hrt_absolute_time();
@@ -1216,11 +1230,11 @@ MulticopterPositionControl::task_main()
 						/* thrust compensation for altitude only control mode */
 						float att_comp;
 
-						if (PX4_R(_att.R, 2, 2) > TILT_COS_MAX) {
-							att_comp = 1.0f / PX4_R(_att.R, 2, 2);
+						if (_R(2, 2) > TILT_COS_MAX) {
+							att_comp = 1.0f / _R(2, 2);
 
-						} else if (PX4_R(_att.R, 2, 2) > 0.0f) {
-							att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * PX4_R(_att.R, 2, 2) + 1.0f;
+						} else if (_R(2, 2) > 0.0f) {
+							att_comp = ((1.0f / TILT_COS_MAX - 1.0f) / TILT_COS_MAX) * _R(2, 2) + 1.0f;
 							saturation_z = true;
 
 						} else {
@@ -1407,7 +1421,7 @@ MulticopterPositionControl::task_main()
 			/* reset yaw setpoint to current position if needed */
 			if (reset_yaw_sp) {
 				reset_yaw_sp = false;
-				_att_sp.yaw_body = _att.yaw;
+				_att_sp.yaw_body = _yaw;
 			}
 
 			/* do not move yaw while arming */
@@ -1417,7 +1431,7 @@ MulticopterPositionControl::task_main()
 
 				_att_sp.yaw_sp_move_rate = _manual.r * _params.man_yaw_max;
 				float yaw_target = _wrap_pi(_att_sp.yaw_body + _att_sp.yaw_sp_move_rate * dt);
-				float yaw_offs = _wrap_pi(yaw_target - _att.yaw);
+				float yaw_offs = _wrap_pi(yaw_target - _yaw);
 
 				// If the yaw offset became too big for the system to track stop
 				// shifting it