FW att controller: pitch controller: separate from ecl_controller

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

src/modules/fw_att_control/CMakeLists.txt

@@ -39,7 +39,7 @@ px4_add_module(
 		FixedwingAttitudeControl.hpp
 
 		ecl_controller.cpp
-		ecl_pitch_controller.cpp
+		fw_pitch_controller.cpp
 		fw_roll_controller.cpp
 		ecl_wheel_controller.cpp
 		ecl_yaw_controller.cpp

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -343,7 +343,8 @@ void FixedwingAttitudeControl::Run()
 				if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) {
 					_roll_ctrl.control_roll(_att_sp.roll_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
 								euler_angles.theta());
-					_pitch_ctrl.control_attitude(dt, control_input);
+					_pitch_ctrl.control_pitch(_att_sp.pitch_body, _yaw_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
+								  euler_angles.theta());
 					_yaw_ctrl.control_attitude(dt, control_input);
 
 					if (wheel_control) {

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -34,7 +34,7 @@
 #pragma once
 
 #include <drivers/drv_hrt.h>
-#include "ecl_pitch_controller.h"
+#include "fw_pitch_controller.h"
 #include "fw_roll_controller.h"
 #include "ecl_wheel_controller.h"
 #include "ecl_yaw_controller.h"
@@ -160,7 +160,7 @@ private:
 	)
 
 	RollController _roll_ctrl;
-	ECL_PitchController _pitch_ctrl;
+	PitchController _pitch_ctrl;
 	ECL_YawController _yaw_ctrl;
 	ECL_WheelController _wheel_ctrl;
 

src/modules/fw_att_control/fw_pitch_controller.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2020 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,38 +32,34 @@
  ****************************************************************************/
 
 /**
- * @file ecl_pitch_controller.cpp
- * Implementation of a simple orthogonal pitch PID controller.
- *
- * Authors and acknowledgements in header.
+ * @file fw_pitch_controller.cpp
+ * Implementation of a simple pitch P controller.
  */
 
-#include "ecl_pitch_controller.h"
+#include "fw_pitch_controller.h"
 #include <float.h>
 #include <lib/geo/geo.h>
 #include <mathlib/mathlib.h>
 
-float ECL_PitchController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
+float PitchController::control_pitch(float pitch_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) &&
-	      PX4_ISFINITE(ctl_data.roll) &&
-	      PX4_ISFINITE(ctl_data.pitch) &&
-	      PX4_ISFINITE(ctl_data.euler_yaw_rate_setpoint))) {
+	if (!(PX4_ISFINITE(pitch_setpoint) &&
+	      PX4_ISFINITE(euler_yaw_rate_setpoint) &&
+	      PX4_ISFINITE(pitch) &&
+	      PX4_ISFINITE(roll))) {
 
 		return _body_rate_setpoint;
 	}
 
-	/* Calculate the error */
-	float pitch_error = ctl_data.pitch_setpoint - ctl_data.pitch;
-
-	/*  Apply P controller: rate setpoint from current error and time constant */
-	_euler_rate_setpoint =  pitch_error / _tc;
+	const float pitch_error = pitch_setpoint - pitch;
+	_euler_rate_setpoint = pitch_error / _tc;
 
 	/* Transform setpoint to body angular rates (jacobian) */
-	const float pitch_body_rate_setpoint_raw = cosf(ctl_data.roll) * _euler_rate_setpoint +
-			cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.euler_yaw_rate_setpoint;
-	_body_rate_setpoint = math::constrain(pitch_body_rate_setpoint_raw, -_max_rate_neg, _max_rate);
+	const float pitch_body_rate_setpoint_raw = cosf(roll) * _euler_rate_setpoint +
+			cosf(pitch) * sinf(roll) * euler_yaw_rate_setpoint;
+
+	_body_rate_setpoint = math::constrain(pitch_body_rate_setpoint_raw, -_max_rate_neg, _max_rate_pos);
 
 	return _body_rate_setpoint;
 }

src/modules/fw_att_control/fw_pitch_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,56 +32,43 @@
  ****************************************************************************/
 
 /**
- * @file ecl_pitch_controller.h
- * Definition of a simple orthogonal pitch PID controller.
- *
- * @author Lorenz Meier <lm@inf.ethz.ch>
- * @author Thomas Gubler <thomasgubler@gmail.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.
+ * @file fw_pitch_controller.h
+ * Definition of a simple pitch P controller.
  */
 
-#ifndef ECL_PITCH_CONTROLLER_H
-#define ECL_PITCH_CONTROLLER_H
+#ifndef FW_PITCH_CONTROLLER_H
+#define FW_PITCH_CONTROLLER_H
 
-#include <mathlib/mathlib.h>
-
-#include "ecl_controller.h"
-
-class ECL_PitchController :
-	public ECL_Controller
+class PitchController
 {
 public:
-	ECL_PitchController() = default;
-	~ECL_PitchController() = default;
+	PitchController() = default;
+	~PitchController() = default;
 
 	/**
 	 * @brief Calculates both euler and body pitch rate setpoints.
 	 *
-	 * @param dt Time step [s]
-	 * @param ctrl_data Various control inputs (attitude, body rates, attitdue stepoints, euler rate setpoints, current speeed)
+	 * @param pitch_setpoint pitch setpoint [rad]
+	 * @param euler_yaw_rate_setpoint euler yaw rate setpoint [rad/s]
+	 * @param roll estimated roll [rad]
+	 * @param pitch estimated pitch [rad]
 	 * @return Pitch body rate setpoint [rad/s]
 	 */
-	float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
+	float control_pitch(float pitch_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch);
 
-	/* Additional Setters */
-	void set_max_rate_pos(float max_rate_pos)
-	{
-		_max_rate = max_rate_pos;
-	}
+	void set_time_constant(float time_constant) { _tc = time_constant; }
+	void set_max_rate_pos(float max_rate_pos) { _max_rate_pos = max_rate_pos; }
+	void set_max_rate_neg(float max_rate_neg) { _max_rate_neg = max_rate_neg; }
 
-	void set_max_rate_neg(float max_rate_neg)
-	{
-		_max_rate_neg = max_rate_neg;
-	}
+	float get_euler_rate_setpoint() { return _euler_rate_setpoint; }
+	float get_body_rate_setpoint() { return _body_rate_setpoint; }
 
-protected:
-	float _max_rate_neg{0.0f};
+private:
+	float _tc;
+	float _max_rate_pos;
+	float _max_rate_neg;
+	float _euler_rate_setpoint;
+	float _body_rate_setpoint;
 };
 
-#endif // ECL_PITCH_CONTROLLER_H
+#endif // FW_PITCH_CONTROLLER_H

src/modules/fw_att_control/fw_roll_controller.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2023 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