multiplatform port of #1741 and #1801: port attitude setpoint generation in manual to mc_pos_control_multiplatform

2 files changed, 78 insertions, 15 deletions

diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
index 1c0298a07..303123202 100644
--- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.cpp
@@ -41,6 +41,9 @@
 
 #include "mc_pos_control.h"
 #include "mc_pos_control_params.h"
+/* The following inclue is needed because the pos controller depens on a parameter from attitude control to set a
+ * reasonable yaw setpoint in manual mode */
+#include <mc_att_control_multiplatform/mc_att_control_params.h>
 
 #define TILT_COS_MAX	0.7f
 #define SIGMA		0.000001f
@@ -83,9 +86,10 @@ MulticopterPositionControl::MulticopterPositionControl() :
 		.tilt_max_air	    = px4::ParameterFloat("MPC_TILTMAX_AIR", PARAM_MPC_TILTMAX_AIR_DEFAULT),
 		.land_speed	    = px4::ParameterFloat("MPC_LAND_SPEED", PARAM_MPC_LAND_SPEED_DEFAULT),
 		.tilt_max_land	    = px4::ParameterFloat("MPC_TILTMAX_LND", PARAM_MPC_TILTMAX_LND_DEFAULT),
-		.man_roll_max	    = px4::ParameterFloat("MC_MAN_R_MAX", PARAM_MPC_MAN_R_MAX_DEFAULT),
-		.man_pitch_max	    = px4::ParameterFloat("MC_MAN_P_MAX", PARAM_MPC_MAN_P_MAX_DEFAULT),
-		.man_yaw_max	    = px4::ParameterFloat("MC_MAN_Y_MAX", PARAM_MPC_MAN_Y_MAX_DEFAULT)
+		.man_roll_max	    = px4::ParameterFloat("MPC_MAN_R_MAX", PARAM_MPC_MAN_R_MAX_DEFAULT),
+		.man_pitch_max	    = px4::ParameterFloat("MPC_MAN_P_MAX", PARAM_MPC_MAN_P_MAX_DEFAULT),
+		.man_yaw_max	    = px4::ParameterFloat("MPC_MAN_Y_MAX", PARAM_MPC_MAN_Y_MAX_DEFAULT),
+		.mc_att_yaw_p	    = px4::ParameterFloat("MC_YAW_P", PARAM_MC_YAW_P_DEFAULT)
 	}),
 	_ref_alt(0.0f),
 	_ref_timestamp(0),
@@ -102,7 +106,6 @@ MulticopterPositionControl::MulticopterPositionControl() :
 	 * Do subscriptions
 	 */
 	_att = _n.subscribe<px4_vehicle_attitude>(&MulticopterPositionControl::handle_vehicle_attitude, this, 0);
-	_v_att_sp = _n.subscribe<px4_vehicle_attitude_setpoint>(0);
 	_control_mode = _n.subscribe<px4_vehicle_control_mode>(0);
 	_parameter_update = _n.subscribe<px4_parameter_update>(
 			&MulticopterPositionControl::handle_parameter_update, this, 1000);
@@ -154,6 +157,8 @@ MulticopterPositionControl::parameters_update()
 	_params.man_pitch_max = math::radians(_params_handles.man_pitch_max.update());
 	_params.man_yaw_max = math::radians(_params_handles.man_yaw_max.update());
 
+	_params.mc_att_yaw_p = _params_handles.mc_att_yaw_p.update();
+
 	_params.pos_p(0) = _params.pos_p(1) = _params_handles.xy_p.update();
 	_params.pos_p(2) = _params_handles.z_p.update();
 	_params.vel_p(0) = _params.vel_p(1) = _params_handles.xy_vel_p.update();
@@ -577,6 +582,7 @@ void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
 	static bool reset_int_z = true;
 	static bool reset_int_z_manual = false;
 	static bool reset_int_xy = true;
+	static bool reset_yaw_sp = true;
 	static bool was_armed = false;
 	static uint64_t t_prev = 0;
 
@@ -590,8 +596,10 @@ void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
 		_reset_alt_sp = true;
 		reset_int_z = true;
 		reset_int_xy = true;
+		reset_yaw_sp = true;
 	}
 
+	/* Update previous arming state */
 	was_armed = _control_mode->data().flag_armed;
 
 	update_ref();
@@ -975,6 +983,60 @@ void  MulticopterPositionControl::handle_vehicle_attitude(const px4_vehicle_atti
 		reset_int_xy = true;
 	}
 
+	/* generate attitude setpoint from manual controls */
+	if(_control_mode->data().flag_control_manual_enabled && _control_mode->data().flag_control_attitude_enabled) {
+
+		/* reset yaw setpoint to current position if needed */
+		if (reset_yaw_sp) {
+			reset_yaw_sp = false;
+			_att_sp_msg.data().yaw_body = _att->data().yaw;
+		}
+
+		// do not move yaw while arming
+		else if (_manual_control_sp->data().z > 0.1f)
+		{
+			const float YAW_OFFSET_MAX = _params.man_yaw_max / _params.mc_att_yaw_p;
+
+			_att_sp_msg.data().yaw_sp_move_rate = _manual_control_sp->data().r * _params.man_yaw_max;
+			_att_sp_msg.data().yaw_body = _wrap_pi(_att_sp_msg.data().yaw_body + _att_sp_msg.data().yaw_sp_move_rate * dt);
+			float yaw_offs = _wrap_pi(_att_sp_msg.data().yaw_body - _att->data().yaw);
+			if (yaw_offs < - YAW_OFFSET_MAX) {
+				_att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw - YAW_OFFSET_MAX);
+
+			} else if (yaw_offs > YAW_OFFSET_MAX) {
+				_att_sp_msg.data().yaw_body = _wrap_pi(_att->data().yaw + YAW_OFFSET_MAX);
+			}
+		}
+
+		/* Control roll and pitch directly if we no aiding velocity controller is active */
+		if(!_control_mode->data().flag_control_velocity_enabled) {
+			_att_sp_msg.data().roll_body = _manual_control_sp->data().y * _params.man_roll_max;
+			_att_sp_msg.data().pitch_body = -_manual_control_sp->data().x * _params.man_pitch_max;
+		}
+
+		/* Control climb rate directly if no aiding altitude controller is active */
+		if(!_control_mode->data().flag_control_climb_rate_enabled) {
+			_att_sp_msg.data().thrust = _manual_control_sp->data().z;
+		}
+
+		/* Construct attitude setpoint rotation matrix */
+		math::Matrix<3,3> R_sp;
+		R_sp.from_euler(_att_sp_msg.data().roll_body,_att_sp_msg.data().pitch_body,_att_sp_msg.data().yaw_body);
+		memcpy(&_att_sp_msg.data().R_body[0], R_sp.data, sizeof(_att_sp_msg.data().R_body));
+		_att_sp_msg.data().timestamp = get_time_micros();
+	}
+	else {
+		reset_yaw_sp = true;
+	}
+
+	/* publish attitude setpoint */
+	if (_att_sp_pub != nullptr) {
+		_att_sp_pub->publish(_att_sp_msg);
+
+	} else {
+		_att_sp_pub = _n.advertise<px4_vehicle_attitude_setpoint>();
+	}
+
 	/* reset altitude controller integral (hovering throttle) to manual throttle after manual throttle control */
 	reset_int_z_manual = _control_mode->data().flag_armed && _control_mode->data().flag_control_manual_enabled && !_control_mode->data().flag_control_climb_rate_enabled;
 }
diff --git a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h
index 59b663b9f..54c6de155 100644
--- a/src/modules/mc_pos_control_multiplatform/mc_pos_control.h
+++ b/src/modules/mc_pos_control_multiplatform/mc_pos_control.h
@@ -88,19 +88,18 @@ protected:
 
 	Publisher<px4_vehicle_attitude_setpoint>	*_att_sp_pub;			/**< attitude setpoint publication */
 	Publisher<px4_vehicle_local_position_setpoint>	*_local_pos_sp_pub;		/**< vehicle local position setpoint publication */
-	Publisher<px4_vehicle_global_velocity_setpoint>		*_global_vel_sp_pub;		/**< vehicle global velocity setpoint publication */
+	Publisher<px4_vehicle_global_velocity_setpoint>	*_global_vel_sp_pub;		/**< vehicle global velocity setpoint publication */
 
 
-	Subscriber<px4_vehicle_attitude> *_att;			    /**< vehicle attitude */
-	Subscriber<px4_vehicle_attitude_setpoint> *_v_att_sp;	    /**< vehicle attitude setpoint */
-	Subscriber<px4_vehicle_control_mode> *_control_mode;	    /**< vehicle control mode */
-	Subscriber<px4_parameter_update> *_parameter_update;	    /**< parameter update */
-	Subscriber<px4_manual_control_setpoint> *_manual_control_sp;   /**< manual control setpoint */
-	Subscriber<px4_actuator_armed> *_armed;			    /**< actuator arming status */
-	Subscriber<px4_vehicle_local_position> *_local_pos;			    /**< local position */
-	Subscriber<px4_position_setpoint_triplet> *_pos_sp_triplet;			    /**< local position */
-	Subscriber<px4_vehicle_local_position_setpoint> *_local_pos_sp;			    /**< local position */
-	Subscriber<px4_vehicle_global_velocity_setpoint> *_global_vel_sp;			    /**< local position */
+	Subscriber<px4_vehicle_attitude> *_att;				    /**< vehicle attitude */
+	Subscriber<px4_vehicle_control_mode> *_control_mode;		    /**< vehicle control mode */
+	Subscriber<px4_parameter_update> *_parameter_update;		    /**< parameter update */
+	Subscriber<px4_manual_control_setpoint> *_manual_control_sp;	    /**< manual control setpoint */
+	Subscriber<px4_actuator_armed> *_armed;				    /**< actuator arming status */
+	Subscriber<px4_vehicle_local_position> *_local_pos;		    /**< local position */
+	Subscriber<px4_position_setpoint_triplet> *_pos_sp_triplet;	    /**< local position */
+	Subscriber<px4_vehicle_local_position_setpoint> *_local_pos_sp;	    /**< local position */
+	Subscriber<px4_vehicle_global_velocity_setpoint> *_global_vel_sp;   /**< local position */
 
 	px4_vehicle_attitude_setpoint _att_sp_msg;
 	px4_vehicle_local_position_setpoint _local_pos_sp_msg;
@@ -129,6 +128,7 @@ protected:
 		px4::ParameterFloat man_roll_max;
 		px4::ParameterFloat man_pitch_max;
 		px4::ParameterFloat man_yaw_max;
+		px4::ParameterFloat mc_att_yaw_p;   // needed for calculating reasonable attitude setpoints in manual
 	}		_params_handles;		/**< handles for interesting parameters */
 
 	struct {
@@ -140,6 +140,7 @@ protected:
 		float man_roll_max;
 		float man_pitch_max;
 		float man_yaw_max;
+		float mc_att_yaw_p;
 
 		math::Vector<3> pos_p;
 		math::Vector<3> vel_p;