FW att controller: roll controller: seperate from ecl_controller

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

src/modules/fw_att_control/CMakeLists.txt

@@ -40,7 +40,7 @@ px4_add_module(
 
 		ecl_controller.cpp
 		ecl_pitch_controller.cpp
-		ecl_roll_controller.cpp
+		fw_roll_controller.cpp
 		ecl_wheel_controller.cpp
 		ecl_yaw_controller.cpp
 	DEPENDS

src/modules/fw_att_control/FixedwingAttitudeControl.cpp

@@ -341,7 +341,8 @@ void FixedwingAttitudeControl::Run()
 
 			if (_vcontrol_mode.flag_control_attitude_enabled && _in_fw_or_transition_wo_tailsitter_transition) {
 				if (PX4_ISFINITE(_att_sp.roll_body) && PX4_ISFINITE(_att_sp.pitch_body)) {
-					_roll_ctrl.control_attitude(dt, control_input);
+					_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);
 					_yaw_ctrl.control_attitude(dt, control_input);
 

src/modules/fw_att_control/FixedwingAttitudeControl.hpp

@@ -35,7 +35,7 @@
 
 #include <drivers/drv_hrt.h>
 #include "ecl_pitch_controller.h"
-#include "ecl_roll_controller.h"
+#include "fw_roll_controller.h"
 #include "ecl_wheel_controller.h"
 #include "ecl_yaw_controller.h"
 #include <lib/mathlib/mathlib.h>
@@ -159,7 +159,7 @@ private:
 
 	)
 
-	ECL_RollController _roll_ctrl;
+	RollController _roll_ctrl;
 	ECL_PitchController _pitch_ctrl;
 	ECL_YawController _yaw_ctrl;
 	ECL_WheelController _wheel_ctrl;

src/modules/fw_att_control/fw_roll_controller.cpp

@@ -1,6 +1,6 @@
 /****************************************************************************
  *
- *   Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved.
+ *   Copyright (c) 2013-2023 Estimation and Control Library (ECL). All rights reserved.
  *
  * Redistribution and use in source and binary forms, with or without
  * modification, are permitted provided that the following conditions
@@ -32,37 +32,32 @@
  ****************************************************************************/
 
 /**
- * @file ecl_roll_controller.cpp
+ * @file fw_roll_controller.cpp
- * Implementation of a simple orthogonal roll PID controller.
+ * Implementation of a simple roll P controller.
- *
- * Authors and acknowledgements in header.
  */
 
-#include "ecl_roll_controller.h"
+#include "fw_roll_controller.h"
 #include <float.h>
 #include <lib/geo/geo.h>
 #include <mathlib/mathlib.h>
 
-float ECL_RollController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
+float RollController::control_roll(float roll_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch)
 {
 	/* Do not calculate control signal with bad inputs */
-	if (!(PX4_ISFINITE(ctl_data.roll_setpoint) &&
+	if (!(PX4_ISFINITE(roll_setpoint) &&
-	      PX4_ISFINITE(ctl_data.euler_yaw_rate_setpoint) &&
+	      PX4_ISFINITE(euler_yaw_rate_setpoint) &&
-	      PX4_ISFINITE(ctl_data.pitch) &&
+	      PX4_ISFINITE(pitch) &&
-	      PX4_ISFINITE(ctl_data.roll))) {
+	      PX4_ISFINITE(roll))) {
 
 		return _body_rate_setpoint;
 	}
 
-	/* Calculate the error */
+	const float roll_error = roll_setpoint - roll;
-	float roll_error = ctl_data.roll_setpoint - ctl_data.roll;
-
-	/*  Apply P controller: rate setpoint from current error and time constant */
 	_euler_rate_setpoint = roll_error / _tc;
 
 	/* Transform setpoint to body angular rates (jacobian) */
-	const float roll_body_rate_setpoint_raw = _euler_rate_setpoint - sinf(ctl_data.pitch) *
+	const float roll_body_rate_setpoint_raw = _euler_rate_setpoint - sinf(pitch) *
-			ctl_data.euler_yaw_rate_setpoint;
+			euler_yaw_rate_setpoint;
 	_body_rate_setpoint = math::constrain(roll_body_rate_setpoint_raw, -_max_rate, _max_rate);
 
 	return _body_rate_setpoint;

src/modules/fw_att_control/fw_roll_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,40 +32,41 @@
  ****************************************************************************/
 
 /**
- * @file ecl_roll_controller.h
+ * @file fw_roll_controller.h
- * Definition of a simple orthogonal roll PID controller.
+ * Definition of a simple roll P 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.
  */
 
-#ifndef ECL_ROLL_CONTROLLER_H
+#ifndef FW_ROLL_CONTROLLER_H
-#define ECL_ROLL_CONTROLLER_H
+#define FW_ROLL_CONTROLLER_H
 
-#include "ecl_controller.h"
+class RollController
-
-class ECL_RollController :
-	public ECL_Controller
 {
 public:
-	ECL_RollController() = default;
+	RollController() = default;
-	~ECL_RollController() = default;
+	~RollController() = default;
 
 	/**
 	 * @brief Calculates both euler and body roll rate setpoints.
 	 *
-	 * @param dt Time step [s]
+	 * @param roll_setpoint roll setpoint [rad]
-	 * @param ctrl_data Various control inputs (attitude, body rates, attitdue stepoints, euler rate setpoints, current speeed)
+	 * @param euler_yaw_rate_setpoint euler yaw rate setpoint [rad/s]
+	 * @param roll estimated roll [rad]
+	 * @param pitch estimated pitch [rad]
 	 * @return Roll body rate setpoint [rad/s]
 	 */
-	float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
+	float control_roll(float roll_setpoint, float euler_yaw_rate_setpoint, float roll, float pitch);
+
+	void set_time_constant(float time_constant) { _tc = time_constant; }
+	void set_max_rate(float max_rate) { _max_rate = max_rate; }
+
+	float get_euler_rate_setpoint() { return _euler_rate_setpoint; }
+	float get_body_rate_setpoint() { return _body_rate_setpoint; }
+
+private:
+	float _tc;
+	float _max_rate;
+	float _euler_rate_setpoint;
+	float _body_rate_setpoint;
 };
 
-#endif // ECL_ROLL_CONTROLLER_H
+#endif // FW_ROLL_CONTROLLER_H