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

attitude_fw: move angular rate limits to body angular rates (#404)

This commit is contained in:
Thomas Stastny 2018-03-09 04:19:19 +01:00 committed by Daniel Agar
parent 78e983073a
commit 041886a289
6 changed files with 18 additions and 27 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

5
attitude_fw/ecl_controller.cpp
View File

@ -104,6 +104,11 @@ void ECL_Controller::set_max_rate(float max_rate)
_max_rate = max_rate;
}
void ECL_Controller::set_bodyrate_setpoint(float rate)
{
_bodyrate_setpoint = math::constrain(rate, -_max_rate, _max_rate);
}
float ECL_Controller::get_rate_error()
{
return _rate_error;

2
attitude_fw/ecl_controller.h
View File

@ -91,7 +91,7 @@ public:
void set_k_ff(float k_ff);
void set_integrator_max(float max);
void set_max_rate(float max_rate);
void set_bodyrate_setpoint(float rate) {_bodyrate_setpoint = rate;}
void set_bodyrate_setpoint(float rate);
/* Getters */
float get_rate_error();

13
attitude_fw/ecl_pitch_controller.cpp
View File

@ -72,17 +72,6 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
/* Apply P controller: rate setpoint from current error and time constant */
_rate_setpoint = pitch_error / _tc;
/* limit the rate */
if (_max_rate > 0.01f && _max_rate_neg > 0.01f) {
if (_rate_setpoint > 0.0f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
} else {
_rate_setpoint = (_rate_setpoint < -_max_rate_neg) ? -_max_rate_neg : _rate_setpoint;
}
}
return _rate_setpoint;
}
@ -148,5 +137,7 @@ float ECL_PitchController::control_euler_rate(const struct ECL_ControlData &ctl_
_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
cosf(ctl_data.pitch) * sinf(ctl_data.roll) * ctl_data.yaw_rate_setpoint;
set_bodyrate_setpoint(_bodyrate_setpoint);
return control_bodyrate(ctl_data);
}

6
attitude_fw/ecl_pitch_controller.h
View File

@ -51,6 +51,7 @@
#include <stdbool.h>
#include <stdint.h>
#include <mathlib/mathlib.h>
#include "ecl_controller.h"
@ -76,6 +77,11 @@ public:
_max_rate_neg = max_rate_neg;
}
void set_bodyrate_setpoint(float rate)
{
_bodyrate_setpoint = math::constrain(rate, -_max_rate_neg, _max_rate);
}
void set_roll_ff(float roll_ff)
{
_roll_ff = roll_ff;

10
attitude_fw/ecl_roll_controller.cpp
View File

@ -65,13 +65,6 @@ float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_dat
/* Apply P controller */
_rate_setpoint = roll_error / _tc;
/* limit the rate */ //XXX: move to body angluar rates
if (_max_rate > 0.01f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
}
return _rate_setpoint;
}
@ -136,7 +129,8 @@ float ECL_RollController::control_euler_rate(const struct ECL_ControlData &ctl_d
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
set_bodyrate_setpoint(_bodyrate_setpoint);
return control_bodyrate(ctl_data);
}

9
attitude_fw/ecl_yaw_controller.cpp
View File

@ -118,13 +118,6 @@ float ECL_YawController::control_attitude_impl_openloop(const struct ECL_Control
ctl_data.airspeed_min : ctl_data.airspeed);
}
/* limit the rate */ //XXX: move to body angluar rates
if (_max_rate > 0.01f) {
_rate_setpoint = (_rate_setpoint > _max_rate) ? _max_rate : _rate_setpoint;
_rate_setpoint = (_rate_setpoint < -_max_rate) ? -_max_rate : _rate_setpoint;
}
if (!PX4_ISFINITE(_rate_setpoint)) {
warnx("yaw rate sepoint not finite");
_rate_setpoint = 0.0f;
@ -215,6 +208,8 @@ float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_da
_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
cosf(ctl_data.roll) * cosf(ctl_data.pitch) * _rate_setpoint;
set_bodyrate_setpoint(_bodyrate_setpoint);
return control_bodyrate(ctl_data);
}