Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

SIYI: more commands (inc settime) #1272

Merged
merged 4 commits into from
Nov 15, 2023
Merged

SIYI: more commands (inc settime) #1272

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
4 commits
Select commit Hold shift + click to select a range
5228b5e
STI: added gain control
tridge Nov 10, 2023
f457762
SIYI: added some viewing tools
tridge Nov 10, 2023
1c7f579
SIYI: added thermal environment param controls
tridge Nov 10, 2023
98cd67f
SIYI: support settime command
tridge Nov 15, 2023
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
202 changes: 179 additions & 23 deletions MAVProxy/modules/mavproxy_SIYI/__init__.py
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -20,6 +20,7 @@
from threading import Thread
import cv2
import traceback
import copy

import socket, time, os, struct

Expand Down Expand Up @@ -68,6 +69,34 @@
GET_THERMAL_MODE = 0x33
SET_THERMAL_MODE = 0x34
GET_TEMP_FRAME = 0x35
SET_WEAK_CONTROL = 0x71
GET_THERMAL_GAIN = 0x37
SET_THERMAL_GAIN = 0x38
GET_THERMAL_PARAM = 0x39
SET_THERMAL_PARAM = 0x3A
GET_THERMAL_ENVSWITCH = 0x3B
SET_THERMAL_ENVSWITCH = 0x3C
SET_TIME = 0x30

class ThermalParameters:
def __init__(self, distance, target_emissivity, humidity, air_temperature, reflection_temperature):
self.distance = distance
self.target_emissivity = target_emissivity
self.humidity = humidity
self.air_temperature = air_temperature
self.reflection_temperature = reflection_temperature

def __repr__(self):
return "Dist=%.2f Emiss=%.2f Hum=%.2f AirTemp=%.2f RefTemp=%.2f" % (self.distance,
self.target_emissivity,
self.humidity,
self.air_temperature,
self.reflection_temperature)

def args(self):
return [int(self.distance*100), int(self.target_emissivity*100),
int(self.humidity*100), int(self.air_temperature*100), int(self.reflection_temperature*100)]


def crc16_from_bytes(bytes, initial=0):
# CRC-16-CCITT
Expand Down Expand Up @@ -194,7 +223,8 @@ def __init__(self, mpstate):
super(SIYIModule, self).__init__(mpstate, "SIYI", "SIYI camera support")

self.add_command('siyi', self.cmd_siyi, "SIYI camera control",
["<rates|connect|autofocus|zoom|yaw|pitch|center|getconfig|angle|photo|recording|lock|follow|fpv|settarget|notarget|thermal|rgbview|tempsnap|get_thermal_mode>",
["<rates|connect|autofocus|zoom|yaw|pitch|center|getconfig|angle|photo|recording|lock|follow|fpv|settarget|notarget|thermal|rgbview|tempsnap|get_thermal_mode|thermal_gain|get_thermal_gain|settime>",
"<therm_getenv|therm_set_distance|therm_set_emissivity|therm_set_humidity|therm_set_airtemp|therm_set_reftemp|therm_getswitch|therm_setswitch>",
"set (SIYISETTING)",
"imode <1|2|3|4|5|6|7|8|wide|zoom|split>",
"palette <WhiteHot|Sepia|Ironbow|Rainbow|Night|Aurora|RedHot|Jungle|Medical|BlackHot|GloryHot>",
Expand All @@ -218,9 +248,9 @@ def __init__(self, mpstate):
('mount_yaw', float, 0),
('lag', float, 0),
('target_rate', float, 10),
('telem_hz', float, 5),
('telem_rate', float, 4),
('att_send_hz', float, 4),
('att_send_hz', float, 10),
('mode_hz', float, 0),
('temp_hz', float, 5),
('rtsp_rgb', str, 'rtsp://192.168.144.25:8554/video1'),
('rtsp_thermal', str, 'rtsp://192.168.144.25:8554/video2'),
Expand All @@ -238,16 +268,17 @@ def __init__(self, mpstate):
('track_size_pct', float, 5.0),
('threshold_temp', int, 50),
('threshold_min', int, 240),
('los_correction', int, 1),
('los_correction', int, 0),
('att_control', int, 0),
('therm_cap_rate', float, 0),
('show_horizon', int, 0),
('track_ROI', int, 1),
MPSetting('thresh_climit', int, 50, range=(10,50)),
MPSetting('thresh_volt', int, 50, range=(20,50)),
MPSetting('thresh_ang', int, 300, range=(30,300)),
MPSetting('thresh_volt', int, 80, range=(20,80)),
MPSetting('thresh_ang', int, 4000, range=(30,4000)),
MPSetting('thresh_climit_dis', int, 20, range=(10,50)),
MPSetting('thresh_volt_dis', int, 40, range=(20,50)),
MPSetting('thresh_ang_dis', int, 40, range=(30,300)),
MPSetting('thresh_volt_dis', int, 40, range=(20,80)),
MPSetting('thresh_ang_dis', int, 40, range=(30,4000)),
])
self.add_completion_function('(SIYISETTING)',
self.siyi_settings.completion)
Expand Down Expand Up @@ -280,7 +311,7 @@ def __init__(self, mpstate):
self.last_enc_t = None
self.last_enc_recv_t = time.time()
self.last_volt_t = None
self.last_mode_t = None
self.last_mode_t = time.time()
self.last_thresh_t = None
self.last_thresh_send_t = None
self.GLOBAL_POSITION_INT = None
Expand All @@ -306,11 +337,13 @@ def __init__(self, mpstate):
self.last_SIEA = time.time()
self.last_therm_cap = time.time()
self.thermal_capture_count = 0
self.last_therm_mode = time.time()

self.recv_thread = Thread(target=self.receive_thread, name='SIYI_Receive')
self.recv_thread.daemon = True
self.recv_thread.start()
self.have_horizon_lines = False
self.thermal_param = None

if mp_util.has_wxpython:
menu = MPMenuSubMenu('SIYI',
Expand All @@ -332,6 +365,10 @@ def __init__(self, mpstate):
MPMenuItem('ResetAttitude', 'ResetAttitude', '# siyi resetattitude '),
MPMenuSubMenu('Zoom',
items=[MPMenuItem('Zoom%u'%z, 'Zoom%u'%z, '# siyi zoom %u ' % z) for z in range(1,11)]),
MPMenuSubMenu('ThermalGain',
items=[MPMenuItem('HighGain', 'HighGain', '# siyi thermal_gain 1'),
MPMenuItem('LowGain', 'LowGain', '# siyi thermal_gain 0')]),

MPMenuSubMenu('Threshold',
items=[MPMenuItem('Threshold%u'%z, 'Threshold%u'%z, '# siyi set threshold_temp %u ' % z) for z in range(20,115,5)])])
map = self.module('map')
Expand Down Expand Up @@ -384,6 +421,10 @@ def cmd_siyi(self, args):
self.send_packet_fmt(SET_THERMAL_MODE, "<B", int(args[1]))
elif args[0] == "get_thermal_mode":
self.send_packet_fmt(GET_THERMAL_MODE, None)
elif args[0] == "get_thermal_gain":
self.send_packet_fmt(GET_THERMAL_GAIN, None)
elif args[0] == "thermal_gain":
self.send_packet_fmt(SET_THERMAL_GAIN, "<B", int(args[1]))
elif args[0] == "recording":
self.send_packet_fmt(PHOTO, "<B", 2)
self.send_packet(FUNCTION_FEEDBACK_INFO, None)
Expand All @@ -407,6 +448,24 @@ def cmd_siyi(self, args):
self.cmd_thermal()
elif args[0] == "rgbview":
self.cmd_rgbview()
elif args[0] == "therm_getenv":
self.send_packet_fmt(GET_THERMAL_PARAM, None)
elif args[0] == "therm_set_distance":
self.therm_set_distance(float(args[1]))
elif args[0] == "therm_set_humidity":
self.therm_set_humidity(float(args[1]))
elif args[0] == "therm_set_emissivity":
self.therm_set_emissivity(float(args[1]))
elif args[0] == "therm_set_airtemp":
self.therm_set_airtemp(float(args[1]))
elif args[0] == "therm_set_reftemp":
self.therm_set_reftemp(float(args[1]))
elif args[0] == "therm_getswitch":
self.send_packet_fmt(GET_THERMAL_ENVSWITCH, None)
elif args[0] == "therm_setswitch":
self.send_packet_fmt(SET_THERMAL_ENVSWITCH, "<B", int(args[1]))
elif args[0] == "settime":
self.cmd_settime()
else:
print(usage)

Expand Down Expand Up @@ -477,6 +536,11 @@ def cmd_palette(self, args):
pal = int(args[0])
self.send_packet_fmt(SET_THERMAL_PALETTE, "<B", pal)

def cmd_settime(self):
'''set camera time'''
t_us = int(time.time()*1.0e6)
self.send_packet_fmt(SET_TIME, "<Q", t_us)

def video_filename(self, base):
'''get a video file name'''
if self.logdir is None:
Expand Down Expand Up @@ -541,6 +605,51 @@ def set_target(self, lat, lon, alt):
(lat, lon),
self.icon, layer='SIYI', rotation=0, follow=False))

def therm_set_distance(self, distance):
'''set thermal distance'''
if self.thermal_param is None:
print("Run therm_getenv first")
return
p = copy.copy(self.thermal_param)
p.distance = distance
self.send_packet_fmt(SET_THERMAL_PARAM, "<HHHHH", *p.args())

def therm_set_emissivity(self, emissivity):
'''set thermal emissivity'''
if self.thermal_param is None:
print("Run therm_getenv first")
return
p = copy.copy(self.thermal_param)
p.target_emissivity = emissivity
self.send_packet_fmt(SET_THERMAL_PARAM, "<HHHHH", *p.args())

def therm_set_humidity(self, humidity):
'''set thermal humidity'''
if self.thermal_param is None:
print("Run therm_getenv first")
return
p = copy.copy(self.thermal_param)
p.humidity = humidity
self.send_packet_fmt(SET_THERMAL_PARAM, "<HHHHH", *p.args())

def therm_set_airtemp(self, airtemp):
'''set thermal airtemp'''
if self.thermal_param is None:
print("Run therm_getenv first")
return
p = copy.copy(self.thermal_param)
p.air_temperature = airtemp
self.send_packet_fmt(SET_THERMAL_PARAM, "<HHHHH", *p.args())

def therm_set_reftemp(self, reftemp):
'''set thermal reftemp'''
if self.thermal_param is None:
print("Run therm_getenv first")
return
p = copy.copy(self.thermal_param)
p.reflection_temperature = reftemp
self.send_packet_fmt(SET_THERMAL_PARAM, "<HHHHH", *p.args())

def clear_target(self):
'''clear target position'''
self.target_pos = None
Expand Down Expand Up @@ -616,11 +725,17 @@ def request_telem(self):
if self.last_thresh_t is None or now - self.last_thresh_t > 10:
self.last_thresh_t = now
self.send_packet_fmt(READ_THRESHOLDS, None)
if self.siyi_settings.mode_hz > 0 and now - self.last_mode_t > 1.0/self.siyi_settings.mode_hz:
self.last_mode_t = now
self.send_packet_fmt(READ_CONTROL_MODE, None)
if self.siyi_settings.therm_cap_rate > 0 and now - self.last_therm_mode > 2:
self.last_therm_mode = now
self.send_packet_fmt(GET_THERMAL_MODE, None)

def send_attitude(self):
'''send attitude to gimbal'''
now = time.time()
if self.siyi_settings.att_send_hz <= 0 or now - self.last_att_send_t < 1.0/self.siyi_settings.telem_hz:
if self.siyi_settings.att_send_hz <= 0 or now - self.last_att_send_t < 1.0/self.siyi_settings.att_send_hz:
return
self.last_att_send_t = now
att = self.master.messages.get('ATTITUDE',None)
Expand All @@ -630,14 +745,6 @@ def send_attitude(self):
self.millis32(),
att.roll, att.pitch, att.yaw,
att.rollspeed, att.pitchspeed, att.yawspeed)
gpi = self.master.messages.get('GLOBAL_POSITION_INT',None)
if gpi is None:
return
#self.send_packet_fmt(VELOCITY_EXTERNAL, "<Ifff",
# self.millis32(),
# gpi.vx*0.01,
# gpi.vy*0.01,
# gpi.vz*0.01)


def send_packet(self, command_id, pkt):
Expand Down Expand Up @@ -667,10 +774,13 @@ def send_packet_fmt(self, command_id, fmt, *args):
def unpack(self, command_id, fmt, data):
'''unpack SIYI data and log'''
fsize = struct.calcsize(fmt)
if fsize != len(data):
print("cmd 0x%02x needs %u bytes got %u" % (command_id, fsize, len(data)))
return None
v = struct.unpack(fmt, data[:fsize])
args = list(v)
args.extend([0]*(10-len(args)))
self.logf.write('SIIN', 'QBffffffffff', 'TimeUS,Cmd,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10', self.micros64(), command_id, *args)
args.extend([0]*(12-len(args)))
self.logf.write('SIIN', 'QBffffffffffff', 'TimeUS,Cmd,P1,P2,P3,P4,P5,P6,P7,P8,P9,P10,P11,P12', self.micros64(), command_id, *args)
return v

def parse_data(self, pkt):
Expand All @@ -695,10 +805,11 @@ def parse_packet(self, pkt):
return

if cmd == ACQUIRE_FIRMWARE_VERSION:
(patch,minor,major,gpatch,gminor,gmajor) = self.unpack(cmd, "<BBBBBB", data)
patch,minor,major,gpatch,gminor,gmajor,zpatch,zminor,zmajor,_,_,_ = self.unpack(cmd, "<BBBBBBBBBBBB", data)
self.have_version = True
print("SIYI CAM %u.%u.%u" % (major, minor, patch))
print("SIYI Gimbal %u.%u.%u" % (gmajor, gminor, gpatch))
print("SIYI Zoom %u.%u.%u" % (zmajor, zminor, zpatch))
# change to white hot
self.send_packet_fmt(SET_THERMAL_PALETTE, "<B", 0)

Expand Down Expand Up @@ -789,15 +900,24 @@ def parse_packet(self, pkt):
new_thresh = (self.siyi_settings.thresh_climit,
self.siyi_settings.thresh_volt,
self.siyi_settings.thresh_ang)
weak_control = 0
else:
new_thresh = (self.siyi_settings.thresh_climit_dis,
self.siyi_settings.thresh_volt_dis,
self.siyi_settings.thresh_ang_dis)
weak_control = 1
if (climit != new_thresh[0] or volt_thresh != new_thresh[1] or ang_thresh != new_thresh[2]):
print("SIYI: Setting thresholds (%u,%u,%u) -> (%u,%u,%u)" %
(climit,volt_thresh,ang_thresh,new_thresh[0],new_thresh[1],new_thresh[2]))
self.send_packet_fmt(SET_THRESHOLDS, "<hhh",
new_thresh[0], new_thresh[1], new_thresh[2])
self.send_packet_fmt(SET_WEAK_CONTROL,"<B", weak_control)

elif cmd == READ_CONTROL_MODE:
self.control_mode, = self.unpack(cmd, "<B", data)
self.send_named_float('CMODE', self.control_mode)
self.logf.write('SIMO', 'QB', 'TimeUS,Mode',
self.micros64(), self.control_mode)

elif cmd == READ_TEMP_FULL_SCREEN:
if len(data) < 12:
Expand Down Expand Up @@ -839,16 +959,52 @@ def parse_packet(self, pkt):
ok, = self.unpack(cmd, "<B", data)
if ok != 1:
print("Threshold set failure")
elif cmd == SET_WEAK_CONTROL:
ok,weak_control, = self.unpack(cmd, "<BB", data)
if ok != 1:
print("Weak control set failure")
else:
print("Weak control is %u" % weak_control)
elif cmd == GET_THERMAL_MODE:
ok, = self.unpack(cmd,"<B", data)
print("ThermalMode: %u" % ok)
if self.siyi_settings.therm_cap_rate > 0 and ok != 1:
print("ThermalMode: %u" % ok)
self.send_packet_fmt(SET_THERMAL_MODE, "<B", 1)

elif cmd == SET_THERMAL_MODE:
ok, = self.unpack(cmd,"<B", data)
print("SetThermalMode: %u" % ok)

elif cmd == SET_THERMAL_GAIN:
ok, = self.unpack(cmd,"<B", data)
print("SetThermalGain: %u" % ok)

elif cmd == GET_TEMP_FRAME:
ok, = self.unpack(cmd,"<B", data)
if ok:
self.thermal_capture_count += 1

elif cmd == GET_THERMAL_ENVSWITCH:
ok, = self.unpack(cmd,"<B", data)
print("ThermalEnvSwitch: %u" % ok)

elif cmd == SET_THERMAL_ENVSWITCH:
ok, = self.unpack(cmd,"<B", data)
print("ThermalEnvSwitch: %u" % ok)

elif cmd == SET_THERMAL_PARAM:
ok, = self.unpack(cmd,"<B", data)
print("SetThermalParam: %u" % ok)

elif cmd == GET_THERMAL_PARAM:
dist,emiss,humidity,airtemp,reftemp, = self.unpack(cmd,"<HHHHH", data)
self.thermal_param = ThermalParameters(dist*0.01, emiss*0.01, humidity*0.01, airtemp*0.01, reftemp*0.01)
print("ThermalParam: %s" % self.thermal_param)

elif cmd == SET_TIME:
ok, = self.unpack(cmd,"<B", data)
print("SetTime: %u" % ok)

elif cmd in [SET_ANGLE, CENTER, GIMBAL_ROTATION, ABSOLUTE_ZOOM, SET_IMAGE_TYPE,
REQUEST_CONTINUOUS_DATA, SET_THERMAL_PALETTE, MANUAL_ZOOM_AND_AUTO_FOCUS]:
# an ack
Expand Down Expand Up @@ -1058,7 +1214,7 @@ def update_target(self):
# added rate of target update

map_module = self.module('map')
if map_module is not None and map_module.current_ROI != self.last_map_ROI:
if self.siyi_settings.track_ROI == 1 and map_module is not None and map_module.current_ROI != self.last_map_ROI:
self.last_map_ROI = map_module.current_ROI
(lat, lon, alt) = self.last_map_ROI
self.clear_target()
Expand Down
Loading