Because the velocity starts with a positive value (down) to ramp-up
slowly the motors, the position setpoint (integral) was going down into
the ground, canceling the input of the velocity controller during the
early takeoff phase.
Instead of a smooth ramp, the behavior was a delay followed by a quick
takeoff or a land detection.
Setting the current altitude setpoint with the current altitude during
this phase avoids the canceling effect and creates a takeoff purely
velocity-based.
When disable_weather_vane is true, the vehicle is aligning for the transition waypoint
(VTOL) and thus the yaw setpoint from Navigator should be used.
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
During takeoff, Navigator is sending a constant yaw value.
However, there is always a reset at 1.5m of the yaw estimate in EKF2
that is not handled by Navigator that produces a glitch in the rate
controller. Given that in FlightTask, the yaw
is already computed and properly corrected in case of an estimator reset
event, we just ignore the yaw value sent by navigator during takeoff.
The acceleration setpoint gets implicitly inherited from the altitude
flight task since #14212. This feed-forward adds an unwanted
acceleration when the right stick is deflected. Instead I'm using it
to command the expected centripetal acceleration when flying
in a circle for better orbit tracking.
For the RC controlled yaw behaviour, we do a yaw setpoint according to
the stick expo. The uncontrolled yaw behaviour behaves undefined.
Switching between yaw behaviours makes the drone stand still for a
moment, which probably can be improved.
This fixes the corner case where a NAV_DELAY command changes the
validity of the next WP but not the rest of the triplet. The logic in
FlightTask was ignoring this change because the check was only based on
WP position change.
* FlightTask: Fix ekf2 reset race condition during task switch
During a loss of GPS data when using GPS as primary height source,
the height is reset to baro and the local position gets invalid at the
same time. This triggers a switch to altitude flight task and a setpoint
reset.
This combination of events had the effect to ignore the height reset,
the large sudden height error could create an abrupt change of altitude
or even a crash.
The ekf2 reset is now done at the beginning of each update call.
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.
Previously acceleration setpoints were not executed and just used
to pass a possible rough initialization value for the next task. Now
they get executed by the multicopter controller and hence
overwriting them with rough estimates doesn't work anymore.
This mode was just kept as an example after
its usage in a single case. It's basically untested
and doesn't make much sense anymore since it's
incompatible with the jerk limited trajectory
implementations. It's implementation only switched
hte configuration parameter of the velocity resulting
from maximum stick deflection to be
MPC_XY_VEL_MAX instead of MPC_VEL_MANUAL.
This is according to todays understanding undesired
because when hitting that limit the position
controller has no room for corrections anymore.
Also it saves some flash space on omnibus to remove
the task at this point and makes romm for the
acceleration feed-forward.
Unfortunately the constraints cannot be implemented using the
_constraints because the controller uses that directly. This
causes discontinuities in the velocity and/or accel at various
points of the flight. In particular this was used in Takeoff.
Instead this was done by changing target accel/velocity in the
jerk-optimal velocity planner for Z.
The modified statements are intended to
1. Set a new Z position setpoint that is equivalent to the current distance
to ground setpoint,
and
2. Set a new distance to ground setpoint that is equivalent to the current Z
position setpoint.
They are only called in terrain hold mode, when activating/deactivating the
holding (typically when coming to a stop and when starting to move again).
The setpoints take the current control error into account, but because the
control error is added, not subtracted, the result is that the new setpoint
is 2 times the control error off from the old setpoint, instead of being
at the same spot as the old setpoint.
Dusan Zivkovic