diff --git a/src/modules/gimbal/gimbal.cpp b/src/modules/gimbal/gimbal.cpp index 20f70d296d..4524080639 100644 --- a/src/modules/gimbal/gimbal.cpp +++ b/src/modules/gimbal/gimbal.cpp @@ -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(" ", "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 ", "Specify an axis and an angle in degrees", false); + PRINT_MODULE_USAGE_ARG("rollrate|pitchrate|yawrate ", "Specify an axis and an angle rate in degrees / second", false); PRINT_MODULE_USAGE_DEFAULT_COMMANDS(); } diff --git a/src/modules/gimbal/input_test.cpp b/src/modules/gimbal/input_test.cpp index 46a8401866..67dc952512 100644 --- a/src/modules/gimbal/input_test.cpp +++ b/src/modules/gimbal/input_test.cpp @@ -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); } diff --git a/src/modules/gimbal/input_test.h b/src/modules/gimbal/input_test.h index 4511838c5c..80f60746aa 100644 --- a/src/modules/gimbal/input_test.h +++ b/src/modules/gimbal/input_test.h @@ -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 _has_been_set {false}; };