mc att: use multiplatform subscriptions type

3 files changed, 40 insertions, 39 deletions

diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp
index bbda1f8aa..59eaaec6d 100644
--- a/src/modules/mc_att_control/mc_att_control.cpp
+++ b/src/modules/mc_att_control/mc_att_control.cpp
@@ -214,7 +214,7 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 		dt = 0.02f;
 	}
 
-	if (_v_control_mode.flag_control_attitude_enabled) {
+	if (_v_control_mode->get().flag_control_attitude_enabled) {
 		control_attitude(dt);
 
 		/* publish the attitude setpoint if needed */
@@ -245,11 +245,11 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 
 	} else {
 		/* attitude controller disabled, poll rates setpoint topic */
-		if (_v_control_mode.flag_control_manual_enabled) {
+		if (_v_control_mode->get().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;
+			_rates_sp = math::Vector<3>(_manual_control_sp->get().y, -_manual_control_sp->get().x,
+						    _manual_control_sp->get().r).emult(_params.acro_rate_max);
+			_thrust_sp = _manual_control_sp->get().z;
 
 			/* reset yaw setpoint after ACRO */
 			_reset_yaw_sp = true;
@@ -278,7 +278,7 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 		}
 	}
 
-	if (_v_control_mode.flag_control_rates_enabled) {
+	if (_v_control_mode->get().flag_control_rates_enabled) {
 		control_attitude_rates(dt);
 
 		/* publish actuator controls */
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 0a05be924..b9a57ce69 100644
--- a/src/modules/mc_att_control/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control/mc_att_control_base.cpp
@@ -58,13 +58,13 @@ MulticopterAttitudeControlBase::MulticopterAttitudeControlBase() :
 	_publish_att_sp(false)
 
 {
-	memset(&_v_att, 0, sizeof(_v_att));
-	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
-	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
-	memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
-	memset(&_v_control_mode, 0, sizeof(_v_control_mode));
-	memset(&_actuators, 0, sizeof(_actuators));
-	memset(&_armed, 0, sizeof(_armed));
+	// memset(&_v_att, 0, sizeof(_v_att));
+	// memset(&_v_att_sp, 0, sizeof(_v_att_sp));
+	// memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
+	// memset(&_manual_control_sp, 0, sizeof(_manual_control_sp));
+	// memset(&_v_control_mode, 0, sizeof(_v_control_mode));
+	// memset(&_actuators, 0, sizeof(_actuators));
+	// memset(&_armed, 0, sizeof(_armed));
 
 	_params.att_p.zero();
 	_params.rate_p.zero();
@@ -95,22 +95,22 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 	float yaw_sp_move_rate = 0.0f;
 	_publish_att_sp = false;
 
-	if (_v_control_mode.flag_control_manual_enabled) {
+	if (_v_control_mode->get().flag_control_manual_enabled) {
 		/* manual input, set or modify attitude setpoint */
 
-		if (_v_control_mode.flag_control_velocity_enabled
-		    || _v_control_mode.flag_control_climb_rate_enabled) {
+		if (_v_control_mode->get().flag_control_velocity_enabled
+		    || _v_control_mode->get().flag_control_climb_rate_enabled) {
 			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
 			//XXX vehicle_attitude_setpoint_poll();
 		}
 
-		if (!_v_control_mode.flag_control_climb_rate_enabled) {
+		if (!_v_control_mode->get().flag_control_climb_rate_enabled) {
 			/* pass throttle directly if not in altitude stabilized mode */
-			_v_att_sp.thrust = _manual_control_sp.z;
+			_v_att_sp.thrust = _manual_control_sp->get().z;
 			_publish_att_sp = true;
 		}
 
-		if (!_armed.armed) {
+		if (!_armed->get().armed) {
 			/* reset yaw setpoint when disarmed */
 			_reset_yaw_sp = true;
 		}
@@ -124,17 +124,17 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 			//}
 		} else {
 			/* move yaw setpoint */
-			yaw_sp_move_rate = _manual_control_sp.r * _params.man_yaw_max;
+			yaw_sp_move_rate = _manual_control_sp->get().r * _params.man_yaw_max;
 			_v_att_sp.yaw_body = _wrap_pi(
 						     _v_att_sp.yaw_body + yaw_sp_move_rate * dt);
 			float yaw_offs_max = _params.man_yaw_max / _params.att_p(2);
-			float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att.yaw);
+			float yaw_offs = _wrap_pi(_v_att_sp.yaw_body - _v_att->get().yaw);
 
 			if (yaw_offs < -yaw_offs_max) {
-				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw - yaw_offs_max);
+				_v_att_sp.yaw_body = _wrap_pi(_v_att->get().yaw - yaw_offs_max);
 
 			} else if (yaw_offs > yaw_offs_max) {
-				_v_att_sp.yaw_body = _wrap_pi(_v_att.yaw + yaw_offs_max);
+				_v_att_sp.yaw_body = _wrap_pi(_v_att->get().yaw + yaw_offs_max);
 			}
 
 			_v_att_sp.R_valid = false;
@@ -144,15 +144,15 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 		/* reset yaw setpint to current position if needed */
 		if (_reset_yaw_sp) {
 			_reset_yaw_sp = false;
-			_v_att_sp.yaw_body = _v_att.yaw;
+			_v_att_sp.yaw_body = _v_att->get().yaw;
 			_v_att_sp.R_valid = false;
 			_publish_att_sp = true;
 		}
 
-		if (!_v_control_mode.flag_control_velocity_enabled) {
+		if (!_v_control_mode->get().flag_control_velocity_enabled) {
 			/* update attitude setpoint if not in position control mode */
-			_v_att_sp.roll_body = _manual_control_sp.y * _params.man_roll_max;
-			_v_att_sp.pitch_body = -_manual_control_sp.x
+			_v_att_sp.roll_body = _manual_control_sp->get().y * _params.man_roll_max;
+			_v_att_sp.pitch_body = -_manual_control_sp->get().x
 					       * _params.man_pitch_max;
 			_v_att_sp.R_valid = false;
 			_publish_att_sp = true;
@@ -188,7 +188,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 
 	/* rotation matrix for current state */
 	math::Matrix<3, 3> R;
-	R.set(_v_att.R);
+	R.set(_v_att->get().R);
 
 	/* all input data is ready, run controller itself */
 
@@ -269,15 +269,15 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
 {
 	/* reset integral if disarmed */
-	if (!_armed.armed) {
+	if (!_armed->get().armed) {
 		_rates_int.zero();
 	}
 
 	/* current body angular rates */
 	math::Vector < 3 > rates;
-	rates(0) = _v_att.rollspeed;
-	rates(1) = _v_att.pitchspeed;
-	rates(2) = _v_att.yawspeed;
+	rates(0) = _v_att->get().rollspeed;
+	rates(1) = _v_att->get().pitchspeed;
+	rates(2) = _v_att->get().yawspeed;
 
 	/* angular rates error */
 	math::Vector < 3 > rates_err = _rates_sp - rates;
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 0783bb71e..05f89abe6 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -86,13 +86,14 @@ public:
 	void set_actuator_controls();
 
 protected:
-	struct vehicle_attitude_s _v_att; /**< vehicle attitude */
-	struct vehicle_attitude_setpoint_s _v_att_sp; /**< vehicle attitude setpoint */
-	struct vehicle_rates_setpoint_s _v_rates_sp; /**< vehicle rates setpoint */
-	struct manual_control_setpoint_s _manual_control_sp; /**< manual control setpoint */
-	struct vehicle_control_mode_s _v_control_mode; /**< vehicle control mode */
-	struct actuator_controls_s _actuators; /**< actuator controls */
-	struct actuator_armed_s _armed; /**< actuator arming status */
+	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_control_mode) *_v_control_mode;	    /**< vehicle control mode */
+	px4::PX4_SUBSCRIBER(actuator_armed) *_armed;			    /**< actuator arming status */
+
+	PX4_TOPIC_T(vehicle_attitude_setpoint)	_v_att_sp;	/**< vehicle attitude setpoint */
+	PX4_TOPIC_T(vehicle_rates_setpoint)	_v_rates_sp;	/**< vehicle rates setpoint */
+	PX4_TOPIC_T(actuator_controls)		_actuators;	/**< actuator controls */
 
 	math::Vector<3> _rates_prev; /**< angular rates on previous step */
 	math::Vector<3> _rates_sp; /**< angular rates setpoint */