Skip to content

Commit

Permalink
MAVLinkInterface: add GimbalManagerProtocol
Browse files Browse the repository at this point in the history
  • Loading branch information
@robertlong13 @meee1
robertlong13 authored and meee1 committed Feb 3, 2025
1 parent 795ed1b commit 8253576
Show file tree
Hide file tree
Showing 3 changed files with 292 additions and 0 deletions.
262 changes: 262 additions & 0 deletions ExtLibs/ArduPilot/Mavlink/GimbalManagerProtocol.cs
View file
Original file line number Diff line number Diff line change
@@ -0,0 +1,262 @@
using System.Collections.Concurrent;
using System.Collections.Generic;

namespace MissionPlanner.ArduPilot.Mavlink
{
public class GimbalManagerProtocol
{
public ConcurrentDictionary<byte, MAVLink.mavlink_gimbal_manager_information_t> ManagerInfo =
new ConcurrentDictionary<byte, MAVLink.mavlink_gimbal_manager_information_t>(
new Dictionary<byte, MAVLink.mavlink_gimbal_manager_information_t>()
{
{0, new MAVLink.mavlink_gimbal_manager_information_t() {cap_flags = 0} }
}
);

public ConcurrentDictionary<byte, MAVLink.mavlink_gimbal_manager_status_t> ManagerStatus =
new ConcurrentDictionary<byte, MAVLink.mavlink_gimbal_manager_status_t>();

public ConcurrentDictionary<byte, MAVLink.mavlink_gimbal_device_attitude_status_t> GimbalStatus =
new ConcurrentDictionary<byte, MAVLink.mavlink_gimbal_device_attitude_status_t>();

private readonly MAVLinkInterface mavint;

public GimbalManagerProtocol(MAVLinkInterface mavint)
{
this.mavint = mavint;
}

public void Discover()
{
mavint.doCommand(0, 0, MAVLink.MAV_CMD.REQUEST_MESSAGE,
(float)MAVLink.MAVLINK_MSG_ID.GIMBAL_MANAGER_INFORMATION,
0, 0, 0, 0, 0, 0, false);

mavint.OnPacketReceived += (sender, message) =>
{
if (message.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GIMBAL_MANAGER_INFORMATION)
{
var gmi = (MAVLink.mavlink_gimbal_manager_information_t)message.data;

// It is invalid to have a gimbal device ID of 0. This field should be a component ID or a number 1-6
if (gmi.gimbal_device_id == 0)
{
return;
}

ManagerInfo[gmi.gimbal_device_id] = gmi;
// Keep a ManagerInfo element 0 to store capabilities of any gimbal
ManagerInfo[0] = new MAVLink.mavlink_gimbal_manager_information_t()
{
cap_flags = gmi.cap_flags | ManagerInfo[0].cap_flags
};
}

if (message.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GIMBAL_MANAGER_STATUS)
{
var gms = (MAVLink.mavlink_gimbal_manager_status_t)message.data;
ManagerStatus[gms.gimbal_device_id] = gms;
}

if (message.msgid == (uint)MAVLink.MAVLINK_MSG_ID.GIMBAL_DEVICE_ATTITUDE_STATUS)
{
var gds = (MAVLink.mavlink_gimbal_device_attitude_status_t)message.data;
GimbalStatus[gds.gimbal_device_id] = gds;
}
};
}

public bool HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS flags, byte gimbal_device_id = 0)
{
return ManagerInfo.TryGetValue(gimbal_device_id, out var info) && ((info.cap_flags & (uint)flags) != 0);
}

public bool HasAllCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS flags, byte gimbal_device_id = 0)
{
return ManagerInfo.TryGetValue(gimbal_device_id, out var info) && ((info.cap_flags & (uint)flags) == (uint)flags);
}

public bool Retract(byte gimbal_device_id = 0)
{
if (!HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_RETRACT))
{
return false;
}
return mavint.doCommand(
(byte)mavint.sysidcurrent,
(byte)mavint.compidcurrent,
MAVLink.MAV_CMD.DO_GIMBAL_MANAGER_PITCHYAW,
float.NaN, // pitch angle
float.NaN, // yaw angle
float.NaN, // pitch rate
float.NaN, // yaw rate
(float)MAVLink.GIMBAL_MANAGER_FLAGS.RETRACT,
0, // unused
gimbal_device_id);
}

public bool Neutral(byte gimbal_device_id = 0)
{
if (!HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_NEUTRAL))
{
return false;
}
return mavint.doCommand(
(byte)mavint.sysidcurrent,
(byte)mavint.compidcurrent,
MAVLink.MAV_CMD.DO_GIMBAL_MANAGER_PITCHYAW,
float.NaN, // pitch angle
float.NaN, // yaw angle
float.NaN, // pitch rate
float.NaN, // yaw rate
(float)MAVLink.GIMBAL_MANAGER_FLAGS.NEUTRAL,
0, // unused
gimbal_device_id);
}

public bool SetYawLock(bool yaw_lock, byte gimbal_device_id = 0)
{
if ((yaw_lock && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_YAW_LOCK)) ||
(!yaw_lock && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_YAW_FOLLOW)))
{
return false;
}

return mavint.doCommand(
(byte)mavint.sysidcurrent,
(byte)mavint.compidcurrent,
MAVLink.MAV_CMD.DO_GIMBAL_MANAGER_PITCHYAW,
float.NaN, // pitch angle
float.NaN, // yaw angle
float.NaN, // pitch rate
float.NaN, // yaw rate
yaw_lock ? (float)MAVLink.GIMBAL_MANAGER_FLAGS.YAW_LOCK : 0,
0, // unused
gimbal_device_id);
}

public bool SetAnglesCommand(float pitch, float yaw, bool yaw_in_earth_frame, byte gimbal_device_id = 0)
{
if (!HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.CAN_POINT_LOCATION_LOCAL) ||
(pitch != 0 && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_PITCH_AXIS)) ||
(yaw != 0 && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_YAW_AXIS)) ||
(yaw != 0 && yaw_in_earth_frame && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_YAW_LOCK)) ||
(yaw != 0 && !yaw_in_earth_frame && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_YAW_FOLLOW)))
{
return false;
}

return mavint.doCommand(
(byte)mavint.sysidcurrent,
(byte)mavint.compidcurrent,
MAVLink.MAV_CMD.DO_GIMBAL_MANAGER_PITCHYAW,
pitch,
yaw,
float.NaN, // pitch rate
float.NaN, // yaw rate
yaw_in_earth_frame ? (float)MAVLink.GIMBAL_MANAGER_FLAGS.YAW_LOCK : 0, // flags
0, // unused
gimbal_device_id);
}

public void SetAnglesStream(float pitch, float yaw, bool yaw_in_earth_frame, byte gimbal_device_id = 0)
{
MAVLink.mavlink_gimbal_manager_set_pitchyaw_t set = new MAVLink.mavlink_gimbal_manager_set_pitchyaw_t()
{
target_system = (byte)mavint.sysidcurrent,
target_component = (byte)mavint.compidcurrent,
gimbal_device_id = gimbal_device_id,
pitch = pitch,
yaw = yaw,
pitch_rate = float.NaN,
yaw_rate = float.NaN,
flags = yaw_in_earth_frame ? (uint)MAVLink.GIMBAL_MANAGER_FLAGS.YAW_LOCK : 0
};
mavint.sendPacket(set, mavint.sysidcurrent, mavint.compidcurrent);
}

public bool SetRatesCommand(float pitchRate, float yawRate, bool yaw_in_earth_frame, byte gimbal_device_id = 0)
{
if ((pitchRate != 0 && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_PITCH_AXIS)) ||
(yawRate != 0 && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_YAW_AXIS)) ||
(yawRate != 0 && yaw_in_earth_frame && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_YAW_LOCK)) ||
(yawRate != 0 && !yaw_in_earth_frame && !HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.HAS_YAW_FOLLOW)))
{
return false;
}

return mavint.doCommand(
(byte)mavint.sysidcurrent,
(byte)mavint.compidcurrent,
MAVLink.MAV_CMD.DO_GIMBAL_MANAGER_PITCHYAW,
float.NaN, // pitch angle
float.NaN, // yaw angle
pitchRate,
yawRate,
yaw_in_earth_frame ? (float)MAVLink.GIMBAL_MANAGER_FLAGS.YAW_LOCK : 0, // flags
0, // unused
gimbal_device_id);
}

public void SetRatesStream(float pitchRate, float yawRate, bool yaw_in_earth_frame, byte gimbal_device_id = 0)
{
MAVLink.mavlink_gimbal_manager_set_pitchyaw_t set = new MAVLink.mavlink_gimbal_manager_set_pitchyaw_t()
{
target_system = (byte)mavint.sysidcurrent,
target_component = (byte)mavint.compidcurrent,
gimbal_device_id = gimbal_device_id,
pitch = float.NaN,
yaw = float.NaN,
pitch_rate = pitchRate,
yaw_rate = yawRate,
flags = yaw_in_earth_frame ? (uint)MAVLink.GIMBAL_MANAGER_FLAGS.YAW_LOCK : 0
};
mavint.sendPacket(set, mavint.sysidcurrent, mavint.compidcurrent);
}

public bool SetROILocation(double lat, double lon, double alt = 0, byte gimbal_device_id = 0, MAVLink.MAV_FRAME frame = MAVLink.MAV_FRAME.GLOBAL_TERRAIN_ALT)
{
if (!HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.CAN_POINT_LOCATION_GLOBAL))
{
return false;
}

return mavint.doCommandInt(
(byte)mavint.sysidcurrent,
(byte)mavint.compidcurrent,
MAVLink.MAV_CMD.DO_SET_ROI_LOCATION,
gimbal_device_id,
0, 0, 0, // unused
(int)(lat * 1e7),
(int)(lon * 1e7),
(float)alt,
frame: frame);
}

public bool SetROINone(byte gimbal_device_id = 0)
{
return mavint.doCommand(
(byte)mavint.sysidcurrent,
(byte)mavint.compidcurrent,
MAVLink.MAV_CMD.DO_SET_ROI_NONE,
gimbal_device_id,
0, 0, 0, 0, 0, 0);
}

public bool SetROISysID(byte sysid, byte gimbal_device_id = 0)
{
if (!HasCapability(MAVLink.GIMBAL_MANAGER_CAP_FLAGS.CAN_POINT_LOCATION_GLOBAL))
{
return false;
}

return mavint.doCommand(
(byte)mavint.sysidcurrent,
(byte)mavint.compidcurrent,
MAVLink.MAV_CMD.DO_SET_ROI_SYSID,
sysid,
gimbal_device_id,
0, 0, 0, 0, 0);
}
}
}
27 changes: 27 additions & 0 deletions ExtLibs/ArduPilot/Mavlink/MAVLinkInterface.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -557,6 +557,33 @@ private void OnMAVDetected(object sender, (byte, byte) tuple)
}
});
}

if (tuple.Item2 == (byte)MAV_COMPONENT.MAV_COMP_ID_AUTOPILOT1 ||
(tuple.Item2 >= (byte)MAV_COMPONENT.MAV_COMP_ID_MISSIONPLANNER &&
tuple.Item2 <= (byte)MAV_COMPONENT.MAV_COMP_ID_ONBOARD_COMPUTER4))
{
MAVlist[tuple.Item1, tuple.Item2].GimbalManager = new GimbalManagerProtocol(this);
Task.Run(async () =>
{
try
{
// Open holds this
while (!_openComplete)
{
await Task.Delay(1000);
}

await Task.Delay(2000);

MAVlist[tuple.Item1, tuple.Item2]
.GimbalManager.Discover();
}
catch (Exception e)
{
log.Error(e);
}
});
}
}

public MAVLinkInterface(Stream logfileStream)
Expand Down
3 changes: 3 additions & 0 deletions ExtLibs/ArduPilot/Mavlink/MAVState.cs
View file
Original file line number Diff line number Diff line change
Expand Up @@ -328,7 +328,10 @@ public ap_product Product_ID
internal int recvpacketcount = 0;
public Int64 time_offset_ns { get; set; }
public CameraProtocol Camera { get; set; }

[Obsolete("Use GimbalManager instead")]
public GimbalProtocol Gimbal { get; set; }
public GimbalManagerProtocol GimbalManager { get; set; }

public override string ToString()
{
Expand Down

0 comments on commit 8253576

Please sign in to comment.