From 7c55229db7c09422f816d4be43d471b59115ea40 Mon Sep 17 00:00:00 2001 From: Matthias Grob Date: Fri, 21 Jan 2022 16:10:13 +0100 Subject: [PATCH] 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. --- .../FlightTaskManualAltitudeSmoothVel.hpp | 11 +++---- .../tasks/Orbit/FlightTaskOrbit.cpp | 32 ++++++------------- .../tasks/Orbit/FlightTaskOrbit.hpp | 3 +- 3 files changed, 15 insertions(+), 31 deletions(-) diff --git a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp index dabbac620d..64f1aec554 100644 --- a/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp +++ b/src/modules/flight_mode_manager/tasks/ManualAltitudeSmoothVel/FlightTaskManualAltitudeSmoothVel.hpp @@ -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) _param_mpc_jerk_max, (ParamFloat) _param_mpc_acc_up_max, (ParamFloat) _param_mpc_acc_down_max ) - -private: - void _updateTrajConstraints(); - void _setOutputState(); - - ManualVelocitySmoothingZ _smoothing; ///< Smoothing in z direction }; diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp index 7612923c55..0adc4f6eae 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.cpp @@ -188,9 +188,7 @@ bool FlightTaskOrbit::activate(const vehicle_local_position_setpoint_s &last_set bool FlightTaskOrbit::update() { - // update altitude - bool ret = FlightTaskManualAltitude::update(); - + bool ret = true; _updateTrajectoryBoundaries(); // stick input adjusts parameters within a fixed time frame @@ -203,7 +201,10 @@ bool FlightTaskOrbit::update() if (_is_position_on_circle()) { if (_in_circle_approach) { _in_circle_approach = false; - _altitude_velocity_smoothing.reset(0, _velocity(2), _position(2)); + FlightTaskManualAltitudeSmoothVel::_smoothing.reset( + PX4_ISFINITE(_acceleration_setpoint(2)) ? _acceleration_setpoint(2) : 0.f, + PX4_ISFINITE(_velocity_setpoint(2)) ? _velocity_setpoint(2) : _velocity(2), + PX4_ISFINITE(_position_setpoint(2)) ? _position_setpoint(2) : _position(2)); } } @@ -211,20 +212,12 @@ bool FlightTaskOrbit::update() _generate_circle_approach_setpoints(); } else { + // update altitude + ret = ret && FlightTaskManualAltitudeSmoothVel::update(); + // this generates x / y setpoints _generate_circle_setpoints(); _generate_circle_yaw_setpoints(); - - // in case we have a velocity setpoint in altititude (from altitude parent) - // smooth this - if (!PX4_ISFINITE(_position_setpoint(2))) { - _altitude_velocity_smoothing.updateDurations(_velocity_setpoint(2)); - _altitude_velocity_smoothing.updateTraj(_deltatime); - _velocity_setpoint(2) = _altitude_velocity_smoothing.getCurrentVelocity(); - _acceleration_setpoint(2) = _altitude_velocity_smoothing.getCurrentAcceleration(); - // set orbit altitude center to expected new altitude - _center(2) = _altitude_velocity_smoothing.getCurrentPosition(); - } } // Apply yaw smoothing @@ -250,21 +243,14 @@ void FlightTaskOrbit::_updateTrajectoryBoundaries() _position_smoothing.setMaxVelocityXY(_param_mpc_xy_vel_max.get()); float max_jerk = _param_mpc_jerk_auto.get(); _position_smoothing.setMaxJerk({max_jerk, max_jerk, max_jerk}); // TODO : Should be computed using heading - _altitude_velocity_smoothing.setMaxJerk(max_jerk); - - const float z_accel_constraint = _param_mpc_acc_up_max.get(); if (_velocity_setpoint(2) < 0.f) { // up _position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_up.get()); - _position_smoothing.setMaxAccelerationZ(z_accel_constraint); - _altitude_velocity_smoothing.setMaxVel(_param_mpc_z_vel_max_up.get()); - _altitude_velocity_smoothing.setMaxAccel(z_accel_constraint); + _position_smoothing.setMaxAccelerationZ(_param_mpc_acc_up_max.get()); } else { // down _position_smoothing.setMaxAccelerationZ(_param_mpc_acc_down_max.get()); _position_smoothing.setMaxVelocityZ(_param_mpc_z_v_auto_dn.get()); - _altitude_velocity_smoothing.setMaxAccel(_param_mpc_acc_down_max.get()); - _altitude_velocity_smoothing.setMaxVel(_param_mpc_z_vel_max_dn.get()); } } diff --git a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp index 5da387c5be..9ebd760dde 100644 --- a/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp +++ b/src/modules/flight_mode_manager/tasks/Orbit/FlightTaskOrbit.hpp @@ -49,7 +49,7 @@ #include -class FlightTaskOrbit : public FlightTaskManualAltitude +class FlightTaskOrbit : public FlightTaskManualAltitudeSmoothVel { public: @@ -119,7 +119,6 @@ private: bool _in_circle_approach = false; PositionSmoothing _position_smoothing; - VelocitySmoothing _altitude_velocity_smoothing; /** yaw behaviour during the orbit flight according to MAVLink's ORBIT_YAW_BEHAVIOUR enum */ int _yaw_behaviour = orbit_status_s::ORBIT_YAW_BEHAVIOUR_HOLD_FRONT_TO_CIRCLE_CENTER;