vmount: add param to use RC input for angular rate

Until now RC input was translated to angles only. I added the param
MNT_RC_IN_MODE which allows the RC input to be used for angular rate.

src/modules/vmount/input_rc.cpp

@@ -41,6 +41,7 @@
 
 #include <math.h>
 #include <errno.h>
+#include <mathlib/mathlib.h>
 #include <matrix/matrix/math.hpp>
 #include <px4_platform_common/posix.h>
 #include <px4_platform_common/defines.h>
@@ -50,11 +51,15 @@ namespace vmount
 {
 
 
-InputRC::InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw)
+InputRC::InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw, float mnt_rate_pitch,
+		 float mnt_rate_yaw, int rc_in_mode)
 {
 	_aux_channels[0] = aux_channel_roll;
 	_aux_channels[1] = aux_channel_pitch;
 	_aux_channels[2] = aux_channel_yaw;
+	_mnt_rate_pitch = mnt_rate_pitch;
+	_mnt_rate_yaw = mnt_rate_yaw;
+	_rc_in_mode = rc_in_mode;
 }
 
 InputRC::~InputRC()
@@ -106,7 +111,7 @@ int InputRC::update_impl(unsigned int timeout_ms, ControlData **control_data, bo
 
 bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bool already_active)
 {
-	manual_control_setpoint_s manual_control_setpoint;
+	manual_control_setpoint_s manual_control_setpoint{};
 	orb_copy(ORB_ID(manual_control_setpoint), _manual_control_setpoint_sub, &manual_control_setpoint);
 	control_data.type = ControlData::Type::Angle;
 
@@ -131,15 +136,40 @@ bool InputRC::_read_control_data_from_subscription(ControlData &control_data, bo
 
 		_first_time = false;
 
-		// We scale manual input from roll -180..180, pitch -90..90, yaw, -180..180 degrees.
+		if (_rc_in_mode == 0) {
-		matrix::Eulerf euler(new_aux_values[0] * M_PI_F, new_aux_values[1] * M_PI_F / 2.0f,
+			// We scale manual input from roll -180..180, pitch -90..90, yaw, -180..180 degrees.
-				     new_aux_values[2] * M_PI_F);
+			matrix::Eulerf euler(new_aux_values[0] * math::radians(180.f),
-		matrix::Quatf q(euler);
+					     new_aux_values[1] * math::radians(90.f),
-		q.copyTo(control_data.type_data.angle.q);
+					     new_aux_values[2] * math::radians(180.f));
+
+			matrix::Quatf q(euler);
+			q.copyTo(control_data.type_data.angle.q);
+
+			control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
+			control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngleAbsoluteFrame;
+			control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
+
+			control_data.type_data.angle.angular_velocity[0] = NAN;
+			control_data.type_data.angle.angular_velocity[1] = NAN;
+			control_data.type_data.angle.angular_velocity[2] = NAN;
+
+		} else {
+			control_data.type_data.angle.q[0] = NAN;
+			control_data.type_data.angle.q[1] = NAN;
+			control_data.type_data.angle.q[2] = NAN;
+			control_data.type_data.angle.q[3] = NAN;
+
+			control_data.type_data.angle.angular_velocity[0] = 0.f;
+			control_data.type_data.angle.angular_velocity[1] = math::radians(_mnt_rate_pitch) * new_aux_values[1];
+			control_data.type_data.angle.angular_velocity[2] = math::radians(_mnt_rate_yaw) * new_aux_values[2];
+
+			control_data.type_data.angle.frames[0] = ControlData::TypeData::TypeAngle::Frame::AngularRate;
+			control_data.type_data.angle.frames[1] = ControlData::TypeData::TypeAngle::Frame::AngularRate;
+			control_data.type_data.angle.frames[2] = ControlData::TypeData::TypeAngle::Frame::AngularRate;
+		}
 
 		for (int i = 0; i < 3; ++i) {
 			// We always use follow mode with RC input for now.
-			control_data.type_data.angle.frames[i] = ControlData::TypeData::TypeAngle::Frame::AngleBodyFrame;
 			_last_set_aux_values[i] = new_aux_values[i];
 		}
 

src/modules/vmount/input_rc.h

@@ -61,7 +61,8 @@ public:
 	 * @param aux_channel_pitch
 	 * @param aux_channel_yaw
 	 */
-	InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw);
+	InputRC(int aux_channel_roll, int aux_channel_pitch, int aux_channel_yaw, float mnt_rate_pitch, float mnt_rate_yaw,
+		int rc_in_mode);
 	virtual ~InputRC();
 
 	virtual void print_status();
@@ -75,16 +76,20 @@ protected:
 	 */
 	virtual bool _read_control_data_from_subscription(ControlData &control_data, bool already_active);
 
-	int _get_subscription_fd() const { return _manual_control_setpoint_sub; }
-
 	float _get_aux_value(const manual_control_setpoint_s &manual_control_setpoint, int channel_idx);
 
 private:
-	int _aux_channels[3];
+	int _aux_channels[3] {};
-	int _manual_control_setpoint_sub = -1;
 
-	bool _first_time = true;
+	float _mnt_rate_pitch{0.f};
-	float _last_set_aux_values[3] = {};
+	float _mnt_rate_yaw{0.f};
+
+	int _rc_in_mode{0};
+
+	int _manual_control_setpoint_sub{-1};
+
+	bool _first_time{true};
+	float _last_set_aux_values[3] {};
 };
 
 

src/modules/vmount/vmount.cpp

@@ -106,6 +106,7 @@ struct Parameters {
 	int32_t mav_comp_id;
 	float mnt_rate_pitch;
 	float mnt_rate_yaw;
+	int32_t mnt_rc_in_mode;
 
 	bool operator!=(const Parameters &p)
 	{
@@ -128,7 +129,8 @@ struct Parameters {
 		       mnt_off_roll != p.mnt_off_roll ||
 		       mnt_off_yaw != p.mnt_off_yaw ||
 		       mav_sys_id != p.mav_sys_id ||
-		       mav_comp_id != p.mav_comp_id;
+		       mav_comp_id != p.mav_comp_id ||
+		       mnt_rc_in_mode != p.mnt_rc_in_mode;
 #pragma GCC diagnostic pop
 
 	}
@@ -155,6 +157,7 @@ struct ParameterHandles {
 	param_t mav_comp_id;
 	param_t mnt_rate_pitch;
 	param_t mnt_rate_yaw;
+	param_t mnt_rc_in_mode;
 };
 
 
@@ -261,7 +264,10 @@ static int vmount_thread_main(int argc, char *argv[])
 					// 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);
+										params.mnt_man_yaw,
+										params.mnt_rate_pitch,
+										params.mnt_rate_yaw,
+										params.mnt_rc_in_mode);
 					thread_data.input_objs_len = 3;
 
 					break;
@@ -269,7 +275,10 @@ static int vmount_thread_main(int argc, char *argv[])
 				case 1: //RC
 					thread_data.input_objs[0] = new InputRC(params.mnt_man_roll,
 										params.mnt_man_pitch,
-										params.mnt_man_yaw);
+										params.mnt_man_yaw,
+										params.mnt_rate_pitch,
+										params.mnt_rate_yaw,
+										params.mnt_rc_in_mode);
 					break;
 
 				case 2: //MAVLINK_ROI
@@ -583,6 +592,7 @@ void update_params(ParameterHandles &param_handles, Parameters &params, bool &go
 	param_get(param_handles.mav_comp_id, &params.mav_comp_id);
 	param_get(param_handles.mnt_rate_pitch, &params.mnt_rate_pitch);
 	param_get(param_handles.mnt_rate_yaw, &params.mnt_rate_yaw);
+	param_get(param_handles.mnt_rc_in_mode, &params.mnt_rc_in_mode);
 
 	got_changes = prev_params != params;
 }
@@ -609,6 +619,7 @@ bool get_params(ParameterHandles &param_handles, Parameters &params)
 	param_handles.mav_comp_id = param_find("MAV_COMP_ID");
 	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");
 
 	if (param_handles.mnt_mode_in == PARAM_INVALID ||
 	    param_handles.mnt_mode_out == PARAM_INVALID ||
@@ -629,7 +640,8 @@ bool get_params(ParameterHandles &param_handles, Parameters &params)
 	    param_handles.mav_sys_id == PARAM_INVALID ||
 	    param_handles.mav_comp_id == PARAM_INVALID ||
 	    param_handles.mnt_rate_pitch == PARAM_INVALID ||
-	    param_handles.mnt_rate_yaw == PARAM_INVALID
+	    param_handles.mnt_rate_yaw == PARAM_INVALID ||
+	    param_handles.mnt_rc_in_mode == PARAM_INVALID
 	   ) {
 		return false;
 	}

src/modules/vmount/vmount_params.c

@@ -261,3 +261,14 @@ PARAM_DEFINE_FLOAT(MNT_RATE_PITCH, 30.0f);
  * @group Mount
  */
 PARAM_DEFINE_FLOAT(MNT_RATE_YAW, 30.0f);
+
+/**
+ * Input mode for RC gimbal input
+ *
+ * @value 0 Angle
+ * @value 1 Angular rate
+ * @min 0
+ * @max 1
+ * @group Mount
+ */
+PARAM_DEFINE_INT32(MNT_RC_IN_MODE, 1);