VTOL/TECS: publish transition throttle after transition until tecs is initialized and fw pos ctl is publishing att setpoint

src/modules/vtol_att_control/standard.cpp

@@ -169,6 +169,7 @@ void Standard::update_vtol_state()
 				// we can turn off the multirotor motors now
 				_flag_enable_mc_motors = false;
 				// don't set pusher throttle here as it's being ramped up elsewhere
+				_trans_finished_ts = hrt_absolute_time();
 			}
 
 		} else if (_vtol_schedule.flight_mode == TRANSITION_TO_MC) {
@@ -316,6 +317,12 @@ void Standard::fill_actuator_outputs()
 	}
 }
 
+void
+Standard::waiting_on_fw_ctl()
+{
+	_v_att_sp->thrust = _pusher_throttle;
+};
+
 /**
 * Disable all multirotor motors when in fw mode.
 */

src/modules/vtol_att_control/standard.h

@@ -61,6 +61,7 @@ public:
 	virtual void update_transition_state();
 	virtual void update_fw_state();
 	virtual void fill_actuator_outputs();
+	virtual void waiting_on_fw_ctl();
 
 private:
 

src/modules/vtol_att_control/vtol_att_control_main.cpp

@@ -80,6 +80,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	_battery_status_sub(-1),
 	_vehicle_cmd_sub(-1),
 	_vehicle_status_sub(-1),
+	_tecs_status_sub(-1),
 
 	//init publication handlers
 	_actuators_0_pub(nullptr),
@@ -111,6 +112,7 @@ VtolAttitudeControl::VtolAttitudeControl() :
 	memset(&_batt_status, 0, sizeof(_batt_status));
 	memset(&_vehicle_cmd, 0, sizeof(_vehicle_cmd));
 	memset(&_vehicle_status, 0, sizeof(_vehicle_status));
+	memset(&_tecs_status, 0, sizeof(_tecs_status));
 
 	_params.idle_pwm_mc = PWM_DEFAULT_MIN;
 	_params.vtol_motor_count = 0;
@@ -425,6 +427,21 @@ VtolAttitudeControl::vehicle_status_poll()
 	}
 }
 
+/**
+* Check for TECS status updates.
+*/
+void
+VtolAttitudeControl::tecs_status_poll()
+{
+	bool updated;
+
+	orb_check(_tecs_status_sub, &updated);
+
+	if (updated) {
+		orb_copy(ORB_ID(tecs_status), _tecs_status_sub , &_tecs_status);
+	}
+}
+
 /**
 * Check received command
 */
@@ -596,6 +613,7 @@ void VtolAttitudeControl::task_main()
 	_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));
+	_tecs_status_sub = orb_subscribe(ORB_ID(tecs_status));
 
 	_actuator_inputs_mc    = orb_subscribe(ORB_ID(actuator_controls_virtual_mc));
 	_actuator_inputs_fw    = orb_subscribe(ORB_ID(actuator_controls_virtual_fw));
@@ -683,6 +701,7 @@ void VtolAttitudeControl::task_main()
 		vehicle_battery_poll();
 		vehicle_cmd_poll();
 		vehicle_status_poll();
+		tecs_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

@@ -84,6 +84,7 @@
 #include <uORB/topics/battery_status.h>
 #include <uORB/topics/vehicle_command.h>
 #include <uORB/topics/vehicle_status.h>
+#include <uORB/topics/tecs_status.h>
 #include <systemlib/param/param.h>
 #include <systemlib/err.h>
 #include <systemlib/systemlib.h>
@@ -111,10 +112,10 @@ public:
 	bool is_fixed_wing_requested();
 	void abort_front_transition();
 
-	struct vehicle_attitude_s 				*get_att() {return &_v_att;}
+	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 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 mc_virtual_rates_setpoint_s 		*get_mc_virtual_rates_sp() {return &_mc_virtual_v_rates_sp;}
 	struct fw_virtual_rates_setpoint_s 		*get_fw_virtual_rates_sp() {return &_fw_virtual_v_rates_sp;}
@@ -125,13 +126,14 @@ public:
 	struct actuator_controls_s 			*get_actuators_out1() {return &_actuators_out_1;}
 	struct actuator_controls_s 			*get_actuators_mc_in() {return &_actuators_mc_in;}
 	struct actuator_controls_s 			*get_actuators_fw_in() {return &_actuators_fw_in;}
-	struct actuator_armed_s 				*get_armed() {return &_armed;}
+	struct actuator_armed_s 			*get_armed() {return &_armed;}
 	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 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 tecs_status_s 				*get_tecs_status() {return &_tecs_status;}
 
-	struct Params 						*get_params() {return &_params;}
+	struct Params 					*get_params() {return &_params;}
 
 
 private:
@@ -141,21 +143,22 @@ private:
 	int _mavlink_fd;		// mavlink log device
 
 	/* handlers for subscriptions */
-	int		_v_att_sub;				//vehicle attitude subscription
-	int		_v_att_sp_sub;			//vehicle attitude setpoint subscription
+	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
-	int		_params_sub;			//parameter updates subscription
-	int		_manual_control_sp_sub;	//manual control setpoint subscription
-	int		_armed_sub;				//arming status subscription
+	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
+	int	_params_sub;			//parameter updates subscription
+	int	_manual_control_sp_sub;	//manual control setpoint subscription
+	int	_armed_sub;				//arming status subscription
 	int 	_local_pos_sub;			// sensor subscription
 	int 	_airspeed_sub;			// airspeed subscription
 	int 	_battery_status_sub;	// battery status subscription
 	int 	_vehicle_cmd_sub;
 	int 	_vehicle_status_sub;
+	int	_tecs_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
@@ -168,25 +171,26 @@ private:
 	orb_advert_t	_v_att_sp_pub;
 //*******************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 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 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 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 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 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)
 	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_armed_s				_armed;				//actuator arming status
-	struct vehicle_local_position_s		_local_pos;
-	struct airspeed_s 					_airspeed;			// airspeed
+	struct vehicle_local_position_s			_local_pos;
+	struct airspeed_s 				_airspeed;			// airspeed
 	struct battery_status_s 			_batt_status; 		// battery status
 	struct vehicle_command_s			_vehicle_cmd;
-	struct vehicle_status_s			_vehicle_status;
+	struct vehicle_status_s				_vehicle_status;
+	struct tecs_status_s				_tecs_status;
 
 	Params _params;	// struct holding the parameters
 
@@ -236,6 +240,7 @@ private:
 	void		vehicle_attitude_poll();  //Check for attitude updates.
 	void 		vehicle_battery_poll();			// Check for battery updates
 	void		vehicle_cmd_poll();
+	void		tecs_status_poll();
 	void 		parameters_update_poll();		//Check if parameters have changed
 	void 		vehicle_status_poll();
 	int 		parameters_update();			//Update local paraemter cache

src/modules/vtol_att_control/vtol_type.cpp

@@ -35,6 +35,7 @@
 * @file vtol_type.cpp
 *
 * @author Roman Bapst 		<bapstroman@gmail.com>
+* @author Andreas Antener	<andreas@uaventure.com>
 *
 */
 
@@ -66,6 +67,7 @@ VtolType::VtolType(VtolAttitudeControl *att_controller) :
 	_airspeed = _attc->get_airspeed();
 	_batt_status = _attc->get_batt_status();
 	_vehicle_status = _attc->get_vehicle_status();
+	_tecs_status = _attc->get_tecs_status();
 	_params = _attc->get_params();
 
 	flag_idle_mc = true;
@@ -152,4 +154,18 @@ void VtolType::update_fw_state()
 	_mc_roll_weight = 0.0f;
 	_mc_pitch_weight = 0.0f;
 	_mc_yaw_weight = 0.0f;
+
+	// tecs didn't publish an update yet after the transition
+	if (_tecs_status->timestamp < _trans_finished_ts) {
+		_tecs_running = false;
+
+	} else if (!_tecs_running) {
+		_tecs_running = true;
+		_tecs_running_ts = hrt_absolute_time();
+	}
+
+	// tecs didn't publish yet and the position controller didn't publish yet AFTER tecs
+	if (!_tecs_running || (_tecs_running && _fw_virtual_att_sp->timestamp <= _tecs_running_ts)) {
+		waiting_on_fw_ctl();
+	}
 }

src/modules/vtol_att_control/vtol_type.h

@@ -36,6 +36,7 @@
 *
 * @author Roman Bapst 		<bapstroman@gmail.com>
 * @author Sander Smeets		<sander@droneslab.com>
+* @author Andreas Antener	<andreas@uaventure.com>
 *
 */
 
@@ -43,6 +44,7 @@
 #define VTOL_TYPE_H
 
 #include <lib/mathlib/mathlib.h>
+#include <drivers/drv_hrt.h>
 
 struct Params {
 	int idle_pwm_mc;			// pwm value for idle in mc mode
@@ -106,6 +108,8 @@ public:
 	 */
 	virtual void fill_actuator_outputs() = 0;
 
+	virtual void waiting_on_fw_ctl() {};
+
 	void set_idle_mc();
 	void set_idle_fw();
 
@@ -130,12 +134,13 @@ protected:
 	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_armed_s				*_armed;					//actuator arming status
-	struct vehicle_local_position_s		*_local_pos;
-	struct airspeed_s 					*_airspeed;					// airspeed
+	struct vehicle_local_position_s			*_local_pos;
+	struct airspeed_s 				*_airspeed;					// airspeed
 	struct battery_status_s 			*_batt_status; 				// battery status
 	struct vehicle_status_s 			*_vehicle_status;			// vehicle status from commander app
+	struct tecs_status_s				*_tecs_status;
 
-	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"
 
@@ -149,6 +154,9 @@ protected:
 	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
+	hrt_abstime _trans_finished_ts;
+	bool _tecs_running;
+	hrt_abstime _tecs_running_ts;
 
 };