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.

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];
 		}

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;
 

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;
 };

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.param1 = _angle_outputs[1] * M_RAD_TO_DEG_F + _config.pitch_offset;
-	vehicle_command.param2 = _angle_outputs[0] * M_RAD_TO_DEG_F;
+	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;
+	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

@@ -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;
+	// _angle_outputs are in radians, actuator_controls are in [-1, 1]
-	actuator_controls.control[1] = _angle_outputs[1] / M_PI_F;
+	actuator_controls.control[0] = (_angle_outputs[0] + _config.roll_offset * M_DEG_TO_RAD_F) /
-	actuator_controls.control[2] = _angle_outputs[2] / M_PI_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

@@ -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

@@ -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);