mirror of
https://github.com/PX4/PX4-Autopilot.git
synced 2026-07-24 15:27:41 +08:00
feat(gimbal): add a fixed mode to the gimbal module (#27649)
Option to keep a gimbal pointing at a fixed attitude, not linked to any user input. Roll and Yaw angle setpoints are 0, Pitch can be set in separate parameter.
This commit is contained in:
@@ -23,6 +23,9 @@ You should set `MNT_MODE_IN` to one of: `RC (1)`, `MAVlink gimbal protocol v2 (4
|
||||
If you select `Auto (0)`, the gimbal will automatically select either RC or MAVLink input based on the latest input.
|
||||
Note that the auto-switch from MAVLink to RC requires a large stick motion!
|
||||
|
||||
To hold a fixed attitude that the pilot cannot control (e.g. for RF/Satellite receiver stabilization), set `MNT_MODE_IN` to `Fixed attitude (5)`.
|
||||
See [Fixed Attitude Gimbal](#fixed-attitude-gimbal) below.
|
||||
|
||||
The output is set using the [MNT_MODE_OUT](../advanced_config/parameter_reference.md#MNT_MODE_OUT) parameter.
|
||||
By default the output is set to a PXM port (`AUX (0)`).
|
||||
If the [MAVLink Gimbal Protocol v2](https://mavlink.io/en/services/gimbal_v2.html) is supported by your gimbal, you should instead select `MAVLink gimbal protocol v2 (2)`.
|
||||
@@ -77,6 +80,20 @@ For example, you might have the following settings to assign the gimbal roll, pi
|
||||
The PWM values to use for the disarmed, maximum, center and minimum values can be determined in the same way as other servo, using the [Actuator Test sliders](../config/actuators.md#actuator-testing) to confirm that each slider moves the appropriate axis, and changing the values so that the gimbal is in the appropriate position at the disarmed, low, center and high position in the slider.
|
||||
The values may also be provided in gimbal documentation.
|
||||
|
||||
## Fixed Attitude Gimbal
|
||||
|
||||
A fixed-attitude gimbal holds a constant world-frame attitude and cannot be controlled by the pilot.
|
||||
This is useful for stabilizing a payload that must keep pointing in a fixed direction regardless of vehicle motion, such as an RF or satellite receiver antenna.
|
||||
|
||||
To enable it, set [MNT_MODE_IN](../advanced_config/parameter_reference.md#MNT_MODE_IN) to `Fixed attitude (5)` and reboot.
|
||||
In this mode no RC or MAVLink input is created, so the attitude cannot be commanded from a transmitter or ground station.
|
||||
|
||||
The gimbal holds roll and yaw level (roll at 0, yaw at north), and pitch at the angle set in [MNT_FIXED_PITCH](../advanced_config/parameter_reference.md#MNT_FIXED_PITCH) (in degrees, world frame).
|
||||
|
||||
Because the setpoint is in the world frame, stabilization against vehicle motion must be enabled with [MNT_DO_STAB](../advanced_config/parameter_reference.md#MNT_DO_STAB):
|
||||
|
||||
- Set `MNT_DO_STAB` to `Stabilize all axis (1)` for the typical servo (AUX) gimbal.
|
||||
|
||||
## Gimbal Control in Missions
|
||||
|
||||
[Gimbal Manager commands](https://mavlink.io/en/services/gimbal_v2.html#gimbal-manager-messages) may be used in missions if supported by the vehicle type.
|
||||
|
||||
@@ -36,6 +36,7 @@ px4_add_module(
|
||||
COMPILE_FLAGS
|
||||
SRCS
|
||||
input.cpp
|
||||
input_fixed.cpp
|
||||
input_mavlink.cpp
|
||||
input_rc.cpp
|
||||
input_test.cpp
|
||||
|
||||
@@ -38,6 +38,7 @@
|
||||
* - MAVLink gimbal protocol v1
|
||||
* - MAVLink gimbal protocol v2
|
||||
* - Test CLI commands
|
||||
* - Fixed (no user input)
|
||||
* Supported outputs:
|
||||
* - PWM
|
||||
* - MAVLink gimbal protocol v1
|
||||
@@ -51,6 +52,7 @@
|
||||
#include <px4_platform_common/tasks.h>
|
||||
|
||||
#include "gimbal_params.h"
|
||||
#include "input_fixed.h"
|
||||
#include "input_mavlink.h"
|
||||
#include "input_rc.h"
|
||||
#include "input_test.h"
|
||||
@@ -98,7 +100,6 @@ static int gimbal_thread_main(int argc, char *argv[])
|
||||
|
||||
if (!initialize_params(param_handles, params)) {
|
||||
PX4_ERR("could not get mount parameters!");
|
||||
delete g_thread_data->test_input;
|
||||
return -1;
|
||||
}
|
||||
|
||||
@@ -139,6 +140,10 @@ static int gimbal_thread_main(int argc, char *argv[])
|
||||
thread_data.input_objs[thread_data.input_objs_len++] = new InputMavlinkGimbalV2(params);
|
||||
break;
|
||||
|
||||
case MNT_MODE_IN_FIXED: // Fixed world-frame attitude, not user controllable
|
||||
thread_data.input_objs[thread_data.input_objs_len++] = new InputFixed(params);
|
||||
break;
|
||||
|
||||
default:
|
||||
PX4_ERR("invalid input mode %" PRId32, params.mnt_mode_in);
|
||||
break;
|
||||
@@ -559,6 +564,7 @@ void update_params(ParameterHandles ¶m_handles, Parameters ¶ms)
|
||||
param_get(param_handles.mnt_lnd_p_min, ¶ms.mnt_lnd_p_min);
|
||||
param_get(param_handles.mnt_lnd_p_max, ¶ms.mnt_lnd_p_max);
|
||||
param_get(param_handles.mnt_tau, ¶ms.mnt_tau);
|
||||
param_get(param_handles.mnt_fixed_pitch, ¶ms.mnt_fixed_pitch);
|
||||
}
|
||||
|
||||
bool initialize_params(ParameterHandles ¶m_handles, Parameters ¶ms)
|
||||
@@ -583,6 +589,7 @@ bool initialize_params(ParameterHandles ¶m_handles, Parameters ¶ms)
|
||||
param_handles.mnt_lnd_p_min = param_find("MNT_LND_P_MIN");
|
||||
param_handles.mnt_lnd_p_max = param_find("MNT_LND_P_MAX");
|
||||
param_handles.mnt_tau = param_find("MNT_TAU");
|
||||
param_handles.mnt_fixed_pitch = param_find("MNT_FIXED_PITCH");
|
||||
|
||||
if (param_handles.mnt_mode_in == PARAM_INVALID ||
|
||||
param_handles.mnt_mode_out == PARAM_INVALID ||
|
||||
@@ -603,7 +610,8 @@ bool initialize_params(ParameterHandles ¶m_handles, Parameters ¶ms)
|
||||
param_handles.mnt_rc_in_mode == PARAM_INVALID ||
|
||||
param_handles.mnt_lnd_p_min == PARAM_INVALID ||
|
||||
param_handles.mnt_lnd_p_max == PARAM_INVALID ||
|
||||
param_handles.mnt_tau == PARAM_INVALID
|
||||
param_handles.mnt_tau == PARAM_INVALID ||
|
||||
param_handles.mnt_fixed_pitch == PARAM_INVALID
|
||||
) {
|
||||
return false;
|
||||
}
|
||||
|
||||
@@ -46,7 +46,8 @@ enum MntModeIn {
|
||||
MNT_MODE_IN_RC,
|
||||
MNT_MODE_IN_MAVLINK_ROI, // MAVLink gimbal protocol v1 (to be deprecated)
|
||||
MNT_MODE_IN_MAVLINK_DO_MOUNT, // MAVLink gimbal protocol v1 (to be deprecated)
|
||||
MNT_MODE_IN_MAVLINK_V2 // MAVLink gimbal protocol v2
|
||||
MNT_MODE_IN_MAVLINK_V2, // MAVLink gimbal protocol v2
|
||||
MNT_MODE_IN_FIXED // Fixed world-frame attitude, not user controllable
|
||||
};
|
||||
|
||||
enum MntModeOut {
|
||||
@@ -76,6 +77,7 @@ struct Parameters {
|
||||
float mnt_lnd_p_min;
|
||||
float mnt_lnd_p_max;
|
||||
float mnt_tau;
|
||||
float mnt_fixed_pitch;
|
||||
};
|
||||
|
||||
struct ParameterHandles {
|
||||
@@ -99,6 +101,7 @@ struct ParameterHandles {
|
||||
param_t mnt_lnd_p_min;
|
||||
param_t mnt_lnd_p_max;
|
||||
param_t mnt_tau;
|
||||
param_t mnt_fixed_pitch;
|
||||
};
|
||||
|
||||
} /* namespace gimbal */
|
||||
|
||||
@@ -18,9 +18,10 @@ parameters:
|
||||
2: MAVLINK_ROI (protocol v1, to be deprecated)
|
||||
3: MAVLINK_DO_MOUNT (protocol v1, to be deprecated)
|
||||
4: MAVlink gimbal protocol v2
|
||||
5: Fixed world-frame attitude, not user controllable
|
||||
default: -1
|
||||
min: -1
|
||||
max: 4
|
||||
max: 5
|
||||
reboot_required: true
|
||||
MNT_MODE_OUT:
|
||||
description:
|
||||
@@ -207,3 +208,12 @@ parameters:
|
||||
max: 90.0
|
||||
unit: deg
|
||||
decimal: 1
|
||||
MNT_FIXED_PITCH:
|
||||
description:
|
||||
short: Tracked pitch angle when in fixed mode (MNT_MODE_IN=5)
|
||||
type: float
|
||||
default: 0.0
|
||||
min: -90
|
||||
max: 90
|
||||
unit: deg
|
||||
decimal: 1
|
||||
|
||||
94
src/modules/gimbal/input_fixed.cpp
Normal file
94
src/modules/gimbal/input_fixed.cpp
Normal file
@@ -0,0 +1,94 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2026 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#include "input_fixed.h"
|
||||
|
||||
#include <px4_platform_common/log.h>
|
||||
#include <px4_platform_common/time.h>
|
||||
#include <lib/matrix/matrix/math.hpp>
|
||||
#include <lib/mathlib/mathlib.h>
|
||||
|
||||
namespace gimbal
|
||||
{
|
||||
|
||||
InputFixed::InputFixed(Parameters ¶meters) :
|
||||
InputBase(parameters)
|
||||
{}
|
||||
|
||||
InputFixed::UpdateResult InputFixed::update(unsigned int timeout_ms, ControlData &control_data, bool already_active)
|
||||
{
|
||||
// Unlike the RC/MAVLink inputs there is no event source to poll, so block for
|
||||
// the requested timeout to pace the gimbal main loop. Without this the loop
|
||||
// never sleeps in fixed mode and busy-spins at 100% CPU, starving other tasks.
|
||||
if (timeout_ms > 0) {
|
||||
px4_usleep(timeout_ms * 1000);
|
||||
}
|
||||
|
||||
// Hold a constant attitude in the world frame: level in roll/yaw and a fixed
|
||||
// pitch from MNT_FIXED_PITCH. All axes are requested in the absolute frame so
|
||||
// the output stabilizes them against vehicle motion (depending on MNT_DO_STAB).
|
||||
// The user cannot influence this: in fixed mode no RC/MAVLink input objects
|
||||
// are created.
|
||||
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::AngleAbsoluteFrame;
|
||||
|
||||
const matrix::Quatf q(matrix::Eulerf(0.f, math::radians(_parameters.mnt_fixed_pitch), 0.f));
|
||||
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;
|
||||
|
||||
// Nobody is in control: this attitude is not commandable.
|
||||
control_data.sysid_primary_control = 0;
|
||||
control_data.compid_primary_control = 0;
|
||||
|
||||
return UpdateResult::UpdatedActive;
|
||||
}
|
||||
|
||||
int InputFixed::initialize()
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
|
||||
void InputFixed::print_status() const
|
||||
{
|
||||
PX4_INFO("Input: Fixed (world-frame stabilized)");
|
||||
PX4_INFO_RAW(" pitch: % .1f deg\n", (double)_parameters.mnt_fixed_pitch);
|
||||
}
|
||||
|
||||
} /* namespace gimbal */
|
||||
56
src/modules/gimbal/input_fixed.h
Normal file
56
src/modules/gimbal/input_fixed.h
Normal file
@@ -0,0 +1,56 @@
|
||||
/****************************************************************************
|
||||
*
|
||||
* Copyright (c) 2026 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
|
||||
* are met:
|
||||
*
|
||||
* 1. Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* 2. Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in
|
||||
* the documentation and/or other materials provided with the
|
||||
* distribution.
|
||||
* 3. Neither the name PX4 nor the names of its contributors may be
|
||||
* used to endorse or promote products derived from this software
|
||||
* without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
|
||||
* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
|
||||
* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
|
||||
* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
|
||||
* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
|
||||
* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
|
||||
* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS
|
||||
* OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED
|
||||
* AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
|
||||
* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
|
||||
* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
|
||||
* POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
****************************************************************************/
|
||||
|
||||
#pragma once
|
||||
|
||||
#include "input.h"
|
||||
|
||||
namespace gimbal
|
||||
{
|
||||
|
||||
// Holds a constant, world-frame-stabilized (level) attitude that cannot be
|
||||
// influenced by the user. Intended for RF/Satellite receiver stabilization.
|
||||
class InputFixed : public InputBase
|
||||
{
|
||||
public:
|
||||
InputFixed() = delete;
|
||||
explicit InputFixed(Parameters ¶meters);
|
||||
virtual ~InputFixed() = default;
|
||||
|
||||
UpdateResult update(unsigned int timeout_ms, ControlData &control_data, bool already_active) override;
|
||||
int initialize() override;
|
||||
void print_status() const override;
|
||||
};
|
||||
|
||||
|
||||
} /* namespace gimbal */
|
||||
Reference in New Issue
Block a user