fw att: add param to select yawrate control methodyawrate

4 files changed, 62 insertions, 2 deletions

diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
index 82ba2c6c7..9c386a3e8 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp
@@ -45,10 +45,12 @@
 #include <ecl/ecl.h>
 #include <mathlib/mathlib.h>
 #include <systemlib/err.h>
+#include <drivers/drv_hrt.h>
 
 ECL_YawController::ECL_YawController() :
 	ECL_Controller("yaw"),
-	_coordinated_min_speed(1.0f)
+	_coordinated_min_speed(1.0f),
+	_coordinated_method(0)
 {
 }
 
@@ -58,6 +60,36 @@ ECL_YawController::~ECL_YawController()
 
 float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data)
 {
+	switch (_coordinated_method) {
+		case 0:
+			return control_attitude_impl_openloop(ctl_data);
+		default:
+			static hrt_abstime last_print = 0;
+			if (hrt_elapsed_time(&last_print) > 5e6) {
+				warnx("invalid param setting FW_YCO_METHOD");
+				last_print = hrt_absolute_time();
+			}
+	}
+	return _rate_setpoint;
+}
+
+float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data)
+{
+	switch (_coordinated_method) {
+		case 0:
+			return control_bodyrate_impl(ctl_data);
+		default:
+			static hrt_abstime last_print = 0;
+			if (hrt_elapsed_time(&last_print) > 5e6) {
+				warnx("invalid param setting FW_YCO_METHOD");
+				last_print = hrt_absolute_time();
+			}
+	}
+	return math::constrain(_last_output, -1.0f, 1.0f);
+}
+
+float ECL_YawController::control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data)
+{
 	/* Do not calculate control signal with bad inputs */
 	if (!(isfinite(ctl_data.roll) &&
 				isfinite(ctl_data.pitch) &&
@@ -109,7 +141,7 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data
 	return _rate_setpoint;
 }
 
-float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data)
+float ECL_YawController::control_bodyrate_impl(const struct ECL_ControlData &ctl_data)
 {
 	/* Do not calculate control signal with bad inputs */
 	if (!(isfinite(ctl_data.roll) && isfinite(ctl_data.pitch) && isfinite(ctl_data.pitch_rate) &&
diff --git a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
index 37c03705e..ab7dce2b1 100644
--- a/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
+++ b/src/lib/ecl/attitude_fw/ecl_yaw_controller.h
@@ -62,6 +62,7 @@ public:
 	~ECL_YawController();
 
 	float control_attitude(const struct ECL_ControlData &ctl_data);
+
 	float control_bodyrate(const struct ECL_ControlData &ctl_data);
 
 	/* Additional setters */
@@ -69,8 +70,17 @@ public:
 		_coordinated_min_speed = coordinated_min_speed;
 	}
 
+	void set_coordinated_method(int32_t coordinated_method) {
+		_coordinated_method = coordinated_method;
+	}
+
 protected:
 	float _coordinated_min_speed;
+	int32_t _coordinated_method;;
+
+	float control_attitude_impl_openloop(const struct ECL_ControlData &ctl_data);
+	float control_bodyrate_impl(const struct ECL_ControlData &ctl_data);
+
 };
 
 #endif // ECL_YAW_CONTROLLER_H
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 6f225bb48..14c6c5916 100644
--- a/src/modules/fw_att_control/fw_att_control_main.cpp
+++ b/src/modules/fw_att_control/fw_att_control_main.cpp
@@ -178,6 +178,7 @@ private:
 		float y_roll_feedforward;
 		float y_integrator_max;
 		float y_coordinated_min_speed;
+		int32_t y_coordinated_method;
 		float y_rmax;
 
 		float airspeed_min;
@@ -220,6 +221,7 @@ private:
 		param_t y_roll_feedforward;
 		param_t y_integrator_max;
 		param_t y_coordinated_min_speed;
+		param_t y_coordinated_method;
 		param_t y_rmax;
 
 		param_t airspeed_min;
@@ -386,6 +388,7 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
 
 	_parameter_handles.y_coordinated_min_speed = param_find("FW_YCO_VMIN");
+	_parameter_handles.y_coordinated_method = param_find("FW_YCO_METHOD");
 
 	_parameter_handles.trim_roll = param_find("TRIM_ROLL");
 	_parameter_handles.trim_pitch = param_find("TRIM_PITCH");
@@ -454,6 +457,7 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.y_ff, &(_parameters.y_ff));
 	param_get(_parameter_handles.y_integrator_max, &(_parameters.y_integrator_max));
 	param_get(_parameter_handles.y_coordinated_min_speed, &(_parameters.y_coordinated_min_speed));
+	param_get(_parameter_handles.y_coordinated_method, &(_parameters.y_coordinated_method));
 	param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
 
 	param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
@@ -496,6 +500,7 @@ FixedwingAttitudeControl::parameters_update()
 	_yaw_ctrl.set_k_ff(_parameters.y_ff);
 	_yaw_ctrl.set_integrator_max(_parameters.y_integrator_max);
 	_yaw_ctrl.set_coordinated_min_speed(_parameters.y_coordinated_min_speed);
+	_yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method);
 	_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
 
 	return OK;
diff --git a/src/modules/fw_att_control/fw_att_control_params.c b/src/modules/fw_att_control/fw_att_control_params.c
index 7cae84678..f48075d71 100644
--- a/src/modules/fw_att_control/fw_att_control_params.c
+++ b/src/modules/fw_att_control/fw_att_control_params.c
@@ -275,6 +275,19 @@ PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
  */
 PARAM_DEFINE_FLOAT(FW_YCO_VMIN, 1000.0f);
 
+/**
+ * Method used for yaw coordination
+ *
+ * The param value sets the method used to calculate the yaw rate
+ * 0: open-loop zero lateral acceleration based on kinematic constraints
+ *
+ * @min 0
+ * @max 0
+ * @unit m/s
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_INT32(FW_YCO_METHOD, 0);
+
 /* Airspeed parameters:
  * The following parameters about airspeed are used by the attitude and the
  * position controller.