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

fix(npfg): use NFPG_DAMPING and NPFG_PERIOD to calculate directional p gain

Browse Source 
Signed-off-by: Silvan <silvan@auterion.com>
This commit is contained in:
Silvan 2026-03-16 16:07:29 +01:00 committed by Silvan Fuhrer
parent 39b0568ea4
commit fab7cbda8d
3 changed files with 9 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
src/lib/npfg/AirspeedDirectionController.hpp
View File

@ -59,13 +59,15 @@
#ifndef PX4_AIRSPEEDDIRECTIONONTROLLER_HPP
#define PX4_AIRSPEEDDIRECTIONONTROLLER_HPP
#include <matrix/math.hpp>
#include <lib/mathlib/mathlib.h>
class AirspeedDirectionController
{
public:
AirspeedDirectionController();
void setPGainFromPeriodAndDamping(float damping, float period) {p_gain_ = 4.f * M_PI_F * damping / math::max(period, FLT_EPSILON);}
float controlHeading(const float heading_sp, const float heading, const float airspeed) const;
private:

2
src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.cpp
View File

@ -115,6 +115,8 @@ FwLateralLongitudinalControl::parameters_update()
_tecs_alt_time_const_slew_rate.setSlewRate(TECS_ALT_TIME_CONST_SLEW_RATE);
_tecs_alt_time_const_slew_rate.setForcedValue(_param_fw_t_h_error_tc.get() * _param_fw_thrtc_sc.get());
_airspeed_direction_control.setPGainFromPeriodAndDamping(_param_npfg_damping.get(), _param_npfg_period.get());
}
void FwLateralLongitudinalControl::Run()

4
src/modules/fw_lateral_longitudinal_control/FwLateralLongitudinalControl.hpp
View File

@ -169,7 +169,9 @@ private:
(ParamFloat<px4::params::FW_LND_THRTC_SC>) _param_fw_thrtc_sc,
(ParamFloat<px4::params::FW_T_THR_LOW_HGT>) _param_fw_t_thr_low_hgt,
(ParamFloat<px4::params::FW_WIND_ARSP_SC>) _param_fw_wind_arsp_sc,
(ParamFloat<px4::params::FW_GND_SPD_MIN>) _param_fw_gnd_spd_min
(ParamFloat<px4::params::FW_GND_SPD_MIN>) _param_fw_gnd_spd_min,
(ParamFloat<px4::params::NPFG_DAMPING>) _param_npfg_damping,
(ParamFloat<px4::params::NPFG_PERIOD>) _param_npfg_period
)
hrt_abstime _last_time_loop_ran{};