yemai/bizhang_-obav
1
0
Fork 0
mirror of https://gitee.com/xiaohuolufeihua/bizhang_-obav.git synced 2026-05-21 01:12:11 +00:00
Code Issues Packages Projects Releases Wiki Activity

renamed heading controller to wheel controller, added groundspeed dependency and separate parameters

Browse Source
This commit is contained in:
Andreas Antener
2015-09-24 15:57:51 +02:00
committed by Roman
parent 6c31421889
commit 234a200e60
9 changed files with 138 additions and 55 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
msg/vehicle_attitude_setpoint.msg
View File

@@ -22,4 +22,4 @@ bool roll_reset_integral # Reset roll integral part (navigation logic change)
bool pitch_reset_integral # Reset pitch integral part (navigation logic change)
bool yaw_reset_integral # Reset yaw integral part (navigation logic change)
bool fw_control_yaw # control heading with rudder (used for auto takeoff on runway)
bool fw_control_wheel # control heading with rudder (used for auto takeoff on runway)

1
src/lib/ecl/attitude_fw/ecl_controller.h
View File

@@ -75,6 +75,7 @@ struct ECL_ControlData {
float airspeed_max;
float airspeed;
float scaler;
float ground_speed;
bool lock_integrator;
};

55
src/lib/ecl/attitude_fw/ecl_heading_controller.cpp → src/lib/ecl/attitude_fw/ecl_wheel_controller.cpp
View File

@@ -32,13 +32,13 @@
****************************************************************************/
/**
* @file ecl_yaw_controller.cpp
* @file ecl_wheel_controller.cpp
* Implementation of a simple orthogonal coordinated turn yaw PID controller.
*
* Authors and acknowledgements in header.
*/
#include "ecl_heading_controller.h"
#include "ecl_wheel_controller.h"
#include <stdint.h>
#include <float.h>
#include <geo/geo.h>
@@ -47,22 +47,20 @@
#include <systemlib/err.h>
#include <ecl/ecl.h>
ECL_HeadingController::ECL_HeadingController() :
ECL_Controller("heading")
ECL_WheelController::ECL_WheelController() :
ECL_Controller("wheel")
{
}
ECL_HeadingController::~ECL_HeadingController()
ECL_WheelController::~ECL_WheelController()
{
}
float ECL_HeadingController::control_bodyrate(const struct ECL_ControlData &ctl_data)
float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) && PX4_ISFINITE(ctl_data.pitch) && PX4_ISFINITE(ctl_data.pitch_rate) &&
PX4_ISFINITE(ctl_data.yaw_rate) && PX4_ISFINITE(ctl_data.pitch_rate_setpoint) &&
PX4_ISFINITE(ctl_data.airspeed_min) && PX4_ISFINITE(ctl_data.airspeed_max) &&
PX4_ISFINITE(ctl_data.scaler))) {
if (!(PX4_ISFINITE(ctl_data.yaw_rate) &&
PX4_ISFINITE(ctl_data.ground_speed))) {
perf_count(_nonfinite_input_perf);
return math::constrain(_last_output, -1.0f, 1.0f);
}
@@ -80,27 +78,21 @@ float ECL_HeadingController::control_bodyrate(const struct ECL_ControlData &ctl_
}
/* input conditioning */
float airspeed = ctl_data.airspeed;
if (!PX4_ISFINITE(airspeed)) {
/* airspeed is NaN, +- INF or not available, pick center of band */
airspeed = 0.5f * (ctl_data.airspeed_min + ctl_data.airspeed_max);
} else if (airspeed < ctl_data.airspeed_min) {
airspeed = ctl_data.airspeed_min;
float min_speed = 1.0f;
/* assume minimum speed to prevent oscillations */
float speed = min_speed;
if (ctl_data.ground_speed > speed) {
speed = ctl_data.ground_speed;
}
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint;
float scaler = 1.0f / speed;
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - ctl_data.yaw_rate; //body angular rate error
_rate_error = _rate_setpoint - ctl_data.yaw_rate; //body angular rate error
if (!lock_integrator && _k_i > 0.0f && airspeed > 0.5f * ctl_data.airspeed_min) {
if (!lock_integrator && _k_i > 0.0f && speed > min_speed) {
float id = _rate_error * dt;
float id = _rate_error * dt * scaler;
/*
* anti-windup: do not allow integrator to increase if actuator is at limit
@@ -122,23 +114,22 @@ float ECL_HeadingController::control_bodyrate(const struct ECL_ControlData &ctl_
float integrator_constrained = math::constrain(_integrator * _k_i, -_integrator_max, _integrator_max);
/* Apply PI rate controller and store non-limited output */
_last_output = (_bodyrate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * ctl_data.scaler *
ctl_data.scaler; //scaler is proportional to 1/airspeed
warnx("yaw:_last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, airspeed %.4f, _k_i %.4f, _k_p: %.4f", (double)_last_output, (double)_integrator, (double)_integrator_max, (double)airspeed, (double)_k_i, (double)_k_p);
_last_output = (_rate_setpoint * _k_ff + _rate_error * _k_p + integrator_constrained) * scaler;
/*warnx("wheel: _last_output: %.4f, _integrator: %.4f, _integrator_max: %.4f, speed %.4f, _k_i %.4f, _k_p: %.4f",
(double)_last_output, (double)_integrator, (double)_integrator_max, (double)speed, (double)_k_i, (double)_k_p);*/
return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_HeadingController::control_attitude(const struct ECL_ControlData &ctl_data)
float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) &&
PX4_ISFINITE(ctl_data.yaw) &&
PX4_ISFINITE(ctl_data.airspeed))) {
PX4_ISFINITE(ctl_data.yaw))) {
perf_count(_nonfinite_input_perf);
warnx("not controlling yaw");
warnx("not controlling wheel");
return _rate_setpoint;
}

8
src/lib/ecl/attitude_fw/ecl_heading_controller.h → src/lib/ecl/attitude_fw/ecl_wheel_controller.h
View File

@@ -32,7 +32,7 @@
****************************************************************************/
/**
* @file ecl_yaw_controller.h
* @file ecl_wheel_controller.h
* Definition of a simple orthogonal coordinated turn yaw PID controller.
*
* @author Lorenz Meier <lm@inf.ethz.ch>
@@ -54,13 +54,13 @@
#include "ecl_controller.h"
class __EXPORT ECL_HeadingController :
class __EXPORT ECL_WheelController :
public ECL_Controller
{
public:
ECL_HeadingController();
ECL_WheelController();
~ECL_HeadingController();
~ECL_WheelController();
float control_attitude(const struct ECL_ControlData &ctl_data);

7
src/lib/runway_takeoff/RunwayTakeoff.cpp
View File

@@ -123,10 +123,10 @@ void RunwayTakeoff::update(float airspeed, float alt_agl, int mavlink_fd)
}
}
bool RunwayTakeoff::controlYaw()
bool RunwayTakeoff::controlWheel()
{
// keep controlling yaw with rudder until we have enough ground clearance
return _state < RunwayTakeoffState::FLY;
// keep controlling wheel until takeoff
return _state < RunwayTakeoffState::TAKEOFF;
}
float RunwayTakeoff::getPitch(float tecsPitch)
@@ -140,6 +140,7 @@ float RunwayTakeoff::getPitch(float tecsPitch)
float RunwayTakeoff::getRoll(float navigatorRoll)
{
// until we have enough ground clearance, set roll to 0
if (_state < RunwayTakeoffState::FLY) {
return 0.0f;
}

2
src/lib/runway_takeoff/RunwayTakeoff.h
View File

@@ -75,7 +75,7 @@ public:
bool runwayTakeoffEnabled() { return (bool)_runway_takeoff_enabled.get(); };
bool controlYaw();
bool controlWheel();
bool climbout() { return _climbout; };
float getPitch(float tecsPitch);
float getRoll(float navigatorRoll);

51
src/modules/fw_att_control/fw_att_control_main.cpp
View File

@@ -82,7 +82,7 @@
#include <ecl/attitude_fw/ecl_pitch_controller.h>
#include <ecl/attitude_fw/ecl_roll_controller.h>
#include <ecl/attitude_fw/ecl_yaw_controller.h>
#include <ecl/attitude_fw/ecl_heading_controller.h>
#include <ecl/attitude_fw/ecl_wheel_controller.h>
#include <platforms/px4_defines.h>
/**
@@ -182,6 +182,11 @@ private:
float y_coordinated_min_speed;
int32_t y_coordinated_method;
float y_rmax;
float w_p;
float w_i;
float w_ff;
float w_integrator_max;
float w_rmax;
float airspeed_min;
float airspeed_trim;
@@ -223,6 +228,11 @@ private:
param_t y_coordinated_min_speed;
param_t y_coordinated_method;
param_t y_rmax;
param_t w_p;
param_t w_i;
param_t w_ff;
param_t w_integrator_max;
param_t w_rmax;
param_t airspeed_min;
param_t airspeed_trim;
@@ -249,7 +259,7 @@ private:
ECL_RollController _roll_ctrl;
ECL_PitchController _pitch_ctrl;
ECL_YawController _yaw_ctrl;
ECL_HeadingController _heading_ctrl;
ECL_WheelController _wheel_ctrl;
/**
@@ -382,6 +392,12 @@ FixedwingAttitudeControl::FixedwingAttitudeControl() :
_parameter_handles.y_integrator_max = param_find("FW_YR_IMAX");
_parameter_handles.y_rmax = param_find("FW_Y_RMAX");
_parameter_handles.w_p = param_find("FW_WR_P");
_parameter_handles.w_i = param_find("FW_WR_I");
_parameter_handles.w_ff = param_find("FW_WR_FF");
_parameter_handles.w_integrator_max = param_find("FW_WR_IMAX");
_parameter_handles.w_rmax = param_find("FW_W_RMAX");
_parameter_handles.airspeed_min = param_find("FW_AIRSPD_MIN");
_parameter_handles.airspeed_trim = param_find("FW_AIRSPD_TRIM");
_parameter_handles.airspeed_max = param_find("FW_AIRSPD_MAX");
@@ -460,6 +476,12 @@ FixedwingAttitudeControl::parameters_update()
param_get(_parameter_handles.y_coordinated_method, &(_parameters.y_coordinated_method));
param_get(_parameter_handles.y_rmax, &(_parameters.y_rmax));
param_get(_parameter_handles.w_p, &(_parameters.w_p));
param_get(_parameter_handles.w_i, &(_parameters.w_i));
param_get(_parameter_handles.w_ff, &(_parameters.w_ff));
param_get(_parameter_handles.w_integrator_max, &(_parameters.w_integrator_max));
param_get(_parameter_handles.w_rmax, &(_parameters.w_rmax));
param_get(_parameter_handles.airspeed_min, &(_parameters.airspeed_min));
param_get(_parameter_handles.airspeed_trim, &(_parameters.airspeed_trim));
param_get(_parameter_handles.airspeed_max, &(_parameters.airspeed_max));
@@ -504,12 +526,12 @@ FixedwingAttitudeControl::parameters_update()
_yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method);
_yaw_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
/* heading control parameters */
_heading_ctrl.set_k_p(_parameters.y_p);
_heading_ctrl.set_k_i(_parameters.y_i);
_heading_ctrl.set_k_ff(_parameters.y_ff);
_heading_ctrl.set_integrator_max(_parameters.y_integrator_max);
_heading_ctrl.set_max_rate(math::radians(_parameters.y_rmax));
/* wheel control parameters */
_wheel_ctrl.set_k_p(_parameters.w_p);
_wheel_ctrl.set_k_i(_parameters.w_i);
_wheel_ctrl.set_k_ff(_parameters.w_ff);
_wheel_ctrl.set_integrator_max(_parameters.w_integrator_max);
_wheel_ctrl.set_max_rate(math::radians(_parameters.w_rmax));
return OK;
}
@@ -846,6 +868,7 @@ FixedwingAttitudeControl::task_main()
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
} else if (_vcontrol_mode.flag_control_velocity_enabled) {
@@ -871,6 +894,7 @@ FixedwingAttitudeControl::task_main()
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
} else if (_vcontrol_mode.flag_control_altitude_enabled) {
@@ -890,6 +914,7 @@ FixedwingAttitudeControl::task_main()
}
if (_att_sp.yaw_reset_integral) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
} else {
/*
@@ -940,6 +965,7 @@ FixedwingAttitudeControl::task_main()
_roll_ctrl.reset_integrator();
_pitch_ctrl.reset_integrator();
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
}
/* Prepare speed_body_u and speed_body_w */
@@ -969,6 +995,8 @@ FixedwingAttitudeControl::task_main()
control_input.airspeed = airspeed;
control_input.scaler = airspeed_scaling;
control_input.lock_integrator = lock_integrator;
control_input.ground_speed = sqrtf(_global_pos.vel_n * _global_pos.vel_n +
_global_pos.vel_e * _global_pos.vel_e);
_yaw_ctrl.set_coordinated_method(_parameters.y_coordinated_method);
@@ -977,7 +1005,7 @@ FixedwingAttitudeControl::task_main()
_roll_ctrl.control_attitude(control_input);
_pitch_ctrl.control_attitude(control_input);
_yaw_ctrl.control_attitude(control_input); //runs last, because is depending on output of roll and pitch attitude
_heading_ctrl.control_attitude(control_input);
_wheel_ctrl.control_attitude(control_input);
/* Update input data for rate controllers */
control_input.roll_rate_setpoint = _roll_ctrl.get_desired_rate();
@@ -1018,8 +1046,8 @@ FixedwingAttitudeControl::task_main()
}
float yaw_u = 0.0f;
if (_att_sp.fw_control_yaw == true) {
yaw_u = _heading_ctrl.control_bodyrate(control_input);
if (_att_sp.fw_control_wheel == true) {
yaw_u = _wheel_ctrl.control_bodyrate(control_input);
}
else {
@@ -1031,6 +1059,7 @@ FixedwingAttitudeControl::task_main()
_actuators.control[2] += yaw_manual;
if (!PX4_ISFINITE(yaw_u)) {
_yaw_ctrl.reset_integrator();
_wheel_ctrl.reset_integrator();
perf_count(_nonfinite_output_perf);
if (_debug && loop_counter % 10 == 0) {
warnx("yaw_u %.4f", (double)yaw_u);

60
src/modules/fw_att_control/fw_att_control_params.c
View File

@@ -220,6 +220,55 @@ PARAM_DEFINE_FLOAT(FW_YR_IMAX, 0.2f);
*/
PARAM_DEFINE_FLOAT(FW_Y_RMAX, 0.0f);
/**
* Wheel steering rate proportional gain
*
* This defines how much the wheel steering input will be commanded depending on the
* current body angular rate error.
*
* @min 0.005
* @max 1.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_WR_P, 0.05f);
/**
* Wheel steering rate integrator gain
*
* This gain defines how much control response will result out of a steady
* state error. It trims any constant error.
*
* @min 0.0
* @max 50.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_WR_I, 0.0f);
/**
* Wheel steering rate integrator limit
*
* The portion of the integrator part in the control surface deflection is
* limited to this value
*
* @min 0.0
* @max 1.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_WR_IMAX, 0.2f);
/**
* Maximum wheel steering rate
*
* This limits the maximum wheel steering rate the controller will output (in degrees per
* second). Setting a value of zero disables the limit.
*
* @unit deg/s
* @min 0.0
* @max 90.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_W_RMAX, 0.0f);
/**
* Roll rate feed forward
*
@@ -255,6 +304,17 @@ PARAM_DEFINE_FLOAT(FW_PR_FF, 0.5f);
*/
PARAM_DEFINE_FLOAT(FW_YR_FF, 0.3f);
/**
* Wheel steering rate feed forward
*
* Direct feed forward from rate setpoint to control surface output
*
* @min 0.0
* @max 10.0
* @group FW Attitude Control
*/
PARAM_DEFINE_FLOAT(FW_WR_FF, 0.2f);
/**
* Minimal speed for yaw coordination
*

7
src/modules/fw_pos_control_l1/fw_pos_control_l1_main.cpp
View File

@@ -1074,7 +1074,7 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
bool setpoint = true;
_att_sp.fw_control_yaw = false; // by default we don't want yaw to be contoller directly with rudder
_att_sp.fw_control_wheel = false; // by default we don't want yaw to be contoller directly with rudder
float eas2tas = 1.0f; // XXX calculate actual number based on current measurements
@@ -1406,9 +1406,10 @@ FixedwingPositionControl::control_position(const math::Vector<2> &current_positi
// assign values
_att_sp.roll_body = _runway_takeoff.getRoll(_l1_control.nav_roll());
_att_sp.yaw_body = _runway_takeoff.getYaw(_l1_control.nav_bearing());
_att_sp.fw_control_yaw = _runway_takeoff.controlYaw(); // tell attitude controller if he should control yaw directly
_att_sp.fw_control_wheel = _runway_takeoff.controlWheel();
_att_sp.pitch_body = _runway_takeoff.getPitch(_tecs.get_pitch_demand());
/*warnx("yaw: %.4f, roll: %.4f, pitch: %.4f", (double)_att_sp.yaw_body,
(double)_att_sp.roll_body, (double)_att_sp.pitch_body);*/
} else {
/* Perform launch detection */