renamed heading controller to wheel controller, added groundspeed dependency and separate parameters

msg/vehicle_attitude_setpoint.msg

@@ -22,4 +22,4 @@ bool roll_reset_integral			# Reset roll integral part (navigation logic change)
 bool pitch_reset_integral			# Reset pitch integral part (navigation logic change)
 bool yaw_reset_integral				# Reset yaw integral part (navigation logic change)
 
-bool fw_control_yaw					# control heading with rudder (used for auto takeoff on runway)
+bool fw_control_wheel					# control heading with rudder (used for auto takeoff on runway)

src/lib/ecl/attitude_fw/ecl_controller.h

@@ -75,6 +75,7 @@ struct ECL_ControlData {
 	float airspeed_max;
 	float airspeed;
 	float scaler;
+	float ground_speed;
 	bool lock_integrator;
 };
 

src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp

@@ -32,13 +32,13 @@
  ****************************************************************************/
 
 /**
- * @file ecl_yaw_controller.cpp
+ * @file ecl_wheel_controller.cpp
  * Implementation of a simple orthogonal coordinated turn yaw PID controller.
  *
  * Authors and acknowledgements in header.
  */
 
-#include "ecl_heading_controller.h"
+#include "ecl_wheel_controller.h"
 #include <stdint.h>
 #include <float.h>
 #include <geo/geo.h>
@@ -47,22 +47,20 @@
 #include <systemlib/err.h>
 #include <ecl/ecl.h>
 
-ECL_HeadingController::ECL_HeadingController() :
+ECL_WheelController::ECL_WheelController() :
-    ECL_Controller("heading")
+    ECL_Controller("wheel")
 {
 }
 
-ECL_HeadingController::~ECL_HeadingController()
+ECL_WheelController::~ECL_WheelController()
 {
 }
 
-float ECL_HeadingController::control_bodyrate(const struct ECL_ControlData &ctl_data)
+float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data)
 {
     /* Do not calculate control signal with bad inputs */
-    if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.pitch_rate) &&
+    if (!(PX4_ISFINITE(ctl_data.yaw_rate) &&
-          PX4_ISFINITE(ctl_data.yaw_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) &&
+          PX4_ISFINITE(ctl_data.ground_speed))) {
-          PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) &&
-          PX4_ISFINITE(ctl_data.scaler))) {
         perf_count(_nonfinite_input_perf);
         return math::constrain(_last_output, -1.0f, 1.0f);
     }
@@ -80,27 +78,21 @@ float ECL_HeadingController::control_bodyrate(const struct ECL_ControlData &ctl_
     }
 
     /* input conditioning */
-    float airspeed = ctl_data.airspeed;
+    float min_speed = 1.0f;
-
+    /* assume minimum speed to prevent oscillations */
-    if (!PX4_ISFINITE(airspeed)) {
+    float speed = min_speed;
-        /* airspeed is NaN, +- INF or not available, pick center of band */
+    if (ctl_data.ground_speed > speed) {
-        airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
+        speed = ctl_data.ground_speed;
-
-    } else if (airspeed < ctl_data.airspeed_min) {
-        airspeed = ctl_data.airspeed_min;
     }
 
-
+    float scaler = 1.0f / speed;
-    /* Transform setpoint to body angular rates (jacobian) */
-    _bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
-                 cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint;
 
     /* Calculate body angular rate error */
-    _rate_error = _bodyrate_setpoint - ctl_data.yaw_rate; //body angular rate error
+    _rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error
 
-    if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
+    if (!lock_integrator && _k_i > 0.0f && speed > min_speed) {
 
-        float id = _rate_error * dt;
+        float id = _rate_error * dt * scaler;
 
         /*
          * anti-windup: do not allow integrator to increase if actuator is at limit
@@ -122,23 +114,22 @@ float ECL_HeadingController::control_bodyrate(const struct ECL_ControlData &ctl_
     float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
 
     /* Apply PI rate controller and store non-limited output */
-    _last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler *
+    _last_output = (_rate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler;
-               ctl_data.scaler;  //scaler is proportional to 1/airspeed
+    /*warnx("wheel: _last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, speed %.4f, _k_i %.4f, _k_p: %.4f",
-    warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
+            (double)_last_output, (double)_integrator, (double)_integrator_max, (double)speed, (double)_k_i, (double)_k_p);*/
 
 
     return math::constrain(_last_output, -1.0f, 1.0f);
 }
 
 
-float ECL_HeadingController::control_attitude(const struct ECL_ControlData &ctl_data)
+float ECL_WheelController::control_attitude(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.yaw))) {
-          PX4_ISFINITE(ctl_data.airspeed))) {
         perf_count(_nonfinite_input_perf);
-        warnx("not controlling yaw");
+        warnx("not controlling wheel");
         return _rate_setpoint;
     }
 

src/lib/ecl/attitude_fw/ecl_wheel_controller.h

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file ecl_yaw_controller.h
+ * @file ecl_wheel_controller.h
  * Definition of a simple orthogonal coordinated turn yaw PID controller.
  *
  * @author Lorenz Meier <lm@inf.ethz.ch>
@@ -54,13 +54,13 @@
 
 #include "ecl_controller.h"
 
-class __EXPORT ECL_HeadingController :
+class __EXPORT ECL_WheelController :
     public ECL_Controller
 {
 public:
-    ECL_HeadingController();
+    ECL_WheelController();
 
-    ~ECL_HeadingController();
+    ~ECL_WheelController();
 
     float control_attitude(const struct ECL_ControlData &ctl_data);
 

src/lib/runway_takeoff/RunwayTakeoff.cpp

@@ -123,10 +123,10 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd)
 	}
 }
 
-bool RunwayTakeoff::controlYaw()
+bool RunwayTakeoff::controlWheel()
 {
-	// keep controlling yaw with rudder until we have enough ground clearance
+	// keep controlling wheel until takeoff
-	return _state < RunwayTakeoffState::FLY;
+	return _state < RunwayTakeoffState::TAKEOFF;
 }
 
 float RunwayTakeoff::getPitch(float tecsPitch)
@@ -140,6 +140,7 @@ float RunwayTakeoff::getPitch(float tecsPitch)
 
 float RunwayTakeoff::getRoll(float navigatorRoll)
 {
+	// until we have enough ground clearance, set roll to 0
 	if (_state < RunwayTakeoffState::FLY) {
 		return 0.0f;
 	}

src/lib/runway_takeoff/RunwayTakeoff.h

@@ -75,7 +75,7 @@ public:
 
 	bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); };
 
-	bool controlYaw();
+	bool controlWheel();
 	bool climbout() { return _climbout; };
 	float getPitch(float tecsPitch);
 	float getRoll(float navigatorRoll);

src/modules/fw_att_control/fw_att_control_main.cpp

@@ -82,7 +82,7 @@
 #include <ecl/attitude_fw/ecl_pitch_controller.h>
 #include <ecl/attitude_fw/ecl_roll_controller.h>
 #include <ecl/attitude_fw/ecl_yaw_controller.h>
-#include <ecl/attitude_fw/ecl_heading_controller.h>
+#include <ecl/attitude_fw/ecl_wheel_controller.h>
 #include <platforms/px4_defines.h>
 
 /**
@@ -182,6 +182,11 @@ private:
 		float y_coordinated_min_speed;
 		int32_t y_coordinated_method;
 		float y_rmax;
+		float w_p;
+		float w_i;
+		float w_ff;
+		float w_integrator_max;
+		float w_rmax;
 
 		float airspeed_min;
 		float airspeed_trim;
@@ -223,6 +228,11 @@ private:
 		param_t y_coordinated_min_speed;
 		param_t y_coordinated_method;
 		param_t y_rmax;
+		param_t w_p;
+		param_t w_i;
+		param_t w_ff;
+		param_t w_integrator_max;
+		param_t w_rmax;
 
 		param_t airspeed_min;
 		param_t airspeed_trim;
@@ -249,7 +259,7 @@ private:
 	ECL_RollController				_roll_ctrl;
 	ECL_PitchController				_pitch_ctrl;
 	ECL_YawController				_yaw_ctrl;
-	ECL_HeadingController			_heading_ctrl;
+	ECL_WheelController			_wheel_ctrl;
 
 
 	/**
@@ -382,6 +392,12 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
 	_parameter_handles.y_integrator_max = param_find("FW_YR_IMAX");
 	_parameter_handles.y_rmax = param_find("FW_Y_RMAX");
 
+	_parameter_handles.w_p = param_find("FW_WR_P");
+	_parameter_handles.w_i = param_find("FW_WR_I");
+	_parameter_handles.w_ff = param_find("FW_WR_FF");
+	_parameter_handles.w_integrator_max = param_find("FW_WR_IMAX");
+	_parameter_handles.w_rmax = param_find("FW_W_RMAX");
+
 	_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
 	_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
 	_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
@@ -460,6 +476,12 @@ FixedwingAttitudeControl::parameters_update()
 	param_get(_parameter_handles.y_coordinated_method, &(_parameters.y_coordinated_method));
 	param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
 
+	param_get(_parameter_handles.w_p, &(_parameters.w_p));
+	param_get(_parameter_handles.w_i, &(_parameters.w_i));
+	param_get(_parameter_handles.w_ff, &(_parameters.w_ff));
+	param_get(_parameter_handles.w_integrator_max, &(_parameters.w_integrator_max));
+	param_get(_parameter_handles.w_rmax, &(_parameters.w_rmax));
+
 	param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
 	param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
 	param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
@@ -504,12 +526,12 @@ FixedwingAttitudeControl::parameters_update()
 	_yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method);
 	_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
 
-	/* heading control parameters */
+	/* wheel control parameters */
-	_heading_ctrl.set_k_p(_parameters.y_p);
+	_wheel_ctrl.set_k_p(_parameters.w_p);
-	_heading_ctrl.set_k_i(_parameters.y_i);
+	_wheel_ctrl.set_k_i(_parameters.w_i);
-	_heading_ctrl.set_k_ff(_parameters.y_ff);
+	_wheel_ctrl.set_k_ff(_parameters.w_ff);
-	_heading_ctrl.set_integrator_max(_parameters.y_integrator_max);
+	_wheel_ctrl.set_integrator_max(_parameters.w_integrator_max);
-	_heading_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
+	_wheel_ctrl.set_max_rate(math::radians(_parameters.w_rmax));
 
 	return OK;
 }
@@ -846,6 +868,7 @@ FixedwingAttitudeControl::task_main()
 					}
 					if (_att_sp.yaw_reset_integral) {
 						_yaw_ctrl.reset_integrator();
+						_wheel_ctrl.reset_integrator();
 					}
 				} else if (_vcontrol_mode.flag_control_velocity_enabled) {
 
@@ -871,6 +894,7 @@ FixedwingAttitudeControl::task_main()
 					}
 					if (_att_sp.yaw_reset_integral) {
 						_yaw_ctrl.reset_integrator();
+						_wheel_ctrl.reset_integrator();
 					}
 
 				} else if (_vcontrol_mode.flag_control_altitude_enabled) {
@@ -890,6 +914,7 @@ FixedwingAttitudeControl::task_main()
 					}
 					if (_att_sp.yaw_reset_integral) {
 						_yaw_ctrl.reset_integrator();
+						_wheel_ctrl.reset_integrator();
 					}
 				} else {
 					/*
@@ -940,6 +965,7 @@ FixedwingAttitudeControl::task_main()
 					_roll_ctrl.reset_integrator();
 					_pitch_ctrl.reset_integrator();
 					_yaw_ctrl.reset_integrator();
+					_wheel_ctrl.reset_integrator();
 				}
 
 				/* Prepare speed_body_u and speed_body_w */
@@ -969,6 +995,8 @@ FixedwingAttitudeControl::task_main()
 				control_input.airspeed = airspeed;
 				control_input.scaler = airspeed_scaling;
 				control_input.lock_integrator = lock_integrator;
+				control_input.ground_speed = sqrtf(_global_pos.vel_n * _global_pos.vel_n +
+						_global_pos.vel_e * _global_pos.vel_e);
 
 				_yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method);
 
@@ -977,7 +1005,7 @@ FixedwingAttitudeControl::task_main()
 					_roll_ctrl.control_attitude(control_input);
 					_pitch_ctrl.control_attitude(control_input);
 					_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
-					_heading_ctrl.control_attitude(control_input);
+					_wheel_ctrl.control_attitude(control_input);
 
 					/* Update input data for rate controllers */
 					control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
@@ -1018,8 +1046,8 @@ FixedwingAttitudeControl::task_main()
 					}
 
 					float yaw_u = 0.0f;
-					if (_att_sp.fw_control_yaw == true) {
+					if (_att_sp.fw_control_wheel == true) {
-						yaw_u = _heading_ctrl.control_bodyrate(control_input);
+						yaw_u = _wheel_ctrl.control_bodyrate(control_input);
 					}
 
 					else {
@@ -1031,6 +1059,7 @@ FixedwingAttitudeControl::task_main()
 					_actuators.control[2] += yaw_manual;
 					if (!PX4_ISFINITE(yaw_u)) {
 						_yaw_ctrl.reset_integrator();
+						_wheel_ctrl.reset_integrator();
 						perf_count(_nonfinite_output_perf);
 						if (_debug && loop_counter % 10 == 0) {
 							warnx("yaw_u %.4f", (double)yaw_u);

src/modules/fw_att_control/fw_att_control_params.c

@@ -220,6 +220,55 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
  */
 PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f);
 
+/**
+ * Wheel steering rate proportional gain
+ *
+ * This defines how much the wheel steering input will be commanded depending on the
+ * current body angular rate error.
+ *
+ * @min 0.005
+ * @max 1.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_WR_P, 0.05f);
+
+/**
+ * Wheel steering rate integrator gain
+ *
+ * This gain defines how much control response will result out of a steady
+ * state error. It trims any constant error.
+ *
+ * @min 0.0
+ * @max 50.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_WR_I, 0.0f);
+
+/**
+ * Wheel steering rate integrator limit
+ *
+ * The portion of the integrator part in the control surface deflection is
+ * limited to this value
+ *
+ * @min 0.0
+ * @max 1.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_WR_IMAX, 0.2f);
+
+/**
+ * Maximum wheel steering rate
+ *
+ * This limits the maximum wheel steering rate the controller will output (in degrees per
+ * second). Setting a value of zero disables the limit.
+ *
+ * @unit deg/s
+ * @min 0.0
+ * @max 90.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_W_RMAX, 0.0f);
+
 /**
  * Roll rate feed forward
  *
@@ -255,6 +304,17 @@ PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f);
  */
 PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
 
+/**
+ * Wheel steering rate feed forward
+ *
+ * Direct feed forward from rate setpoint to control surface output
+ *
+ * @min 0.0
+ * @max 10.0
+ * @group FW Attitude Control
+ */
+PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f);
+
 /**
  * Minimal speed for yaw coordination
  *

src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp

@@ -1074,7 +1074,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 
 	bool setpoint = true;
 
-	_att_sp.fw_control_yaw = false;		// by default we don't want yaw to be contoller directly with rudder
+	_att_sp.fw_control_wheel = false;		// by default we don't want yaw to be contoller directly with rudder
 
 	float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
 
@@ -1406,9 +1406,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
 				// assign values
 				_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll());
 				_att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing());
-				_att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); // tell attitude controller if he should control yaw directly
+				_att_sp.fw_control_wheel = _runway_takeoff.controlWheel();
 				_att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand());
-
+				/*warnx("yaw: %.4f, roll: %.4f, pitch: %.4f", (double)_att_sp.yaw_body,
+					(double)_att_sp.roll_body, (double)_att_sp.pitch_body);*/
 
 			} else {
 				/* Perform launch detection */