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

fw_att_control: pass time through from run

Browse Source
This commit is contained in:
Daniel Agar
2020-08-10 10:51:52 -04:00
parent 560c22e612
commit 9992bd05db
11 changed files with 59 additions and 106 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/fw_att_control/ecl_pitch_controller.cpp
+5 -18
View File
@@ -43,9 +43,8 @@
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_data)
float ECL_PitchController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.pitch_setpoint) &&
PX4_ISFINITE(ctl_data.roll) &&
@@ -64,7 +63,7 @@ float ECL_PitchController::control_attitude(const struct ECL_ControlData &ctl_da
return _rate_setpoint;
}
float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_data)
float ECL_PitchController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) &&
@@ -79,22 +78,10 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
return math::constrain(_last_output, -1.0f, 1.0f);
}
/* get the usual dt estimate */
uint64_t dt_micros = hrt_elapsed_time(&_last_run);
_last_run = hrt_absolute_time();
float dt = (float)dt_micros * 1e-6f;
/* lock integral for long intervals */
bool lock_integrator = ctl_data.lock_integrator;
if (dt_micros > 500000) {
lock_integrator = true;
}
/* Calculate body angular rate error */
_rate_error = _bodyrate_setpoint - ctl_data.body_y_rate;
if (!lock_integrator && _k_i > 0.0f) {
if (!ctl_data.lock_integrator && _k_i > 0.0f) {
/* Integral term scales with 1/IAS^2 */
float id = _rate_error * dt * ctl_data.scaler * ctl_data.scaler;
@@ -124,7 +111,7 @@ float ECL_PitchController::control_bodyrate(const struct ECL_ControlData &ctl_da
return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_PitchController::control_euler_rate(const struct ECL_ControlData &ctl_data)
float ECL_PitchController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data)
{
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = cosf(ctl_data.roll) * _rate_setpoint +
@@ -132,5 +119,5 @@ float ECL_PitchController::control_euler_rate(const struct ECL_ControlData &ctl_
set_bodyrate_setpoint(_bodyrate_setpoint);
return control_bodyrate(ctl_data);
return control_bodyrate(dt, ctl_data);
}