Skip to content
Snippets Groups Projects
Commit 2f40bc3a authored by Matthew Edwards's avatar Matthew Edwards Committed by Beat Küng
Browse files

vmount: Add parameters for servo range and offset and whether to stabilize (#8120). 

Adds MNT_DO_STAB for whether to stabilize by default.
Adds MNT_RANGE_{PITCH,ROLL,YAW} for the output range of each output channel in AUX mode (instead of hardcoded 360 degrees).
Adds MNT_OFF_{PITCH,ROLL,YAW} for adjusting the zero point of each output channel.
parent 8ec59f0b
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
src/drivers/vmount/input_rc.cpp
+ 3
− 2
View file @ 2f40bc3a
......@@ -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];
}
......
src/drivers/vmount/input_rc.h
+ 3
− 1
View file @ 2f40bc3a
......@@ -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;
......
src/drivers/vmount/output.h
+ 7
− 0
View file @ 2f40bc3a
......@@ -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;
};
......
src/drivers/vmount/output_mavlink.cpp
+ 3
− 3
View file @ 2f40bc3a
......@@ -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);
......
src/drivers/vmount/output_rc.cpp
+ 7
− 3
View file @ 2f40bc3a
......@@ -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;
......
src/drivers/vmount/vmount.cpp
+ 57
− 4
View file @ 2f40bc3a
......@@ -36,6 +36,7 @@
* @author Leon Müller (thedevleon)
* @author Beat Küng <beat-kueng@gmx.net>
* @author Julian Oes <julian@oes.ch>
* @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 &param_handles, Parameters &params, bool &go
param_get(param_handles.mnt_man_pitch, &params.mnt_man_pitch);
param_get(param_handles.mnt_man_roll, &params.mnt_man_roll);
param_get(param_handles.mnt_man_yaw, &params.mnt_man_yaw);
param_get(param_handles.mnt_do_stab, &params.mnt_do_stab);
param_get(param_handles.mnt_range_pitch, &params.mnt_range_pitch);
param_get(param_handles.mnt_range_roll, &params.mnt_range_roll);
param_get(param_handles.mnt_range_yaw, &params.mnt_range_yaw);
param_get(param_handles.mnt_off_pitch, &params.mnt_off_pitch);
param_get(param_handles.mnt_off_roll, &params.mnt_off_roll);
param_get(param_handles.mnt_off_yaw, &params.mnt_off_yaw);
got_changes = prev_params != params;
}
......@@ -540,6 +579,13 @@ bool get_params(ParameterHandles &param_handles, Parameters &params)
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 &param_handles, Parameters &params)
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;
}
......
src/drivers/vmount/vmount_params.c
+ 69
− 0
View file @ 2f40bc3a
......@@ -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
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment