fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-10-22 16:03:34 +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_wheel_controller.cpp
+3 -15
View File
@@ -46,7 +46,7 @@
using matrix::wrap_pi;
float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_data)
float ECL_WheelController::control_bodyrate(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.body_z_rate) &&
@@ -56,25 +56,13 @@ float ECL_WheelController::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;
}
/* input conditioning */
float min_speed = 1.0f;
/* Calculate body angular rate error */
_rate_error = _rate_setpoint - ctl_data.body_z_rate; //body angular rate error
if (!lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) {
if (!ctl_data.lock_integrator && _k_i > 0.0f && ctl_data.groundspeed > min_speed) {
float id = _rate_error * dt * ctl_data.groundspeed_scaler;
@@ -101,7 +89,7 @@ float ECL_WheelController::control_bodyrate(const struct ECL_ControlData &ctl_da
return math::constrain(_last_output, -1.0f, 1.0f);
}
float ECL_WheelController::control_attitude(const struct ECL_ControlData &ctl_data)
float ECL_WheelController::control_attitude(const float dt, const ECL_ControlData &ctl_data)
{
/* Do not calculate control signal with bad inputs */
if (!(PX4_ISFINITE(ctl_data.yaw_setpoint) &&