VTOL: pull up generic setpoint publishing

src/modules/vtol_att_control/standard.cpp

@@ -36,6 +36,7 @@
  *
  * @author Simon Wilks		<simon@uaventure.com>
  * @author Roman Bapst		<bapstroman@gmail.com>
+ * @author Andreas Antener	<andreas@uaventure.com>
  * @author Sander Smeets	<sander@droneslab.com>
  *
 */
@@ -265,16 +266,9 @@ void Standard::update_transition_state()
 	_mc_throttle_weight = math::constrain(_mc_throttle_weight, 0.0f, 1.0f);
 }
 
-void Standard::update_mc_state()
-{
-	// 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()
 {
-	// copy virtual attitude setpoint to real attitude setpoint
+	VtolType::update_fw_state();
-	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) {
@@ -284,10 +278,6 @@ void Standard::update_fw_state()
 	}
 }
 
-void Standard::update_external_state()
-{
-}
-
 /**
  * Prepare message to acutators with data from mc and fw attitude controllers. An mc attitude weighting will determine
  * what proportion of control should be applied to each of the control groups (mc and fw).

src/modules/vtol_att_control/standard.h

@@ -38,6 +38,7 @@
 *
 * @author Simon Wilks 		<simon@uaventure.com>
 * @author Roman Bapst 		<bapstroman@gmail.com>
+* @author Andreas Antener	<andreas@uaventure.com>
 * @author Sander Smeets 	<sander@droneslab.com>
 *
 */
@@ -56,11 +57,10 @@ public:
 	Standard(VtolAttitudeControl *_att_controller);
 	~Standard();
 
-	void update_vtol_state();
+	virtual void update_vtol_state();
-	void update_mc_state();
+	virtual void update_transition_state();
-	void update_fw_state();
+	virtual void update_fw_state();
-	void update_transition_state();
+	virtual void fill_actuator_outputs();
-	void update_external_state();
 
 private:
 
@@ -98,7 +98,6 @@ private:
 	float _pusher_throttle;
 	float _airspeed_trans_blend_margin;
 
-	void fill_actuator_outputs();
 	void set_max_mc(unsigned pwm_value);
 
 	int parameters_update();

src/modules/vtol_att_control/tailsitter.cpp

@@ -227,39 +227,6 @@ void Tailsitter::update_vtol_state()
 	}
 }
 
-
-
-void Tailsitter::update_mc_state()
-{
-	// set idle speed for rotary wing mode
-	if (!flag_idle_mc) {
-		set_idle_mc();
-		flag_idle_mc = true;
-	}
-
-	_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()
-{
-	if (flag_idle_mc) {
-		set_idle_fw();
-		flag_idle_mc = false;
-	}
-
-	_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()
 {
 	if (!_flag_was_in_trans_mode) {
@@ -381,12 +348,6 @@ void Tailsitter::update_transition_state()
 	memcpy(&_v_att_sp->q_d[0], &q_sp.data[0], sizeof(_v_att_sp->q_d));
 }
 
-
-void Tailsitter::update_external_state()
-{
-
-}
-
 void Tailsitter::calc_tot_airspeed()
 {
 	float airspeed = math::max(1.0f, _airspeed->true_airspeed_m_s);	// prevent numerical drama
@@ -439,6 +400,27 @@ void Tailsitter::scale_mc_output()
 				       -1.0f, 1.0f);
 }
 
+void Tailsitter::update_mc_state()
+{
+	VtolType::update_mc_state();
+
+	// set idle speed for rotary wing mode
+	if (!flag_idle_mc) {
+		set_idle_mc();
+		flag_idle_mc = true;
+	}
+}
+
+void Tailsitter::update_fw_state()
+{
+	VtolType::update_fw_state();
+
+	if (flag_idle_mc) {
+		set_idle_fw();
+		flag_idle_mc = false;
+	}
+}
+
 /**
 * Write data to actuator output topic.
 */

src/modules/vtol_att_control/tailsitter.h

@@ -54,35 +54,14 @@ public:
 	Tailsitter(VtolAttitudeControl *_att_controller);
 	~Tailsitter();
 
-	/**
+	virtual void update_vtol_state();
-	 * Update vtol state.
+	virtual void update_transition_state();
-	 */
+	virtual void update_mc_state();
-	void update_vtol_state();
+	virtual void update_fw_state();
-
+	virtual void fill_actuator_outputs();
-	/**
-	 * Update multicopter state.
-	 */
-	void update_mc_state();
-
-	/**
-	 * Update fixed wing state.
-	 */
-	void update_fw_state();
-
-	/**
-	 * Update transition state.
-	 */
-	void update_transition_state();
-
-	/**
-	 * Update external state.
-	 */
-	void update_external_state();
 
 private:
 
-
-
 	struct {
 		float front_trans_dur;			/**< duration of first part of front transition */
 		float front_trans_dur_p2;
@@ -135,11 +114,6 @@ private:
 	/** is this one still needed? */
 	void scale_mc_output();
 
-	/**
-	 * Write control values to actuator output topics.
-	 */
-	void fill_actuator_outputs();
-
 	/**
 	 * Update parameters.
 	 */

src/modules/vtol_att_control/tiltrotor.cpp

@@ -252,8 +252,7 @@ void Tiltrotor::update_vtol_state()
 
 void Tiltrotor::update_mc_state()
 {
-	// copy virtual attitude setpoint to real attitude setpoint
+	VtolType::update_mc_state();
-	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;
@@ -268,16 +267,11 @@ void Tiltrotor::update_mc_state()
 		set_idle_mc();
 		flag_idle_mc = true;
 	}
-
-	_mc_roll_weight = 1.0f;
-	_mc_pitch_weight = 1.0f;
-	_mc_yaw_weight = 1.0f;
 }
 
 void Tiltrotor::update_fw_state()
 {
-	// copy virtual attitude setpoint to real attitude setpoint
+	VtolType::update_fw_state();
-	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;
@@ -292,10 +286,6 @@ void Tiltrotor::update_fw_state()
 		set_idle_fw();
 		flag_idle_mc = false;
 	}
-
-	_mc_roll_weight = 0.0f;
-	_mc_pitch_weight = 0.0f;
-	_mc_yaw_weight = 0.0f;
 }
 
 void Tiltrotor::update_transition_state()
@@ -374,11 +364,6 @@ void Tiltrotor::update_transition_state()
 	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
 }
 
-void Tiltrotor::update_external_state()
-{
-
-}
-
 /**
 * Write data to actuator output topic.
 */

src/modules/vtol_att_control/tiltrotor.h

@@ -52,30 +52,11 @@ public:
 	Tiltrotor(VtolAttitudeControl *_att_controller);
 	~Tiltrotor();
 
-	/**
+	virtual void update_vtol_state();
-	 * Update vtol state.
+	virtual void update_transition_state();
-	 */
+	virtual void fill_actuator_outputs();
-	void update_vtol_state();
+	virtual void update_mc_state();
-
+	virtual void update_fw_state();
-	/**
-	 * Update multicopter state.
-	 */
-	void update_mc_state();
-
-	/**
-	 * Update fixed wing state.
-	 */
-	void update_fw_state();
-
-	/**
-	 * Update transition state.
-	 */
-	void update_transition_state();
-
-	/**
-	 * Update external state.
-	 */
-	void update_external_state();
 
 private:
 
@@ -143,11 +124,6 @@ private:
 	 */
 	bool is_motor_off_channel(const int channel);
 
-	/**
-	 * Write control values to actuator output topics.
-	 */
-	void fill_actuator_outputs();
-
 	/**
 	 * Adjust the state of the rear motors. In fw mode they shouldn't spin.
 	 */

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -79,6 +79,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_airspeed_sub(-1),
 	_battery_status_sub(-1),
 	_vehicle_cmd_sub(-1),
+	_vehicle_status_sub(-1),
 
 	//init publication handlers
 	_actuators_0_pub(nullptr),
@@ -109,6 +110,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	memset(&_airspeed, 0, sizeof(_airspeed));
 	memset(&_batt_status, 0, sizeof(_batt_status));
 	memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd));
+	memset(&_vehicle_status, 0, sizeof(_vehicle_status));
 
 	_params.idle_pwm_mc = PWM_DEFAULT_MIN;
 	_params.vtol_motor_count = 0;
@@ -408,6 +410,21 @@ VtolAttitudeControl::vehicle_cmd_poll()
 	}
 }
 
+/**
+* Check for vehicle status updates.
+*/
+void
+VtolAttitudeControl::vehicle_status_poll()
+{
+	bool updated;
+
+	orb_check(_vehicle_status_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(vehicle_status), _vehicle_status_sub , &_vehicle_status);
+	}
+}
+
 /**
 * Check received command
 */
@@ -578,6 +595,7 @@ void VtolAttitudeControl::task_main()
 	_airspeed_sub          = orb_subscribe(ORB_ID(airspeed));
 	_battery_status_sub	   = orb_subscribe(ORB_ID(battery_status));
 	_vehicle_cmd_sub	   = orb_subscribe(ORB_ID(vehicle_command));
+	_vehicle_status_sub    = orb_subscribe(ORB_ID(vehicle_status));
 
 	_actuator_inputs_mc    = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
 	_actuator_inputs_fw    = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));
@@ -664,6 +682,7 @@ void VtolAttitudeControl::task_main()
 		vehicle_airspeed_poll();
 		vehicle_battery_poll();
 		vehicle_cmd_poll();
+		vehicle_status_poll();
 
 		// update the vtol state machine which decides which mode we are in
 		_vtol_type->update_vtol_state();

src/modules/vtol_att_control/vtol_att_control_main.h

@@ -83,6 +83,7 @@
 #include <uORB/topics/vehicle_local_position.h>
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/vehicle_command.h>
+#include <uORB/topics/vehicle_status.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
@@ -119,7 +120,7 @@ public:
 	struct fw_virtual_rates_setpoint_s 		*get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;}
 	struct manual_control_setpoint_s 		*get_manual_control_sp() {return &_manual_control_sp;}
 	struct vehicle_control_mode_s 			*get_control_mode() {return &_v_control_mode;}
-	struct vtol_vehicle_status_s			*get_vehicle_status() {return &_vtol_vehicle_status;}
+	struct vtol_vehicle_status_s			*get_vtol_vehicle_status() {return &_vtol_vehicle_status;}
 	struct actuator_controls_s 			*get_actuators_out0() {return &_actuators_out_0;}
 	struct actuator_controls_s 			*get_actuators_out1() {return &_actuators_out_1;}
 	struct actuator_controls_s 			*get_actuators_mc_in() {return &_actuators_mc_in;}
@@ -128,6 +129,7 @@ public:
 	struct vehicle_local_position_s 		*get_local_pos() {return &_local_pos;}
 	struct airspeed_s 						*get_airspeed() {return &_airspeed;}
 	struct battery_status_s 				*get_batt_status() {return &_batt_status;}
+	struct vehicle_status_s 				*get_vehicle_status() {return &_vehicle_status;}
 
 	struct Params 						*get_params() {return &_params;}
 
@@ -153,6 +155,7 @@ private:
 	int 	_airspeed_sub;			// airspeed subscription
 	int 	_battery_status_sub;	// battery status subscription
 	int 	_vehicle_cmd_sub;
+	int 	_vehicle_status_sub;
 
 	int 	_actuator_inputs_mc;	//topic on which the mc_att_controller publishes actuator inputs
 	int 	_actuator_inputs_fw;	//topic on which the fw_att_controller publishes actuator inputs
@@ -164,25 +167,26 @@ private:
 	orb_advert_t	_v_rates_sp_pub;
 	orb_advert_t	_v_att_sp_pub;
 //*******************data containers***********************************************************
-	struct vehicle_attitude_s				_v_att;				//vehicle attitude
+	struct vehicle_attitude_s			_v_att;				//vehicle attitude
-	struct vehicle_attitude_setpoint_s		_v_att_sp;			//vehicle attitude setpoint
+	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 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 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 	_v_rates_sp;		//vehicle rates setpoint
-	struct mc_virtual_rates_setpoint_s		_mc_virtual_v_rates_sp;		// virtual mc vehicle rates setpoint
+	struct mc_virtual_rates_setpoint_s 	_mc_virtual_v_rates_sp;		// virtual mc vehicle rates setpoint
-	struct fw_virtual_rates_setpoint_s		_fw_virtual_v_rates_sp;		// virtual fw vehicle rates setpoint
+	struct fw_virtual_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 manual_control_setpoint_s	_manual_control_sp; //manual control setpoint
-	struct vehicle_control_mode_s			_v_control_mode;	//vehicle control mode
+	struct vehicle_control_mode_s		_v_control_mode;	//vehicle control mode
-	struct vtol_vehicle_status_s			_vtol_vehicle_status;
+	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_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)
+	struct actuator_controls_s			_actuators_out_1;	//actuator controls going to the fw mixer (used for elevons)
-	struct actuator_controls_s				_actuators_mc_in;	//actuator controls from mc_att_control
+	struct actuator_controls_s			_actuators_mc_in;	//actuator controls from mc_att_control
-	struct actuator_controls_s				_actuators_fw_in;	//actuator controls from fw_att_control
+	struct actuator_controls_s			_actuators_fw_in;	//actuator controls from fw_att_control
-	struct actuator_armed_s					_armed;				//actuator arming status
+	struct actuator_armed_s				_armed;				//actuator arming status
-	struct vehicle_local_position_s			_local_pos;
+	struct vehicle_local_position_s		_local_pos;
-	struct airspeed_s						_airspeed;			// airspeed
+	struct airspeed_s 					_airspeed;			// airspeed
-	struct battery_status_s					_batt_status; 		// battery status
+	struct battery_status_s 			_batt_status; 		// battery status
-	struct vehicle_command_s				_vehicle_cmd;
+	struct vehicle_command_s			_vehicle_cmd;
+	struct vehicle_status_s			_vehicle_status;
 
 	Params _params;	// struct holding the parameters
 
@@ -233,6 +237,7 @@ private:
 	void 		vehicle_battery_poll();			// Check for battery updates
 	void		vehicle_cmd_poll();
 	void 		parameters_update_poll();		//Check if parameters have changed
+	void 		vehicle_status_poll();
 	int 		parameters_update();			//Update local paraemter cache
 	void 		fill_mc_att_rates_sp();
 	void 		fill_fw_att_rates_sp();

src/modules/vtol_att_control/vtol_type.cpp

@@ -56,7 +56,7 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
 	_fw_virtual_v_rates_sp = _attc->get_fw_virtual_rates_sp();
 	_manual_control_sp = _attc->get_manual_control_sp();
 	_v_control_mode = _attc->get_control_mode();
-	_vtol_vehicle_status = _attc->get_vehicle_status();
+	_vtol_vehicle_status = _attc->get_vtol_vehicle_status();
 	_actuators_out_0 = _attc->get_actuators_out0();
 	_actuators_out_1 = _attc->get_actuators_out1();
 	_actuators_mc_in = _attc->get_actuators_mc_in();
@@ -65,6 +65,7 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
 	_local_pos = _attc->get_local_pos();
 	_airspeed = _attc->get_airspeed();
 	_batt_status = _attc->get_batt_status();
+	_vehicle_status = _attc->get_vehicle_status();
 	_params = _attc->get_params();
 
 	flag_idle_mc = true;
@@ -132,3 +133,23 @@ void VtolType::set_idle_fw()
 
 	px4_close(fd);
 }
+
+void VtolType::update_mc_state()
+{
+	// copy virtual attitude setpoint to real attitude setpoint
+	memcpy(_v_att_sp, _mc_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+
+	_mc_roll_weight = 1.0f;
+	_mc_pitch_weight = 1.0f;
+	_mc_yaw_weight = 1.0f;
+}
+
+void VtolType::update_fw_state()
+{
+	// copy virtual attitude setpoint to real attitude setpoint
+	memcpy(_v_att_sp, _fw_virtual_att_sp, sizeof(vehicle_attitude_setpoint_s));
+
+	_mc_roll_weight = 0.0f;
+	_mc_pitch_weight = 0.0f;
+	_mc_yaw_weight = 0.0f;
+}

src/modules/vtol_att_control/vtol_type.h

@@ -76,11 +76,34 @@ public:
 
 	virtual ~VtolType();
 
+	/**
+	 * Update vtol state.
+	 */
 	virtual void update_vtol_state() = 0;
-	virtual void update_mc_state() = 0;
+
-	virtual void update_fw_state() = 0;
+	/**
+	 * Update transition state.
+	 */
 	virtual void update_transition_state() = 0;
-	virtual void update_external_state() = 0;
+
+	/**
+	 * Update multicopter state.
+	 */
+	virtual void update_mc_state();
+
+	/**
+	 * Update fixed wing state.
+	 */
+	virtual void update_fw_state();
+
+	/**
+	 * Update external state.
+	 */
+	virtual void update_external_state() {};
+
+	/**
+	 * Write control values to actuator output topics.
+	 */
 	virtual void fill_actuator_outputs() = 0;
 
 	void set_idle_mc();
@@ -92,26 +115,27 @@ protected:
 	VtolAttitudeControl *_attc;
 	mode _vtol_mode;
 
-	struct vehicle_attitude_s				*_v_att;				//vehicle attitude
+	struct vehicle_attitude_s		*_v_att;				//vehicle attitude
-	struct vehicle_attitude_setpoint_s		*_v_att_sp;			//vehicle attitude setpoint
+	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 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 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 	*_v_rates_sp;		//vehicle rates setpoint
-	struct mc_virtual_rates_setpoint_s		*_mc_virtual_v_rates_sp;		// virtual mc vehicle rates setpoint
+	struct mc_virtual_rates_setpoint_s 	*_mc_virtual_v_rates_sp;		// virtual mc vehicle rates setpoint
-	struct fw_virtual_rates_setpoint_s		*_fw_virtual_v_rates_sp;		// virtual fw vehicle rates setpoint
+	struct fw_virtual_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 manual_control_setpoint_s	*_manual_control_sp; //manual control setpoint
-	struct vehicle_control_mode_s			*_v_control_mode;	//vehicle control mode
+	struct vehicle_control_mode_s		*_v_control_mode;	//vehicle control mode
-	struct vtol_vehicle_status_s			*_vtol_vehicle_status;
+	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_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)
+	struct actuator_controls_s			*_actuators_out_1;			//actuator controls going to the fw mixer (used for elevons)
-	struct actuator_controls_s				*_actuators_mc_in;			//actuator controls from mc_att_control
+	struct actuator_controls_s			*_actuators_mc_in;			//actuator controls from mc_att_control
-	struct actuator_controls_s				*_actuators_fw_in;			//actuator controls from fw_att_control
+	struct actuator_controls_s			*_actuators_fw_in;			//actuator controls from fw_att_control
-	struct actuator_armed_s					*_armed;					//actuator arming status
+	struct actuator_armed_s				*_armed;					//actuator arming status
-	struct vehicle_local_position_s			*_local_pos;
+	struct vehicle_local_position_s		*_local_pos;
-	struct airspeed_s						*_airspeed;					// airspeed
+	struct airspeed_s 					*_airspeed;					// airspeed
-	struct battery_status_s					*_batt_status; 				// battery status
+	struct battery_status_s 			*_batt_status; 				// battery status
+	struct vehicle_status_s 			*_vehicle_status;			// vehicle status from commander app
 
-	struct Params							*_params;
+	struct Params 						*_params;
 
 	bool flag_idle_mc;		//false = "idle is set for fixed wing mode"; true = "idle is set for multicopter mode"