manual_control: add reset to diff

This should prevent triggering user override right after RC re-appears.
This commit is contained in:
Julian Oes
2021-06-10 11:48:17 +02:00
committed by Matthias Grob
parent 93c505b163
commit d0fea93d72
2 changed files with 19 additions and 8 deletions
+4 -4
View File
@@ -275,10 +275,10 @@ void ManualControl::Run()
_manual_control_setpoint_pub.publish(_selector.setpoint());
}
_x_diff.update(0.0f, dt_s);
_y_diff.update(0.0f, dt_s);
_z_diff.update(0.0f, dt_s);
_r_diff.update(0.0f, dt_s);
_x_diff.reset();
_y_diff.reset();
_z_diff.reset();
_r_diff.reset();
}
_last_time = now;
+15 -4
View File
@@ -66,8 +66,12 @@ public:
dt_s = _time_period_s;
}
const float new_diff = value - _last_value;
_diff = new_diff * dt_s + _diff * (_time_period_s - dt_s);
// Leave _diff at 0.0f if we don't have a _last_value yet.
if (PX4_ISFINITE(_last_value)) {
const float new_diff = value - _last_value;
_diff = new_diff * dt_s + _diff * (_time_period_s - dt_s);
}
_last_value = value;
}
@@ -76,10 +80,17 @@ public:
return _diff;
}
void reset()
{
_diff = 0.0f;
_last_value = NAN;
}
private:
static constexpr float _time_period_s{1.0f};
float _diff{0.0f};
float _last_value{0.0f};
const float _time_period_s{1.0f};
float _last_value{NAN};
};