Add FlightTaskManualPositionSmooth

src/lib/FlightTasks/CMakeLists.txt

@@ -40,6 +40,7 @@ px4_add_module(
 		tasks/FlightTaskManualAltitude.cpp
 		tasks/FlightTaskManualAltitudeSmooth.cpp
 		tasks/FlightTaskManualPosition.cpp
+		tasks/FlightTaskManualPositionSmooth.cpp
 		tasks/Utility/ManualSmoothingZ.cpp
 		tasks/Utility/ManualSmoothingXY.cpp
 		SubscriptionArray.cpp

src/lib/FlightTasks/FlightTasks.cpp

@@ -68,6 +68,10 @@ int FlightTasks::switchTask(int task_number)
 		_current_task = new (&_task_union.altitude_smooth) FlightTaskManualAltitudeSmooth(this, "MANALTSM");
 		break;
 
+	case 7:
+		_current_task = new (&_task_union.position_smooth) FlightTaskManualPositionSmooth(this, "MANPOSSM");
+		break;
+
 	case -1:
 		/* disable tasks is a success */
 		return 0;

src/lib/FlightTasks/FlightTasks.hpp

@@ -45,6 +45,7 @@
 #include "tasks/FlightTaskManualAltitude.hpp"
 #include "tasks/FlightTaskManualAltitudeSmooth.hpp"
 #include "tasks/FlightTaskManualPosition.hpp"
+#include "tasks/FlightTaskManualPositionSmooth.hpp"
 #include "tasks/FlightTaskManualStabilized.hpp"
 #include "tasks/FlightTaskOrbit.hpp"
 #include "tasks/FlightTaskSport.hpp"
@@ -126,6 +127,7 @@ private:
 		FlightTaskManualAltitude altitude;
 		FlightTaskManualAltitudeSmooth altitude_smooth;
 		FlightTaskManualPosition position;
+		FlightTaskManualPosition position_smooth;
 		FlightTaskOrbit orbit;
 		FlightTaskSport sport;
 	} _task_union; /**< storage for the currently active task */

src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.cpp

+/****************************************************************************
+ *
+ *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightManualAltitude.cpp
+ */
+
+#include "FlightTaskManualPositionSmooth.hpp"
+#include <mathlib/mathlib.h>
+#include <float.h>
+
+using namespace matrix;
+
+FlightTaskManualPositionSmooth::FlightTaskManualPositionSmooth(control::SuperBlock *parent, const char *name) :
+	FlightTaskManualPosition(parent, name),
+	_smoothing(&_velocity(0))
+{}
+
+void FlightTaskManualPositionSmooth::_updateSetpoints()
+{
+	/* Get yaw, thrust */
+	FlightTaskManualStabilized::_updateSetpoints();
+
+	/* Smooth velocity setpoint */
+	matrix::Vector2f vel(&_velocity(0));
+	_smoothing.smoothVelFromSticks(_vel_sp_xy, vel, _deltatime);
+
+	/* Check for altitude lock*/
+	_updateXYlock();
+
+}

src/lib/FlightTasks/tasks/FlightTaskManualPositionSmooth.hpp

+/****************************************************************************
+ *
+ *   Copyright (c) 2017 PX4 Development Team. All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * 1. Redistributions of source code must retain the above copyright
+ *    notice, this list of conditions and the following disclaimer.
+ * 2. Redistributions in binary form must reproduce the above copyright
+ *    notice, this list of conditions and the following disclaimer in
+ *    the documentation and/or other materials provided with the
+ *    distribution.
+ * 3. Neither the name PX4 nor the names of its contributors may be
+ *    used to endorse or promote products derived from this software
+ *    without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
+ * OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
+ * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ *
+ ****************************************************************************/
+
+/**
+ * @file FlightManualAltitude.hpp
+ *
+ * Flight task for manual controlled altitude.
+ */
+
+
+#include "FlightTaskManualPosition.hpp"
+#include "Utility/ManualSmoothingXY.hpp"
+
+class FlightTaskManualPositionSmooth : public FlightTaskManualPosition
+{
+public:
+	FlightTaskManualPositionSmooth(control::SuperBlock *parent, const char *name);
+
+	virtual ~FlightTaskManualPositionSmooth() = default;
+
+
+protected:
+
+	virtual void _updateSetpoints() override;
+
+private:
+
+	ManualSmoothingXY _smoothing; // Smoothing for velocity setpoints.
+
+};

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

@@ -159,7 +159,7 @@ ManualSmoothingXY::_getStateAcceleration(const matrix::Vector2f &vel_sp, const m
 	case Intention::brake: {
 
 			/* First iteration where user demands brake */
-			if (intention != intention) {
+			if (intention != _intention) {
 				/* we start braking with lowest acceleration
 				 * This make stopping smoother. */
 				_acc_state_dependent = _dec_xy_min;