From 8f584a14960db8a08ba013d58c1e6efca6187661 Mon Sep 17 00:00:00 2001
From: bresch <brescianimathieu@gmail.com>
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
--
GitLab