Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-05-24 15:40:31 +08:00
Code Issues Actions 7 Packages Projects Releases Wiki Activity

vmount: add param to use RC input for angular rate

Browse Source 
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.
This commit is contained in:
Julian Oes
2021-10-05 10:08:43 +02:00
committed by Daniel Agar
parent 08b0ac9654
commit 9cb2bf389c
4 changed files with 77 additions and 19 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/vmount/input_rc.cpp
+38 -8
View File
@@ -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.
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];
}
src/modules/vmount/input_rc.h
+12 -7
View File
@@ -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] {};
};
src/modules/vmount/vmount.cpp
+16 -4
View File
@@ -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
+11
View File
@@ -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);