diff --git a/src/modules/gimbal/gimbal.cpp b/src/modules/gimbal/gimbal.cpp index c32e885736..6052044f3e 100644 --- a/src/modules/gimbal/gimbal.cpp +++ b/src/modules/gimbal/gimbal.cpp @@ -549,9 +549,6 @@ void update_params(ParameterHandles ¶m_handles, Parameters ¶ms) param_get(param_handles.mnt_min_pitch, ¶ms.mnt_min_pitch); param_get(param_handles.mnt_range_roll, ¶ms.mnt_range_roll); param_get(param_handles.mnt_range_yaw, ¶ms.mnt_range_yaw); - param_get(param_handles.mnt_off_pitch, ¶ms.mnt_off_pitch); - param_get(param_handles.mnt_off_roll, ¶ms.mnt_off_roll); - param_get(param_handles.mnt_off_yaw, ¶ms.mnt_off_yaw); param_get(param_handles.mav_sysid, ¶ms.mav_sysid); param_get(param_handles.mav_compid, ¶ms.mav_compid); param_get(param_handles.mnt_rate_pitch, ¶ms.mnt_rate_pitch); @@ -575,9 +572,6 @@ bool initialize_params(ParameterHandles ¶m_handles, Parameters ¶ms) param_handles.mnt_min_pitch = param_find("MNT_MIN_PITCH"); param_handles.mnt_range_roll = param_find("MNT_RANGE_ROLL"); param_handles.mnt_range_yaw = param_find("MNT_RANGE_YAW"); - param_handles.mnt_off_pitch = param_find("MNT_OFF_PITCH"); - param_handles.mnt_off_roll = param_find("MNT_OFF_ROLL"); - param_handles.mnt_off_yaw = param_find("MNT_OFF_YAW"); param_handles.mav_sysid = param_find("MAV_SYS_ID"); param_handles.mav_compid = param_find("MAV_COMP_ID"); param_handles.mnt_rate_pitch = param_find("MNT_RATE_PITCH"); @@ -598,9 +592,6 @@ bool initialize_params(ParameterHandles ¶m_handles, Parameters ¶ms) param_handles.mnt_min_pitch == PARAM_INVALID || param_handles.mnt_range_roll == PARAM_INVALID || param_handles.mnt_range_yaw == PARAM_INVALID || - param_handles.mnt_off_pitch == PARAM_INVALID || - param_handles.mnt_off_roll == PARAM_INVALID || - param_handles.mnt_off_yaw == PARAM_INVALID || param_handles.mav_sysid == PARAM_INVALID || param_handles.mav_compid == PARAM_INVALID || param_handles.mnt_rate_pitch == PARAM_INVALID || diff --git a/src/modules/gimbal/gimbal_params.c b/src/modules/gimbal/gimbal_params.c index 3f889c4273..f452c42b76 100644 --- a/src/modules/gimbal/gimbal_params.c +++ b/src/modules/gimbal/gimbal_params.c @@ -152,7 +152,9 @@ PARAM_DEFINE_INT32(MNT_MAN_YAW, 0); PARAM_DEFINE_INT32(MNT_DO_STAB, 0); /** -* Max angle of pitch channel output in degrees (only in AUX output mode). +* Max positive angle of pitch setpoint (only in MNT_MODE_OUT=AUX). +* +* Use output driver settings to calibrate (e.g. PWM_CENT/_MIN/_MAX). * * @unit deg * @decimal 1 @@ -161,7 +163,9 @@ PARAM_DEFINE_INT32(MNT_DO_STAB, 0); PARAM_DEFINE_FLOAT(MNT_MAX_PITCH, 45.0f); /** -* Min angle of pitch channel output in degrees (only in AUX output mode). +* Min negative angle of pitch setpoint (only in MNT_MODE_OUT=AUX). +* +* Use output driver settings to calibrate (e.g. PWM_CENT/_MIN/_MAX). * * @unit deg * @decimal 1 @@ -170,7 +174,9 @@ PARAM_DEFINE_FLOAT(MNT_MAX_PITCH, 45.0f); PARAM_DEFINE_FLOAT(MNT_MIN_PITCH, -45.0f); /** -* Range of roll channel output in degrees (only in AUX output mode). +* Range of roll channel output (only in MNT_MODE_OUT=AUX). +* +* Use output driver settings to calibrate (e.g. PWM_CENT/_MIN/_MAX). Note that only symmetric angular ranges are supported. * * @min 1.0 * @max 720.0 @@ -181,7 +187,9 @@ PARAM_DEFINE_FLOAT(MNT_MIN_PITCH, -45.0f); PARAM_DEFINE_FLOAT(MNT_RANGE_ROLL, 90.0f); /** -* Range of yaw channel output in degrees (only in AUX output mode). +* Range of yaw channel output (only in MNT_MODE_OUT=AUX). +* +* Use output driver settings to calibrate (e.g. PWM_CENT/_MIN/_MAX). Note that only symmetric angular ranges are supported. * * @min 1.0 * @max 720.0 @@ -191,38 +199,6 @@ PARAM_DEFINE_FLOAT(MNT_RANGE_ROLL, 90.0f); */ PARAM_DEFINE_FLOAT(MNT_RANGE_YAW, 360.0f); -/** -* Offset for pitch channel output in degrees. -* -* @min -360.0 -* @max 360.0 -* @unit deg -* @decimal 1 -* @group Mount -*/ -PARAM_DEFINE_FLOAT(MNT_OFF_PITCH, 0.0f); - -/** -* Offset for roll channel output in degrees. -* -* @min -360.0 -* @max 360.0 -* @unit deg -* @decimal 1 -* @group Mount -*/ -PARAM_DEFINE_FLOAT(MNT_OFF_ROLL, 0.0f); - -/** -* Offset for yaw channel output in degrees. -* -* @min -360.0 -* @max 360.0 -* @unit deg -* @decimal 1 -* @group Mount -*/ -PARAM_DEFINE_FLOAT(MNT_OFF_YAW, 0.0f); /** * Angular pitch rate for manual input in degrees/second. diff --git a/src/modules/gimbal/gimbal_params.h b/src/modules/gimbal/gimbal_params.h index bfe7c7f557..5649067b42 100644 --- a/src/modules/gimbal/gimbal_params.h +++ b/src/modules/gimbal/gimbal_params.h @@ -68,9 +68,6 @@ struct Parameters { float mnt_min_pitch; float mnt_range_roll; float mnt_range_yaw; - float mnt_off_pitch; - float mnt_off_roll; - float mnt_off_yaw; int32_t mav_sysid; int32_t mav_compid; float mnt_rate_pitch; @@ -93,9 +90,6 @@ struct ParameterHandles { param_t mnt_min_pitch; param_t mnt_range_roll; param_t mnt_range_yaw; - param_t mnt_off_pitch; - param_t mnt_off_roll; - param_t mnt_off_yaw; param_t mav_sysid; param_t mav_compid; param_t mnt_rate_pitch; diff --git a/src/modules/gimbal/output_mavlink.cpp b/src/modules/gimbal/output_mavlink.cpp index ae5222c674..c3ceaab678 100644 --- a/src/modules/gimbal/output_mavlink.cpp +++ b/src/modules/gimbal/output_mavlink.cpp @@ -100,9 +100,9 @@ void OutputMavlinkV1::update(const ControlData &control_data, bool new_setpoints // gimbal spec has roll, pitch on channels 0, 1, respectively; MAVLink spec has roll, pitch on channels 1, 0, respectively // gimbal uses radians, MAVLink uses degrees - vehicle_command.param1 = math::degrees(_angle_outputs[1] + math::radians(_parameters.mnt_off_pitch)); - vehicle_command.param2 = math::degrees(_angle_outputs[0] + math::radians(_parameters.mnt_off_roll)); - vehicle_command.param3 = math::degrees(_angle_outputs[2] + math::radians(_parameters.mnt_off_yaw)); + vehicle_command.param1 = math::degrees(_angle_outputs[1]); + vehicle_command.param2 = math::degrees(_angle_outputs[0]); + vehicle_command.param3 = math::degrees(_angle_outputs[2]); vehicle_command.param7 = 2.0f; // MAV_MOUNT_MODE_MAVLINK_TARGETING; _gimbal_v1_command_pub.publish(vehicle_command); diff --git a/src/modules/gimbal/output_rc.cpp b/src/modules/gimbal/output_rc.cpp index e89d8f1741..79f4dc14b4 100644 --- a/src/modules/gimbal/output_rc.cpp +++ b/src/modules/gimbal/output_rc.cpp @@ -82,11 +82,9 @@ float OutputRC::anglesMappedToOutput(const uint8_t index) float value = 0.f; float min_value = 0.f; float max_value = 0.f; - float offset = 0.f; switch (index) { case gimbal_controls_s::INDEX_ROLL: { - offset = math::radians(_parameters.mnt_off_roll); value = _angle_outputs[0]; max_value = math::radians(_parameters.mnt_range_roll) * 0.5f; min_value = -math::radians(_parameters.mnt_range_roll) * 0.5f; @@ -95,7 +93,6 @@ float OutputRC::anglesMappedToOutput(const uint8_t index) case gimbal_controls_s::INDEX_PITCH: { value = _angle_outputs[1]; - offset = math::radians(_parameters.mnt_off_pitch); max_value = math::radians(_parameters.mnt_max_pitch); min_value = math::radians(_parameters.mnt_min_pitch); break; @@ -103,7 +100,6 @@ float OutputRC::anglesMappedToOutput(const uint8_t index) case gimbal_controls_s::INDEX_YAW: { value = _angle_outputs[2]; - offset = math::radians(_parameters.mnt_off_yaw); max_value = math::radians(_parameters.mnt_range_yaw) * 0.5f; min_value = -math::radians(_parameters.mnt_range_yaw) * 0.5f; break; @@ -115,11 +111,11 @@ float OutputRC::anglesMappedToOutput(const uint8_t index) } } - if (value + offset >= FLT_EPSILON) { - return math::interpolate(value + offset, 0.f, max_value + offset, 0.f, 1.f); + if (value >= FLT_EPSILON) { + return math::interpolate(value, 0.f, max_value, 0.f, 1.f); } else { - return math::interpolate(value + offset, min_value + offset, 0.f, -1.f, 0.f); + return math::interpolate(value, min_value, 0.f, -1.f, 0.f); } }