diff --git a/src/modules/gimbal/gimbal.cpp b/src/modules/gimbal/gimbal.cpp index 08dec3323b..49c2591234 100644 --- a/src/modules/gimbal/gimbal.cpp +++ b/src/modules/gimbal/gimbal.cpp @@ -462,6 +462,8 @@ void update_params(ParameterHandles ¶m_handles, Parameters ¶ms) param_get(param_handles.mnt_rate_pitch, ¶ms.mnt_rate_pitch); param_get(param_handles.mnt_rate_yaw, ¶ms.mnt_rate_yaw); param_get(param_handles.mnt_rc_in_mode, ¶ms.mnt_rc_in_mode); + param_get(param_handles.mnt_lnd_p_min, ¶ms.mnt_lnd_p_min); + param_get(param_handles.mnt_lnd_p_max, ¶ms.mnt_lnd_p_max); } bool initialize_params(ParameterHandles ¶m_handles, Parameters ¶ms) @@ -487,6 +489,8 @@ bool initialize_params(ParameterHandles ¶m_handles, Parameters ¶ms) 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 ¶m_handles, Parameters ¶ms) 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; } diff --git a/src/modules/gimbal/gimbal_params.c b/src/modules/gimbal/gimbal_params.c index 2bd4b44ea5..d2013d20e7 100644 --- a/src/modules/gimbal/gimbal_params.c +++ b/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); diff --git a/src/modules/gimbal/gimbal_params.h b/src/modules/gimbal/gimbal_params.h index 88d2fe209e..9781d8e81e 100644 --- a/src/modules/gimbal/gimbal_params.h +++ b/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 */ diff --git a/src/modules/gimbal/output.cpp b/src/modules/gimbal/output.cpp index a96afed287..d72ffd6a07 100644 --- a/src/modules/gimbal/output.cpp +++ b/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) diff --git a/src/modules/gimbal/output.h b/src/modules/gimbal/output.h index a2b301bca5..9b2e3693ea 100644 --- a/src/modules/gimbal/output.h +++ b/src/modules/gimbal/output.h @@ -43,6 +43,7 @@ #include #include #include +#include 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_pub{ORB_ID(mount_orientation)}; + + bool _landed{true}; };