FlightTaskTransition: Transition improvements

- use fw pitch setpoint offset during transition
- take over previous vertical velocity and smooth out over transition

Signed-off-by: RomanBapst <bapstroman@gmail.com>

src/modules/flight_mode_manager/tasks/Transition/CMakeLists.txt

@@ -35,5 +35,5 @@ px4_add_library(FlightTaskTransition
 	FlightTaskTransition.cpp
 )
 
-target_link_libraries(FlightTaskTransition PUBLIC FlightTask)
+target_link_libraries(FlightTaskTransition PUBLIC FlightTask FlightTaskUtility)
 target_include_directories(FlightTaskTransition PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})

src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.cpp

@@ -36,26 +36,77 @@
  */
 
 #include "FlightTaskTransition.hpp"
+#include "Sticks.hpp"
+
+FlightTaskTransition::FlightTaskTransition()
+{
+	_param_handle_pitch_cruise_degrees = param_find("FW_PSP_OFF");
+
+	if (_param_handle_pitch_cruise_degrees != PARAM_INVALID) {
+		param_get(_param_handle_pitch_cruise_degrees, &_param_pitch_cruise_degrees);
+	}
+
+}
 
 bool FlightTaskTransition::updateInitialize()
 {
+
+	updateParameters();
 	return FlightTask::updateInitialize();
 }
 
+void FlightTaskTransition::updateParameters()
+{
+// check for parameter updates
+	if (_parameter_update_sub.updated()) {
+		// clear update
+		parameter_update_s pupdate;
+		_parameter_update_sub.copy(&pupdate);
+
+		// update parameters from storage
+		if (_param_handle_pitch_cruise_degrees != PARAM_INVALID) {
+			param_get(_param_handle_pitch_cruise_degrees, &_param_pitch_cruise_degrees);
+		}
+	}
+}
+
 bool FlightTaskTransition::activate(const vehicle_local_position_setpoint_s &last_setpoint)
 {
-	return FlightTask::activate(last_setpoint);
+	bool ret = FlightTask::activate(last_setpoint);
+
+	_vel_z_filter.setParameters(math::constrain(_deltatime, 0.01f, 0.1f), _vel_z_filter_time_const);
+
+	if (PX4_ISFINITE(last_setpoint.vz)) {
+		_vel_z_filter.reset(last_setpoint.vz);
+
+	} else {
+		_vel_z_filter.reset(_velocity(2));
+	}
+
+	_velocity_setpoint(2) = _vel_z_filter.getState();
+
+	return ret;
+
+
 }
 
 bool FlightTaskTransition::update()
 {
-	bool ret = FlightTask::update();
-	_acceleration_setpoint.xy() = matrix::Vector2f(0.f, 0.f);
-	// demand zero vertical velocity and level attitude
 	// tailsitters will override attitude and thrust setpoint
 	// tiltrotors and standard vtol will overrride roll and pitch setpoint but keep vertical thrust setpoint
+	bool ret = FlightTask::update();
+
 	_position_setpoint.setAll(NAN);
-	_velocity_setpoint(2) = 0.0f;
+
+	// calculate a horizontal acceleration vector which corresponds to an attitude composed of pitch up by _param_pitch_cruise_degrees
+	// and zero roll angle
+	matrix::Vector2f tmp(-1.0f, 0.0f);
+	Sticks::rotateIntoHeadingFrameXY(tmp, _yaw, NAN);
+	_acceleration_setpoint.xy() = tmp * tanf(math::radians(_param_pitch_cruise_degrees)) * CONSTANTS_ONE_G;
+
+	// slowly move vertical velocity setpoint to zero
+	_vel_z_filter.setParameters(math::constrain(_deltatime, 0.01f, 0.1f), _vel_z_filter_time_const);
+	_velocity_setpoint(2) = _vel_z_filter.update(0.0f);
 
 	_yaw_setpoint = NAN;
 	return ret;

src/modules/flight_mode_manager/tasks/Transition/FlightTaskTransition.hpp

@@ -40,14 +40,34 @@
 #pragma once
 
 #include "FlightTask.hpp"
+#include <lib/mathlib/math/filter/AlphaFilter.hpp>
+#include <uORB/SubscriptionInterval.hpp>
+#include <uORB/topics/parameter_update.h>
+#include <drivers/drv_hrt.h>
+
+using namespace time_literals;
+
 
 class FlightTaskTransition : public FlightTask
 {
 public:
-	FlightTaskTransition() = default;
+	FlightTaskTransition();
 
 	virtual ~FlightTaskTransition() = default;
 	bool activate(const vehicle_local_position_setpoint_s &last_setpoint) override;
 	bool updateInitialize() override;
 	bool update() override;
+
+private:
+
+	static constexpr float _vel_z_filter_time_const = 2.0f;
+
+	uORB::SubscriptionInterval _parameter_update_sub{ORB_ID(parameter_update), 1_s};
+	param_t _param_handle_pitch_cruise_degrees{PARAM_INVALID};
+	float _param_pitch_cruise_degrees{0.f};
+
+	AlphaFilter<float> _vel_z_filter;
+
+	void updateParameters();
+
 };