FlightTaskOrbit: Fix altitude adjustment by stick

This is done by inheriting from FlightTaskManualAltitudeSmoothVel again.
The altitude change by command is taken care of by switching
to the apporach when the altitude difference is big enough and
switching back once the altitude is close enough.

The altitude of the command is not perfectly reached but this can
only be done smoothly when the Orbit has full control over the
altitude smoothing. The independent altitude smoothing is not kept
because it was lacking stick handling like altitude lock and smooth
transitions when opening and closing the vertical position loop.
This commit is contained in:
Matthias Grob
2022-01-25 11:45:31 +01:00
parent d73d8f390b
commit 7c55229db7
3 changed files with 15 additions and 31 deletions
@@ -57,15 +57,14 @@ protected:
void _ekfResetHandlerPositionZ(float delta_z) override;
void _ekfResetHandlerVelocityZ(float delta_vz) override;
void _updateTrajConstraints();
void _setOutputState();
ManualVelocitySmoothingZ _smoothing; ///< Smoothing in z direction
DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAltitude,
(ParamFloat<px4::params::MPC_JERK_MAX>) _param_mpc_jerk_max,
(ParamFloat<px4::params::MPC_ACC_UP_MAX>) _param_mpc_acc_up_max,
(ParamFloat<px4::params::MPC_ACC_DOWN_MAX>) _param_mpc_acc_down_max
)
private:
void _updateTrajConstraints();
void _setOutputState();
ManualVelocitySmoothingZ _smoothing; ///< Smoothing in z direction
};