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

RpmControl: Better consider the case where there's no rpm measurement (anymore)

Browse Source
This commit is contained in:
Matthias Grob 2024-12-17 21:02:51 +01:00
parent b584f8381c
commit bc92008885
3 changed files with 7 additions and 1 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

1
msg/Rpm.msg
View File

@ -1,4 +1,5 @@
uint64 timestamp # time since system start (microseconds) uint64 timestamp # time since system start (microseconds)
# rpm values of 0.0 mean within a timeout there is no movement measured
float32 rpm_estimate # filtered revolutions per minute float32 rpm_estimate # filtered revolutions per minute
float32 rpm_raw float32 rpm_raw

6
src/modules/control_allocator/ActuatorEffectiveness/RpmControl.cpp
View File

@ -68,11 +68,15 @@ float RpmControl::getActuatorCorrection()
const float gain_scale = math::interpolate(_spoolup_progress, .8f, 1.f, 0.f, 1e-3f); 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); _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); _actuator_correction = _pid.update(rpm.rpm_estimate, dt, true);
_rpm_invalid = rpm.rpm_estimate < 1.f;
} }
} }
// Timeout // Timeout
if (now > _timestamp_last_measurement + 1_s) { const bool timeout = now > _timestamp_last_measurement + 1_s;
if (_rpm_invalid || timeout) {
_pid.resetIntegral(); _pid.resetIntegral();
_actuator_correction = 0.f; _actuator_correction = 0.f;
} }

1
src/modules/control_allocator/ActuatorEffectiveness/RpmControl.hpp
View File

@ -63,6 +63,7 @@ private:
static constexpr float PID_OUTPUT_LIMIT = .5f; // [0,1] static constexpr float PID_OUTPUT_LIMIT = .5f; // [0,1]
uORB::Subscription _rpm_sub{ORB_ID(rpm)}; uORB::Subscription _rpm_sub{ORB_ID(rpm)};
bool _rpm_invalid{true};
PID _pid; PID _pid;
float _spoolup_progress{0.f}; // [0,1] float _spoolup_progress{0.f}; // [0,1]
hrt_abstime _timestamp_last_measurement{0}; // for dt and timeout hrt_abstime _timestamp_last_measurement{0}; // for dt and timeout