mc att: correctly handle topics which are simultaneously subscribed and published

3 files changed, 51 insertions, 51 deletions

diff --git a/src/modules/mc_att_control/mc_att_control.cpp b/src/modules/mc_att_control/mc_att_control.cpp
index 59eaaec6d..13bff9561 100644
--- a/src/modules/mc_att_control/mc_att_control.cpp
+++ b/src/modules/mc_att_control/mc_att_control.cpp
@@ -219,10 +219,10 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 
 		/* publish the attitude setpoint if needed */
 		if (_publish_att_sp) {
-			_v_att_sp.timestamp = hrt_absolute_time();
+			_v_att_sp_mod.timestamp = hrt_absolute_time();
 
 			if (_att_sp_pub != nullptr) {
-				_att_sp_pub->publish(_v_att_sp);
+				_att_sp_pub->publish(_v_att_sp_mod);
 
 			} else {
 				_att_sp_pub = PX4_ADVERTISE(_n, vehicle_attitude_setpoint);
@@ -230,15 +230,14 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 		}
 
 		/* publish attitude rates setpoint */
-		_v_rates_sp.roll = _rates_sp(0);
-		_v_rates_sp.pitch = _rates_sp(1);
-		_v_rates_sp.yaw = _rates_sp(2);
-		_v_rates_sp.thrust = _thrust_sp;
-		_v_rates_sp.timestamp = hrt_absolute_time();
+		_v_rates_sp_mod.roll = _rates_sp(0);
+		_v_rates_sp_mod.pitch = _rates_sp(1);
+		_v_rates_sp_mod.yaw = _rates_sp(2);
+		_v_rates_sp_mod.thrust = _thrust_sp;
+		_v_rates_sp_mod.timestamp = hrt_absolute_time();
 
 		if (_v_rates_sp_pub != nullptr) {
-			_v_rates_sp_pub->publish(_v_rates_sp);
-
+			_v_rates_sp_pub->publish(_v_rates_sp_mod);
 		} else {
 			_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
 		}
@@ -255,14 +254,14 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 			_reset_yaw_sp = true;
 
 			/* publish attitude rates setpoint */
-			_v_rates_sp.roll = _rates_sp(0);
-			_v_rates_sp.pitch = _rates_sp(1);
-			_v_rates_sp.yaw = _rates_sp(2);
-			_v_rates_sp.thrust = _thrust_sp;
-			_v_rates_sp.timestamp = hrt_absolute_time();
+			_v_rates_sp_mod.roll = _rates_sp(0);
+			_v_rates_sp_mod.pitch = _rates_sp(1);
+			_v_rates_sp_mod.yaw = _rates_sp(2);
+			_v_rates_sp_mod.thrust = _thrust_sp;
+			_v_rates_sp_mod.timestamp = hrt_absolute_time();
 
 			if (_v_rates_sp_pub != nullptr) {
-				_v_rates_sp_pub->publish(_v_rates_sp);
+				_v_rates_sp_pub->publish(_v_rates_sp_mod);
 
 			} else {
 				_v_rates_sp_pub = PX4_ADVERTISE(_n, vehicle_rates_setpoint);
@@ -271,10 +270,10 @@ void  MulticopterAttitudeControl::handle_vehicle_attitude(const PX4_TOPIC_T(vehi
 		} else {
 			/* attitude controller disabled, poll rates setpoint topic */
 			//XXX vehicle_rates_setpoint_poll();
-			_rates_sp(0) = _v_rates_sp.roll;
-			_rates_sp(1) = _v_rates_sp.pitch;
-			_rates_sp(2) = _v_rates_sp.yaw;
-			_thrust_sp = _v_rates_sp.thrust;
+			_rates_sp(0) = _v_rates_sp->get().roll;
+			_rates_sp(1) = _v_rates_sp->get().pitch;
+			_rates_sp(2) = _v_rates_sp->get().yaw;
+			_thrust_sp = _v_rates_sp->get().thrust;
 		}
 	}
 
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 b9a57ce69..871f93ab8 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,9 @@ 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_rates_sp, 0, sizeof(_v_att_sp_mod));
+	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp_mod));
+	memset(&_actuators, 0, sizeof(_actuators));
 
 	_params.att_p.zero();
 	_params.rate_p.zero();
@@ -95,18 +91,19 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 	float yaw_sp_move_rate = 0.0f;
 	_publish_att_sp = false;
 
+
 	if (_v_control_mode->get().flag_control_manual_enabled) {
 		/* manual input, set or modify attitude setpoint */
 
 		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();
+			_v_att_sp_mod = _v_att_sp->get();
 		}
 
 		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->get().z;
+			_v_att_sp_mod.thrust = _manual_control_sp->get().z;
 			_publish_att_sp = true;
 		}
 
@@ -116,7 +113,7 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 		}
 
 		/* move yaw setpoint in all modes */
-		if (_v_att_sp.thrust < 0.1f) {
+		if (_v_att_sp_mod.thrust < 0.1f) {
 			// TODO
 			//if (_status.condition_landed) {
 			/* reset yaw setpoint if on ground */
@@ -125,65 +122,65 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 		} else {
 			/* move yaw setpoint */
 			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);
+			_v_att_sp_mod.yaw_body = _wrap_pi(
+						     _v_att_sp_mod.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->get().yaw);
+			float yaw_offs = _wrap_pi(_v_att_sp_mod.yaw_body - _v_att->get().yaw);
 
 			if (yaw_offs < -yaw_offs_max) {
-				_v_att_sp.yaw_body = _wrap_pi(_v_att->get().yaw - yaw_offs_max);
+				_v_att_sp_mod.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->get().yaw + yaw_offs_max);
+				_v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().yaw + yaw_offs_max);
 			}
 
-			_v_att_sp.R_valid = false;
+			_v_att_sp_mod.R_valid = false;
 			_publish_att_sp = true;
 		}
 
 		/* reset yaw setpint to current position if needed */
 		if (_reset_yaw_sp) {
 			_reset_yaw_sp = false;
-			_v_att_sp.yaw_body = _v_att->get().yaw;
-			_v_att_sp.R_valid = false;
+			_v_att_sp_mod.yaw_body = _v_att->get().yaw;
+			_v_att_sp_mod.R_valid = false;
 			_publish_att_sp = true;
 		}
 
 		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->get().y * _params.man_roll_max;
-			_v_att_sp.pitch_body = -_manual_control_sp->get().x
+			_v_att_sp_mod.roll_body = _manual_control_sp->get().y * _params.man_roll_max;
+			_v_att_sp_mod.pitch_body = -_manual_control_sp->get().x
 					       * _params.man_pitch_max;
-			_v_att_sp.R_valid = false;
+			_v_att_sp_mod.R_valid = false;
 			_publish_att_sp = true;
 		}
 
 	} else {
 		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
-		//XXX vehicle_attitude_setpoint_poll();
+		_v_att_sp_mod = _v_att_sp->get();
 
 		/* reset yaw setpoint after non-manual control mode */
 		_reset_yaw_sp = true;
 	}
 
-	_thrust_sp = _v_att_sp.thrust;
+	_thrust_sp = _v_att_sp_mod.thrust;
 
 	/* construct attitude setpoint rotation matrix */
 	math::Matrix<3, 3> R_sp;
 
-	if (_v_att_sp.R_valid) {
+	if (_v_att_sp_mod.R_valid) {
 		/* rotation matrix in _att_sp is valid, use it */
-		R_sp.set(&_v_att_sp.R_body[0]);
+		R_sp.set(&_v_att_sp_mod.R_body[0]);
 
 	} else {
 		/* rotation matrix in _att_sp is not valid, use euler angles instead */
-		R_sp.from_euler(_v_att_sp.roll_body, _v_att_sp.pitch_body,
-				_v_att_sp.yaw_body);
+		R_sp.from_euler(_v_att_sp_mod.roll_body, _v_att_sp_mod.pitch_body,
+				_v_att_sp_mod.yaw_body);
 
 		/* copy rotation matrix back to setpoint struct */
-		memcpy(&_v_att_sp.R_body[0], &R_sp.data[0][0],
-		       sizeof(_v_att_sp.R_body));
-		_v_att_sp.R_valid = true;
+		memcpy(&_v_att_sp_mod.R_body[0], &R_sp.data[0][0],
+		       sizeof(_v_att_sp_mod.R_body));
+		_v_att_sp_mod.R_valid = true;
 	}
 
 	/* rotation matrix for current state */
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 05f89abe6..2135b61d8 100644
--- a/src/modules/mc_att_control/mc_att_control_base.h
+++ b/src/modules/mc_att_control/mc_att_control_base.h
@@ -90,9 +90,13 @@ protected:
 	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::PX4_SUBSCRIBER(vehicle_attitude_setpoint) * _v_att_sp;	/**< vehicle attitude setpoint */
+	px4::PX4_SUBSCRIBER(vehicle_rates_setpoint) * _v_rates_sp;	/**< vehicle rates setpoint */
 
-	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(vehicle_attitude_setpoint)	_v_att_sp_mod;	/**< modified vehicle attitude setpoint
+								  that gets published eventually */
+	PX4_TOPIC_T(vehicle_rates_setpoint)	_v_rates_sp_mod;	/**< vehicle rates setpoint
+								  that gets published eventually*/
 	PX4_TOPIC_T(actuator_controls)		_actuators;	/**< actuator controls */
 
 	math::Vector<3> _rates_prev; /**< angular rates on previous step */