With a DLPF of 62.5 Hz, the sampling rate is apperently not 1 kHz anymore,
because the driver got duplicate samples and published only at 128 Hz.
We have to increase the filter back to 500 Hz so that we get 1 kHz sampling
rate, with 250 Hz publications.

Co-Authored-By: EliaTarasov <elias.tarasov@gmail.com>

Signed-off-by: Roman <bapstroman@gmail.com>

[Position|AutoLine]SmoothVel - Update position and velocity states of the trajectories in case of EKF reset. Until now, only the position reset on XY axis was properly handled. Now, xy, vxy, z and vz are handled

   This also fixes a typo in the GPIO defines

ManualPositionSmoothVel - Force velocity and acceleration setpoints to zero if the velocity setpoint is smaller than 1mm/s and that the acceleration setpoint is null

ManualSmoothVel - Set jerk to 1.0 during position hold to help the optimizer to converge towards 0 vel and acc