Merge pull request #491 from thomasgubler/l1_max_roll_angle

added parameter for maximal roll angle

4 files changed, 20 insertions, 1 deletions

diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
index daf136d49..196ded26c 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.cpp
@@ -43,7 +43,7 @@
 float ECL_L1_Pos_Controller::nav_roll()
 {
 	float ret = atanf(_lateral_accel * 1.0f / CONSTANTS_ONE_G);
-	ret = math::constrain(ret, (-M_PI_F) / 2.0f, M_PI_F / 2.0f);
+	ret = math::constrain(ret, -_roll_lim_rad, _roll_lim_rad);
 	return ret;
 }
 
diff --git a/src/lib/ecl/l1/ecl_l1_pos_controller.h b/src/lib/ecl/l1/ecl_l1_pos_controller.h
index 5a17346cb..7a3c42a92 100644
--- a/src/lib/ecl/l1/ecl_l1_pos_controller.h
+++ b/src/lib/ecl/l1/ecl_l1_pos_controller.h
@@ -222,6 +222,15 @@ public:
 		_K_L1 = 4.0f * _L1_damping * _L1_damping;
 	}
 
+
+	/**
+	 * Set the maximum roll angle output in radians
+	 *
+	 */
+	void set_l1_roll_limit(float roll_lim_rad) {
+		_roll_lim_rad = roll_lim_rad;
+	}
+
 private:
 
 	float _lateral_accel;		///< Lateral acceleration setpoint in m/s^2
@@ -238,6 +247,8 @@ private:
 	float _K_L1;			///< L1 control gain for _L1_damping
 	float _heading_omega;		///< Normalized frequency
 
+	float _roll_lim_rad;  ///<maximum roll angle
+
 	/**
 	 * Convert a 2D vector from WGS84 to planar coordinates.
 	 *
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 cd4a0d58e..f09e44b39 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
@@ -192,6 +192,7 @@ private:
 
 		float pitch_limit_min;
 		float pitch_limit_max;
+		float roll_limit;
 		float throttle_min;
 		float throttle_max;
 		float throttle_cruise;
@@ -223,6 +224,7 @@ private:
 
 		param_t pitch_limit_min;
 		param_t pitch_limit_max;
+		param_t roll_limit;
 		param_t throttle_min;
 		param_t throttle_max;
 		param_t throttle_cruise;
@@ -339,6 +341,7 @@ FixedwingPositionControl::FixedwingPositionControl() :
 
 	_parameter_handles.pitch_limit_min = param_find("FW_P_LIM_MIN");
 	_parameter_handles.pitch_limit_max = param_find("FW_P_LIM_MAX");
+	_parameter_handles.roll_limit = param_find("FW_R_LIM");
 	_parameter_handles.throttle_min = param_find("FW_THR_MIN");
 	_parameter_handles.throttle_max = param_find("FW_THR_MAX");
 	_parameter_handles.throttle_cruise = param_find("FW_THR_CRUISE");
@@ -400,6 +403,7 @@ FixedwingPositionControl::parameters_update()
 
 	param_get(_parameter_handles.pitch_limit_min, &(_parameters.pitch_limit_min));
 	param_get(_parameter_handles.pitch_limit_max, &(_parameters.pitch_limit_max));
+	param_get(_parameter_handles.roll_limit, &(_parameters.roll_limit));
 	param_get(_parameter_handles.throttle_min, &(_parameters.throttle_min));
 	param_get(_parameter_handles.throttle_max, &(_parameters.throttle_max));
 	param_get(_parameter_handles.throttle_cruise, &(_parameters.throttle_cruise));
@@ -419,6 +423,7 @@ FixedwingPositionControl::parameters_update()
 
 	_l1_control.set_l1_damping(_parameters.l1_damping);
 	_l1_control.set_l1_period(_parameters.l1_period);
+	_l1_control.set_l1_roll_limit(math::radians(_parameters.roll_limit));
 
 	_tecs.set_time_const(_parameters.time_const);
 	_tecs.set_min_sink_rate(_parameters.min_sink_rate);
diff --git a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
index d210ec712..bf4b3b32a 100644
--- a/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
+++ b/src/modules/fw_pos_control_l1/fw_pos_control_l1_params.c
@@ -67,6 +67,9 @@ PARAM_DEFINE_FLOAT(FW_P_LIM_MIN, -45.0f);
 PARAM_DEFINE_FLOAT(FW_P_LIM_MAX, 45.0f);
 
 
+PARAM_DEFINE_FLOAT(FW_R_LIM, 45.0f);
+
+
 PARAM_DEFINE_FLOAT(FW_THR_MIN, 0.0f);