fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 14:53:35 +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_yaw_controller.cpp
+5 -18
View File
@@ -43,9 +43,8 @@
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data)
float ECL_YawController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) &&
PX4_ISFINITE(ctl_data.pitch) &&
@@ -95,7 +94,7 @@ float ECL_YawController::control_attitude(const struct ECL_ControlData &ctl_data
return _rate_setpoint;
}
float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data)
float ECL_YawController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll) &&
@@ -110,22 +109,10 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
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_z_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;
@@ -155,7 +142,7 @@ float ECL_YawController::control_bodyrate(const struct ECL_ControlData &ctl_data
return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_data)
float ECL_YawController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data)
{
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = -sinf(ctl_data.roll) * ctl_data.pitch_rate_setpoint +
@@ -163,5 +150,5 @@ float ECL_YawController::control_euler_rate(const struct ECL_ControlData &ctl_da
set_bodyrate_setpoint(_bodyrate_setpoint);
return control_bodyrate(ctl_data);
return control_bodyrate(dt, ctl_data);
}