FlightTaskManualPositionSmooth: smooth xy and z

src/lib/FlightTasks/tasks/FlightTaskManualPosition.cpp

@@ -45,7 +45,7 @@ using namespace matrix;
 FlightTaskManualPosition::FlightTaskManualPosition(control::SuperBlock *parent, const char *name) :
 	FlightTaskManualAltitude(parent, name),
 	_vel_xy_manual_max(parent, "MPC_VEL_MANUAL", false),
-	_acc_xy_max(parent, "MPC_ACC_HOR_MAX", false), \
+	_acc_xy_max(parent, "MPC_ACC_HOR_MAX", false),
 	_vel_xy_dz(parent,  "MPC_HOLD_MAX_XY", false)
 {}
 

src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp

@@ -32,7 +32,7 @@
  ****************************************************************************/
 
 /**
- * @file FlightManualAltitude.cpp
+ * @file FlightManualPositionSmooth.cpp
  */
 
 #include "FlightTaskManualPositionSmooth.hpp"
@@ -43,19 +43,41 @@ using namespace matrix;
 
 FlightTaskManualPositionSmooth::FlightTaskManualPositionSmooth(control::SuperBlock *parent, const char *name) :
 	FlightTaskManualPosition(parent, name),
-	_smoothing(&_velocity(0))
-{}
+	_smoothingXY(matrix::Vector2f(&_velocity(0))),
+	_smoothingZ(_velocity(2), _sticks(2))
+
+{
+
+}
+
+
+bool FlightTaskManualPositionSmooth::activate()
+{
+	_vel_sp_prev_z = _velocity(2);
+	return FlightTaskManualPosition::activate();
+}
 
 void FlightTaskManualPositionSmooth::_updateSetpoints()
 {
-	/* Get yaw, thrust */
-	FlightTaskManualStabilized::_updateSetpoints();
+	/* Get yaw, unsmoothe position setpoints */
+	FlightTaskManualPosition::_updateSetpoints();
 
-	/* Smooth velocity setpoint */
+	/* Smooth velocity setpoint in xy */
 	matrix::Vector2f vel(&_velocity(0));
-	_smoothing.smoothVelFromSticks(_vel_sp_xy, vel, _deltatime);
+	_smoothingXY.smoothVelFromSticks(_vel_sp_xy, vel, _deltatime);
 
 	/* Check for altitude lock*/
 	_updateXYlock();
 
+	/* Smooth velocity in z*/
+	float vel_sp[2] = { _vel_sp_z, _vel_sp_prev_z };
+	_smoothingZ.smoothVelFromSticks(vel_sp, _deltatime);
+	_vel_sp_z = vel_sp[0];
+
+	/* Check for altitude lock*/
+	_updateAltitudeLock();
+
+	/* Update previous velocity setpoint for next smoothing iteration */
+	_vel_sp_prev_z = _vel_sp_z;
+
 }

src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.hpp

@@ -32,14 +32,15 @@
  ****************************************************************************/
 
 /**
- * @file FlightManualAltitude.hpp
+ * @file FlightManualPositionSmooth.hpp
  *
- * Flight task for manual controlled altitude.
+ * Flight task for smooth manual controlled position.
  */
 
 
 #include "FlightTaskManualPosition.hpp"
 #include "Utility/ManualSmoothingXY.hpp"
+#include "Utility/ManualSmoothingZ.hpp"
 
 class FlightTaskManualPositionSmooth : public FlightTaskManualPosition
 {
@@ -48,13 +49,14 @@ public:
 
 	virtual ~FlightTaskManualPositionSmooth() = default;
 
+	bool activate() override;
 
 protected:
 
 	virtual void _updateSetpoints() override;
+	ManualSmoothingXY _smoothingXY; // Smoothing for velocity setpoints.
+	ManualSmoothingZ _smoothingZ; //Smoothing for velocity in z.
+	float _vel_sp_prev_z{}; // Velocity setpoint from previous iteration.
 
-private:
-
-	ManualSmoothingXY _smoothing; // Smoothing for velocity setpoints.
 
 };

src/lib/FlightTasks/tasks/Utility/ManualSmoothingXY.cpp

@@ -116,8 +116,9 @@ ManualSmoothingXY::_getIntention(const matrix::Vector2f &vel_sp)
 		/* Distinguish between acceleration, deceleration and direction change */
 
 		/* Check if stick direction and current velocity are within 120.
-		 * If previous and current setpoint are more than 120 apart, we assume
-		 * that the user demanded a direction change. */
+		 * If current setpoint and velocity are more than 120 apart, we assume
+		 * that the user demanded a direction change.
+		 * The detecton has to happen in body frame.*/
 		matrix::Vector2f vel_sp_unit = vel_sp;;
 		matrix::Vector2f vel_sp_prev_unit = _vel_sp_prev;
 
@@ -238,4 +239,6 @@ ManualSmoothingXY::_velocitySlewRate(matrix::Vector2f &vel_sp, const float dt)
 	if (acc.length() > _acc_state_dependent) {
 		vel_sp = acc.normalized() * _acc_state_dependent  * dt + _vel_sp_prev;
 	}
+
+	_vel_sp_prev = vel_sp;
 }