diff --git a/src/drivers/vmount/input_rc.cpp b/src/drivers/vmount/input_rc.cpp index 4319e7803f..1ab04a85c2 100644 --- a/src/drivers/vmount/input_rc.cpp +++ b/src/drivers/vmount/input_rc.cpp @@ -48,7 +48,8 @@ namespace vmount { -InputRC::InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw) +InputRC::InputRC(bool do_stabilization, int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw) + : _do_stabilization(do_stabilization) { _aux_channels[0] = aux_channel_roll; _aux_channels[1] = aux_channel_pitch; @@ -132,7 +133,7 @@ bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bo for (int i = 0; i < 3; ++i) { control_data.type_data.angle.is_speed[i] = false; control_data.type_data.angle.angles[i] = new_aux_values[i] * M_PI_F; - control_data.stabilize_axis[i] = false; + control_data.stabilize_axis[i] = _do_stabilization; _last_set_aux_values[i] = new_aux_values[i]; } diff --git a/src/drivers/vmount/input_rc.h b/src/drivers/vmount/input_rc.h index 04a31a12b4..14c39c4053 100644 --- a/src/drivers/vmount/input_rc.h +++ b/src/drivers/vmount/input_rc.h @@ -57,11 +57,12 @@ class InputRC : public InputBase public: /** + * @param do_stabilization * @param aux_channel_roll which aux channel to use for roll (set to 0 to use a fixed angle of 0) * @param aux_channel_pitch * @param aux_channel_yaw */ - InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw); + InputRC(bool do_stabilization, int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw); virtual ~InputRC(); virtual void print_status(); @@ -80,6 +81,7 @@ protected: float _get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx); private: + const bool _do_stabilization; int _aux_channels[3]; int _manual_control_setpoint_sub = -1; diff --git a/src/drivers/vmount/output.h b/src/drivers/vmount/output.h index 29adf98c09..e7e06403fe 100644 --- a/src/drivers/vmount/output.h +++ b/src/drivers/vmount/output.h @@ -53,6 +53,13 @@ struct OutputConfig { float gimbal_retracted_mode_value; /**< mixer output value for selecting gimbal retracted mode */ float gimbal_normal_mode_value; /**< mixer output value for selecting gimbal normal mode */ + float pitch_range; + float roll_range; + float yaw_range; + float pitch_offset; + float roll_offset; + float yaw_offset; + uint32_t mavlink_sys_id; /**< mavlink target system id for mavlink output */ uint32_t mavlink_comp_id; }; diff --git a/src/drivers/vmount/output_mavlink.cpp b/src/drivers/vmount/output_mavlink.cpp index 8dcd45ef29..9f06f828a9 100644 --- a/src/drivers/vmount/output_mavlink.cpp +++ b/src/drivers/vmount/output_mavlink.cpp @@ -113,9 +113,9 @@ int OutputMavlink::update(const ControlData *control_data) // vmount spec has roll, pitch on channels 0, 1, respectively; MAVLink spec has roll, pitch on channels 1, 0, respectively // vmount uses radians, MAVLink uses degrees - vehicle_command.param1 = _angle_outputs[1] * M_RAD_TO_DEG_F; - vehicle_command.param2 = _angle_outputs[0] * M_RAD_TO_DEG_F; - vehicle_command.param3 = _angle_outputs[2] * M_RAD_TO_DEG_F; + vehicle_command.param1 = _angle_outputs[1] * M_RAD_TO_DEG_F + _config.pitch_offset; + vehicle_command.param2 = _angle_outputs[0] * M_RAD_TO_DEG_F + _config.roll_offset; + vehicle_command.param3 = _angle_outputs[2] * M_RAD_TO_DEG_F + _config.yaw_offset; orb_publish(ORB_ID(vehicle_command), _vehicle_command_pub, &vehicle_command); diff --git a/src/drivers/vmount/output_rc.cpp b/src/drivers/vmount/output_rc.cpp index 5c0010365c..591a6065df 100644 --- a/src/drivers/vmount/output_rc.cpp +++ b/src/drivers/vmount/output_rc.cpp @@ -73,9 +73,13 @@ int OutputRC::update(const ControlData *control_data) actuator_controls_s actuator_controls; actuator_controls.timestamp = hrt_absolute_time(); - actuator_controls.control[0] = _angle_outputs[0] / M_PI_F; - actuator_controls.control[1] = _angle_outputs[1] / M_PI_F; - actuator_controls.control[2] = _angle_outputs[2] / M_PI_F; + // _angle_outputs are in radians, actuator_controls are in [-1, 1] + actuator_controls.control[0] = (_angle_outputs[0] + _config.roll_offset * M_DEG_TO_RAD_F) / + (_config.roll_range * (M_DEG_TO_RAD_F / 2.0f)); + actuator_controls.control[1] = (_angle_outputs[1] + _config.pitch_offset * M_DEG_TO_RAD_F) / + (_config.pitch_range * (M_DEG_TO_RAD_F / 2.0f)); + actuator_controls.control[2] = (_angle_outputs[2] + _config.yaw_offset * M_DEG_TO_RAD_F) / + (_config.yaw_range * (M_DEG_TO_RAD_F / 2.0f)); actuator_controls.control[3] = _retract_gimbal ? _config.gimbal_retracted_mode_value : _config.gimbal_normal_mode_value; int instance; diff --git a/src/drivers/vmount/vmount.cpp b/src/drivers/vmount/vmount.cpp index ce5ca4e91a..0f5d7c0d40 100644 --- a/src/drivers/vmount/vmount.cpp +++ b/src/drivers/vmount/vmount.cpp @@ -36,6 +36,7 @@ * @author Leon Müller (thedevleon) * @author Beat Küng * @author Julian Oes + * @author Matthew Edwards (mje-nz) * * Driver for to control mounts such as gimbals or servos. * Inputs for the mounts can RC and/or mavlink commands. @@ -90,9 +91,18 @@ struct Parameters { int32_t mnt_man_pitch; int32_t mnt_man_roll; int32_t mnt_man_yaw; + int32_t mnt_do_stab; + float mnt_range_pitch; + float mnt_range_roll; + float mnt_range_yaw; + float mnt_off_pitch; + float mnt_off_roll; + float mnt_off_yaw; bool operator!=(const Parameters &p) { +#pragma GCC diagnostic push +#pragma GCC diagnostic ignored "-Wfloat-equal" return mnt_mode_in != p.mnt_mode_in || mnt_mode_out != p.mnt_mode_out || mnt_mav_sysid != p.mnt_mav_sysid || @@ -101,7 +111,16 @@ struct Parameters { mnt_ob_norm_mode != p.mnt_ob_norm_mode || mnt_man_pitch != p.mnt_man_pitch || mnt_man_roll != p.mnt_man_roll || - mnt_man_yaw != p.mnt_man_yaw; + mnt_man_yaw != p.mnt_man_yaw || + mnt_do_stab != p.mnt_do_stab || + mnt_range_pitch != p.mnt_range_pitch || + mnt_range_roll != p.mnt_range_roll || + mnt_range_yaw != p.mnt_range_yaw || + mnt_off_pitch != p.mnt_off_pitch || + mnt_off_roll != p.mnt_off_roll || + mnt_off_yaw != p.mnt_off_yaw; +#pragma GCC diagnostic pop + } }; @@ -115,6 +134,13 @@ struct ParameterHandles { param_t mnt_man_pitch; param_t mnt_man_roll; param_t mnt_man_yaw; + param_t mnt_do_stab; + param_t mnt_range_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; }; @@ -230,6 +256,12 @@ static int vmount_thread_main(int argc, char *argv[]) output_config.gimbal_normal_mode_value = params.mnt_ob_norm_mode; output_config.gimbal_retracted_mode_value = params.mnt_ob_lock_mode; + output_config.pitch_range = params.mnt_range_pitch; + output_config.roll_range = params.mnt_range_roll; + output_config.yaw_range = params.mnt_range_yaw; + output_config.pitch_offset = params.mnt_off_pitch; + output_config.roll_offset = params.mnt_off_roll; + output_config.yaw_offset = params.mnt_off_yaw; output_config.mavlink_sys_id = params.mnt_mav_sysid; output_config.mavlink_comp_id = params.mnt_mav_compid; @@ -250,13 +282,13 @@ static int vmount_thread_main(int argc, char *argv[]) // RC is on purpose last here so that if there are any mavlink // messages, they will take precedence over RC. // This logic is done further below while update() is called. - thread_data.input_objs[2] = new InputRC(params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw); + thread_data.input_objs[2] = new InputRC(params.mnt_do_stab, params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw); thread_data.input_objs_len = 3; break; case 1: //RC - thread_data.input_objs[0] = new InputRC(params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw); + thread_data.input_objs[0] = new InputRC(params.mnt_do_stab, params.mnt_man_roll, params.mnt_man_pitch, params.mnt_man_yaw); break; case 2: //MAVLINK_ROI @@ -525,6 +557,13 @@ void update_params(ParameterHandles ¶m_handles, Parameters ¶ms, bool &go param_get(param_handles.mnt_man_pitch, ¶ms.mnt_man_pitch); param_get(param_handles.mnt_man_roll, ¶ms.mnt_man_roll); param_get(param_handles.mnt_man_yaw, ¶ms.mnt_man_yaw); + param_get(param_handles.mnt_do_stab, ¶ms.mnt_do_stab); + param_get(param_handles.mnt_range_pitch, ¶ms.mnt_range_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); got_changes = prev_params != params; } @@ -540,6 +579,13 @@ bool get_params(ParameterHandles ¶m_handles, Parameters ¶ms) param_handles.mnt_man_pitch = param_find("MNT_MAN_PITCH"); param_handles.mnt_man_roll = param_find("MNT_MAN_ROLL"); param_handles.mnt_man_yaw = param_find("MNT_MAN_YAW"); + param_handles.mnt_do_stab = param_find("MNT_DO_STAB"); + param_handles.mnt_range_pitch = param_find("MNT_RANGE_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"); if (param_handles.mnt_mode_in == PARAM_INVALID || param_handles.mnt_mode_out == PARAM_INVALID || @@ -549,7 +595,14 @@ bool get_params(ParameterHandles ¶m_handles, Parameters ¶ms) param_handles.mnt_ob_norm_mode == PARAM_INVALID || param_handles.mnt_man_pitch == PARAM_INVALID || param_handles.mnt_man_roll == PARAM_INVALID || - param_handles.mnt_man_yaw == PARAM_INVALID) { + param_handles.mnt_man_yaw == PARAM_INVALID || + param_handles.mnt_do_stab == PARAM_INVALID || + param_handles.mnt_range_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) { return false; } diff --git a/src/drivers/vmount/vmount_params.c b/src/drivers/vmount/vmount_params.c index 906af500c1..6ed17f0dfa 100644 --- a/src/drivers/vmount/vmount_params.c +++ b/src/drivers/vmount/vmount_params.c @@ -34,6 +34,7 @@ /** * @file vmount_params.c * @author Leon Müller (thedevleon) +* @author Matthew Edwards (mje-nz) * */ @@ -156,3 +157,71 @@ PARAM_DEFINE_INT32(MNT_MAN_PITCH, 0); */ PARAM_DEFINE_INT32(MNT_MAN_YAW, 0); +/** +* Stabilize the mount (true for servo gimbal, false for passthrough). +* Only affects RC input; Mavlink input can set this with the DO_MOUNT_CONFIGURE command. +* +* @boolean +* @group Mount +*/ +PARAM_DEFINE_INT32(MNT_DO_STAB, 0); + +/** +* Range of pitch channel output in degrees (only in AUX output mode). +* +* @min 1.0 +* @max 720.0 +* @decimal 1 +* @group Mount +*/ +PARAM_DEFINE_FLOAT(MNT_RANGE_PITCH, 360.0f); + +/** +* Range of roll channel output in degrees (only in AUX output mode). +* +* @min 1.0 +* @max 720.0 +* @decimal 1 +* @group Mount +*/ +PARAM_DEFINE_FLOAT(MNT_RANGE_ROLL, 360.0f); + +/** +* Range of yaw channel output in degrees (only in AUX output mode). +* +* @min 1.0 +* @max 720.0 +* @decimal 1 +* @group Mount +*/ +PARAM_DEFINE_FLOAT(MNT_RANGE_YAW, 360.0f); + +/** +* Offset for pitch channel output in degrees. +* +* @min -360.0 +* @max 360.0 +* @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 +* @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 +* @decimal 1 +* @group Mount +*/ +PARAM_DEFINE_FLOAT(MNT_OFF_YAW, 0.0f); \ No newline at end of file