The mapping itself was seprated out into a calls because it was reused
for the experimental nudging implementation.
The position resets which were handled correctly before now
change the wrong setpoints and I adjusted.
The nudging has to be before any filtering, then these member setpoints
which are essentially copies are not needed anymore.
The velocity setpoint of the position controller
does a jump when unlocking position with a non-zero position error.
This is solved by using the velocity setpoint feedback to smoothly
take over.
With a higher responsiveness, after centering the stick, the velocity and
acceleration setpoints could oscillate around 0 and never reach 0, due to
discretization.
This also prevented position lock engagement.
In some cases e.g.: (VTOL in wind) a good tracking cannot be
achieved. This condition then needs to be relaxed, otherwise the
drone cannot land properly.
Moving the command handling to a separate function that gets called
whenever a vehicle command is available to always react on commands
and not just when already a task is running.
This solves e.g. commanding an Orbit when in Staibilized.
It is important that setpoints get initialized with NAN and not overwritten
if specifically set by a successful flight task execution. It's then
clear if any setpoints were intentionally and successfully set.
Crucial for the position controller's emergency failsafe and the
seamless setpoint handover to the next flight task.
- parameter updates can be quite expensive because they trigger nearly all modules to reload all of their parameters immediately
- limit modules from updating faster than once per second
Matthias Grob