From fe4e6779f3b248af9c992f6a514022efb0bfe25e Mon Sep 17 00:00:00 2001
From: Roman <bapstroman@gmail.com>
Date: Mon, 23 Jul 2018 16:30:08 +0200
Subject: [PATCH] l1 position control: added roll angle setpoint slew rate
 limiting

Signed-off-by: Roman <bapstroman@gmail.com>
---
 l1/ecl_l1_pos_controller.cpp | 32 ++++++++++++++++++++++++++++----
 l1/ecl_l1_pos_controller.h   | 23 +++++++++++++++++++++--
 2 files changed, 49 insertions(+), 6 deletions(-)

diff --git a/l1/ecl_l1_pos_controller.cpp b/l1/ecl_l1_pos_controller.cpp
index 010952070b..196c5c0194 100644
--- a/l1/ecl_l1_pos_controller.cpp
+++ b/l1/ecl_l1_pos_controller.cpp
@@ -45,11 +45,27 @@
 using matrix::Vector2f;
 using matrix::wrap_pi;
 
-float ECL_L1_Pos_Controller::nav_roll()
+
+void ECL_L1_Pos_Controller::update_roll_setpoint()
 {
-	float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
-	ret = math::constrain(ret, -_roll_lim_rad, _roll_lim_rad);
-	return ret;
+	float roll_new = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
+	roll_new = math::constrain(roll_new, -_roll_lim_rad, _roll_lim_rad);
+
+	// no slew rate limiting active
+	if (_dt <= 0.0f || _roll_slew_rate <= 0.0f) {
+		_roll_setpoint = roll_new;
+		return;
+	}
+
+	// slew rate limiting active
+	if ((roll_new - _roll_setpoint) / _dt > _roll_slew_rate) {
+		roll_new = _roll_setpoint + _roll_slew_rate * _dt;
+	} else if ((roll_new - _roll_setpoint) / _dt < -_roll_slew_rate) {
+		roll_new = _roll_setpoint - _roll_slew_rate * _dt;
+	}
+
+	_roll_setpoint = roll_new;
+
 }
 
 float ECL_L1_Pos_Controller::switch_distance(float wp_radius)
@@ -182,6 +198,8 @@ ECL_L1_Pos_Controller::navigate_waypoints(const Vector2f &vector_A, const Vector
 
 	/* the bearing angle, in NED frame */
 	_bearing_error = eta;
+
+	update_roll_setpoint();
 }
 
 void
@@ -281,6 +299,8 @@ ECL_L1_Pos_Controller::navigate_loiter(const Vector2f &vector_A, const Vector2f
 		/* bearing from current position to L1 point */
 		_nav_bearing = atan2f(-vector_A_to_airplane_unit(1), -vector_A_to_airplane_unit(0));
 	}
+
+	update_roll_setpoint();
 }
 
 void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float current_heading,
@@ -316,6 +336,8 @@ void ECL_L1_Pos_Controller::navigate_heading(float navigation_heading, float cur
 	/* limit eta to 90 degrees */
 	eta = math::constrain(eta, (-M_PI_F) / 2.0f, +M_PI_F / 2.0f);
 	_lateral_accel = 2.0f * sinf(eta) * omega_vel;
+
+	update_roll_setpoint();
 }
 
 void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
@@ -331,6 +353,8 @@ void ECL_L1_Pos_Controller::navigate_level_flight(float current_heading)
 
 	/* not circling a waypoint when flying level */
 	_circle_mode = false;
+
+	update_roll_setpoint();
 }
 
 Vector2f ECL_L1_Pos_Controller::get_local_planar_vector(const Vector2f &origin, const Vector2f &target) const
diff --git a/l1/ecl_l1_pos_controller.h b/l1/ecl_l1_pos_controller.h
index 5928c66658..610a54d503 100644
--- a/l1/ecl_l1_pos_controller.h
+++ b/l1/ecl_l1_pos_controller.h
@@ -105,7 +105,7 @@ public:
 	 *
 	 * @return Roll angle (in NED frame)
 	 */
-	float nav_roll();
+	float get_roll_setpoint(){ return _roll_setpoint; }
 
 	/**
 	 * Get the current crosstrack error.
@@ -195,6 +195,16 @@ public:
 	 */
 	void set_l1_roll_limit(float roll_lim_rad) { _roll_lim_rad = roll_lim_rad; }
 
+	/**
+	 * Set roll angle slew rate. Set to zero to deactivate.
+	 */
+	void set_roll_slew_rate(float roll_slew_rate) { _roll_slew_rate = roll_slew_rate; }
+
+	/**
+	 * Set control loop dt. The value will be used to apply roll angle setpoint slew rate limiting.
+	 */
+	void set_dt(float dt) { _dt = dt;}
+
 private:
 
 	float _lateral_accel{0.0f};		///< Lateral acceleration setpoint in m/s^2
@@ -211,7 +221,10 @@ private:
 	float _K_L1{2.0f};			///< L1 control gain for _L1_damping
 	float _heading_omega{1.0f};		///< Normalized frequency
 
-	float _roll_lim_rad{math::radians(30.0f)};  ///<maximum roll angle
+	float _roll_lim_rad{math::radians(30.0f)};  ///<maximum roll angle in radians
+	float _roll_setpoint{0.0f};	///< current roll angle setpoint in radians
+	float _roll_slew_rate{0.0f};	///< roll angle setpoint slew rate limit in rad/s
+	float _dt{0};				///< control loop time in seconds
 
 	/**
 	 * Convert a 2D vector from WGS84 to planar coordinates.
@@ -226,6 +239,12 @@ private:
 	 */
 	matrix::Vector2f get_local_planar_vector(const matrix::Vector2f &origin, const matrix::Vector2f &target) const;
 
+	/**
+	 * Update roll angle setpoint. This will also apply slew rate limits if set.
+	 *
+	 */
+	void update_roll_setpoint();
+
 };