fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

FW att controller: yaw controller: separate from ecl_controller

Browse Source 
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
This commit is contained in:
Silvan Fuhrer 2023-05-17 17:15:44 +02:00
parent d4206195c6
commit 448292c980
6 changed files with 53 additions and 46 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

@ -42,7 +42,7 @@ px4_add_module(
fw_pitch_controller.cpp
fw_roll_controller.cpp
ecl_wheel_controller.cpp
ecl_yaw_controller.cpp
fw_yaw_controller.cpp
DEPENDS
px4_work_queue
)

3
src/modules/fw_att_control/FixedwingAttitudeControl.cpp
View File

@ -345,7 +345,8 @@ void FixedwingAttitudeControl::Run()
euler_angles.theta());
_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);
_yaw_ctrl.control_yaw(_att_sp.roll_body, _pitch_ctrl.get_euler_rate_setpoint(), euler_angles.phi(),
euler_angles.theta(), get_airspeed_constrained());
if (wheel_control) {
_wheel_ctrl.control_attitude(dt, control_input);

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

@ -37,7 +37,7 @@
#include "fw_pitch_controller.h"
#include "fw_roll_controller.h"
#include "ecl_wheel_controller.h"
#include "ecl_yaw_controller.h"
#include "fw_yaw_controller.h"
#include <lib/mathlib/mathlib.h>
#include <lib/parameters/param.h>
#include <lib/perf/perf_counter.h>
@ -161,7 +161,7 @@ private:
RollController _roll_ctrl;
PitchController _pitch_ctrl;
ECL_YawController _yaw_ctrl;
YawController _yaw_ctrl;
ECL_WheelController _wheel_ctrl;
void parameters_update();

2
src/modules/fw_att_control/ecl_controller.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013 Estimation and Control Library (ECL). All rights reserved.
* Copyright (c) 2020-2022 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

43
src/modules/fw_att_control/ecl_yaw_controller.cpp → src/modules/fw_att_control/fw_yaw_controller.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2020 Estimation and Control Library (ECL). All rights reserved.
* Copyright (c) 2020-2022 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,24 +32,24 @@
****************************************************************************/
/**
* @file ecl_yaw_controller.cpp
* Implementation of a simple orthogonal coordinated turn yaw PID controller.
*
* Authors and acknowledgements in header.
* @file fw_yaw_controller.cpp
* Implementation of a simple coordinated turn yaw controller.
*/
#include "ecl_yaw_controller.h"
#include "fw_yaw_controller.h"
#include <float.h>
#include <lib/geo/geo.h>
#include <mathlib/mathlib.h>
float ECL_YawController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
float YawController::control_yaw(float roll_setpoint, float euler_pitch_rate_setpoint, float roll, float pitch,
float airspeed)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
PX4_ISFINITE(ctl_data.euler_pitch_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed_constrained))) {
if (!(PX4_ISFINITE(roll_setpoint) &&
PX4_ISFINITE(roll) &&
PX4_ISFINITE(pitch) &&
PX4_ISFINITE(euler_pitch_rate_setpoint) &&
PX4_ISFINITE(airspeed))) {
return _body_rate_setpoint;
}
@ -58,41 +58,40 @@ float ECL_YawController::control_attitude(const float dt, const ECL_ControlData
bool inverted = false;
/* roll is used as feedforward term and inverted flight needs to be considered */
if (fabsf(ctl_data.roll) < math::radians(90.0f)) {
if (fabsf(roll) < math::radians(90.f)) {
/* not inverted, but numerically still potentially close to infinity */
constrained_roll = math::constrain(ctl_data.roll, math::radians(-80.0f), math::radians(80.0f));
constrained_roll = math::constrain(roll, math::radians(-80.f), math::radians(80.f));
} else {
inverted = true;
// inverted flight, constrain on the two extremes of -pi..+pi to avoid infinity
//note: the ranges are extended by 10 deg here to avoid numeric resolution effects
if (ctl_data.roll > 0.0f) {
if (roll > 0.f) {
/* right hemisphere */
constrained_roll = math::constrain(ctl_data.roll, math::radians(100.0f), math::radians(180.0f));
constrained_roll = math::constrain(roll, math::radians(100.f), math::radians(180.f));
} else {
/* left hemisphere */
constrained_roll = math::constrain(ctl_data.roll, math::radians(-180.0f), math::radians(-100.0f));
constrained_roll = math::constrain(roll, math::radians(-180.f), math::radians(-100.f));
}
}
constrained_roll = math::constrain(constrained_roll, -fabsf(ctl_data.roll_setpoint), fabsf(ctl_data.roll_setpoint));
constrained_roll = math::constrain(constrained_roll, -fabsf(roll_setpoint), fabsf(roll_setpoint));
if (!inverted) {
/* Calculate desired yaw rate from coordinated turn constraint / (no side forces) */
_euler_rate_setpoint = tanf(constrained_roll) * cosf(ctl_data.pitch) * CONSTANTS_ONE_G / ctl_data.airspeed_constrained;
_euler_rate_setpoint = tanf(constrained_roll) * cosf(pitch) * CONSTANTS_ONE_G / airspeed;
/* Transform setpoint to body angular rates (jacobian) */
const float yaw_body_rate_setpoint_raw = -sinf(ctl_data.roll) * ctl_data.euler_pitch_rate_setpoint +
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _euler_rate_setpoint;
const float yaw_body_rate_setpoint_raw = -sinf(roll) * euler_pitch_rate_setpoint +
cosf(roll) * cosf(pitch) * _euler_rate_setpoint;
_body_rate_setpoint = math::constrain(yaw_body_rate_setpoint_raw, -_max_rate, _max_rate);
}
if (!PX4_ISFINITE(_body_rate_setpoint)) {
PX4_WARN("yaw rate sepoint not finite");
_body_rate_setpoint = 0.0f;
_body_rate_setpoint = 0.f;
}
return _body_rate_setpoint;

45
src/modules/fw_att_control/ecl_yaw_controller.h → src/modules/fw_att_control/fw_yaw_controller.h
View File

@ -32,11 +32,8 @@
****************************************************************************/
/**
* @file ecl_yaw_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>
* @file fw_yaw_controller.h
* Definition of a simple coordinated turn controller.
*
* Acknowledgements:
*
@ -46,27 +43,37 @@
* Jonathan Challinger, 2012.
*/
#ifndef ECL_YAW_CONTROLLER_H
#define ECL_YAW_CONTROLLER_H
#ifndef FW_YAW_CONTROLLER_H
#define FW_YAW_CONTROLLER_H
#include "ecl_controller.h"
class ECL_YawController :
public ECL_Controller
class YawController
{
public:
ECL_YawController() = default;
~ECL_YawController() = default;
YawController() = default;
~YawController() = default;
/**
* @brief Calculates both euler and body yaw rate setpoints.
* @brief Calculates both euler and body yaw rate setpoints for coordinated turn based on current attitude and airspeed
*
* @param dt Time step [s]
* @param ctrl_data Various control inputs (attitude, body rates, attitdue stepoints, euler rate setpoints, current speeed)
* @return Yaw body rate setpoint [rad/s]
* @param roll_setpoint roll setpoint [rad]
* @param euler_pitch_rate_setpoint euler pitch rate setpoint [rad/s]
* @param roll estimated roll [rad]
* @param pitch estimated pitch [rad]
* @param airspeed airspeed [m/s]
* @return Roll body rate setpoint [rad/s]
*/
float control_attitude(const float dt, const ECL_ControlData &ctl_data) override;
float control_yaw(float roll_setpoint, float euler_pitch_rate_setpoint, float roll, float pitch,
float airspeed);
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 _max_rate;
float _euler_rate_setpoint;
float _body_rate_setpoint;
};
#endif // ECL_YAW_CONTROLLER_H
#endif // FW_YAW_CONTROLLER_H