Merge pull request #1476 from friekopter/vel_alt_hold

Implemented altitude and velocity hold mode

2 files changed, 108 insertions, 3 deletions

diff --git a/src/modules/fw_att_control/fw_att_control_main.cpp b/src/modules/fw_att_control/fw_att_control_main.cpp
index e770c11a2..2d5744b8a 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -751,8 +751,7 @@ FixedwingAttitudeControl::task_main()
 				 * - manual control is disabled (another app may send the setpoint, but it should
 				 *   for sure not be set from the remote control values)
 				 */
-				if (_vcontrol_mode.flag_control_velocity_enabled ||
-						_vcontrol_mode.flag_control_position_enabled ||
+				if (_vcontrol_mode.flag_control_auto_enabled ||
 						!_vcontrol_mode.flag_control_manual_enabled) {
 					/* read in attitude setpoint from attitude setpoint uorb topic */
 					roll_sp = _att_sp.roll_body + _parameters.rollsp_offset_rad;
@@ -769,6 +768,25 @@ FixedwingAttitudeControl::task_main()
 					if (_att_sp.yaw_reset_integral) {
 						_yaw_ctrl.reset_integrator();
 					}
+				} else if (_vcontrol_mode.flag_control_velocity_enabled) {
+ 					/* 
+					 * Velocity should be controlled and manual is enabled.
+					*/
+					roll_sp = (_manual.y * _parameters.man_roll_max - _parameters.trim_roll)
+											+ _parameters.rollsp_offset_rad;
+					pitch_sp = _att_sp.pitch_body + _parameters.pitchsp_offset_rad;
+					throttle_sp = _att_sp.thrust;
+
+					/* reset integrals where needed */
+					if (_att_sp.roll_reset_integral) {
+						_roll_ctrl.reset_integrator();
+					}
+					if (_att_sp.pitch_reset_integral) {
+						_pitch_ctrl.reset_integrator();
+					}
+					if (_att_sp.yaw_reset_integral) {
+						_yaw_ctrl.reset_integrator();
+					}
 				} else {
 					/*
 					 * Scale down roll and pitch as the setpoints are radians
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
index 6017369aa..e7c95cc86 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
@@ -163,6 +163,9 @@ private:
 
 	perf_counter_t	_loop_perf;			/**< loop performance counter */
 
+	float	_hold_alt;				/**< hold altitude for velocity mode */
+	hrt_abstime _control_position_last_called; /**<last call of control_position  */
+
 	/* land states */
 	bool land_noreturn_horizontal;
 	bool land_noreturn_vertical;
@@ -198,6 +201,8 @@ private:
 	TECS						_tecs;
 	fwPosctrl::mTecs				_mTecs;
 	bool						_was_pos_control_mode;
+	bool						_was_velocity_control_mode;
+	bool						_was_alt_control_mode;
 
 	struct {
 		float l1_period;
@@ -317,6 +322,11 @@ private:
 	void		vehicle_status_poll();
 
 	/**
+	 * Check for manual setpoint updates.
+	 */
+	bool		vehicle_manual_control_setpoint_poll();
+
+	/**
 	 * Check for airspeed updates.
 	 */
 	bool		vehicle_airspeed_poll();
@@ -439,6 +449,9 @@ FixedwingPositionControl::FixedwingPositionControl() :
 /* performance counters */
 	_loop_perf(perf_alloc(PC_ELAPSED, "fw l1 control")),
 
+	_hold_alt(0.0f),
+	_control_position_last_called(0),
+
 	land_noreturn_horizontal(false),
 	land_noreturn_vertical(false),
 	land_stayonground(false),
@@ -692,6 +705,22 @@ FixedwingPositionControl::vehicle_airspeed_poll()
 	return airspeed_updated;
 }
 
+bool
+FixedwingPositionControl::vehicle_manual_control_setpoint_poll()
+{
+	bool manual_updated;
+
+	/* Check if manual setpoint has changed */
+	orb_check(_manual_control_sub, &manual_updated);
+
+	if (manual_updated) {
+		orb_copy(ORB_ID(manual_control_setpoint), _manual_control_sub, &_manual);
+	}
+
+	return manual_updated;
+}
+
+
 void
 FixedwingPositionControl::vehicle_attitude_poll()
 {
@@ -852,6 +881,12 @@ bool
 FixedwingPositionControl::control_position(const math::Vector<2> &current_position, const math::Vector<3> &ground_speed,
 		const struct position_setpoint_triplet_s &pos_sp_triplet)
 {
+	float dt = FLT_MIN; // Using non zero value to a avoid division by zero
+	if (_control_position_last_called > 0) {
+		dt = (float)hrt_elapsed_time(&_control_position_last_called) * 1e-6f;
+	}
+	_control_position_last_called = hrt_absolute_time();
+
 	bool setpoint = true;
 
 	math::Vector<2> ground_speed_2d = {ground_speed(0), ground_speed(1)};
@@ -888,6 +923,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 		}
 
 		_was_pos_control_mode = true;
+		_was_velocity_control_mode = false;
+
+		/* reset hold altitude */
+		_hold_alt = _global_pos.alt;
 
 		/* get circle mode */
 		bool was_circle_mode = _l1_control.circle_mode();
@@ -1209,12 +1248,59 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 			_att_sp.roll_reset_integral = true;
 		}
 
+	} else if (_control_mode.flag_control_velocity_enabled) {
+		const float deadBand = (60.0f/1000.0f);
+		const float factor = 1.0f - deadBand;
+		if (!_was_velocity_control_mode) {
+			_hold_alt = _global_pos.alt;
+			_was_alt_control_mode = false;
+		}
+		_was_velocity_control_mode = true;
+		_was_pos_control_mode = false;
+		// Get demanded airspeed
+		float altctrl_airspeed = _parameters.airspeed_min +
+					  (_parameters.airspeed_max - _parameters.airspeed_min) *
+					  _manual.z;
+
+		// Get demanded vertical velocity from pitch control
+		float pitch = 0.0f;
+		if (_manual.x > deadBand) {
+			pitch = (_manual.x - deadBand) / factor;
+		} else if (_manual.x < - deadBand) {
+			pitch = (_manual.x + deadBand) / factor;
+		}
+		if (pitch > 0.0f) {
+			_hold_alt -= (_parameters.max_climb_rate * dt) * pitch;
+			_was_alt_control_mode = false;
+		} else if (pitch < 0.0f) {
+			_hold_alt -= (_parameters.max_sink_rate * dt) * pitch;
+			_was_alt_control_mode = false;
+		} else if (!_was_alt_control_mode) {
+			_hold_alt = _global_pos.alt;
+			_was_alt_control_mode = true;
+		}
+		tecs_update_pitch_throttle(_hold_alt,
+									altctrl_airspeed,
+									eas2tas,
+									math::radians(_parameters.pitch_limit_min),
+									math::radians(_parameters.pitch_limit_max),
+									_parameters.throttle_min,
+									_parameters.throttle_max,
+									_parameters.throttle_cruise,
+									false,
+									math::radians(_parameters.pitch_limit_min),
+									_global_pos.alt,
+									ground_speed,
+									TECS_MODE_NORMAL);
 	} else {
-
+		_was_velocity_control_mode = false;
 		_was_pos_control_mode = false;
 
 		/** MANUAL FLIGHT **/
 
+		/* reset hold altitude */
+		_hold_alt = _global_pos.alt;
+
 		/* no flight mode applies, do not publish an attitude setpoint */
 		setpoint = false;
 
@@ -1350,6 +1436,7 @@ FixedwingPositionControl::task_main()
 			vehicle_setpoint_poll();
 			vehicle_sensor_combined_poll();
 			vehicle_airspeed_poll();
+			vehicle_manual_control_setpoint_poll();
 			// vehicle_baro_poll();
 
 			math::Vector<3> ground_speed(_global_pos.vel_n, _global_pos.vel_e,  _global_pos.vel_d);