fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-07-17 05:10: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-07 21:35:39 -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_roll_controller.cpp
+5 -17
View File
@@ -43,7 +43,7 @@
#include <lib/ecl/geo/geo.h>
#include <mathlib/mathlib.h>
float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_data)
float ECL_RollController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.roll_setpoint) &&
@@ -61,7 +61,7 @@ float ECL_RollController::control_attitude(const struct ECL_ControlData &ctl_dat
return _rate_setpoint;
}
float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_data)
float ECL_RollController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.pitch) &&
@@ -75,22 +75,10 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
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_x_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;
@@ -120,12 +108,12 @@ float ECL_RollController::control_bodyrate(const struct ECL_ControlData &ctl_dat
return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_RollController::control_euler_rate(const struct ECL_ControlData &ctl_data)
float ECL_RollController::control_euler_rate(const float dt, const ECL_ControlData &ctl_data)
{
/* Transform setpoint to body angular rates (jacobian) */
_bodyrate_setpoint = ctl_data.roll_rate_setpoint - sinf(ctl_data.pitch) * ctl_data.yaw_rate_setpoint;
set_bodyrate_setpoint(_bodyrate_setpoint);
return control_bodyrate(ctl_data);
return control_bodyrate(dt, ctl_data);
}