Mavlink Camera

Mavlink Camera Component for sending commands to a camera on a companion computer or GCS

import time
from UAV.cameras.gst_cam import GSTCamera
from UAV.logging import LogLevels
from UAV.manager import Gui
from UAV.mavlink import CameraClient, CameraServer, MAVCom, mavlink, GimbalServerViewsheen


from UAV.mavlink.camera_client import *
from UAV.mavlink.camera_server import *
from UAV.utils.display import *
from fastcore.test import *

Btn_State.RUNNING = 1

Implementation of these commands:

MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS = 527 MAV_CMD_REQUEST_CAMERA_INFORMATION = 523 MAV_CMD_REQUEST_CAMERA_SETTINGS = 524 MAV_CMD_REQUEST_STORAGE_INFORMATION = 525 MAV_CMD_STORAGE_FORMAT = 526 MAV_CMD_SET_CAMERA_ZOOM = 531 MAV_CMD_SET_CAMERA_FOCUS = 532 MAV_CMD_IMAGE_START_CAPTURE = 2000 MAV_CMD_IMAGE_STOP_CAPTURE = 2001

MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION = 2504 MAV_CMD_REQUEST_VIDEO_STREAM_STATUS = 2505 MAV_CMD_VIDEO_START_CAPTURE = 2500 MAV_CMD_VIDEO_STOP_CAPTURE = 2501 MAV_CMD_SET_CAMERA_MODE = 530

Note The simulated camera is implemented in PX4 gazebo_camera_manager_plugin.cpp.

# from pymavlink.dialects.v20 import ardupilotmega as mav
# from pymavlink.dialects.v20.ardupilotmega import MAVLink


NAN = float("nan")

"""
MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS = 527 # https://mavlink.io/en/messages/common.html#MAV_CMD_REQUEST_CAMERA_CAPTURE_STATUS
MAV_CMD_REQUEST_CAMERA_INFORMATION = 521 # https://mavlink.io/en/messages/common.html#MAV_CMD_REQUEST_CAMERA_INFORMATION
MAV_CMD_REQUEST_CAMERA_SETTINGS = 522 # https://mavlink.io/en/messages/common.html#MAV_CMD_REQUEST_CAMERA_SETTINGS
MAV_CMD_REQUEST_STORAGE_INFORMATION = 525 # https://mavlink.io/en/messages/common.html#MAV_CMD_REQUEST_STORAGE_INFORMATION
MAV_CMD_STORAGE_FORMAT = 526 # https://mavlink.io/en/messages/common.html#MAV_CMD_STORAGE_FORMAT
MAV_CMD_SET_CAMERA_ZOOM = 531 # https://mavlink.io/en/messages/common.html#MAV_CMD_SET_CAMERA_ZOOM
MAV_CMD_SET_CAMERA_FOCUS = 532 # https://mavlink.io/en/messages/common.html#MAV_CMD_SET_CAMERA_FOCUS
MAV_CMD_IMAGE_START_CAPTURE = 2000  # https://mavlink.io/en/messages/common.html#MAV_CMD_IMAGE_START_CAPTURE
MAV_CMD_IMAGE_STOP_CAPTURE = 2001  # https://mavlink.io/en/messages/common.html#MAV_CMD_IMAGE_STOP_CAPTURE
MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION = 2504 # https://mavlink.io/en/messages/common.html#MAV_CMD_REQUEST_VIDEO_STREAM_INFORMATION
MAV_CMD_REQUEST_VIDEO_STREAM_STATUS = 2505 # https://mavlink.io/en/messages/common.html#MAV_CMD_REQUEST_VIDEO_STREAM_STATUS
MAV_CMD_VIDEO_START_CAPTURE = 2500 # https://mavlink.io/en/messages/common.html#MAV_CMD_VIDEO_START_CAPTURE
MAV_CMD_VIDEO_STOP_CAPTURE = 2501 # https://mavlink.io/en/messages/common.html#MAV_CMD_VIDEO_STOP_CAPTURE
MAV_CMD_SET_CAMERA_MODE = 530 # https://mavlink.io/en/messages/common.html#MAV_CMD_SET_CAMERA_MODE

"""
CAMERA_INFORMATION = mavlink.MAVLINK_MSG_ID_CAMERA_INFORMATION # https://mavlink.io/en/messages/common.html#CAMERA_INFORMATION
CAMERA_SETTINGS = mavlink.MAVLINK_MSG_ID_CAMERA_SETTINGS # https://mavlink.io/en/messages/common.html#CAMERA_SETTINGS
STORAGE_INFORMATION = mavlink.MAVLINK_MSG_ID_STORAGE_INFORMATION # https://mavlink.io/en/messages/common.html#STORAGE_INFORMATION
CAMERA_CAPTURE_STATUS = mavlink.MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS # https://mavlink.io/en/messages/common.html#CAMERA_CAPTURE_STATUS
CAMERA_IMAGE_CAPTURED = mavlink.MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED # https://mavlink.io/en/messages/common.html#CAMERA_IMAGE_CAPTURED

Camera Server

The server is on the companion computer and is used to receive commands from the camera on the ground station PC.

---

CameraServer

 CameraServer (source_component=100, mav_type=30, camera=None,
               loglevel:UAV.logging.LogLevels|int=20)

Create a mavlink Camera server Component, cameras argument will normally be a gstreamer pipeline

	Type	Default	Details
source_component	int	100	used for component indication
mav_type	int	30	used for heartbeat MAV_TYPE indication
camera	NoneType	None	cameras (or FakeCamera for testing)
loglevel	LogLevels | int	20	logging level

# run a server that can receive commands from a client
from UAV.logging import LogLevels

# start a mavlink server that can receive commands from a client
with MAVCom("udpout:localhost:14445", source_system=222, loglevel=LogLevels.DEBUG) as UAV_server:
    # add the camera server components to the server
    UAV_server.add_component(CameraServer(mav_type=mavlink.MAV_TYPE_CAMERA, source_component=mavlink.MAV_COMP_ID_CAMERA, camera=None))
    UAV_server.add_component(CameraServer(mavlink.MAV_COMP_ID_CAMERA2))
    UAV_server.add_component(CameraServer(mavlink.MAV_COMP_ID_CAMERA3))
    
    time.sleep(1)

INFO |28.955| mavcom.MAVCom   | mavcom.py :386 | Thread-6 ( | MainProces | MAVLink Mav2: True, source_system: 222
WARNI|28.957| mavcom.CameraSe | camera_ser:114 | MainThread | MainProces | Component has no cameras object
INFO |28.957| mavcom.CameraSe | basecompon:123 | MainThread | MainProces | Component Started self.source_component = 100, self.mav_type = 30, self.source_system = 222
WARNI|28.958| mavcom.CameraSe | camera_ser:114 | MainThread | MainProces | Component has no cameras object
INFO |28.958| mavcom.CameraSe | basecompon:123 | MainThread | MainProces | Component Started self.source_component = 101, self.mav_type = 30, self.source_system = 222
WARNI|28.959| mavcom.CameraSe | camera_ser:114 | MainThread | MainProces | Component has no cameras object
INFO |28.959| mavcom.CameraSe | basecompon:123 | MainThread | MainProces | Component Started self.source_component = 102, self.mav_type = 30, self.source_system = 222
INFO |30.959| mavcom.CameraSe | basecompon:417 | MainThread | MainProces | CameraServer closed (not waiting for _t_heartbeat daemon thread)
INFO |30.960| mavcom.CameraSe | basecompon:417 | MainThread | MainProces | CameraServer closed (not waiting for _t_heartbeat daemon thread)
INFO |30.960| mavcom.CameraSe | basecompon:417 | MainThread | MainProces | CameraServer closed (not waiting for _t_heartbeat daemon thread)
INFO |30.960| mavcom.MAVCom   | mavcom.py :442 | MainThread | MainProces | MAVCom  closed

UAV                             
UAV                             
UAV                             

CameraServer().list_commands()

Supported Commands: https://mavlink.io/en/messages/common.html#mav_commands
 Cmd = MAV_CMD_REQUEST_MESSAGE: 512 
 Cmd = MAV_CMD_STORAGE_FORMAT: 526 
 Cmd = MAV_CMD_SET_CAMERA_ZOOM: 531 
 Cmd = MAV_CMD_IMAGE_START_CAPTURE: 2000 
 Cmd = MAV_CMD_IMAGE_STOP_CAPTURE: 2001 
 Cmd = MAV_CMD_VIDEO_START_CAPTURE: 2500 
 Cmd = MAV_CMD_VIDEO_STOP_CAPTURE: 2501 
 Cmd = MAV_CMD_SET_CAMERA_MODE: 530 
 Cmd = MAV_CMD_VIDEO_START_STREAMING: 2502 
 Cmd = MAV_CMD_VIDEO_STOP_STREAMING: 2503 
Supported Message Requests:  https://mavlink.io/en/messages/common.html#messages

MAVLINK_MSG_ID_CAMERA_INFORMATION:  259
MAVLINK_MSG_ID_CAMERA_SETTINGS:  260
MAVLINK_MSG_ID_STORAGE_INFORMATION:  261
MAVLINK_MSG_ID_CAMERA_CAPTURE_STATUS:  262
MAVLINK_MSG_ID_CAMERA_IMAGE_CAPTURED:  263
MAVLINK_MSG_ID_VIDEO_STREAM_INFORMATION:  269
MAVLINK_MSG_ID_VIDEO_STREAM_STATUS:  270

Camera Client

The client is on the ground station PC and is used to send commands to the camera on the companion computer.

---

CameraClient

 CameraClient (source_component:int, mav_type:int,
               loglevel:UAV.logging.LogLevels|int=20)

Create a client component to send commands to a companion computer or GCS that will control a cameras via a CameraServer instance

	Type	Default	Details
source_component	int		used for component indication
mav_type	int		used for heartbeat MAV_TYPE indication
loglevel	LogLevels | int	20	logging level

Example: CameraClient

# run a client that can send commands to a server
with MAVCom("udpin:localhost:14445", source_system=111, loglevel=LogLevels.CRITICAL) as client: 
    # add the camera client component to the client
    gcs:CameraClient = client.add_component( CameraClient(mav_type=mavlink.MAV_TYPE_GCS, source_component=11, loglevel=LogLevels.DEBUG) )

DEBUG|31.104| mavcom.CameraCl | basecompon:119 | MainThread | MainProces | set_mav_connection CameraClient general.py:119 self.mav_com = <MAVCom>
DEBUG|31.104| mavcom.CameraCl | basecompon:165 | Thread-16  | MainProces | Starting heartbeat type: 6 to all Systems and Components
DEBUG|31.105| mavcom.CameraCl | basecompon:127 | MainThread | MainProces | Called from Component.start_mav_connection(), override to add startup behaviour
DEBUG|31.105| mavcom.CameraCl | basecompon:168 | Thread-16  | MainProces | Sent heartbeat 6 self.source_system = 111 self.source_component = 11
INFO |31.105| mavcom.CameraCl | basecompon:123 | MainThread | MainProces | Component Started self.source_component = 11, self.mav_type = 6, self.source_system = 111
INFO |32.105| mavcom.CameraCl | basecompon:417 | MainThread | MainProces | CameraClient closed (not waiting for _t_heartbeat daemon thread)
DEBUG|32.107| mavcom.CameraCl | basecompon:168 | Thread-16  | MainProces | Sent heartbeat 6 self.source_system = 111 self.source_component = 11

doc_class(CameraClient)

---

BaseComponent.append_message_handler

 BaseComponent.append_message_handler (callback:Callable)

append the callback function for when a command is received.

---

BaseComponent.close

 BaseComponent.close ()

---

BaseComponent.count_message

 BaseComponent.count_message (msg)

Count a message by adding it to the message_cnts dictionary. indexed by system and message type

---

CameraClient.image_start_capture

 CameraClient.image_start_capture (target_system=None,
                                   target_component=None, interval=0,
                                   count=1)

Start image capture sequence.

	Type	Default	Details
target_system	NoneType	None
target_component	NoneType	None
interval	int	0	Image capture interval
count	int	1	Number of images to capture (0 for unlimited)

---

CameraClient.image_stop_capture

 CameraClient.image_stop_capture (target_system=None,
                                  target_component=None)

Stop image capture sequence

---

Component.message_callback_cond

 Component.message_callback_cond (msg_id, target_system, target_component,
                                  timeout=1)

Register a callback for a message received from a component server Returns the message

Note: async function

---

Component.on_mav_connection

 Component.on_mav_connection ()

---

Component.on_message

 Component.on_message
                       (msg:pymavlink.dialects.v20.ardupilotmega.MAVLink_m
                       essage)

Callback for a command received from a component server

---

Component.request_message

 Component.request_message (msg_id, params=None, target_system=None,
                            target_component=None, timeout=1)

Request the target system(s) emit a single instance of a specified message (i.e. a “one-shot” version of MAV_CMD_SET_MESSAGE_INTERVAL). https://mavlink.io/en/messages/common.html#MAV_CMD_REQUEST_MESSAGE

Note: async function

---

Component.request_message_stream

 Component.request_message_stream (target_system=None,
                                   target_component=None, msg_id:int=147,
                                   interval:int=1000000,
                                   response_target:int=0)

A system can request that additional messages are sent as a stream, or change the rate at which existing streamed messages are sent, using the MAV_CMD_SET_MESSAGE_INTERVAL command. A single instance of a message can be requested by sending MAV_CMD_REQUEST_MESSAGE. See https://mavlink.io/en/mavgen_python/howto_requestmessages.html See https://mavlink.io/en/messages/common.html#MAV_CMD_SET_MESSAGE_INTERVAL

Note: async function

---

BaseComponent.send_ack

 BaseComponent.send_ack (msg, ack_result:object=0)

Send an ACK message to indicate a command was received.

---

BaseComponent.send_command

 BaseComponent.send_command (target_system:int, target_component:int,
                             command_id:int, params:list, timeout=0.5)

Note: async function

---

Component.send_message

 Component.send_message (msg)

Send a message to a component server

---

BaseComponent.send_ping

 BaseComponent.send_ping (target_system:int, target_component:int,
                          ping_num:int=None)

Send self.max_pings * ping messages to test if the server is alive.

---

CameraClient.set_camera_mode

 CameraClient.set_camera_mode (target_system=None, target_component=None,
                               mode_id=0)

Set the camera mode

	Type	Default	Details
target_system	NoneType	None
target_component	NoneType	None
mode_id	int	0	https://mavlink.io/en/messages/common.html#CAMERA_MODE

---

CameraClient.set_camera_zoom

 CameraClient.set_camera_zoom (target_system=None, target_component=None,
                               zoom_type=0, zoom_value=1)

Set the cameras zoom

	Type	Default	Details
target_system	NoneType	None
target_component	NoneType	None
zoom_type	int	0
zoom_value	int	1	0 to 100 zoom value

---

BaseComponent.set_log

 BaseComponent.set_log (loglevel)

---

BaseComponent.set_mav_connection

 BaseComponent.set_mav_connection (mav_com:MAVCom)

Set the mav_connection for the component

---

Component.set_message_callback_cond

 Component.set_message_callback_cond (msg_id, target_system,
                                      target_component)

Register a callback condition for a message received from a component server

---

BaseComponent.set_source_compenent

 BaseComponent.set_source_compenent ()

Set the source component for the master.mav

---

BaseComponent.set_target

 BaseComponent.set_target (target_system, target_component)

Set the target system and component for the gimbal

---

CameraClient.storage_format

 CameraClient.storage_format (target_system=None, target_component=None)

Format storage (for cases where cameras has storage)

---

BaseComponent.test_command

 BaseComponent.test_command (target_system:int, target_component:int,
                             camera_id:int)

example: MAV_CMD_DO_DIGICAM_CONTROL to trigger a cameras

Note: async function

---

CameraClient.video_start_capture

 CameraClient.video_start_capture (target_system=None,
                                   target_component=None,
                                   video_stream_id=0, frequency=1)

Start video capture

	Type	Default	Details
target_system	NoneType	None
target_component	NoneType	None
video_stream_id	int	0
frequency	int	1	Frequency CAMERA_CAPTURE_STATUS messages should be sent while recording (0 for no messages, otherwise frequency in Hz)

---

CameraClient.video_start_streaming

 CameraClient.video_start_streaming (target_system=None,
                                     target_component=None,
                                     video_stream_id=0)

Start video streaming

---

CameraClient.video_stop_capture

 CameraClient.video_stop_capture (target_system=None,
                                  target_component=None,
                                  video_stream_id=0)

Stop video capture

	Type	Default	Details
target_system	NoneType	None
target_component	NoneType	None
video_stream_id	int	0	Video stream id (0 for all streams)

---

CameraClient.video_stop_streaming

 CameraClient.video_stop_streaming (target_system=None,
                                    target_component=None,
                                    video_stream_id=0)

Stop the video stream

	Type	Default	Details
target_system	NoneType	None
target_component	NoneType	None
video_stream_id	int	0	Video Stream ID (0 for all streams)

---

BaseComponent.wait_ack

 BaseComponent.wait_ack (target_system, target_component, command_id=None,
                         timeout=0.1)

Wait for an ack from target_system and target_component.

Note: async function

---

BaseComponent.wait_heartbeat

 BaseComponent.wait_heartbeat (remote_mav_type=None, target_system=None,
                               target_component=None, timeout:float=1.0)

Wait for a heartbeat from target_system and target_component.

Note: async function

---

Component.wait_message_callback

 Component.wait_message_callback (cond, timeout=1, remove_after=True)

Wait for the callback for a message received from a component server

Note: async function

Example: Test locally using UDP ports

on the same machine using UDP ports 14445 with server_system_ID=111, client_system_ID=222 CameraClient is set to udpin:localhost:14445 and CameraServer is set to udpout:localhost:14445 udpin is so that the client can receive UDP from the mavproxy server at localhost:14445 CameraClient uses async/await to send commands to the CameraServer

import asyncio
async def main():
    MAV_TYPE_GCS = mavlink.MAV_TYPE_GCS
    MAV_TYPE_CAMERA = mavlink.MAV_TYPE_CAMERA
    
    con1, con2 = "udpin:localhost:14445", "udpout:localhost:14445"
    # con1, con2 = "/dev/ttyACM0", "/dev/ttyUSB0"
    with MAVCom(con1, source_system=111) as client:
        with MAVCom(con2, source_system=222) as server:
            gcs:CameraClient = client.add_component(
                CameraClient(mav_type=MAV_TYPE_GCS, source_component=11))
            # server.add_component(CameraServer(mav_type=MAV_TYPE_CAMERA, source_component=22, camera=cam_fake1, debug=False))
            server.add_component(CameraServer(mav_type=MAV_TYPE_CAMERA, source_component=22, camera=None))
    
            ret = await gcs.wait_heartbeat(remote_mav_type=mavlink.MAV_TYPE_CAMERA)
            print(f"Heartbeat received {ret = }")
    
            msg = await gcs.request_message(mavlink.MAVLINK_MSG_ID_CAMERA_INFORMATION, target_system=222, target_component=22)

            print( f"MAVLINK_MSG_ID_CAMERA_INFORMATION {msg}")
            
            # msg = await cam.request_storage_information()
            # print (msg)
            
            time.sleep(1)
            
await main()

INFO |32.270| mavcom.MAVCom   | mavcom.py :386 | Thread-18  | MainProces | MAVLink Mav2: True, source_system: 111
INFO |32.370| mavcom.MAVCom   | mavcom.py :386 | Thread-19  | MainProces | MAVLink Mav2: True, source_system: 222
INFO |32.372| mavcom.CameraCl | basecompon:123 | MainThread | MainProces | Component Started self.source_component = 11, self.mav_type = 6, self.source_system = 111
WARNI|32.373| mavcom.CameraSe | camera_ser:114 | MainThread | MainProces | Component has no cameras object
INFO |32.374| mavcom.CameraSe | basecompon:123 | MainThread | MainProces | Component Started self.source_component = 22, self.mav_type = 30, self.source_system = 222
INFO |34.374| mavcom.CameraSe | basecompon:417 | MainThread | MainProces | CameraServer closed (not waiting for _t_heartbeat daemon thread)
INFO |34.375| mavcom.MAVCom   | mavcom.py :442 | MainThread | MainProces | MAVCom  closed
INFO |34.375| mavcom.CameraCl | basecompon:417 | MainThread | MainProces | CameraClient closed (not waiting for _t_heartbeat daemon thread)
INFO |34.376| mavcom.MAVCom   | mavcom.py :442 | MainThread | MainProces | MAVCom  closed

UAV                             
Heartbeat received ret = (222, 22)
MAVLINK_MSG_ID_CAMERA_INFORMATION CAMERA_INFORMATION {time_boot_ms : 3985, vendor_name : UAV, model_name : FakeCamera, firmware_version : 1, focal_length : 2.799999952316284, sensor_size_h : 3.200000047683716, sensor_size_v : 2.4000000953674316, resolution_h : 640, resolution_v : 480, lens_id : 0, flags : 0, cam_definition_version : 1, cam_definition_uri : , gimbal_device_id : 0}

Starting a client and server

on the same machine using UDP ports 14445 with server_system_ID=111, client_system_ID=222

from UAV.mavlink import CameraClient, CameraServer, MAVCom, mavlink, GimbalServerViewsheen

import time

def on_message(message):
    print(f"on_message: {message}")
    return True # Return True to indicate that command was ok and send ack

class Cam1(Component):
    def __init__(self, source_component, mav_type, debug=False):
        super().__init__(source_component=source_component, mav_type=mav_type)
        self.append_message_handler(on_message)


class Cam2(Component):
    def __init__(self, source_component, mav_type, debug=False):
        super().__init__(source_component=source_component, mav_type=mav_type)
        self.append_message_handler(on_message)


class Cli(Component):
    def __init__(self, source_component, mav_type, debug=False):
        super().__init__(source_component=source_component, mav_type=mav_type)
        self.append_message_handler(on_message)

# assert False, "Stop here"

Test with Serial ports

Test using a Pixhawk connected via telemetry 2 and USB serial ports. CamClient is set to udpin:localhost:14445 and CamServer is set to udpout:localhost:14435 udpin is so that the client can receive UDP from the mavproxy server at localhost:14445 mavproxy.py –master=/dev/ttyACM1 –baudrate 57600 –out udpout:localhost:14445 mavproxy.py –master=/dev/ttyACM3 –baudrate 57600 –out udpout:localhost:14435

# Test sending a command and receiving an ack from client to server
with MAVCom("/dev/ttyACM0", source_system=111) as client:
    with MAVCom("/dev/ttyUSB0", source_system=222) as server:
        client.add_component(Cli(client, mav_type=mavlink.MAV_TYPE_GCS))
        server.add_component(Cam1(server, mav_type=mavlink.MAV_TYPE_CAMERA))
        server.add_component(Cam1(server, mav_type=mavlink.MAV_TYPE_CAMERA))
        
        for key, comp in client.component.items():
            if comp.wait_heartbeat(target_system=222, target_component=22, timeout=0.1):
                print ("*** Received heartbeat **** " )
        NUM_TO_SEND = 2
        for i in range(NUM_TO_SEND):
            client.component[11]._test_command(222, 22, 1)
            client.component[11]._test_command(222, 23, 1)
            
        client.component[11]._test_command(222, 24, 1)

    print(f"{server.source_system = };  {server.message_cnts = }")
    print(f"{client.source_system = };  {client.message_cnts = }")
    print()
    print(f"{client.source_system = } \n{client.summary()} \n")
    print(f"{server.source_system = } \n{server.summary()} \n")

    assert client.component[11].num_cmds_sent == NUM_TO_SEND * 2 + 1
    assert client.component[11].num_acks_rcvd == NUM_TO_SEND * 2
    assert client.component[11].num_acks_drop == 1
    assert server.component[22].num_cmds_rcvd == NUM_TO_SEND
    assert server.component[23].num_cmds_rcvd == NUM_TO_SEND

ERROR|34.431| mavcom.MAVCom   | mavcom.py :296 | MainThread | MainProces | [Errno 2] could not open port /dev/ttyACM0: [Errno 2] No such file or directory: '/dev/ttyACM0'

SerialException: [Errno 2] could not open port /dev/ttyACM0: [Errno 2] No such file or directory: '/dev/ttyACM0'

For debugging help see http://localhost:3000/tutorials/mavlink_doc&debug.html and http://localhost:3000/tutorials/mavlink_doc&debug.html#debugging