extended ecl yaw controller for yaw tracking with rudder

src/lib/ecl/attitude_fw/ecl_yaw_controller.cpp

@@ -67,6 +67,9 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data
 	case COORD_METHOD_CLOSEACC:
 		return control_attitude_impl_accclosedloop(ctl_data);
 
+	case COORD_METHOD_HEADING:
+		return control_heading(ctl_data);
+
 	default:
 		static hrt_abstime last_print = 0;
 
@@ -238,3 +241,34 @@ float ECL_YawController::control_attitude_impl_accclosedloop(const struct ECL_Co
 	/* dont set a rate setpoint */
 	return 0.0f;
 }
+
+float ECL_YawController::control_heading(const struct ECL_ControlData &ctl_data)
+{
+	/* Do not calculate control signal with bad inputs */
+	if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) &&
+	      PX4_ISFINITE(ctl_data.yaw) &&
+	      PX4_ISFINITE(ctl_data.airspeed))) {
+		perf_count(_nonfinite_input_perf);
+		warnx("not controlling yaw");
+		return _rate_setpoint;
+	}
+
+	/* Calculate the error */
+	float yaw_error = ctl_data.yaw_setpoint - ctl_data.yaw;
+
+	/*  Apply P controller: rate setpoint from current error and time constant */
+	_rate_setpoint =  yaw_error / _tc;
+
+	/* limit the rate */
+	if (_max_rate > 0.01f) {
+		if (_rate_setpoint > 0.0f) {
+			_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
+
+		} else {
+			_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
+		}
+
+	}
+
+	return _rate_setpoint;
+}

src/lib/ecl/attitude_fw/ecl_yaw_controller.h

@@ -76,14 +76,16 @@ public:
 		_coordinated_method = coordinated_method;
 	}
 
-protected:
-	float _coordinated_min_speed;
-
 	enum {
 		COORD_METHOD_OPEN = 0,
 		COORD_METHOD_CLOSEACC = 1,
+		COORD_METHOD_HEADING = 2
 	};
 
+protected:
+	float _coordinated_min_speed;
+	float _max_rate;
+
 	int32_t _coordinated_method;
 
 	float control_bodyrate_impl(const struct ECL_ControlData &ctl_data);
@@ -92,6 +94,8 @@ protected:
 
 	float control_attitude_impl_accclosedloop(const struct ECL_ControlData &ctl_data);
 
+	float control_heading(const struct ECL_ControlData &ctl_data);
+
 };
 
 #endif // ECL_YAW_CONTROLLER_H