dark0707
/
px4-firmware
forked from Archive/PX4-Autopilot
1
0
Fork 0
Code Pull Requests Activity

FW att controller: wheel controller: separate from ecl_controller 

Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-05-17 18:02:38 +02:00
parent 448292c980
commit 00f5bba5e0
4 changed files with 59 additions and 69 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

2
src/modules/fw_att_control/CMakeLists.txt
View File

@ -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

4
src/modules/fw_att_control/FixedwingAttitudeControl.hpp
View File

@ -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);

62
src/modules/fw_att_control/ecl_wheel_controller.cpp → src/modules/fw_att_control/fw_wheel_controller.cpp
View File

@ -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) &&
PX4_ISFINITE(ctl_data.groundspeed) &&
PX4_ISFINITE(ctl_data.groundspeed_scaler))) {
if (!(PX4_ISFINITE(body_z_rate) &&
PX4_ISFINITE(groundspeed) &&
PX4_ISFINITE(groundspeed_scaler))) {
return math::constrain(_last_output, -1.0f, 1.0f);
return math::constrain(_last_output, -1.f, 1.f);
}
/* input conditioning */
float min_speed = 1.0f;
const float rate_error = _body_rate_setpoint - body_z_rate;
/* Calculate body angular rate error */
const float rate_error = _body_rate_setpoint - ctl_data.body_z_rate; //body angular rate error
if (_k_i > 0.f && groundspeed > 1.f) { // only start integrating when above 1m/s
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;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
*/
if (_last_output < -1.0f) {
if (_last_output < -1.f) {
/* 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 +
ctl_data.groundspeed_scaler * ctl_data.groundspeed_scaler * (rate_error * _k_p + _integrator);
_last_output = _body_rate_setpoint * _k_ff * groundspeed_scaler +
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) &&
PX4_ISFINITE(ctl_data.yaw))) {
if (!(PX4_ISFINITE(yaw_setpoint) &&
PX4_ISFINITE(yaw))) {
return _body_rate_setpoint;
}
/* Calculate the error */
float yaw_error = wrap_pi(ctl_data.yaw_setpoint - ctl_data.yaw);
const float yaw_error = wrap_pi(yaw_setpoint - yaw);
/* Apply P controller: rate setpoint from current error and time constant */
_euler_rate_setpoint = yaw_error / _tc;
_body_rate_setpoint = _euler_rate_setpoint; // assume 0 pitch and roll angle, thus jacobian is simply identity matrix
_body_rate_setpoint = yaw_error / _tc; // 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 {

60
src/modules/fw_att_control/ecl_wheel_controller.h → src/modules/fw_att_control/fw_wheel_controller.h
View File

@ -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
* Definition of a simple orthogonal coordinated turn yaw PID 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.
* @file fw_wheel_controller.h
* Definition of a simple wheel controller.
*/
#ifndef ECL_HEADING_CONTROLLER_H
#define ECL_HEADING_CONTROLLER_H
#ifndef FW_WHEEL_CONTROLLER_H
#define FW_WHEEL_CONTROLLER_H
#include "ecl_controller.h"
class ECL_WheelController :
public ECL_Controller
class WheelController
{
public:
ECL_WheelController() = default;
~ECL_WheelController() = default;
WheelController() = default;
~WheelController() = default;
/**
* @brief Calculates wheel body rate setpoint.
*
* @param dt Time step [s]
* @param ctrl_data Various control inputs (attitude, body rates, attitdue stepoints, euler rate setpoints, current speeed)
* @param yaw_setpoint yaw setpoint [rad]
* @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