Hello everyone,
I am currently working on developing and optimizing my gimbal control system, integrated with a camera on a drone, for tracking mode. Due to limitations in my gimbal’s firmware, I am using Mavlink Protocol v1.0 instead of v2.0. This restricts me to controlling the gimbal’s angle directly over time using the MAV_CMD_DO_MOUNT_CONTROL command, rather than controlling angular velocity with the MAV_CMD_DO_GIMBAL_MANAGER_PITCHYAW command available in Mavlink v2.0.
System Overview
- Bounding Box Feedback: Bounding boxes are returned at a frequency of approximately 20Hz (20 fps).
- PID Loop: I use a PID loop to calculate and control the gimbal’s velocity indirectly by adjusting its angle over time. The loop operates at 30Hz, which is higher than the bounding box frequency to avoid missing updates.
- PID Tuning: The PID parameters (P, I, D) have been tuned to work well for static objects.
Issue Encountered
The gimbal’s responsiveness in tracking moving objects is slow and lacks agility. When tracking a moving target, the gimbal cannot adjust quickly enough to keep the target centered in the frame. The gimbal’s velocity seems insufficient to catch up with the target, resulting in the target maintaining a noticeable offset from the center of the screen.
Assistance Needed
I am seeking advice on how to improve the speed and agility of the gimbal when tracking moving objects using Mavlink v1.0 (MAV_CMD_DO_MOUNT_CONTROL). My goal is to control the gimbal so that the tracked object remains as close to the center of the frame as possible.
I would greatly appreciate any suggestions, insights, or solutions that the community can provide. Thank you in advance for your support and guidance!
This is my simple code and video working
video : https://youtu.be/-xJqryzOLcY?si=jI2D3lp7rT9CFEeb
from simple_pid import PID
from threading import Thread
import time
from pymavlink import mavutil
class GimbalControl:
def __init__(self):
self.master = mavutil.mavlink_connection('dev/ttyUSB0', baud=57600)
self.pid_p = PID(0.058, 0.0, 0.0, sample_time=None)
self.pid_t = PID(0.064, 0.0, 0.0, sample_time=None)
self.pan_rate = 0
self.tilt_rate = 0
self.signal_tracking = False
self.resolution = (1280, 720)
self.hfov = 61.5
self.vfov = 37
self.zoom_ratio = 1
# Start thread to get orientation of Gimbal
Thread_PID= Thread(target=self.PID_callback, daemon=True)
Thread_PID.start()
def PID_callback(self):
while True:
if self.signal_tracking:
# PID feedback value
pan = self.pid_p(self.pan_rate)
tilt = self.pid_t(self.tilt_rate)
tilt_move, pan_move = self.cal_pos(pan, -tilt)
# Send command to Gimbal
self.send_mount_control(tilt_move, pan_move, 0)
time.sleep(1/30) #30Hz
def Track(self, bbox):
# Get bbox data from camera with 20Hz
if bbox[4] > 0.5:
# calculate center of bbox
x, y = self.cal_box_center(bbox)
self.pan_rate, self.tilt_rate = self.cal_error_pos(x, y, self.zoom_ratio)
# Start tracking
self.signal_tracking = True
else:
# Stop tracking when completely lost object
self.signal_tracking = False
# Reset PID when lost object
self.pid_p.reset()
self.pid_t.reset()
def cal_box_center(self, bbox):
center_x = bbox[0] + bbox[2] // 2
center_y = bbox[1] + bbox[3] // 2
return center_x, center_y
def cal_error_pos(self, box_center_x, box_center_y, zoom_ratio=1):
# Set params tracking for mode camera
# Calculate origin center of frame
origin_center_x = self.resolution[0] // 2
origin_center_y = self.resolution[1] // 2
# Calculate difference between gimbal center and filtered position
dbox_x = origin_center_x - box_center_x
dbox_y = origin_center_y - box_center_y
# Stop gimbal when object is in the center of the frame
dbox_x = 0 if abs(dbox_x) < 10 else dbox_x
dbox_y = 0 if abs(dbox_y) < 10 else dbox_y
# Convert pixel to angle
pan_angle = (dbox_x / origin_center_x) * (self.hfov / 2) / zoom_ratio
tilt_angle = (dbox_y / origin_center_y) * (self.vfov / 2) / zoom_ratio
return pan_angle, tilt_angle
def send_mount_control(self, tilt, pan, roll):
self.master.mav.command_long_send(
1, # System ID
154, # Component ID
mavutil.mavlink.MAV_CMD_DO_MOUNT_CONTROL,
1, # Confirmation
tilt, # Param 1
roll, # Param 2
pan, # Param 3
0, 0, 3, # Param 4, 5, 6
mavutil.mavlink.MAV_MOUNT_MODE_MAVLINK_TARGETING)