fly316/PX4-Autopilot
7
0
Fork 0
mirror of https://gitee.com/mirrors_PX4/PX4-Autopilot.git synced 2026-04-14 10:07:39 +08:00
Code Issues Packages Projects Releases Wiki Activity

gimbal: Add test command for angle rates

Browse Source 
This is handy to test angular rate input, not just angles.
This commit is contained in:
Julian Oes 2024-08-01 13:20:14 +12:00 committed by Beat Küng
parent 22950a94ec
commit 55ec6df751
3 changed files with 116 additions and 36 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

79
src/modules/gimbal/gimbal.cpp
View File

@ -1,6 +1,6 @@
/****************************************************************************
*
* Copyright (c) 2013-2023 PX4 Development Team. All rights reserved.
* Copyright (c) 2013-2024 PX4 Development Team. All rights reserved.
*
* Redistribution and use in source and binary forms, with or without
* modification, are permitted provided that the following conditions
@ -369,27 +369,77 @@ int gimbal_main(int argc, char *argv[])
if (thread_running.load() && g_thread_data && g_thread_data->test_input) {
if (argc >= 4) {
bool found_axis = false;
const char *axis_names[3] = {"roll", "pitch", "yaw"};
int angles[3] = { 0, 0, 0 };
float roll_deg = 0.0f;
float pitch_deg = 0.0f;
float yaw_deg = 0.0f;
float rollrate_deg_s = 0.0f;
float pitchrate_deg_s = 0.0f;
float yawrate_deg_s = 0.0f;
bool angles_set = false;
bool rates_set = false;
for (int arg_i = 2 ; arg_i < (argc - 1); ++arg_i) {
for (int axis_i = 0; axis_i < 3; ++axis_i) {
if (!strcmp(argv[arg_i], axis_names[axis_i])) {
int angle_deg = (int)strtol(argv[arg_i + 1], nullptr, 0);
angles[axis_i] = angle_deg;
found_axis = true;
}
if (!strcmp(argv[arg_i], "roll")) {
roll_deg = (int)strtof(argv[arg_i + 1], nullptr);
angles_set = true;
} else if (!strcmp(argv[arg_i], "pitch")) {
pitch_deg = (int)strtof(argv[arg_i + 1], nullptr);
angles_set = true;
} else if (!strcmp(argv[arg_i], "yaw")) {
yaw_deg = (int)strtof(argv[arg_i + 1], nullptr);
angles_set = true;
} else if (!strcmp(argv[arg_i], "rollrate")) {
rollrate_deg_s = (int)strtof(argv[arg_i + 1], nullptr);
rates_set = true;
} else if (!strcmp(argv[arg_i], "pitchrate")) {
pitchrate_deg_s = (int)strtof(argv[arg_i + 1], nullptr);
rates_set = true;
} else if (!strcmp(argv[arg_i], "yawrate")) {
yawrate_deg_s = (int)strtof(argv[arg_i + 1], nullptr);
rates_set = true;
} else {
PX4_ERR("Unknown argument: %s", argv[arg_i]);
usage();
return -1;
}
}
if (!found_axis) {
if (angles_set && rates_set) {
PX4_ERR("This driver doesn't support both, angles and rates, to be set");
usage();
return -1;
} else if (angles_set) {
g_thread_data->test_input->set_test_input_angles(
roll_deg,
pitch_deg,
yaw_deg
);
return 0;
} else if (rates_set) {
g_thread_data->test_input->set_test_input_angle_rates(
rollrate_deg_s,
pitchrate_deg_s,
yawrate_deg_s
);
return 0;
} else {
PX4_ERR("No angles or angle rates set");
usage();
return -1;
}
g_thread_data->test_input->set_test_input(angles[0], angles[1], angles[2]);
return 0;
}
} else {
@ -573,5 +623,6 @@ $ gimbal test pitch -45 yaw 30
PRINT_MODULE_USAGE_ARG("<sysid> <compid>", "MAVLink system ID and MAVLink component ID", false);
PRINT_MODULE_USAGE_COMMAND_DESCR("test", "Test the output: set a fixed angle for one or multiple axes (gimbal must be running)");
PRINT_MODULE_USAGE_ARG("roll|pitch|yaw <angle>", "Specify an axis and an angle in degrees", false);
PRINT_MODULE_USAGE_ARG("rollrate|pitchrate|yawrate <angle rate>", "Specify an axis and an angle rate in degrees / second", false);
PRINT_MODULE_USAGE_DEFAULT_COMMANDS();
}

60
src/modules/gimbal/input_test.cpp
View File

@ -55,21 +55,30 @@ InputTest::UpdateResult InputTest::update(unsigned int timeout_ms, ControlData &
control_data.type = ControlData::Type::Angle;
control_data.timestamp_last_update = hrt_absolute_time();
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;
if (PX4_ISFINITE(_roll_deg) && PX4_ISFINITE(_pitch_deg) && PX4_ISFINITE(_yaw_deg)) {
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;
matrix::Eulerf euler(
math::radians((float)_roll_deg),
math::radians((float)_pitch_deg),
math::radians((float)_yaw_deg));
matrix::Quatf q(euler);
q.copyTo(control_data.type_data.angle.q);
matrix::Eulerf euler(
math::radians((float)_roll_deg),
math::radians((float)_pitch_deg),
math::radians((float)_yaw_deg));
matrix::Quatf q(euler);
} else {
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;
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] = math::radians(_rollrate_deg_s);
control_data.type_data.angle.angular_velocity[1] = math::radians(_pitchrate_deg_s);
control_data.type_data.angle.angular_velocity[2] = math::radians(_yawrate_deg_s);
}
q.copyTo(control_data.type_data.angle.q);
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;
// For testing we mark ourselves as in control.
control_data.sysid_primary_control = _parameters.mav_sysid;
@ -87,17 +96,32 @@ int InputTest::initialize()
void InputTest::print_status() const
{
PX4_INFO("Input: Test");
PX4_INFO_RAW(" roll : % 3d deg\n", _roll_deg);
PX4_INFO_RAW(" pitch: % 3d deg\n", _pitch_deg);
PX4_INFO_RAW(" yaw : % 3d deg\n", _yaw_deg);
PX4_INFO_RAW(" roll : % .1f deg\n", (double)_roll_deg);
PX4_INFO_RAW(" pitch: % .1f deg\n", (double)_pitch_deg);
PX4_INFO_RAW(" yaw : % .1f deg\n", (double)_yaw_deg);
PX4_INFO_RAW(" rollrate : % .1f deg/s\n", (double)_rollrate_deg_s);
PX4_INFO_RAW(" pitchrate: % .1f deg/s\n", (double)_pitchrate_deg_s);
PX4_INFO_RAW(" yawrate : % .1f deg/s\n", (double)_yawrate_deg_s);
}
void InputTest::set_test_input(int roll_deg, int pitch_deg, int yaw_deg)
void InputTest::set_test_input_angles(float roll_deg, float pitch_deg, float yaw_deg)
{
_roll_deg = roll_deg;
_pitch_deg = pitch_deg;
_yaw_deg = yaw_deg;
_rollrate_deg_s = NAN;
_pitchrate_deg_s = NAN;
_yawrate_deg_s = NAN;
_has_been_set.store(true);
}
void InputTest::set_test_input_angle_rates(float rollrate_deg_s, float pitchrate_deg_s, float yawrate_deg_s)
{
_roll_deg = NAN;
_pitch_deg = NAN;
_yaw_deg = NAN;
_rollrate_deg_s = rollrate_deg_s;
_pitchrate_deg_s = pitchrate_deg_s;
_yawrate_deg_s = yawrate_deg_s;
_has_been_set.store(true);
}

13
src/modules/gimbal/input_test.h
View File

@ -50,12 +50,17 @@ public:
int initialize() override;
void print_status() const override;
void set_test_input(int roll_deg, int pitch_deg, int yaw_deg);
void set_test_input_angles(
float roll_deg, float pitch_deg, float yaw_deg);
void set_test_input_angle_rates(float rollrate_deg_s, float pitchrate_deg_s, float yawrate_deg_s);
private:
int _roll_deg {0};
int _pitch_deg {0};
int _yaw_deg {0};
float _roll_deg {NAN};
float _pitch_deg {NAN};
float _yaw_deg {NAN};
float _rollrate_deg_s {NAN};
float _pitchrate_deg_s {NAN};
float _yawrate_deg_s {NAN};
px4::atomic<bool> _has_been_set {false};
};