Mirrors_Framework/PX4-Autopilot
1
0
Fork 0
mirror of https://github.com/PX4/PX4-Autopilot.git synced 2026-10-06 09:02:52 +08:00
Code Issues Packages Projects Releases Wiki Activity

refactor(gimbal): let external manager do control arbitration

Browse Source 
Tracking the manager's control state ourselves could desync from the
manager and stop setpoints. Acquire once when onboard intent starts,
stream setpoints while it lasts, release with -3 (was -2, which means
"set myself in control").

Assisted-by: Claude:claude-opus-5
Signed-off-by: Julian Oes <julian@oes.ch>
This commit is contained in:
Julian Oes
2026-09-22 15:15:30 +12:00
parent a707206a28
commit fad6590345
2 changed files with 45 additions and 117 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/modules/gimbal/output_gimbal_manager.cpp
+36 -100
View File
@@ -50,86 +50,45 @@ void OutputToGimbalManager::update(const ControlData &control_data, bool new_set
// gimbal device, so we must not claim one via control_data.device_compid.
(void)gimbal_device_id;
const hrt_abstime now = hrt_absolute_time();
_update_manager_status();
if (!_manager_found) {
// Nothing to talk to yet. The external manager streams its
// information/status, so we just wait until we've seen it.
// Nothing to talk to yet. The external manager streams its status, so we
// just wait until we've seen it.
return;
}
// Only forward intent that originated onboard (RC, ROI, mission), i.e. where
// the autopilot itself holds primary control. A ground station commands an
// external manager directly, so we must not relay its commands here -
// otherwise we would fight it for control.
// the autopilot itself holds primary control in our input arbitration. A
// ground station commands an external manager directly, so we must not relay
// its commands here.
const bool onboard_originated =
control_data.sysid_primary_control == (uint8_t)_parameters.mav_sysid &&
control_data.compid_primary_control == (uint8_t)_parameters.mav_compid;
const bool want_control = onboard_originated && (control_data.type != ControlData::Type::Neutral);
const bool onboard_intent = onboard_originated && (control_data.type != ControlData::Type::Neutral);
// Acquire only on a fresh edge of onboard intent, never continuously. This
// gives "assert once, then yield if overridden" rather than fighting whoever
// took control (a moved RC stick or a new ROI re-triggers the edge).
const bool want_control_edge = want_control && !_prev_want_control;
_prev_want_control = want_control;
if (onboard_intent && !_onboard_intent) {
// Intent started: ask for control once. The command is retried a few times by
// the MAVLink command sender if not acknowledged. Whether we actually get
// control is up to the manager.
_send_configure(true);
switch (_control_state) {
case ControlState::Released:
if (want_control_edge) {
_send_configure(true);
_last_acquire_request = now;
_control_state = ControlState::Acquiring;
}
break;
case ControlState::Acquiring:
if (_have_primary_control()) {
_control_state = ControlState::InControl;
} else if (!want_control) {
_control_state = ControlState::Released;
} else if (_someone_else_in_control()) {
// We lost the race for control; yield instead of fighting.
_control_state = ControlState::Released;
} else if (now - _last_acquire_request > kAcquireRetryInterval) {
// No one holds control yet, our request may have been lost: retry.
_send_configure(true);
_last_acquire_request = now;
}
break;
case ControlState::InControl:
if (!want_control) {
// Our intent ended: release so a ground station can take over.
_send_configure(false);
_control_state = ControlState::Released;
} else if (!_have_primary_control()) {
// Someone took control from us: yield, don't fight. A new onboard
// intent edge is required to reclaim.
_control_state = ControlState::Released;
}
break;
} else if (!onboard_intent && _onboard_intent) {
// Intent ended: release so another client can take over.
_send_configure(false);
}
if (_control_state == ControlState::InControl) {
// Apply the current setpoint every cycle, not just on new_setpoints.
// Unlike a direct device output we can reach InControl several cycles
// after new_setpoints (after the control handshake), so gating on it
// could miss the setpoint entirely and stream NaN. control_data always
// holds the latest setpoint, and for a location target _set_angle_setpoints
// re-runs the geo pointing so it keeps tracking as the vehicle moves.
_onboard_intent = onboard_intent;
if (_onboard_intent) {
// Apply the current setpoint every cycle, not just on new_setpoints, so
// the manager keeps receiving it. For a location target
// _set_angle_setpoints re-runs the geo pointing so it keeps tracking as the
// vehicle moves. If we are not in control, the manager ignores this.
_set_angle_setpoints(control_data);
_publish_set_pitchyaw();
_last_update = now;
_last_update = hrt_absolute_time();
}
}
@@ -139,10 +98,9 @@ void OutputToGimbalManager::_update_manager_status()
if (_status_sub.update(&status)) {
_status = status;
_status_valid = true;
// The status is streamed periodically, so we use it to discover the
// manager as well as to track control ownership.
// manager.
if (!_manager_found) {
_manager_found = true;
_manager_sysid = status.manager_sysid;
@@ -152,30 +110,16 @@ void OutputToGimbalManager::_update_manager_status()
}
}
bool OutputToGimbalManager::_have_primary_control() const
{
return _status_valid
&& _status.primary_control_sysid == (uint8_t)_parameters.mav_sysid
&& _status.primary_control_compid == (uint8_t)_parameters.mav_compid;
}
bool OutputToGimbalManager::_someone_else_in_control() const
{
// A non-zero primary control that isn't us means another component holds it.
return _status_valid
&& (_status.primary_control_sysid != 0 || _status.primary_control_compid != 0)
&& !_have_primary_control();
}
void OutputToGimbalManager::_send_configure(bool acquire)
{
// Special values per MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: -2 releases
// control, -1 leaves it unchanged. To acquire, we set ourselves as primary.
// Special values per MAV_CMD_DO_GIMBAL_MANAGER_CONFIGURE: -1 leaves a field
// unchanged, -3 removes control if the sender is currently in control.
// To acquire, we set ourselves as primary.
vehicle_command_s cmd{};
cmd.timestamp = hrt_absolute_time();
cmd.command = vehicle_command_s::VEHICLE_CMD_DO_GIMBAL_MANAGER_CONFIGURE;
cmd.param1 = acquire ? (float)_parameters.mav_sysid : -2.f; // primary control sysid
cmd.param2 = acquire ? (float)_parameters.mav_compid : -2.f; // primary control compid
cmd.param1 = acquire ? (float)_parameters.mav_sysid : -3.f; // primary control sysid
cmd.param2 = acquire ? (float)_parameters.mav_compid : -3.f; // primary control compid
cmd.param3 = -1.f; // secondary control sysid: leave unchanged
cmd.param4 = -1.f; // secondary control compid: leave unchanged
cmd.param7 = _gimbal_device_id;
@@ -224,25 +168,17 @@ void OutputToGimbalManager::print_status() const
{
PX4_INFO("Output: external gimbal manager");
if (_manager_found) {
PX4_INFO_RAW(" manager: %d/%d, gimbal device id: %d\n",
_manager_sysid, _manager_compid, _gimbal_device_id);
} else {
if (!_manager_found) {
PX4_INFO_RAW(" manager: not found yet\n");
return;
}
const char *state_str = "released";
switch (_control_state) {
case ControlState::Acquiring: state_str = "acquiring"; break;
case ControlState::InControl: state_str = "in control"; break;
default: break;
}
PX4_INFO_RAW(" control: %s\n", state_str);
PX4_INFO_RAW(" manager: %d/%d, gimbal device id: %d\n",
_manager_sysid, _manager_compid, _gimbal_device_id);
PX4_INFO_RAW(" onboard intent: %s\n", _onboard_intent ? "active" : "idle");
PX4_INFO_RAW(" manager reports primary control: %d/%d, secondary control: %d/%d\n",
_status.primary_control_sysid, _status.primary_control_compid,
_status.secondary_control_sysid, _status.secondary_control_compid);
}
} /* namespace gimbal */
src/modules/gimbal/output_gimbal_manager.h
+9 -17
View File
@@ -46,9 +46,13 @@ namespace gimbal
// Output that makes PX4 act as a gimbal manager *client*: instead of driving a
// gimbal device directly, it forwards the setpoints to an external gimbal
// manager (e.g. a smart camera-gimbal that runs its own manager). It discovers
// the manager, requests control while there is an active setpoint, and streams
// GIMBAL_MANAGER_SET_PITCHYAW to it.
// manager (e.g. a smart camera-gimbal that runs its own manager).
//
// The external manager does the deconfliction between its clients, so we don't
// track or second-guess who is in control. When onboard intent starts we ask
// for control once, while it lasts we stream GIMBAL_MANAGER_SET_PITCHYAW, and
// when it ends we release control. If another client has taken control, the
// manager ignores our setpoints.
class OutputToGimbalManager : public OutputBase
{
public:
@@ -60,15 +64,7 @@ public:
void print_status() const override;
private:
enum class ControlState {
Released, // we don't hold control of the manager
Acquiring, // we requested control and wait for confirmation
InControl // the manager reports us as primary control
};
void _update_manager_status();
bool _have_primary_control() const;
bool _someone_else_in_control() const;
void _send_configure(bool acquire);
void _publish_set_pitchyaw();
@@ -81,14 +77,10 @@ private:
uint8_t _manager_compid{0};
uint8_t _gimbal_device_id{0};
// Last status from the manager, for print_status only.
external_gimbal_manager_status_s _status{};
bool _status_valid{false};
ControlState _control_state{ControlState::Released};
bool _prev_want_control{false};
hrt_abstime _last_acquire_request{0};
static constexpr hrt_abstime kAcquireRetryInterval{3000000}; // 3 s
bool _onboard_intent{false};
};
} /* namespace gimbal */