From 8f584a14960db8a08ba013d58c1e6efca6187661 Mon Sep 17 00:00:00 2001 From: bresch Date: Thu, 14 Feb 2019 12:24:35 +0100 Subject: [PATCH] smooth takeoff - Support smooth takeoff triggered by jerk setpoint --- .../FlightTaskManualPositionSmoothVel.cpp | 11 +++++++++-- .../FlightTaskManualPositionSmoothVel.hpp | 7 ++++++- src/modules/mc_pos_control/mc_pos_control_main.cpp | 9 +++++---- 3 files changed, 20 insertions(+), 7 deletions(-) diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp index b0ffe8378b..958b8b57c2 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.cpp @@ -49,10 +49,12 @@ bool FlightTaskManualPositionSmoothVel::activate() void FlightTaskManualPositionSmoothVel::reActivate() { - reset(Axes::XY); + // The task is reacivated while the vehicle is on the ground. To detect takeoff in mc_pos_control_main properly + // using the generated jerk, reset the z derivatives to zero + reset(Axes::XYZ, true); } -void FlightTaskManualPositionSmoothVel::reset(Axes axes) +void FlightTaskManualPositionSmoothVel::reset(Axes axes, bool force_z_zero) { int count; @@ -75,6 +77,11 @@ void FlightTaskManualPositionSmoothVel::reset(Axes axes) _smoothing[i].reset(0.f, _velocity(i), _position(i)); } + // Set the z derivatives to zero + if (force_z_zero) { + _smoothing[2].reset(0.f, 0.f, _position(2)); + } + _position_lock_xy_active = false; _position_lock_z_active = false; _position_setpoint_xy_locked(0) = NAN; diff --git a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp index 350bc580c8..038d9ddd85 100644 --- a/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp +++ b/src/lib/FlightTasks/tasks/ManualPositionSmoothVel/FlightTaskManualPositionSmoothVel.hpp @@ -65,7 +65,12 @@ protected: private: enum class Axes {XY, XYZ}; - void reset(Axes axes); + + /** + * Reset the required axes. when force_z_zero is set to true, the z derivatives are set to sero and not to the estimated states + */ + void reset(Axes axes, bool force_z_zero = false); + VelocitySmoothing _smoothing[3]; ///< Smoothing in x, y and z directions matrix::Vector3f _vel_sp_smooth; bool _position_lock_xy_active{false}; diff --git a/src/modules/mc_pos_control/mc_pos_control_main.cpp b/src/modules/mc_pos_control/mc_pos_control_main.cpp index 237f3d53bd..421ad96001 100644 --- a/src/modules/mc_pos_control/mc_pos_control_main.cpp +++ b/src/modules/mc_pos_control/mc_pos_control_main.cpp @@ -243,7 +243,7 @@ private: * @param velocity setpoint_z the velocity setpoint in the z-Direction */ void check_for_smooth_takeoff(const float &position_setpoint_z, const float &velocity_setpoint_z, - const vehicle_constraints_s &constraints); + const float &jerk_sp, const vehicle_constraints_s &constraints); /** * Check if smooth takeoff has ended and updates accordingly. @@ -702,7 +702,7 @@ MulticopterPositionControl::run() // do smooth takeoff after delay if there's a valid vertical velocity or position if (_spoolup_time_hysteresis.get_state() && PX4_ISFINITE(_states.position(2)) && PX4_ISFINITE(_states.velocity(2))) { - check_for_smooth_takeoff(setpoint.z, setpoint.vz, constraints); + check_for_smooth_takeoff(setpoint.z, setpoint.vz, setpoint.jerk_z, constraints); update_smooth_takeoff(setpoint.z, setpoint.vz); } @@ -760,7 +760,6 @@ MulticopterPositionControl::run() // Generate desired thrust and yaw. _control.generateThrustYawSetpoint(_dt); - // Fill local position, velocity and thrust setpoint. // This message contains setpoints where each type of setpoint is either the input to the PositionController // or was generated by the PositionController and therefore corresponds to the PositioControl internal states (states that were generated by P-PID). @@ -1034,7 +1033,7 @@ MulticopterPositionControl::start_flight_task() void MulticopterPositionControl::check_for_smooth_takeoff(const float &z_sp, const float &vz_sp, - const vehicle_constraints_s &constraints) + const float &jerk_sp, const vehicle_constraints_s &constraints) { // Check for smooth takeoff if (_vehicle_land_detected.landed && !_in_smooth_takeoff) { @@ -1047,6 +1046,8 @@ MulticopterPositionControl::check_for_smooth_takeoff(const float &z_sp, const fl // takeoff was initiated through velocity setpoint _smooth_velocity_takeoff = PX4_ISFINITE(vz_sp) && vz_sp < -0.1f; + bool jerk_triggered_takeoff = PX4_ISFINITE(jerk_sp) && jerk_sp < -FLT_EPSILON; + _smooth_velocity_takeoff |= jerk_triggered_takeoff; if ((PX4_ISFINITE(z_sp) && z_sp < _states.position(2) - min_altitude) || _smooth_velocity_takeoff) { // There is a position setpoint above current position or velocity setpoint larger than