FW att controller: wheel controller: separate from ecl_controller

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>

src/modules/fw_att_control/CMakeLists.txt

@@ -41,7 +41,7 @@ px4_add_module(
 		ecl_controller.cpp
 		fw_pitch_controller.cpp
 		fw_roll_controller.cpp
-		ecl_wheel_controller.cpp
+		fw_wheel_controller.cpp
 		fw_yaw_controller.cpp
 	DEPENDS
 		px4_work_queue

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -36,7 +36,7 @@
 #include <drivers/drv_hrt.h>
 #include "fw_pitch_controller.h"
 #include "fw_roll_controller.h"
-#include "ecl_wheel_controller.h"
+#include "fw_wheel_controller.h"
 #include "fw_yaw_controller.h"
 #include <lib/mathlib/mathlib.h>
 #include <lib/parameters/param.h>
@@ -162,7 +162,7 @@ private:
 	RollController _roll_ctrl;
 	PitchController _pitch_ctrl;
 	YawController _yaw_ctrl;
-	ECL_WheelController _wheel_ctrl;
+	WheelController _wheel_ctrl;
 
 	void parameters_update();
 	void vehicle_manual_poll(const float yaw_body);

src/modules/fw_att_control/fw_wheel_controller.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2016 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,13 +32,11 @@
  ****************************************************************************/
 
 /**
- * @file ecl_wheel_controller.cpp
+ * @file fw_wheel_controller.cpp
  * Implementation of a simple PID wheel controller for heading tracking.
- *
- * Authors and acknowledgements in header.
  */
 
-#include "ecl_wheel_controller.h"
+#include "fw_wheel_controller.h"
 #include <float.h>
 #include <lib/geo/geo.h>
 #include <mathlib/mathlib.h>
@@ -46,68 +44,56 @@
 
 using matrix::wrap_pi;
 
-float ECL_WheelController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
+float WheelController::control_bodyrate(float dt, float body_z_rate, float groundspeed, float groundspeed_scaler)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(ctl_data.body_z_rate) &&
+	if (!(PX4_ISFINITE(body_z_rate) &&
-	      PX4_ISFINITE(ctl_data.groundspeed) &&
+	      PX4_ISFINITE(groundspeed) &&
-	      PX4_ISFINITE(ctl_data.groundspeed_scaler))) {
+	      PX4_ISFINITE(groundspeed_scaler))) {
 
-		return math::constrain(_last_output, -1.0f, 1.0f);
+		return math::constrain(_last_output, -1.f, 1.f);
 	}
 
-	/* input conditioning */
+	const float rate_error = _body_rate_setpoint - body_z_rate;
-	float min_speed = 1.0f;
 
-	/* Calculate body angular rate error */
+	if (_k_i > 0.f && groundspeed > 1.f) { // only start integrating when above 1m/s
-	const float rate_error = _body_rate_setpoint - ctl_data.body_z_rate; //body angular rate error
 
-	if (_k_i > 0.0f && ctl_data.groundspeed > min_speed) {
+		float id = rate_error * dt * groundspeed_scaler;
 
-		float id = rate_error * dt * ctl_data.groundspeed_scaler;
+		if (_last_output < -1.f) {
-
-		/*
-		 * anti-windup: do not allow integrator to increase if actuator is at limit
-		 */
-		if (_last_output < -1.0f) {
 			/* only allow motion to center: increase value */
-			id = math::max(id, 0.0f);
+			id = math::max(id, 0.f);
 
-		} else if (_last_output > 1.0f) {
+		} else if (_last_output > 1.f) {
 			/* only allow motion to center: decrease value */
-			id = math::min(id, 0.0f);
+			id = math::min(id, 0.f);
 		}
 
-		/* add and constrain */
 		_integrator = math::constrain(_integrator + id * _k_i, -_integrator_max, _integrator_max);
 	}
 
 	/* Apply PI rate controller and store non-limited output */
-	_last_output = _body_rate_setpoint * _k_ff * ctl_data.groundspeed_scaler +
+	_last_output = _body_rate_setpoint * _k_ff * groundspeed_scaler +
-		       ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (rate_error * _k_p + _integrator);
+		       groundspeed_scaler * groundspeed_scaler * (rate_error * _k_p + _integrator);
 
-	return math::constrain(_last_output, -1.0f, 1.0f);
+	return math::constrain(_last_output, -1.f, 1.f);
 }
 
-float ECL_WheelController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
+float WheelController::control_attitude(float yaw_setpoint, float yaw)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) &&
+	if (!(PX4_ISFINITE(yaw_setpoint) &&
-	      PX4_ISFINITE(ctl_data.yaw))) {
+	      PX4_ISFINITE(yaw))) {
 
 		return _body_rate_setpoint;
 	}
 
-	/* Calculate the error */
+	const float yaw_error = wrap_pi(yaw_setpoint - yaw);
-	float yaw_error = wrap_pi(ctl_data.yaw_setpoint - ctl_data.yaw);
 
-	/*  Apply P controller: rate setpoint from current error and time constant */
+	_body_rate_setpoint = yaw_error / _tc; // assume 0 pitch and roll angle, thus jacobian is simply identity matrix
-	_euler_rate_setpoint =  yaw_error / _tc;
-	_body_rate_setpoint = _euler_rate_setpoint; // assume 0 pitch and roll angle, thus jacobian is simply identity matrix
 
-	/* limit the rate */
 	if (_max_rate > 0.01f) {
-		if (_body_rate_setpoint > 0.0f) {
+		if (_body_rate_setpoint > 0.f) {
 			_body_rate_setpoint = (_body_rate_setpoint > _max_rate) ? _max_rate : _body_rate_setpoint;
 
 		} else {

src/modules/fw_att_control/fw_wheel_controller.h

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2020-2022 PX4 Development Team. All rights reserved.
+ *   Copyright (c) 2020-2023 PX4 Development Team. All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,44 +32,48 @@
  ****************************************************************************/
 
 /**
- * @file ecl_wheel_controller.h
+ * @file fw_wheel_controller.h
- * Definition of a simple orthogonal coordinated turn yaw PID controller.
+ * Definition of a simple wheel controller.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@gmail.com>
- * @author Andreas Antener <andreas@uaventure.com>
- *
- * Acknowledgements:
- *
- *   The control design is based on a design
- *   by Paul Riseborough and Andrew Tridgell, 2013,
- *   which in turn is based on initial work of
- *   Jonathan Challinger, 2012.
  */
-#ifndef ECL_HEADING_CONTROLLER_H
+#ifndef FW_WHEEL_CONTROLLER_H
-#define ECL_HEADING_CONTROLLER_H
+#define FW_WHEEL_CONTROLLER_H
 
-#include "ecl_controller.h"
+class WheelController
-
-class ECL_WheelController :
-	public ECL_Controller
 {
 public:
-	ECL_WheelController() = default;
+	WheelController() = default;
-	~ECL_WheelController() = default;
+	~WheelController() = default;
 
 	/**
 	 * @brief Calculates wheel body rate setpoint.
 	 *
-	 * @param dt Time step [s]
+	 * @param yaw_setpoint yaw setpoint [rad]
-	 * @param ctrl_data Various control inputs (attitude, body rates, attitdue stepoints, euler rate setpoints, current speeed)
+	 * @param yaw estimated yaw [rad]
 	 * @return Wheel body rate setpoint [rad/s]
 	 */
-	float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
+	float control_attitude(float yaw_setpoint, float yaw);
 
-	float control_bodyrate(const float dt, const ECL_ControlData &ctl_data);
+	float control_bodyrate(float dt, float body_z_rate, float groundspeed, float groundspeed_scaler);
 
-	float control_euler_rate(const float dt, const ECL_ControlData &ctl_data, float bodyrate_ff) { (void)ctl_data; return 0; }
+	void set_time_constant(float time_constant) { _tc = time_constant; }
+	void set_k_p(float k_p) { _k_p = k_p; }
+	void set_k_i(float k_i) { _k_i = k_i; }
+	void set_k_ff(float k_ff) { _k_ff = k_ff; }
+	void set_integrator_max(float max) { _integrator_max = max; }
+	void set_max_rate(float max_rate) { _max_rate = max_rate; }
+
+	void reset_integrator() { _integrator = 0.f; }
+
+private:
+	float _tc;
+	float _k_p;
+	float _k_i;
+	float _k_ff;
+	float _integrator_max;
+	float _max_rate;
+	float _last_output;
+	float _integrator;
+	float _body_rate_setpoint;
 };
 
-#endif // ECL_HEADING_CONTROLLER_H
+#endif // FW_WHEEL_CONTROLLER_H