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.
Use yawrate setpoint from weather vane only if velocity setpoint is small
(smaller than half the cruise velocity)
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
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.
This will allow navigator to not output any setpoints while disarmed.
Otherwise the position controller outputs warnings in the form of:
Auto activation failed with error: Activation Failed
The risk here is that it could hide problems, where navigator sends invalid
triplets when it should not.
- rename of flag in position sp: from allow_weather_vane to disable_weather_vane
- flag now doesn't have to be set for all auto modes, meaning that weather vane is also active outside of mission
- flag is set before front transition to align with wp, and unset after alignment is over
Signed-off-by: Silvan Fuhrer <silvan@auterion.com>
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.
Martina Rivizzigno