1 files changed, 39 insertions, 39 deletions

diff --git a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
index aff59778e..0de832df9 100644
--- a/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
+++ b/src/modules/mc_att_control_multiplatform/mc_att_control_base.cpp
@@ -92,28 +92,28 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 	_publish_att_sp = false;
 
 
-	if (_v_control_mode->get().flag_control_manual_enabled) {
+	if (_v_control_mode->data().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) {
+		if (_v_control_mode->data().flag_control_velocity_enabled
+		    || _v_control_mode->data().flag_control_climb_rate_enabled) {
 			/* in assisted modes poll 'vehicle_attitude_setpoint' topic and modify it */
-			memcpy(&_v_att_sp_mod, _v_att_sp->get_void_ptr(), sizeof(_v_att_sp_mod));
+			memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod));
 		}
 
-		if (!_v_control_mode->get().flag_control_climb_rate_enabled) {
+		if (!_v_control_mode->data().flag_control_climb_rate_enabled) {
 			/* pass throttle directly if not in altitude stabilized mode */
-			_v_att_sp_mod.thrust = _manual_control_sp->get().z;
+			_v_att_sp_mod.data().thrust = _manual_control_sp->data().z;
 			_publish_att_sp = true;
 		}
 
-		if (!_armed->get().armed) {
+		if (!_armed->data().armed) {
 			/* reset yaw setpoint when disarmed */
 			_reset_yaw_sp = true;
 		}
 
 		/* move yaw setpoint in all modes */
-		if (_v_att_sp_mod.thrust < 0.1f) {
+		if (_v_att_sp_mod.data().thrust < 0.1f) {
 			// TODO
 			//if (_status.condition_landed) {
 			/* reset yaw setpoint if on ground */
@@ -121,71 +121,71 @@ 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_mod.yaw_body = _wrap_pi(
-						     _v_att_sp_mod.yaw_body + yaw_sp_move_rate * dt);
+			yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max;
+			_v_att_sp_mod.data().yaw_body = _wrap_pi(
+						     _v_att_sp_mod.data().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_mod.yaw_body - _v_att->get().yaw);
+			float yaw_offs = _wrap_pi(_v_att_sp_mod.data().yaw_body - _v_att->data().yaw);
 
 			if (yaw_offs < -yaw_offs_max) {
-				_v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().yaw - yaw_offs_max);
+				_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw - yaw_offs_max);
 
 			} else if (yaw_offs > yaw_offs_max) {
-				_v_att_sp_mod.yaw_body = _wrap_pi(_v_att->get().yaw + yaw_offs_max);
+				_v_att_sp_mod.data().yaw_body = _wrap_pi(_v_att->data().yaw + yaw_offs_max);
 			}
 
-			_v_att_sp_mod.R_valid = false;
+			_v_att_sp_mod.data().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_mod.yaw_body = _v_att->get().yaw;
-			_v_att_sp_mod.R_valid = false;
+			_v_att_sp_mod.data().yaw_body = _v_att->data().yaw;
+			_v_att_sp_mod.data().R_valid = false;
 			// _publish_att_sp = true;
 		}
 
-		if (!_v_control_mode->get().flag_control_velocity_enabled) {
+		if (!_v_control_mode->data().flag_control_velocity_enabled) {
 			/* update attitude setpoint if not in position control mode */
-			_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
+			_v_att_sp_mod.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max;
+			_v_att_sp_mod.data().pitch_body = -_manual_control_sp->data().x
 					       * _params.man_pitch_max;
-			_v_att_sp_mod.R_valid = false;
+			_v_att_sp_mod.data().R_valid = false;
 			// _publish_att_sp = true;
 		}
 
 	} else {
 		/* in non-manual mode use 'vehicle_attitude_setpoint' topic */
-		memcpy(&_v_att_sp_mod, _v_att_sp->get_void_ptr(), sizeof(_v_att_sp_mod));
+		memcpy(&_v_att_sp_mod, &_v_att_sp->data(), sizeof(_v_att_sp_mod));
 
 		/* reset yaw setpoint after non-manual control mode */
 		_reset_yaw_sp = true;
 	}
 
-	_thrust_sp = _v_att_sp_mod.thrust;
+	_thrust_sp = _v_att_sp_mod.data().thrust;
 
 	/* construct attitude setpoint rotation matrix */
 	math::Matrix<3, 3> R_sp;
 
-	if (_v_att_sp_mod.R_valid) {
+	if (_v_att_sp_mod.data().R_valid) {
 		/* rotation matrix in _att_sp is valid, use it */
-		R_sp.set(&_v_att_sp_mod.R_body[0]);
+		R_sp.set(&_v_att_sp_mod.data().R_body[0]);
 
 	} else {
 		/* rotation matrix in _att_sp is not valid, use euler angles instead */
-		R_sp.from_euler(_v_att_sp_mod.roll_body, _v_att_sp_mod.pitch_body,
-				_v_att_sp_mod.yaw_body);
+		R_sp.from_euler(_v_att_sp_mod.data().roll_body, _v_att_sp_mod.data().pitch_body,
+				_v_att_sp_mod.data().yaw_body);
 
 		/* copy rotation matrix back to setpoint struct */
-		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;
+		memcpy(&_v_att_sp_mod.data().R_body[0], &R_sp.data[0][0],
+		       sizeof(_v_att_sp_mod.data().R_body));
+		_v_att_sp_mod.data().R_valid = true;
 	}
 
 	/* rotation matrix for current state */
 	math::Matrix<3, 3> R;
-	R.set(_v_att->get().R);
+	R.set(_v_att->data().R);
 
 	/* all input data is ready, run controller itself */
 
@@ -266,15 +266,15 @@ void MulticopterAttitudeControlBase::control_attitude(float dt)
 void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
 {
 	/* reset integral if disarmed */
-	if (!_armed->get().armed || !_v_status->get().is_rotary_wing) {
+	if (!_armed->data().armed || !_v_status->data().is_rotary_wing) {
 		_rates_int.zero();
 	}
 
 	/* current body angular rates */
 	math::Vector < 3 > rates;
-	rates(0) = _v_att->get().rollspeed;
-	rates(1) = _v_att->get().pitchspeed;
-	rates(2) = _v_att->get().yawspeed;
+	rates(0) = _v_att->data().rollspeed;
+	rates(1) = _v_att->data().pitchspeed;
+	rates(2) = _v_att->data().yawspeed;
 
 	/* angular rates error */
 	math::Vector < 3 > rates_err = _rates_sp - rates;
@@ -302,8 +302,8 @@ void MulticopterAttitudeControlBase::control_attitude_rates(float dt)
 
 void MulticopterAttitudeControlBase::set_actuator_controls()
 {
-	_actuators.control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
-	_actuators.control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
-	_actuators.control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
-	_actuators.control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
+	_actuators.data().control[0] = (isfinite(_att_control(0))) ? _att_control(0) : 0.0f;
+	_actuators.data().control[1] = (isfinite(_att_control(1))) ? _att_control(1) : 0.0f;
+	_actuators.data().control[2] = (isfinite(_att_control(2))) ? _att_control(2) : 0.0f;
+	_actuators.data().control[3] = (isfinite(_thrust_sp)) ? _thrust_sp : 0.0f;
 }