FlightTaskManual: Smooth flight integration: Copy over set_manual_acceleration_xy without any changes

src/lib/FlightTasks/tasks/FlightTaskManual.hpp

@@ -162,4 +162,214 @@ private:
 	uORB::Subscription<control_state_s> _sub_control_state;
 
 	matrix::Vector3f _hold_position; /**< position at which the vehicle stays while the input is zero velocity */
+
+	void set_manual_acceleration_xy(matrix::Vector2f &stick_xy, const float dt)
+	{
+
+		/*
+		 * In manual mode we consider four states with different acceleration handling:
+		 * 1. user wants to stop
+		 * 2. user wants to quickly change direction
+		 * 3. user wants to accelerate
+		 * 4. user wants to decelerate
+		 */
+
+		/* get normalized stick input vector */
+		matrix::Vector2f stick_xy_norm = (stick_xy.length() > 0.0f) ? stick_xy.normalized() : stick_xy;
+		matrix::Vector2f stick_xy_prev_norm = (_stick_input_xy_prev.length() > 0.0f) ? _stick_input_xy_prev.normalized() :
+						      _stick_input_xy_prev;
+
+		/* check if stick direction and current velocity are within 60angle */
+		const bool is_aligned = (stick_xy_norm * stick_xy_prev_norm) > 0.5f;
+
+		/* check if zero input stick */
+		const bool is_prev_zero = (fabsf(_stick_input_xy_prev.length()) <= FLT_EPSILON);
+		const bool is_current_zero = (fabsf(stick_xy.length()) <= FLT_EPSILON);
+
+		/* check acceleration */
+		const bool do_acceleration = is_prev_zero || (is_aligned &&
+					     ((stick_xy.length() > _stick_input_xy_prev.length()) || (fabsf(stick_xy.length() - 1.0f) < FLT_EPSILON)));
+
+		const bool do_deceleration = (is_aligned && (stick_xy.length() <= _stick_input_xy_prev.length()));
+
+		const bool do_direction_change = !is_aligned;
+
+		manual_stick_input intention;
+
+		if (is_current_zero) {
+			/* we want to stop */
+			intention = brake;
+
+		} else if (do_acceleration) {
+			/* we do manual acceleration */
+			intention = acceleration;
+
+		} else if (do_deceleration) {
+			/* we do manual deceleration */
+			intention = deceleration;
+
+		} else if (do_direction_change) {
+			/* we have a direction change */
+			intention = direction_change;
+
+		} else {
+			/* catchall: acceleration */
+			intention = acceleration;
+		}
+
+
+		/*
+		 * update user intention
+		 */
+
+		/* we always want to break starting with slow deceleration */
+		if ((_user_intention_xy != brake) && (intention  == brake)) {
+
+			if (_jerk_hor_max.get() > _jerk_hor_min.get()) {
+				_manual_jerk_limit_xy = (_jerk_hor_max.get() - _jerk_hor_min.get()) / _velocity_hor_manual.get() *
+							sqrtf(_vel(0) * _vel(0) + _vel(1) * _vel(1)) + _jerk_hor_min.get();
+
+				/* we start braking with lowest accleration */
+				_acceleration_state_dependent_xy = _deceleration_hor_slow.get();
+
+			} else {
+
+				/* set the jerk limit large since we don't know it better*/
+				_manual_jerk_limit_xy = 1000000.f;
+
+				/* at brake we use max acceleration */
+				_acceleration_state_dependent_xy = _acceleration_hor_max.get();
+
+			}
+
+			/* reset slew rate */
+			_vel_sp_prev(0) = _vel(0);
+			_vel_sp_prev(1) = _vel(1);
+
+		}
+
+		switch (_user_intention_xy) {
+		case brake: {
+				if (intention != brake) {
+					_user_intention_xy = acceleration;
+					/* we initialize with lowest acceleration */
+					_acceleration_state_dependent_xy = _deceleration_hor_slow.get();
+				}
+
+				break;
+			}
+
+		case direction_change: {
+				/* only exit direction change if brake or aligned */
+				matrix::Vector2f vel_xy(_vel(0), _vel(1));
+				matrix::Vector2f vel_xy_norm = (vel_xy.length() > 0.0f) ? vel_xy.normalized() : vel_xy;
+				bool stick_vel_aligned = (vel_xy_norm * stick_xy_norm > 0.0f);
+
+				/* update manual direction change hysteresis */
+				_manual_direction_change_hysteresis.set_state_and_update(!stick_vel_aligned);
+
+
+				/* exit direction change if one of the condition is met */
+				if (intention == brake) {
+					_user_intention_xy = intention;
+
+				} else if (stick_vel_aligned) {
+					_user_intention_xy = acceleration;
+
+				} else if (_manual_direction_change_hysteresis.get_state()) {
+
+					/* TODO: find conditions which are always continuous
+					 * only if stick input is large*/
+					if (stick_xy.length() > 0.6f) {
+						_acceleration_state_dependent_xy = _acceleration_hor_max.get();
+					}
+				}
+
+				break;
+			}
+
+		case acceleration: {
+				_user_intention_xy = intention;
+
+				if (_user_intention_xy == direction_change) {
+					_vel_sp_prev(0) = _vel(0);
+					_vel_sp_prev(1) = _vel(1);
+				}
+
+				break;
+			}
+
+		case deceleration: {
+				_user_intention_xy = intention;
+
+				if (_user_intention_xy == direction_change) {
+					_vel_sp_prev(0) = _vel(0);
+					_vel_sp_prev(1) = _vel(1);
+				}
+
+				break;
+			}
+		}
+
+		/*
+		 * apply acceleration based on state
+		*/
+		switch (_user_intention_xy) {
+		case brake: {
+
+				/* limit jerk when braking to zero */
+				float jerk = (_acceleration_hor_max.get() - _acceleration_state_dependent_xy) / dt;
+
+				if (jerk > _manual_jerk_limit_xy) {
+					_acceleration_state_dependent_xy = _manual_jerk_limit_xy * dt + _acceleration_state_dependent_xy;
+
+				} else {
+					_acceleration_state_dependent_xy = _acceleration_hor_max.get();
+				}
+
+				break;
+			}
+
+		case direction_change: {
+
+				/* limit acceleration linearly on stick input*/
+				_acceleration_state_dependent_xy = (_acceleration_hor_manual.get() - _deceleration_hor_slow.get()) * stick_xy.length() +
+								   _deceleration_hor_slow.get();
+				break;
+			}
+
+		case acceleration: {
+				/* limit acceleration linearly on stick input*/
+				float acc_limit  = (_acceleration_hor_manual.get() - _deceleration_hor_slow.get()) * stick_xy.length()
+						   + _deceleration_hor_slow.get();
+
+				if (_acceleration_state_dependent_xy > acc_limit) {
+					acc_limit = _acceleration_state_dependent_xy;
+				}
+
+				_acceleration_state_dependent_xy = acc_limit;
+				break;
+			}
+
+		case deceleration: {
+				_acceleration_state_dependent_xy = _deceleration_hor_slow.get();
+				break;
+			}
+
+		default :
+			warn_rate_limited("User intention not recognized");
+			_acceleration_state_dependent_xy = _acceleration_hor_max.get();
+
+		}
+
+		/* update previous stick input */
+		_stick_input_xy_prev = matrix::Vector2f(_filter_manual_pitch.apply(stick_xy(0)),
+							_filter_manual_roll.apply(stick_xy(1)));
+
+
+		if (_stick_input_xy_prev.length() > 1.0f) {
+			_stick_input_xy_prev = _stick_input_xy_prev.normalized();
+		}
+	}
+
 };