Gimbal: getting pitch input from gimbal_manager_set_manual_control

FlightTaskManualAccelerationSlow: avoid misalignment of gimbal and vehicle before takeoff

src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.cpp

@@ -132,16 +132,18 @@ bool FlightTaskManualAccelerationSlow::update()
 	bool ret = FlightTaskManualAcceleration::update();
 
 	// Optimize input-to-video latency gimbal control
-	if (_gimbal.checkForTelemetry(_time_stamp_current)) {
+	if (_gimbal.checkForTelemetry(_time_stamp_current) && haveTakenOff()) {
 		_gimbal.acquireGimbalControlIfNeeded();
 
-		const float pitchrate_gimbal = getInputFromSanitizedAuxParameterIndex(_param_mc_slow_map_pitch.get()) * yaw_rate;
-		_yawspeed_setpoint = shapeYawStickToGimbalRate(_sticks.getYaw(), yaw_rate);
+		// the exact same _yawspeed_setpoint is setpoint for the gimbal and vehicle feed-forward
+		const float pitchrate_gimbal = shapeUnitlessStickToGimbalRate(_gimbal.getPitch(_time_stamp_current), yaw_rate);
+		_yawspeed_setpoint = shapeUnitlessStickToGimbalRate(_sticks.getYaw(), yaw_rate);
 
 		_gimbal.publishGimbalManagerSetAttitude(Gimbal::FLAGS_ALL_AXES_LOCKED, Quatf(NAN, NAN, NAN, NAN),
 							Vector3f(NAN, pitchrate_gimbal, _yawspeed_setpoint));
 
 		if (_gimbal.allAxesLockedConfirmed()) {
+			// but the vehicle makes sure it stays alligned with the gimbal absolute yaw
 			_yaw_setpoint = _gimbal.getTelemetryYaw();
 		}
 
@@ -158,7 +160,15 @@ float FlightTaskManualAccelerationSlow::getInputFromSanitizedAuxParameterIndex(i
 	return _sticks.getAux()(sanitized_index);
 }
 
-float FlightTaskManualAccelerationSlow::shapeYawStickToGimbalRate(float stick_yaw, float maximum_yawrate)
+float FlightTaskManualAccelerationSlow::shapeUnitlessStickToGimbalRate(float stick_input, float maximum_rate)
 {
-	return stick_yaw * maximum_yawrate;
+	return stick_input * maximum_rate;
+}
+
+bool FlightTaskManualAccelerationSlow::haveTakenOff()
+{
+	takeoff_status_s takeoff_status{};
+	_takeoff_status_sub.copy(&takeoff_status);
+
+	return takeoff_status.takeoff_state == takeoff_status_s::TAKEOFF_STATE_FLIGHT;
 }

src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/FlightTaskManualAccelerationSlow.hpp

@@ -67,12 +67,12 @@ class FlightTaskManualAccelerationSlow : public FlightTaskManualAcceleration
 	float getInputFromSanitizedAuxParameterIndex(int parameter_value);
 
 	/**
-	 * Input shaping of yaw stick input for gimbal setpoint
-	 * @param stick_yaw raw calibrated yaw stick value [-1, 1]
-	 * @param maximum_yawrate yaw rate [rad/s] with maximum stick deflection
-	 * @return gimbal yaw rate setpoint [rad/s] positive clockwise
+	 * Input shaping of unitless stick input for gimbal setpoint
+	 * @param stick_input raw calibrated stick value [-1, 1]
+	 * @param maximum_rate rate [rad/s] with maximum stick deflection
+	 * @return gimbal rate setpoint [rad/s] positive clockwise
 	 */
-	float shapeYawStickToGimbalRate(float stick_yaw, float maximum_yawrate);
+	float shapeUnitlessStickToGimbalRate(float stick_input, float maximum_rate);
 
 	bool _velocity_limits_received_before{false};
 
@@ -82,6 +82,9 @@ class FlightTaskManualAccelerationSlow : public FlightTaskManualAcceleration
 	uORB::Subscription _velocity_limits_sub{ORB_ID(velocity_limits)};
 	velocity_limits_s _velocity_limits{};
 
+	uORB::Subscription _takeoff_status_sub{ORB_ID(takeoff_status)};
+	bool haveTakenOff();
+
 	DEFINE_PARAMETERS_CUSTOM_PARENT(FlightTaskManualAcceleration,
 					(ParamInt<px4::params::MC_SLOW_MAP_HVEL>) _param_mc_slow_map_hvel,
 					(ParamInt<px4::params::MC_SLOW_MAP_VVEL>) _param_mc_slow_map_vvel,
@@ -95,7 +98,6 @@ class FlightTaskManualAccelerationSlow : public FlightTaskManualAcceleration
 					(ParamFloat<px4::params::MPC_VEL_MANUAL>) _param_mpc_vel_manual,
 					(ParamFloat<px4::params::MPC_Z_VEL_MAX_UP>) _param_mpc_z_vel_max_up,
 					(ParamFloat<px4::params::MPC_Z_VEL_MAX_DN>) _param_mpc_z_vel_max_dn,
-					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max,
-					(ParamInt<px4::params::MC_SLOW_MAP_PTCH>) _param_mc_slow_map_pitch
+					(ParamFloat<px4::params::MPC_MAN_Y_MAX>) _param_mpc_man_y_max
 				       )
 };

src/modules/flight_mode_manager/tasks/ManualAccelerationSlow/flight_task_acceleration_slow_params.c

@@ -156,17 +156,3 @@ PARAM_DEFINE_FLOAT(MC_SLOW_DEF_VVEL, 1.f);
  * @group Multicopter Position Slow Mode
  */
 PARAM_DEFINE_FLOAT(MC_SLOW_DEF_YAWR, 45.f);
-
-/**
- * RC_MAP_AUX{N} to allow for gimbal pitch rate control in position slow mode
- *
- * @value 0 No pitch rate input
- * @value 1 AUX1
- * @value 2 AUX2
- * @value 3 AUX3
- * @value 4 AUX4
- * @value 5 AUX5
- * @value 6 AUX6
- * @group Multicopter Position Slow Mode
- */
-PARAM_DEFINE_INT32(MC_SLOW_MAP_PTCH, 0);

src/modules/flight_mode_manager/tasks/Utility/Gimbal.cpp

@@ -124,3 +124,23 @@ void Gimbal::publishGimbalManagerSetAttitude(const uint16_t gimbal_flags,
 	gimbal_setpoint.timestamp = hrt_absolute_time();
 	_gimbal_manager_set_attitude_pub.publish(gimbal_setpoint);
 }
+
+float Gimbal::getPitch(const hrt_abstime now)
+{
+	if (_gimbal_manager_set_manual_control_sub.updated()) {
+		gimbal_manager_set_manual_control_s gimbal_manager_set_manual_control{};
+
+		if (_gimbal_manager_set_manual_control_sub.copy(&gimbal_manager_set_manual_control)
+		    && gimbal_manager_set_manual_control.origin_compid != _param_mav_comp_id.get()
+		    && gimbal_manager_set_manual_control.origin_sysid != _param_mav_sys_id.get()) {
+			_set_manual_control_timestamp = gimbal_manager_set_manual_control.timestamp;
+			_set_manual_control_pitchrate = gimbal_manager_set_manual_control.pitch_rate;
+		}
+	}
+
+	if (now > _set_manual_control_timestamp + 2_s) {
+		_set_manual_control_pitchrate = 0.f;
+	}
+
+	return _set_manual_control_pitchrate;
+}

src/modules/flight_mode_manager/tasks/Utility/Gimbal.hpp

@@ -45,10 +45,12 @@
 #include <uORB/Subscription.hpp>
 #include <uORB/topics/gimbal_manager_set_attitude.h>
 #include <uORB/topics/gimbal_device_attitude_status.h>
+#include <uORB/topics/gimbal_manager_set_manual_control.h>
 #include <uORB/topics/vehicle_command.h>
 
 #include "Sticks.hpp"
 
+
 class Gimbal : public ModuleParams
 {
 public:
@@ -64,6 +66,15 @@ class Gimbal : public ModuleParams
 	float getTelemetryYaw() { return _telemetry_yaw; }
 	bool allAxesLockedConfirmed() { return _telemetry_flags == FLAGS_ALL_AXES_LOCKED; }
 
+	/**
+	 * Workaround to mix commands from the gcs which should be
+	 * ignored in gimbal manager
+	 * @param now: current time stamp in loop [ms]
+	 * @return raw calibrated stick value [-1, 1] if
+	 * last sample was within expected time, otherwise NAN.
+	 */
+	float getPitch(const hrt_abstime now);
+
 	static constexpr uint16_t FLAGS_ALL_AXES_LOCKED = gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_ROLL_LOCK
 			| gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_PITCH_LOCK
 			| gimbal_manager_set_attitude_s::GIMBAL_MANAGER_FLAGS_YAW_LOCK;
@@ -75,11 +86,14 @@ class Gimbal : public ModuleParams
 	bool _have_gimbal_control{false};
 
 	uORB::Subscription _gimbal_device_attitude_status_sub{ORB_ID(gimbal_device_attitude_status)};
-	uint16_t _telemetry_flags{};
 	hrt_abstime _telemtry_timestamp{};
+	uint16_t _telemetry_flags{};
 	float _telemetry_yaw{};
 
-	// Publications
+	uORB::Subscription _gimbal_manager_set_manual_control_sub{ORB_ID(gimbal_manager_set_manual_control)};
+	hrt_abstime _set_manual_control_timestamp{};
+	float _set_manual_control_pitchrate{};
+
 	uORB::Publication<gimbal_manager_set_attitude_s> _gimbal_manager_set_attitude_pub{ORB_ID(gimbal_manager_set_attitude)};
 	uORB::Publication<vehicle_command_s> _vehicle_command_pub{ORB_ID(vehicle_command)};