fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-06-26 13:20:34 +08:00
Code Issues Packages Projects Releases Wiki Activity

MC attitude control move to Vector3f LPF

Browse Source
This commit is contained in:
Daniel Agar
2018-10-15 15:11:41 -04:00
parent ac85230700
commit efac6f2807
2 changed files with 8 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/mc_att_control/mc_att_control.hpp
+2 -2
View File
@@ -32,7 +32,7 @@
****************************************************************************/
#include <lib/mixer/mixer.h>
#include <mathlib/math/filter/LowPassFilter2p.hpp>
#include <mathlib/math/filter/LowPassFilter2pVector3f.hpp>
#include <matrix/matrix/math.hpp>
#include <perf/perf_counter.h>
#include <px4_config.h>
@@ -169,7 +169,7 @@ private:
perf_counter_t _loop_perf; /**< loop performance counter */
math::LowPassFilter2p _lp_filters_d[3]; /**< low-pass filters for D-term (roll, pitch & yaw) */
math::LowPassFilter2pVector3f _lp_filters_d{initial_update_rate_hz, 50.f}; /**< low-pass filters for D-term (roll, pitch & yaw) */
static constexpr const float initial_update_rate_hz = 250.f; /**< loop update rate used for initialization */
float _loop_update_rate_hz{initial_update_rate_hz}; /**< current rate-controller loop update rate in [Hz] */
src/modules/mc_att_control/mc_att_control_main.cpp
+6 -19
View File
@@ -98,11 +98,7 @@ To reduce control latency, the module directly polls on the gyro topic published
MulticopterAttitudeControl::MulticopterAttitudeControl() :
ModuleParams(nullptr),
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control")),
_lp_filters_d{
{initial_update_rate_hz, 50.f},
{initial_update_rate_hz, 50.f},
{initial_update_rate_hz, 50.f}} // will be initialized correctly when params are loaded
_loop_perf(perf_alloc(PC_ELAPSED, "mc_att_control"))
{
for (uint8_t i = 0; i < MAX_GYRO_COUNT; i++) {
_sensor_gyro_sub[i] = -1;
@@ -161,13 +157,9 @@ MulticopterAttitudeControl::parameters_updated()
_rate_d(2) = _yaw_rate_d.get();
_rate_ff(2) = _yaw_rate_ff.get();
if (fabsf(_lp_filters_d[0].get_cutoff_freq() - _d_term_cutoff_freq.get()) > 0.01f) {
_lp_filters_d[0].set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
_lp_filters_d[1].set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
_lp_filters_d[2].set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
_lp_filters_d[0].reset(_rates_prev(0));
_lp_filters_d[1].reset(_rates_prev(1));
_lp_filters_d[2].reset(_rates_prev(2));
if (fabsf(_lp_filters_d.get_cutoff_freq() - _d_term_cutoff_freq.get()) > 0.01f) {
_lp_filters_d.set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
_lp_filters_d.reset(_rates_prev);
}
/* angular rate limits */
@@ -526,10 +518,7 @@ MulticopterAttitudeControl::control_attitude_rates(float dt)
Vector3f rates_err = _rates_sp - rates;
/* apply low-pass filtering to the rates for D-term */
Vector3f rates_filtered(
_lp_filters_d[0].apply(rates(0)),
_lp_filters_d[1].apply(rates(1)),
_lp_filters_d[2].apply(rates(2)));
Vector3f rates_filtered(_lp_filters_d.apply(rates));
_att_control = rates_p_scaled.emult(rates_err) +
_rates_int -
@@ -819,9 +808,7 @@ MulticopterAttitudeControl::run()
_loop_update_rate_hz = _loop_update_rate_hz * 0.5f + loop_update_rate * 0.5f;
dt_accumulator = 0;
loop_counter = 0;
_lp_filters_d[0].set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
_lp_filters_d[1].set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
_lp_filters_d[2].set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
_lp_filters_d.set_cutoff_frequency(_loop_update_rate_hz, _d_term_cutoff_freq.get());
}
}