use virtual attitude setpoint

Conflicts:
	src/modules/vtol_att_control/tailsitter.cpp
	src/modules/vtol_att_control/tiltrotor.cpp
	src/modules/vtol_att_control/vtol_att_control_main.cpp
	src/modules/vtol_att_control/vtol_att_control_main.h
	src/modules/vtol_att_control/vtol_type.h

msg/fw_virtual_attitude_setpoint.msg

@@ -0,0 +1,23 @@
+
+uint64 timestamp				# in microseconds since system start, is set whenever the writing thread stores new data
+
+float32 roll_body				# body angle in NED frame
+float32 pitch_body				# body angle in NED frame
+float32 yaw_body				# body angle in NED frame
+
+float32 yaw_sp_move_rate		# rad/s (commanded by user)
+
+float32[9] R_body				# Rotation matrix describing the setpoint as rotation from the current body frame
+bool R_valid					# Set to true if rotation matrix is valid
+
+# For quaternion-based attitude control
+float32[4] q_d					# Desired quaternion for quaternion control
+bool q_d_valid					# Set to true if quaternion vector is valid
+float32[4] q_e					# Attitude error in quaternion
+bool q_e_valid					# Set to true if quaternion error vector is valid
+
+float32 thrust					# Thrust in Newton the power system should generate
+
+bool roll_reset_integral			# Reset roll integral part (navigation logic change)
+bool pitch_reset_integral			# Reset pitch integral part (navigation logic change)
+bool yaw_reset_integral				# Reset yaw integral part (navigation logic change)

msg/mc_virtual_attitude_setpoint.msg

@@ -0,0 +1,23 @@
+
+uint64 timestamp				# in microseconds since system start, is set whenever the writing thread stores new data
+
+float32 roll_body				# body angle in NED frame
+float32 pitch_body				# body angle in NED frame
+float32 yaw_body				# body angle in NED frame
+
+float32 yaw_sp_move_rate		# rad/s (commanded by user)
+
+float32[9] R_body				# Rotation matrix describing the setpoint as rotation from the current body frame
+bool R_valid					# Set to true if rotation matrix is valid
+
+# For quaternion-based attitude control
+float32[4] q_d					# Desired quaternion for quaternion control
+bool q_d_valid					# Set to true if quaternion vector is valid
+float32[4] q_e					# Attitude error in quaternion
+bool q_e_valid					# Set to true if quaternion error vector is valid
+
+float32 thrust					# Thrust in Newton the power system should generate
+
+bool roll_reset_integral			# Reset roll integral part (navigation logic change)
+bool pitch_reset_integral			# Reset pitch integral part (navigation logic change)
+bool yaw_reset_integral				# Reset yaw integral part (navigation logic change)

src/modules/uORB/objects_common.cpp

@@ -164,8 +164,12 @@ ORB_DEFINE(fence_vertex, struct fence_vertex_s);
 
 #include "topics/vehicle_attitude_setpoint.h"
 ORB_DEFINE(vehicle_attitude_setpoint, struct vehicle_attitude_setpoint_s);
-ORB_DEFINE(mc_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s);
-ORB_DEFINE(fw_virtual_attitude_setpoint, struct vehicle_attitude_setpoint_s);
+
+#include "topics/mc_virtual_attitude_setpoint.h"
+ORB_DEFINE(mc_virtual_attitude_setpoint, struct mc_virtual_attitude_setpoint_s);
+
+#include "topics/fw_virtual_attitude_setpoint.h"
+ORB_DEFINE(fw_virtual_attitude_setpoint, struct fw_virtual_attitude_setpoint_s);
 
 #include "topics/manual_control_setpoint.h"
 ORB_DEFINE(manual_control_setpoint, struct manual_control_setpoint_s);

src/modules/uORB/uORB.h

@@ -366,7 +366,6 @@ __END_DECLS
 
 /* Diverse uORB header defines */ //XXX: move to better location
 #define ORB_ID_VEHICLE_ATTITUDE_CONTROLS    ORB_ID(actuator_controls_0)
-typedef struct vehicle_attitude_setpoint_s fw_virtual_attitude_setpoint_s;
 typedef uint8_t arming_state_t;
 typedef uint8_t main_state_t;
 typedef uint8_t hil_state_t;

src/modules/vtol_att_control/standard.cpp

@@ -186,6 +186,9 @@ void Standard::update_vtol_state()
 
 void Standard::update_transition_state()
 {
+	// copy virtual attitude setpoint to real attitude setpoint
+	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+
 	if (_vtol_schedule.flight_mode == TRANSITION_TO_FW) {
 		if (_params_standard.front_trans_dur <= 0.0f) {
 			// just set the final target throttle value
@@ -242,12 +245,16 @@ void Standard::update_transition_state()
 
 void Standard::update_mc_state()
 {
-	// do nothing
+	// copy virtual attitude setpoint to real attitude setpoint
+	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 }
 
 void Standard::update_fw_state()
 {
-	// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again
+	// copy virtual attitude setpoint to real attitude setpoint
+	memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+
+	// in fw mode we need the multirotor motors to stop spinning, in backtransition mode we let them spin up again	
 	if (!_flag_enable_mc_motors) {
 		set_max_mc(950);
 		set_idle_fw();  // force them to stop, not just idle

src/modules/vtol_att_control/tailsitter.cpp

@@ -231,6 +231,9 @@ void Tailsitter::update_mc_state()
 	_mc_roll_weight = 1.0f;
 	_mc_pitch_weight = 1.0f;
 	_mc_yaw_weight = 1.0f;
+
+	// copy virtual attitude setpoint to real attitude setpoint
+	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 }
 
 void Tailsitter::update_fw_state()
@@ -243,6 +246,9 @@ void Tailsitter::update_fw_state()
 	_mc_roll_weight = 0.0f;
 	_mc_pitch_weight = 0.0f;
 	_mc_yaw_weight = 0.0f;
+
+	// copy virtual attitude setpoint to real attitude setpoint
+	memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 }
 
 void Tailsitter::update_transition_state()

src/modules/vtol_att_control/tiltrotor.cpp

@@ -252,6 +252,9 @@ void Tiltrotor::update_vtol_state()
 
 void Tiltrotor::update_mc_state()
 {
+	// copy virtual attitude setpoint to real attitude setpoint
+	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+
 	// make sure motors are not tilted
 	_tilt_control = _params_tiltrotor.tilt_mc;
 
@@ -273,6 +276,9 @@ void Tiltrotor::update_mc_state()
 
 void Tiltrotor::update_fw_state()
 {
+	// copy virtual attitude setpoint to real attitude setpoint
+	memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+
 	// make sure motors are tilted forward
 	_tilt_control = _params_tiltrotor.tilt_fw;
 
@@ -363,22 +369,8 @@ void Tiltrotor::update_transition_state()
 	_mc_roll_weight = math::constrain(_mc_roll_weight, 0.0f, 1.0f);
 	_mc_yaw_weight = math::constrain(_mc_yaw_weight, 0.0f, 1.0f);
 
-	// compute desired attitude and thrust setpoint for the transition
-	_v_att_sp->timestamp = hrt_absolute_time();
-	_v_att_sp->roll_body = 0;
-	_v_att_sp->pitch_body = 0;
-	_v_att_sp->yaw_body = _yaw_transition;
-	_v_att_sp->thrust = _throttle_transition;
-
-	math::Matrix<3,3> R_sp;
-	R_sp.from_euler(_v_att_sp->roll_body,_v_att_sp->pitch_body,_v_att_sp->yaw_body);
-	memcpy(&_v_att_sp->R_body[0], R_sp.data, sizeof(_v_att_sp->R_body));
-	_v_att_sp->R_valid = true;
-
-	math::Quaternion q_sp;
-	q_sp.from_dcm(R_sp);
-	_v_att_sp->q_d_valid = true;
-	memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d));
+	// copy virtual attitude setpoint to real attitude setpoint (we use multicopter att sp)
+	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 }
 
 void Tiltrotor::update_external_state()

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -60,7 +60,9 @@ VtolAttitudeControl::VtolAttitudeControl() :
 
 	//init subscription handlers
 	_v_att_sub(-1),
-	_v_att_sp_sub(-1), 
+	_v_att_sp_sub(-1),
+	_mc_virtual_att_sp_sub(-1),
+	_fw_virtual_att_sp_sub(-1),
 	_mc_virtual_v_rates_sp_sub(-1),
 	_fw_virtual_v_rates_sp_sub(-1),
 	_v_control_mode_sub(-1),
@@ -84,6 +86,8 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_vtol_vehicle_status.vtol_in_rw_mode = true;	/* start vtol in rotary wing mode*/
 	memset(&_v_att, 0, sizeof(_v_att));
 	memset(&_v_att_sp, 0, sizeof(_v_att_sp));
+	memset(&_mc_virtual_att_sp, 0, sizeof(_mc_virtual_att_sp));
+	memset(&_fw_virtual_att_sp, 0, sizeof(_fw_virtual_att_sp));
 	memset(&_v_rates_sp, 0, sizeof(_v_rates_sp));
 	memset(&_mc_virtual_v_rates_sp, 0, sizeof(_mc_virtual_v_rates_sp));
 	memset(&_fw_virtual_v_rates_sp, 0, sizeof(_fw_virtual_v_rates_sp));
@@ -351,6 +355,38 @@ VtolAttitudeControl::vehicle_local_pos_poll()
 
 }
 
+/**
+* Check for mc virtual attitude setpoint updates.
+*/
+void
+VtolAttitudeControl::mc_virtual_att_sp_poll()
+{
+	bool updated;
+
+	orb_check(_mc_virtual_att_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(mc_virtual_attitude_setpoint), _mc_virtual_att_sp_sub , &_mc_virtual_att_sp);
+	}
+
+}
+
+/**
+* Check for fw virtual attitude setpoint updates.
+*/
+void
+VtolAttitudeControl::fw_virtual_att_sp_poll()
+{
+	bool updated;
+
+	orb_check(_fw_virtual_att_sp_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(fw_virtual_attitude_setpoint), _fw_virtual_att_sp_sub , &_fw_virtual_att_sp);
+	}
+
+}
+
 /**
 * Check for command updates.
 */
@@ -471,7 +507,7 @@ void VtolAttitudeControl::fill_fw_att_rates_sp()
 	_v_rates_sp.thrust 	= _fw_virtual_v_rates_sp.thrust;
 }
 
-void VtolAttitudeControl::publish_transition_att_sp()
+void VtolAttitudeControl::publish_att_sp()
 {
 	if (_v_att_sp_pub != nullptr) {
 		/* publish the attitude setpoint */
@@ -494,6 +530,9 @@ void VtolAttitudeControl::task_main()
 	fflush(stdout);
 
 	/* do subscriptions */
+	_v_att_sp_sub          = orb_subscribe(ORB_ID(vehicle_attitude_setpoint));
+	_mc_virtual_att_sp_sub = orb_subscribe(ORB_ID(mc_virtual_attitude_setpoint));
+	_fw_virtual_att_sp_sub = orb_subscribe(ORB_ID(fw_virtual_attitude_setpoint));
 	_mc_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(mc_virtual_rates_setpoint));
 	_fw_virtual_v_rates_sp_sub = orb_subscribe(ORB_ID(fw_virtual_rates_setpoint));
 	_v_att_sub             = orb_subscribe(ORB_ID(vehicle_attitude));
@@ -566,6 +605,8 @@ void VtolAttitudeControl::task_main()
 
 		_vtol_vehicle_status.fw_permanent_stab = _params.vtol_fw_permanent_stab == 1 ? true : false;
 
+		mc_virtual_att_sp_poll();
+		fw_virtual_att_sp_poll();
 		vehicle_control_mode_poll();	//Check for changes in vehicle control mode.
 		vehicle_manual_poll();			//Check for changes in manual inputs.
 		arming_status_poll();			//Check for arming status updates.
@@ -645,7 +686,7 @@ void VtolAttitudeControl::task_main()
 			if (got_new_data) {
 				_vtol_type->update_transition_state();
 				fill_mc_att_rates_sp();
-				publish_transition_att_sp();
+				publish_att_sp();
 			}
 
 		} else if (_vtol_type->get_mode() == EXTERNAL) {
@@ -653,6 +694,7 @@ void VtolAttitudeControl::task_main()
 			_vtol_type->update_external_state();
 		}
 
+		publish_att_sp();
 		_vtol_type->fill_actuator_outputs();
 
 		/* Only publish if the proper mode(s) are enabled */

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -63,6 +63,8 @@
 #include <drivers/drv_pwm_output.h>
 #include <uORB/uORB.h>
 #include <uORB/topics/vehicle_attitude_setpoint.h>
+#include <uORB/topics/mc_virtual_attitude_setpoint.h>
+#include <uORB/topics/fw_virtual_attitude_setpoint.h>
 #include <uORB/topics/manual_control_setpoint.h>
 #include <uORB/topics/actuator_controls.h>
 #include <uORB/topics/actuator_controls_virtual_mc.h>
@@ -107,6 +109,8 @@ public:
 
 	struct vehicle_attitude_s 			*get_att() {return &_v_att;}
 	struct vehicle_attitude_setpoint_s *get_att_sp() {return &_v_att_sp;}
+	struct mc_virtual_attitude_setpoint_s 	*get_mc_virtual_att_sp () {return &_mc_virtual_att_sp;}
+	struct fw_virtual_attitude_setpoint_s 	*get_fw_virtual_att_sp () {return &_fw_virtual_att_sp;}
 	struct vehicle_rates_setpoint_s 	*get_rates_sp() {return &_v_rates_sp;}
 	struct vehicle_rates_setpoint_s 	*get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;}
 	struct vehicle_rates_setpoint_s 	*get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;}
@@ -124,7 +128,6 @@ public:
 
 	struct Params 						*get_params() {return &_params;}
 
-
 private:
 //******************flags & handlers******************************************************
 	bool _task_should_exit;
@@ -133,6 +136,8 @@ private:
 	/* handlers for subscriptions */
 	int		_v_att_sub;				//vehicle attitude subscription
 	int		_v_att_sp_sub;			//vehicle attitude setpoint subscription
+	int 	_mc_virtual_att_sp_sub;
+	int 	_fw_virtual_att_sp_sub;
 	int		_mc_virtual_v_rates_sp_sub;		//vehicle rates setpoint subscription
 	int		_fw_virtual_v_rates_sp_sub;		//vehicle rates setpoint subscription
 	int		_v_control_mode_sub;	//vehicle control mode subscription
@@ -156,6 +161,8 @@ private:
 //*******************data containers***********************************************************
 	struct vehicle_attitude_s			_v_att;				//vehicle attitude
 	struct vehicle_attitude_setpoint_s	_v_att_sp;			//vehicle attitude setpoint
+	struct mc_virtual_attitude_setpoint_s _mc_virtual_att_sp;	// virtual mc attitude setpoint
+	struct fw_virtual_attitude_setpoint_s _fw_virtual_att_sp;	// virtual fw attitude setpoint
 	struct vehicle_rates_setpoint_s		_v_rates_sp;		//vehicle rates setpoint
 	struct vehicle_rates_setpoint_s		_mc_virtual_v_rates_sp;		// virtual mc vehicle rates setpoint
 	struct vehicle_rates_setpoint_s		_fw_virtual_v_rates_sp;		// virtual fw vehicle rates setpoint
@@ -204,6 +211,8 @@ private:
 	void		vehicle_control_mode_poll();	//Check for changes in vehicle control mode.
 	void		vehicle_manual_poll();			//Check for changes in manual inputs.
 	void		arming_status_poll();			//Check for arming status updates.
+	void 		mc_virtual_att_sp_poll();
+	void 		fw_virtual_att_sp_poll();
 	void 		actuator_controls_mc_poll();	//Check for changes in mc_attitude_control output
 	void 		actuator_controls_fw_poll();	//Check for changes in fw_attitude_control output
 	void 		vehicle_rates_sp_mc_poll();
@@ -219,7 +228,7 @@ private:
 	void 		fill_mc_att_rates_sp();
 	void 		fill_fw_att_rates_sp();
 	void		handle_command();
-	void 		publish_transition_att_sp();
+	void 		publish_att_sp();
 };
 
 #endif

src/modules/vtol_att_control/vtol_type.cpp

@@ -49,6 +49,8 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
 {
 	_v_att = _attc->get_att();
 	_v_att_sp = _attc->get_att_sp();
+	_mc_virtual_att_sp = _attc->get_mc_virtual_att_sp();
+	_fw_virtual_att_sp = _attc->get_fw_virtual_att_sp();
 	_v_rates_sp = _attc->get_rates_sp();
 	_mc_virtual_v_rates_sp = _attc->get_mc_virtual_rates_sp();
 	_fw_virtual_v_rates_sp = _attc->get_fw_virtual_rates_sp();

src/modules/vtol_att_control/vtol_type.h

@@ -91,13 +91,15 @@ protected:
 	VtolAttitudeControl *_attc;
 	mode _vtol_mode;
 
-	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 vehicle_rates_setpoint_s		*_mc_virtual_v_rates_sp;	// virtual mc vehicle rates setpoint
-	struct vehicle_rates_setpoint_s		*_fw_virtual_v_rates_sp;	// virtual fw 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 vehicle_attitude_s		*_v_att;				//vehicle attitude
+	struct vehicle_attitude_setpoint_s	*_v_att_sp;			//vehicle attitude setpoint
+	struct mc_virtual_attitude_setpoint_s *_mc_virtual_att_sp;	// virtual mc attitude setpoint
+	struct fw_virtual_attitude_setpoint_s *_fw_virtual_att_sp;	// virtual fw attitude setpoint
+	struct vehicle_rates_setpoint_s		*_v_rates_sp;		//vehicle rates setpoint
+	struct vehicle_rates_setpoint_s		*_mc_virtual_v_rates_sp;		// virtual mc vehicle rates setpoint
+	struct vehicle_rates_setpoint_s		*_fw_virtual_v_rates_sp;		// virtual fw 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 vtol_vehicle_status_s 		*_vtol_vehicle_status;
 	struct actuator_controls_s			*_actuators_out_0;			//actuator controls going to the mc mixer
 	struct actuator_controls_s			*_actuators_out_1;			//actuator controls going to the fw mixer (used for elevons)
@@ -117,8 +119,8 @@ protected:
 	float _mc_yaw_weight;	// weight for multicopter attitude controller yaw output
 
 	float _yaw_transition;	// yaw angle in which transition will take place
-	float _throttle_transition; // throttle value used for the transition phase	
 	float _pitch_transition_start;  // pitch angle at the start of transition (tailsitter)
+	float _throttle_transition; // throttle value used for the transition phase
 	bool _flag_was_in_trans_mode;	// true if mode has just switched to transition
 
 };