-always reset roll/pitch/yaw integrators at the same time
-reset them while waiting for launch or during FW Takeoff before Climbout
-reset wheel rate integrator only when disarmed
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
Since the horizontal and vertical velocity controllers are now
decoupled, it can be that the horizontal acceleration produced by the
controller is actually greater than the desired one (by design). This
condition would actually make the ARW run "backwards", degrading the
controller performance.
- Avoid constantly adjusting the velocity gains with the HTE
- Make sure the hover thrust integral update doesn't break
even though its unit is acceleration and not unit thrust anymore
We need to convert the velocity gains to not contain/depend on the
hover thrust. In horizontal direction it doesn't make sense to scale
them with the hover thrust and in vertical direction the adjustments are
already done in the acceleration to collective thrust conversion.
Adjusting the tilt limit can lead to diverging position control
and should only be used by setting a sanity limit for the controller
and not to adjust during the descent phase of a Land or RTL.
Otherwise it leads to flyaways in important failsafe modes when
there's stronger disturbance e.g. wind.
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.
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.
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.
Matthias Grob