fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 09:43:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

mc_rate_control: use vehicle_angular_acceleration topic instead of differentiating

Browse Source 
* mc_rate_control: use vehicle_angular_acceleration topic
* IMU_DGYRO_CUTOFF change default to 30 Hz
* improve IMU_DGYRO_CUTOFF param documentation (updated from MC_DTERM_CUTOFF)
This commit is contained in:
Daniel Agar
2020-03-12 15:07:03 -04:00
committed by GitHub
parent 2b03aa3d1b
commit 093e9ba1ce
19 changed files with 56 additions and 105 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_rate_control/RateControl/RateControl.cpp
+3 -22
View File
@@ -47,15 +47,6 @@ void RateControl::setGains(const Vector3f &P, const Vector3f &I, const Vector3f
_gain_d = D;
}
void RateControl::setDTermCutoff(const float loop_rate, const float cutoff, const bool force)
{
// only do expensive filter update if the cutoff changed
if (force || fabsf(_lp_filters_d.get_cutoff_freq() - cutoff) > 0.01f) {
_lp_filters_d.set_cutoff_frequency(loop_rate, cutoff);
_lp_filters_d.reset(_rate_prev);
}
}
void RateControl::setSaturationStatus(const MultirotorMixer::saturation_status &status)
{
_mixer_saturation_positive[0] = status.flags.roll_pos;
@@ -66,24 +57,14 @@ void RateControl::setSaturationStatus(const MultirotorMixer::saturation_status &
_mixer_saturation_negative[2] = status.flags.yaw_neg;
}
Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const float dt, const bool landed)
Vector3f RateControl::update(const Vector3f &rate, const Vector3f &rate_sp, const Vector3f &angular_accel,
const float dt, const bool landed)
{
// angular rates error
Vector3f rate_error = rate_sp - rate;
// prepare D-term based on low-pass filtered rates
const Vector3f rate_filtered(_lp_filters_d.apply(rate));
Vector3f rate_d;
if (dt > FLT_EPSILON) {
rate_d = (rate_filtered - _rate_prev_filtered) / dt;
}
// PID control with feed forward
const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(rate_d) + _gain_ff.emult(rate_sp);
_rate_prev = rate;
_rate_prev_filtered = rate_filtered;
const Vector3f torque = _gain_p.emult(rate_error) + _rate_int - _gain_d.emult(angular_accel) + _gain_ff.emult(rate_sp);
// update integral only if we are not landed
if (!landed) {
src/modules/mc_rate_control/RateControl/RateControl.hpp
+3 -13
View File
@@ -65,14 +65,6 @@ public:
*/
void setIntegratorLimit(const matrix::Vector3f &integrator_limit) { _lim_int = integrator_limit; };
/**
* Set update frequency and low-pass filter cutoff that is applied to the derivative term
* @param loop_rate [Hz] rate with which update function is called
* @param cutoff [Hz] cutoff frequency for the low-pass filter on the dervative term
* @param force flag to force an expensive update even if the cutoff didn't change
*/
void setDTermCutoff(const float loop_rate, const float cutoff, const bool force);
/**
* Set direct rate to torque feed forward gain
* @see _gain_ff
@@ -93,8 +85,8 @@ public:
* @param dt desired vehicle angular rate setpoint
* @return [-1,1] normalized torque vector to apply to the vehicle
*/
matrix::Vector3f update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp, const float dt,
const bool landed);
matrix::Vector3f update(const matrix::Vector3f &rate, const matrix::Vector3f &rate_sp,
const matrix::Vector3f &angular_accel, const float dt, const bool landed);
/**
* Set the integral term to 0 to prevent windup
@@ -119,10 +111,8 @@ private:
matrix::Vector3f _gain_ff; ///< direct rate to torque feed forward gain only useful for helicopters
// States
matrix::Vector3f _rate_prev; ///< angular rates of previous update
matrix::Vector3f _rate_prev_filtered; ///< low-pass filtered angular rates of previous update
matrix::Vector3f _rate_int; ///< integral term of the rate controller
math::LowPassFilter2pVector3f _lp_filters_d{0.f, 0.f}; ///< low-pass filters for D-term (roll, pitch & yaw)
bool _mixer_saturation_positive[3] {};
bool _mixer_saturation_negative[3] {};
};
src/modules/mc_rate_control/RateControl/RateControlTest.cpp
+1 -1
View File
@@ -39,6 +39,6 @@ using namespace matrix;
TEST(RateControlTest, AllZeroCase)
{
RateControl rate_control;
Vector3f torque = rate_control.update(Vector3f(), Vector3f(), 0.f, false);
Vector3f torque = rate_control.update(Vector3f(), Vector3f(), Vector3f(), 0.f, false);
EXPECT_EQ(torque, Vector3f());
}