fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-16 02:40:35 +08:00
Code Issues Packages Projects Releases Wiki Activity

RpmControl: simplify the entire control logic

Browse Source
This commit is contained in:
Matthias Grob
2024-12-16 18:39:15 +01:00
parent 1c4325db6d
commit 2506bd3b5d
2 changed files with 26 additions and 27 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp
+20 -22
View File
@@ -37,47 +37,45 @@
using namespace time_literals;
RpmControl::RpmControl(ModuleParams *parent) : ModuleParams(parent) {};
RpmControl::RpmControl(ModuleParams *parent) : ModuleParams(parent)
{
_pid.setOutputLimit(PID_OUTPUT_LIMIT);
_pid.setIntegralLimit(PID_OUTPUT_LIMIT);
};
void RpmControl::setSpoolupProgress(float spoolup_progress)
{
_spoolup_progress = spoolup_progress;
_pid.setSetpoint(_spoolup_progress * _param_ca_heli_rpm_sp.get());
if (_spoolup_progress < .8f) {
if (_spoolup_progress < SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED) {
_pid.resetIntegral();
}
}
float RpmControl::getActuatorCorrection()
{
hrt_abstime now = hrt_absolute_time();
// RPM measurement update
if (_rpm_sub.updated()) {
rpm_s rpm{};
if (_rpm_sub.copy(&rpm)) {
_rpm_estimate = rpm.rpm_estimate;
_timestamp_last_rpm_measurement = rpm.timestamp;
const float dt = math::min((now - _timestamp_last_measurement) * 1e-6f, 1.f);
_timestamp_last_measurement = rpm.timestamp;
const float gain_scale = math::interpolate(_spoolup_progress, .8f, 1.f, 0.f, 1e-3f);
_pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f);
_actuator_correction = _pid.update(rpm.rpm_estimate, dt, true);
}
}
hrt_abstime now = hrt_absolute_time();
const float dt = math::constrain((now - _timestamp_last_update) * 1e-6f, 1e-3f, 1.f);
_timestamp_last_update = now;
const bool rpm_measurement_timeout = (now - _timestamp_last_rpm_measurement) < 1_s;
if (rpm_measurement_timeout) {
const float gain_scale = math::max((_spoolup_progress - .8f) * 5.f, 0.f) * 1e-3f;
_pid.setGains(_param_ca_heli_rpm_p.get() * gain_scale, _param_ca_heli_rpm_i.get() * gain_scale, 0.f);
} else {
_pid.setGains(0.f, 0.f, 0.f);
// Timeout
if (now > _timestamp_last_measurement + 1_s) {
_pid.resetIntegral();
_actuator_correction = 0.f;
}
_pid.setOutputLimit(.5f);
_pid.setIntegralLimit(.5f);
float output = _pid.update(_rpm_estimate, dt, true);
return output;
return _actuator_correction;
}
src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp
+6 -5
View File
@@ -59,13 +59,14 @@ public:
float getActuatorCorrection();
private:
uORB::Subscription _rpm_sub{ORB_ID(rpm)};
static constexpr float SPOOLUP_PROGRESS_WITH_CONTROLLER_ENGAGED = .8f; // [0,1]
static constexpr float PID_OUTPUT_LIMIT = .5f; // [0,1]
float _rpm_estimate{0.f};
float _spoolup_progress{0.f};
uORB::Subscription _rpm_sub{ORB_ID(rpm)};
PID _pid;
hrt_abstime _timestamp_last_rpm_measurement{0};
hrt_abstime _timestamp_last_update{0};
float _spoolup_progress{0.f}; // [0,1]
hrt_abstime _timestamp_last_measurement{0}; // for dt and timeout
float _actuator_correction{0.f};
DEFINE_PARAMETERS(
(ParamFloat<px4::params::CA_HELI_RPM_SP>) _param_ca_heli_rpm_sp,