This function updates the vertical velocity integrator with the change
in hover thrust to avoid propagating discontinuities through the
controller when changing the hover thrust.
This is also important when using the hover thrust estimator as its
estimate has unconstrained dynamics and can cause drops or kicks when
the estimate updates faster than the velocity integrator.
* MC_HTE: unitialize with hover_thrust parameter
* MC_HTE: constrain hover thrust setter between 0.1 and 0.9
* MC_HTE: integrate with land detector and velocity controller
* MCHoverThrustEstimator: Always publish an estimate even when not fusing measurements. This is required as the land detector and the position controller need to receive a hover thrust value.
* MC_HTE: use altitude agl threshold to start the estimator
local_position.z is relative to the origin of the EKF while dist_bottom
is above ground
Co-authored-by: bresch <brescianimathieu@gmail.com>
with measurement noise auto-tuning
The purpose of this estimator is to improve land detection and vertical
velocity feedforward
Recovery strategy:
This is required when the setpoint suddenly changes in air or that the
EKF is diverging. A lowpassed test ratio is used as a trigger for the recovery logic
Also, a lowpassed residual is used to estimate the steady-state value
and remove it when estimating the accel noise to avoid increasing the
accel noise when the redisual is caused by an offset.
These assignments were temporary for correct logging of the input
setpoint which now properly also comes out of the controller again.
This is reverting the hotfix from:
dee8d200d8
Removing the skip_controller and interfaceMapping
concept and replacing it with a single method checking
if the position control update was successful and
return the result in the PositionControl.update().
This restores original horizontal thrust setpoint execution
that I intentionally broke three commits ago. It's necessary
for correct stick to tilt mapping of FlightTaskManualAltitude.
This commit temporarily breaks direct horizontal thrust setpoint execution
which is used by FlightTaskManualAltitude. This is necessary to allow for
PositionControl cleanup namely calculating the whole velocity PID in one
Vector3f formula.
Having this in a separate commit is useful since it reduces indentation
of a bigger code block.
When having no velocity estimate the derivative was updated with zero.
When losing the velocity estimate this is fine since the resulting
derivative spike doesn't get used and acceleration is set to NAN.
But when regaining the velocity estimate the spike from zero to
the first estimated velocity gets used as acceleration in the position
controller and results in a twitch.
To solve this I use the derivative reset I introduced in pr #13522
commit b64abf48b2
This caused bad altitude control performance when enabling
terrain following. It even leads to complete vertical control
instability in case dist_bottom is inaccurate.
Relying on the estimator states is the way to go instead of
silently using one altitude source as state.
The test was testing the result of 3D float vector operations with binary equality of the floating point numbers, which is not a valid assumption to make for floating point math. This change switches to proper comparisons with float accuracy and compares vectors using the norm of their difference.
used back in time when no soothing was done to avoid large steps when
the estimator stops sending a maximum velocity (e.g.:GPS gained during
optical flow flight). Since smoothing is applied later, this is not
required anymore.
This issue was found by @khabir and reported over slack.
It resulted from the split up of the big pr #12072 into #13262.
I took over the interface while the internal states still stayed the
hard to understand internal ones. One of the follow up refactors will
fix this completely and the entire legacy setpoint restore block can
be removed.
The plot of the attitude setpoint in the log did not show any values
because the message timestamp that the position control module sets
was overwritten by the PositionControl attitude generation.
by applying it directly to the attitude setpoint which is the output of
the position controller.
The problem was that before the input to the attitude setpoint generation
was adjusted to generate a level attitude with zero thrust keeping the
heading. I refactored the PositionControl class in #13262 to directly
generate the attitude setpoint output. So here I'm adjusting the attitude
setpoint to do the exact same thing as before but without interleaving
with the PositionControl logic.
bresch