diff --git a/src/modules/vmount/input_rc.cpp b/src/modules/vmount/input_rc.cpp index 12f9b1670f..0731bb1150 100644 --- a/src/modules/vmount/input_rc.cpp +++ b/src/modules/vmount/input_rc.cpp @@ -41,6 +41,7 @@ #include #include +#include #include #include #include @@ -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. - matrix::Eulerf euler(new_aux_values[0] * M_PI_F, new_aux_values[1] * M_PI_F / 2.0f, - new_aux_values[2] * M_PI_F); - matrix::Quatf q(euler); - q.copyTo(control_data.type_data.angle.q); + if (_rc_in_mode == 0) { + // We scale manual input from roll -180..180, pitch -90..90, yaw, -180..180 degrees. + matrix::Eulerf euler(new_aux_values[0] * math::radians(180.f), + new_aux_values[1] * math::radians(90.f), + 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]; } diff --git a/src/modules/vmount/input_rc.h b/src/modules/vmount/input_rc.h index 89cf3a6034..134a1406b6 100644 --- a/src/modules/vmount/input_rc.h +++ b/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 _manual_control_setpoint_sub = -1; + int _aux_channels[3] {}; - bool _first_time = true; - float _last_set_aux_values[3] = {}; + float _mnt_rate_pitch{0.f}; + 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] {}; }; diff --git a/src/modules/vmount/vmount.cpp b/src/modules/vmount/vmount.cpp index c6c5fcc8e8..d2882467d6 100644 --- a/src/modules/vmount/vmount.cpp +++ b/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 ¶m_handles, Parameters ¶ms, bool &go param_get(param_handles.mav_comp_id, ¶ms.mav_comp_id); param_get(param_handles.mnt_rate_pitch, ¶ms.mnt_rate_pitch); param_get(param_handles.mnt_rate_yaw, ¶ms.mnt_rate_yaw); + param_get(param_handles.mnt_rc_in_mode, ¶ms.mnt_rc_in_mode); got_changes = prev_params != params; } @@ -609,6 +619,7 @@ bool get_params(ParameterHandles ¶m_handles, Parameters ¶ms) 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 ¶m_handles, Parameters ¶ms) 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; } diff --git a/src/modules/vmount/vmount_params.c b/src/modules/vmount/vmount_params.c index 4687af117a..09e41344e3 100644 --- a/src/modules/vmount/vmount_params.c +++ b/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);