gimbal: new pitch limits [MNT_LND_P_MIN, MNT_LND_P_MAX] when landed

src/modules/gimbal/gimbal.cpp

@@ -462,6 +462,8 @@ void update_params(ParameterHandles &param_handles, Parameters &params)
 	param_get(param_handles.mnt_rate_pitch, &params.mnt_rate_pitch);
 	param_get(param_handles.mnt_rate_yaw, &params.mnt_rate_yaw);
 	param_get(param_handles.mnt_rc_in_mode, &params.mnt_rc_in_mode);
+	param_get(param_handles.mnt_lnd_p_min, &params.mnt_lnd_p_min);
+	param_get(param_handles.mnt_lnd_p_max, &params.mnt_lnd_p_max);
 }
 
 bool initialize_params(ParameterHandles &param_handles, Parameters &params)
@@ -487,6 +489,8 @@ bool initialize_params(ParameterHandles &param_handles, Parameters &params)
 	param_handles.mnt_rate_pitch = param_find("MNT_RATE_PITCH");
 	param_handles.mnt_rate_yaw = param_find("MNT_RATE_YAW");
 	param_handles.mnt_rc_in_mode = param_find("MNT_RC_IN_MODE");
+	param_handles.mnt_lnd_p_min = param_find("MNT_LND_P_MIN");
+	param_handles.mnt_lnd_p_max = param_find("MNT_LND_P_MAX");
 
 	if (param_handles.mnt_mode_in == PARAM_INVALID ||
 	    param_handles.mnt_mode_out == PARAM_INVALID ||
@@ -508,7 +512,9 @@ bool initialize_params(ParameterHandles &param_handles, Parameters &params)
 	    param_handles.mav_compid == PARAM_INVALID ||
 	    param_handles.mnt_rate_pitch == PARAM_INVALID ||
 	    param_handles.mnt_rate_yaw == PARAM_INVALID ||
-	    param_handles.mnt_rc_in_mode == PARAM_INVALID
+	    param_handles.mnt_rc_in_mode == PARAM_INVALID ||
+	    param_handles.mnt_lnd_p_min == PARAM_INVALID ||
+	    param_handles.mnt_lnd_p_max == PARAM_INVALID
 	   ) {
 		return false;
 	}

src/modules/gimbal/gimbal_params.c

@@ -276,3 +276,25 @@ PARAM_DEFINE_FLOAT(MNT_RATE_YAW, 30.0f);
  * @group Mount
  */
 PARAM_DEFINE_INT32(MNT_RC_IN_MODE, 1);
+
+/**
+* Pitch minimum when landed
+*
+* @min -90.0
+* @max 90.0
+* @unit deg
+* @decimal 1
+* @group Mount
+*/
+PARAM_DEFINE_FLOAT(MNT_LND_P_MIN, -90.0f);
+
+/**
+* Pitch maximum when landed
+*
+* @min -90.0
+* @max 90.0
+* @unit deg
+* @decimal 1
+* @group Mount
+*/
+PARAM_DEFINE_FLOAT(MNT_LND_P_MAX, 90.0f);

src/modules/gimbal/gimbal_params.h

@@ -62,6 +62,8 @@ struct Parameters {
 	float mnt_rate_pitch;
 	float mnt_rate_yaw;
 	int32_t mnt_rc_in_mode;
+	float mnt_lnd_p_min;
+	float mnt_lnd_p_max;
 };
 
 struct ParameterHandles {
@@ -86,6 +88,8 @@ struct ParameterHandles {
 	param_t mnt_rate_pitch;
 	param_t mnt_rate_yaw;
 	param_t mnt_rc_in_mode;
+	param_t mnt_lnd_p_min;
+	param_t mnt_lnd_p_max;
 };
 
 } /* namespace gimbal */

src/modules/gimbal/output.cpp

@@ -182,6 +182,14 @@ void OutputBase::_handle_position_update(const ControlData &control_data, bool f
 
 void OutputBase::_calculate_angle_output(const hrt_abstime &t)
 {
+	if (_vehicle_land_detected_sub.updated()) {
+		vehicle_land_detected_s vehicle_land_detected;
+
+		if (_vehicle_land_detected_sub.copy(&vehicle_land_detected)) {
+			_landed = vehicle_land_detected.landed || vehicle_land_detected.maybe_landed;
+		}
+	}
+
 	// We only need to apply additional compensation if the required angle is
 	// absolute (world frame) as well as the gimbal is not capable of doing that
 	// calculation. (Most gimbals stabilize at least roll and pitch
@@ -196,7 +204,7 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t)
 	matrix::Eulerf euler_vehicle{};
 
 	if (compensate[0] || compensate[1] || compensate[2]) {
-		vehicle_attitude_s vehicle_attitude{};
+		vehicle_attitude_s vehicle_attitude;
 
 		if (_vehicle_attitude_sub.copy(&vehicle_attitude)) {
 			euler_vehicle = matrix::Quatf(vehicle_attitude.q);
@@ -232,6 +240,16 @@ void OutputBase::_calculate_angle_output(const hrt_abstime &t)
 			_angle_outputs[i] = matrix::wrap_pi(_angle_outputs[i]);
 		}
 	}
+
+
+	// constrain pitch to [MNT_LND_P_MIN, MNT_LND_P_MAX] if landed
+	if (_landed) {
+		if (PX4_ISFINITE(_angle_outputs[1])) {
+			_angle_outputs[1] = math::constrain(_angle_outputs[1],
+							    math::radians(_parameters.mnt_lnd_p_min),
+							    math::radians(_parameters.mnt_lnd_p_max));
+		}
+	}
 }
 
 void OutputBase::set_stabilize(bool roll_stabilize, bool pitch_stabilize, bool yaw_stabilize)

src/modules/gimbal/output.h

@@ -43,6 +43,7 @@
 #include <uORB/topics/mount_orientation.h>
 #include <uORB/topics/vehicle_attitude.h>
 #include <uORB/topics/vehicle_global_position.h>
+#include <uORB/topics/vehicle_land_detected.h>
 
 namespace gimbal
 {
@@ -93,8 +94,11 @@ protected:
 private:
 	uORB::Subscription _vehicle_attitude_sub{ORB_ID(vehicle_attitude)};
 	uORB::Subscription _vehicle_global_position_sub{ORB_ID(vehicle_global_position)};
+	uORB::Subscription _vehicle_land_detected_sub{ORB_ID(vehicle_land_detected)};
 
 	uORB::Publication<mount_orientation_s> _mount_orientation_pub{ORB_ID(mount_orientation)};
+
+	bool _landed{true};
 };