FlightTaskManualAltitudeSmooth: create taks

src/lib/FlightTasks/CMakeLists.txt

@@ -38,6 +38,7 @@ px4_add_module(
 		tasks/FlightTaskManualStabilized.cpp
 		tasks/FlightTaskOrbit.cpp
 		tasks/FlightTaskManualAltitude.cpp
+		tasks/FlightTaskManualAltitudeSmooth.cpp
 		tasks/FlightTaskManualPosition.cpp
 		tasks/Utility/ManualSmoothingZ.cpp
 		SubscriptionArray.cpp

src/lib/FlightTasks/FlightTasks.cpp

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

src/lib/FlightTasks/FlightTasks.hpp

@@ -43,6 +43,7 @@
 
 #include "tasks/FlightTask.hpp"
 #include "tasks/FlightTaskManualAltitude.hpp"
+#include "tasks/FlightTaskManualAltitudeSmooth.hpp"
 #include "tasks/FlightTaskManualPosition.hpp"
 #include "tasks/FlightTaskManualStabilized.hpp"
 #include "tasks/FlightTaskOrbit.hpp"
@@ -123,6 +124,7 @@ private:
 
 		FlightTaskManualStabilized stabilized;
 		FlightTaskManualAltitude altitude;
+		FlightTaskManualAltitudeSmooth altitude_smooth;
 		FlightTaskManualPosition position;
 		FlightTaskOrbit orbit;
 		FlightTaskSport sport;

src/lib/FlightTasks/tasks/FlightTaskManualAltitude.hpp

@@ -60,9 +60,8 @@ protected:
 	control::BlockParamFloat _vel_max_up; /**< Maximum speed allowed to go down. */
 	control::BlockParamFloat _vel_z_dz; /**< velocity threshold/deadzone to switch into vertical position hold */
 
+	void _updateZsetpoints(); /**< Sets position or velocity setpoints. */
 	void _updateSetpoints() override; /**< Updates all setpoints. */
 	void _scaleSticks() override; /**< Scales sticks to velocity in z. */
 
-private:
-	void _updateZsetpoints(); /**< Sets position or velocity setpoints. */
 };

src/lib/FlightTasks/tasks/FlightTaskManualAltitudeSmooth.cpp

@@ -0,0 +1,70 @@
+/****************************************************************************
+ *
+ *   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 "FlightTaskManualAltitudeSmooth.hpp"
+#include <mathlib/mathlib.h>
+#include <float.h>
+
+using namespace matrix;
+
+FlightTaskManualAltitudeSmooth::FlightTaskManualAltitudeSmooth(control::SuperBlock *parent, const char *name) :
+	FlightTaskManualAltitude(parent, name),
+	_smoothing(_velocity(2), _sticks(2))
+{}
+
+
+bool FlightTaskManualAltitudeSmooth::activate()
+{
+	_vel_sp_prev_z = _velocity(2);
+	return FlightTaskManualStabilized::activate();
+}
+
+void FlightTaskManualAltitudeSmooth::_updateSetpoints()
+{
+	/* Get yaw, thrust */
+	FlightTaskManualStabilized::_updateSetpoints();
+
+	/* Smooth velocity setpoint */
+	float vel_sp[2] = {_vel_sp_z, _vel_sp_prev_z};
+	_smoothing.smoothVelFromSticks(vel_sp, _deltatime);
+
+	/* Update position setpoint in z direction based on lock criteria*/
+	_updateZsetpoints();
+
+	/* Update previous velocity setpoint for next smoothing iteration */
+	_vel_sp_prev_z = _vel_sp_z;
+}

src/lib/FlightTasks/tasks/FlightTaskManualAltitudeSmooth.hpp

@@ -0,0 +1,63 @@
+/****************************************************************************
+ *
+ *   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 "FlightTaskManualAltitude.hpp"
+#include "Utility/ManualSmoothingZ.hpp"
+
+class FlightTaskManualAltitudeSmooth : public FlightTaskManualAltitude
+{
+public:
+	FlightTaskManualAltitudeSmooth(control::SuperBlock *parent, const char *name);
+
+	virtual ~FlightTaskManualAltitudeSmooth() = default;
+
+	bool activate() override;
+
+
+protected:
+
+	virtual void _updateSetpoints() override;
+
+private:
+
+	ManualSmoothingZ _smoothing; // Smoothing for velocity setpoints.
+	float _vel_sp_prev_z{}; // Velocity setpoint from previous iteration.
+
+};