diff --git a/MAVProxy/mavproxy.py b/MAVProxy/mavproxy.py index df28904424..488fc985c4 100644 --- a/MAVProxy/mavproxy.py +++ b/MAVProxy/mavproxy.py @@ -281,6 +281,7 @@ def __init__(self): MPSetting('dist_unit', str, 'm', 'distance unit', choice=['m', 'nm', 'miles'], tab='Units'), MPSetting('height_unit', str, 'm', 'height unit', choice=['m', 'feet']), MPSetting('speed_unit', str, 'm/s', 'height unit', choice=['m/s', 'knots', 'mph']), + MPSetting('flytoframe', str, 'AboveHome', 'frame for FlyTo', choice=['AboveHome', 'AGL', 'AMSL']), MPSetting('fwdpos', bool, False, 'Forward GLOBAL_POSITION_INT on all links'), MPSetting('checkdelay', bool, True, 'check for link delay'), diff --git a/MAVProxy/modules/lib/mission_item_protocol.py b/MAVProxy/modules/lib/mission_item_protocol.py index 529a7a76f7..17be337b24 100644 --- a/MAVProxy/modules/lib/mission_item_protocol.py +++ b/MAVProxy/modules/lib/mission_item_protocol.py @@ -694,6 +694,8 @@ def cmd_changealt(self, args): for wpnum in range(idx, idx+count): offset = self.item_num_to_offset(wpnum) wp = self.wploader.wp(offset) + if wp is None: + continue if not self.wploader.is_location_command(wp.command): continue wp.z = newalt @@ -712,6 +714,49 @@ def cmd_changealt(self, args): mission_type=self.mav_mission_type()) print("Changed alt for WPs %u:%u to %f" % (idx, idx+(count-1), newalt)) + def cmd_changeframe(self, args): + '''handle wp change frame of multiple waypoints''' + if not self.check_have_list(): + return + if len(args) < 2: + print("usage: %s changeframe WPNUM NEWFRAME " % self.command_name()) + return + idx = int(args[0]) + if not self.good_item_num_to_manipulate(idx): + print("Invalid %s number %u" % (self.itemtype(), idx)) + return + newframe = int(args[1]) + if len(args) >= 3: + count = int(args[2]) + else: + count = 1 + if not self.good_item_num_to_manipulate(idx+count-1): + print("Invalid %s number %u" % (self.itemtype(), idx+count-1)) + return + + for wpnum in range(idx, idx+count): + offset = self.item_num_to_offset(wpnum) + wp = self.wploader.wp(offset) + if wp is None: + continue + if not self.wploader.is_location_command(wp.command): + continue + wp.frame = newframe + wp.target_system = self.target_system + wp.target_component = self.target_component + self.wploader.set(wp, offset) + + self.wploader.last_change = time.time() + self.loading_waypoints = True + self.loading_waypoint_lasttime = time.time() + self.master.mav.mission_write_partial_list_send( + self.target_system, + self.target_component, + offset, + offset, + mission_type=self.mav_mission_type()) + print("Changed frame for WPs %u:%u to %u" % (idx, idx+(count-1), newframe)) + def fix_jumps(self, idx, delta): # nothing by default as only waypoints need worry pass diff --git a/MAVProxy/modules/lib/mp_menu.py b/MAVProxy/modules/lib/mp_menu.py index d2a25e4537..732b6b83d4 100644 --- a/MAVProxy/modules/lib/mp_menu.py +++ b/MAVProxy/modules/lib/mp_menu.py @@ -336,17 +336,22 @@ def call(self): class MPMenuCallTextDialog(object): '''used to create a value dialog callback''' - def __init__(self, title='Enter Value', default=''): + def __init__(self, title='Enter Value', default='', settings=None): self.title = title self.default = default + self.settings = settings def call(self): '''show a value dialog''' from MAVProxy.modules.lib.wx_loader import wx + title = self.title + if title.find('FLYTOFRAMEUNITS') != -1 and self.settings is not None: + frameunits = "%s %s" % (self.settings.flytoframe, self.settings.height_unit) + title = title.replace('FLYTOFRAMEUNITS', frameunits) try: - dlg = wx.TextEntryDialog(None, self.title, self.title, defaultValue=str(self.default)) + dlg = wx.TextEntryDialog(None, title, title, defaultValue=str(self.default)) except TypeError: - dlg = wx.TextEntryDialog(None, self.title, self.title, value=str(self.default)) + dlg = wx.TextEntryDialog(None, title, title, value=str(self.default)) if dlg.ShowModal() != wx.ID_OK: return None return dlg.GetValue() diff --git a/MAVProxy/modules/lib/mp_module.py b/MAVProxy/modules/lib/mp_module.py index 832c7a9be3..8c84026462 100644 --- a/MAVProxy/modules/lib/mp_module.py +++ b/MAVProxy/modules/lib/mp_module.py @@ -1,4 +1,5 @@ import time +from pymavlink import mavutil class MPModule(object): ''' @@ -153,6 +154,18 @@ def remove_command(self, name): def add_completion_function(self, name, callback): self.mpstate.completion_functions[name] = callback + def flyto_frame_units(self): + '''return a frame string and unit''' + return "%s %s" % (self.settings.height_unit, self.settings.flytoframe) + + def flyto_frame(self): + '''return mavlink frame flyto frame setting''' + if self.settings.flytoframe == "AGL": + return mavutil.mavlink.MAV_FRAME_GLOBAL_TERRAIN_ALT + if self.settings.flytoframe == "AMSL": + return mavutil.mavlink.MAV_FRAME_GLOBAL + return mavutil.mavlink.MAV_FRAME_GLOBAL_RELATIVE_ALT + def dist_string(self, val_meters): '''return a distance as a string''' if self.settings.dist_unit == 'nm': @@ -167,6 +180,12 @@ def height_convert_units(self, val_meters): return val_meters * 3.28084 return val_meters + def height_convert_from_units(self, val): + '''convert a height from configured units''' + if self.settings.height_unit == 'feet': + return val / 3.28084 + return val + def height_string(self, val_meters): '''return a height as a string''' if self.settings.height_unit == 'feet': diff --git a/MAVProxy/modules/mavproxy_SIYI/__init__.py b/MAVProxy/modules/mavproxy_SIYI/__init__.py index ce9b0e8dfa..fdb07470b9 100644 --- a/MAVProxy/modules/mavproxy_SIYI/__init__.py +++ b/MAVProxy/modules/mavproxy_SIYI/__init__.py @@ -34,6 +34,7 @@ from MAVProxy.modules.lib.mp_image import MPImageOSD_None from MAVProxy.modules.mavproxy_SIYI.camera_view import CameraView +from MAVProxy.modules.mavproxy_SIYI.raw_thermal import RawThermal SIYI_RATE_MAX_DPS = 90.0 SIYI_HEADER1 = 0x55 @@ -77,6 +78,12 @@ GET_THERMAL_ENVSWITCH = 0x3B SET_THERMAL_ENVSWITCH = 0x3C SET_TIME = 0x30 +GET_THERMAL_THRESH_STATE = 0x42 +SET_THERMAL_THRESH_STATE = 0x43 +GET_THERMAL_THRESH = 0x44 +SET_THERMAL_THRESH = 0x45 +GET_THERMAL_THRESH_PREC = 0x46 +SET_THERMAL_THRESH_PREC = 0x47 class ThermalParameters: def __init__(self, distance, target_emissivity, humidity, air_temperature, reflection_temperature): @@ -173,7 +180,9 @@ def write(self, name, fmt, fields, *args): if not name in self.formats: self.formats[name] = self.mlog.add_format(DFReader.DFFormat(0, name, 0, fmt, fields)) self.outf.write(self.mlog.make_format_msgbuf(self.formats[name])) - self.outf.write(self.mlog.make_msgbuf(self.formats[name], args)) + nfields = len(fields.split(',')) + if nfields <= 14: + self.outf.write(self.mlog.make_msgbuf(self.formats[name], args)) now = time.time() if now - self.last_flush > 5: self.last_flush = now @@ -225,6 +234,7 @@ def __init__(self, mpstate): self.add_command('siyi', self.cmd_siyi, "SIYI camera control", ["", "", + "", "set (SIYISETTING)", "imode <1|2|3|4|5|6|7|8|wide|zoom|split>", "palette ", @@ -272,7 +282,13 @@ def __init__(self, mpstate): ('att_control', int, 0), ('therm_cap_rate', float, 0), ('show_horizon', int, 0), + ('autoflag_temp', float, 120), + ('autoflag_enable', bool, False), + ('autoflag_dist', float, 30), + ('autoflag_history', float, 50), + ('autoflag_slices', int, 4), ('track_ROI', int, 1), + ('fetch_timeout', float, 2.0), MPSetting('thresh_climit', int, 50, range=(10,50)), MPSetting('thresh_volt', int, 80, range=(20,80)), MPSetting('thresh_ang', int, 4000, range=(30,4000)), @@ -329,6 +345,7 @@ def __init__(self, mpstate): self.last_att_send_t = time.time() self.last_temp_t = time.time() self.thermal_view = None + self.rawthermal_view = None self.rgb_view = None self.last_zoom = 1.0 self.rgb_lens = "wide" @@ -344,6 +361,10 @@ def __init__(self, mpstate): self.recv_thread.start() self.have_horizon_lines = False self.thermal_param = None + self.last_armed = False + self.getconfig_pending = False + self.last_getconfig = time.time() + self.click_mode = "Flag" if mp_util.has_wxpython: menu = MPMenuSubMenu('SIYI', @@ -361,6 +382,7 @@ def __init__(self, mpstate): MPMenuItem('Recording', 'Recording', '# siyi recording '), MPMenuItem('ClearTarget', 'ClearTarget', '# siyi notarget '), MPMenuItem('ThermalView', 'Thermalview', '# siyi thermal '), + MPMenuItem('RawThermalView', 'RawThermalview', '# siyi rawthermal '), MPMenuItem('RGBView', 'RGBview', '# siyi rgbview '), MPMenuItem('ResetAttitude', 'ResetAttitude', '# siyi resetattitude '), MPMenuSubMenu('Zoom', @@ -411,6 +433,7 @@ def cmd_siyi(self, args): self.cmd_zoom(args[1:]) elif args[0] == "getconfig": self.send_packet(ACQUIRE_GIMBAL_CONFIG_INFO, None) + self.getconfig_pending = False elif args[0] == "angle": self.cmd_angle(args[1:]) elif args[0] == "photo": @@ -428,6 +451,7 @@ def cmd_siyi(self, args): elif args[0] == "recording": self.send_packet_fmt(PHOTO, " 8: + args = args[:8] args.extend([0]*(8-len(args))) self.logf.write('SIOU', 'QBffffffff', 'TimeUS,Cmd,P1,P2,P3,P4,P5,P6,P7,P8', self.micros64(), command_id, *args) @@ -835,12 +898,24 @@ def parse_packet(self, pkt): z,y,x,sz,sy,sx) elif cmd == ACQUIRE_GIMBAL_CONFIG_INFO: - res, hdr_sta, res2, record_sta, gim_motion, gim_mount, video = self.unpack(cmd, " 5: + self.getconfig_pending = True + self.last_getconfig = now + self.send_packet(ACQUIRE_GIMBAL_CONFIG_INFO, None) def mavlink_packet(self, m): '''process a mavlink message''' mtype = m.get_type() + + self.armed_checks() + if mtype == 'GPS_RAW_INT': - # ?!? why off by 18 hours - gwk, gms = mp_util.get_gps_time(time.time()+18*3600) + gwk, gms = mp_util.get_gps_time(time.time()) self.logf.write('GPS', "QBIHLLff", "TimeUS,Status,GMS,GWk,Lat,Lng,Alt,Spd", self.micros64(), m.fix_type, gms, gwk, m.lat, m.lon, m.alt*0.001, m.vel*0.01) if mtype == 'ATTITUDE': @@ -1392,6 +1530,7 @@ def remove_horizon_lines(self): self.rgb_view.im.add_OSD(MPImageOSD_None('hor1')) self.rgb_view.im.add_OSD(MPImageOSD_None('hor2')) + def init(mpstate): '''initialise module''' return SIYIModule(mpstate) diff --git a/MAVProxy/modules/mavproxy_SIYI/camera_view.py b/MAVProxy/modules/mavproxy_SIYI/camera_view.py index 5f3c59c7d0..0a4958a422 100644 --- a/MAVProxy/modules/mavproxy_SIYI/camera_view.py +++ b/MAVProxy/modules/mavproxy_SIYI/camera_view.py @@ -26,7 +26,6 @@ def __init__(self, siyi, rtsp_url, filename, res, thermal=False, fps=10, video_i self.res = res self.rtsp_url = rtsp_url self.filename = filename - self.mode = "Flag" self.last_frame_t = time.time() self.fps = fps self.frame_counter = -1 @@ -93,6 +92,9 @@ def __init__(self, siyi, rtsp_url, filename, res, thermal=False, fps=10, video_i popup.add_to_submenu( ["Zoom"], MPMenuItem("%ux" % z, returnkey="Zoom:%u" % z) ) + popup.add_to_submenu(["Marker"], MPMenuItem("Flame", returnkey="Marker:flame")) + popup.add_to_submenu(["Marker"], MPMenuItem("Flag", returnkey="Marker:flag")) + popup.add_to_submenu(["Marker"], MPMenuItem("Barrell", returnkey="Marker:barrell")) gst_pipeline = "rtspsrc location={0} latency=0 buffer-mode=auto ! rtph265depay ! tee name=tee1 tee1. ! queue ! h265parse ! avdec_h265 ! videoconvert ! video/x-raw,format=BGRx ! appsink tee1. ! queue ! h265parse config-interval=15 ! video/x-h265 ! mpegtsmux ! filesink location={1}".format( self.rtsp_url, self.filename @@ -137,6 +139,7 @@ def set_title(self, title): """set image title""" if self.im is None: return + title += " (mode %s)" % self.siyi.click_mode self.im.set_title(title) def get_pixel_temp(self, event): @@ -210,8 +213,10 @@ def check_events(self): self.im.set_colormap(event.returnkey[9:]) self.siyi.cmd_palette(["WhiteHot"]) elif event.returnkey.startswith("Mode:"): - self.mode = event.returnkey[5:] - print("ViewMode: %s" % self.mode) + self.siyi.click_mode = event.returnkey[5:] + print("ViewMode: %s" % self.siyi.click_mode) + elif event.returnkey.startswith("Marker:"): + self.siyi.handle_marker(event.returnkey[7:]) elif event.returnkey.startswith("Lens:") and self.siyi is not None: self.siyi.cmd_imode([event.returnkey[5:]]) elif event.returnkey.startswith("Zoom:") and self.siyi is not None: @@ -255,13 +260,8 @@ def check_events(self): self.tracking = True elif event.controlDown: self.siyi.end_tracking() - elif self.mode == "ClickTrack": - self.siyi.set_target(latlonalt[0], latlonalt[1], latlonalt[2]) else: - latlon = (latlonalt[0], latlonalt[1]) - self.siyi.mpstate.map.add_object( - mp_slipmap.SlipIcon("SIYIClick", latlon, self.siyi.click_icon, layer="SIYI") - ) + self.siyi.camera_click(self.siyi.click_mode, latlonalt) if __name__ == '__main__': from optparse import OptionParser diff --git a/MAVProxy/modules/mavproxy_SIYI/raw_thermal.py b/MAVProxy/modules/mavproxy_SIYI/raw_thermal.py new file mode 100644 index 0000000000..89431e375a --- /dev/null +++ b/MAVProxy/modules/mavproxy_SIYI/raw_thermal.py @@ -0,0 +1,360 @@ +#!/usr/bin/env python3 + +from threading import Thread +import cv2 +import os +import socket +import struct +import datetime +import math + +import time, sys + +from MAVProxy.modules.lib.mp_menu import MPMenuItem +from MAVProxy.modules.lib.mp_image import MPImage +from MAVProxy.modules.lib.mp_image import MPImageTrackPos +from MAVProxy.modules.lib.mp_image import MPImageFrameCounter +from MAVProxy.modules.mavproxy_map import mp_slipmap +from MAVProxy.modules.lib import mp_util +import numpy as np + +EXPECTED_DATA_SIZE = 640 * 512 * 2 +C_TO_KELVIN = 273.15 + +class RawThermal: + """handle raw thermal image""" + + def __init__(self, siyi, res): + self.siyi = siyi + self.uri = ("192.168.144.25", 7345) + self.im = None + self.tracking = False + self.cap = None + self.res = res + self.FOV = 24.2 + self.last_frame_t = time.time() + self.logdir = 'thermal' + self.should_exit = False + self.last_data = None + self.tmin = -1 + self.tmax = -1 + self.mouse_temp = -1 + self.image_count = 0 + self.marker_history = [] + self.last_tstamp = None + + + if siyi is not None: + self.logdir = self.siyi.logdir + + self.im = MPImage( + title="Raw Thermal", + mouse_events=True, + mouse_movement_events=True, + width=self.res[0], + height=self.res[1], + key_events=True, + can_drag=False, + can_zoom=False, + auto_size=False, + auto_fit=True + ) + + popup = self.im.get_popup_menu() + popup.add_to_submenu(["Mode"], MPMenuItem("ClickTrack", returnkey="Mode:ClickTrack")) + popup.add_to_submenu(["Mode"], MPMenuItem("Flag", returnkey="Mode:Flag")) + popup.add_to_submenu(["Marker"], MPMenuItem("Flame", returnkey="Marker:flame")) + popup.add_to_submenu(["Marker"], MPMenuItem("Flag", returnkey="Marker:flag")) + popup.add_to_submenu(["Marker"], MPMenuItem("Barrell", returnkey="Marker:barrell")) + + dname = os.path.join(self.logdir, 'thermal') + try: + os.mkdir(dname) + except Exception as ex: + pass + + self.thread = Thread(target=self.fetch_loop, name='fetch_loop') + self.thread.daemon = False + self.thread.start() + + def fetch_loop(self): + '''main thread''' + while True: + if self.im is None: + break + time.sleep(0.25) + ret = self.fetch_latest() + if ret is None: + continue + (fname, tstamp, data) = ret + if self.last_tstamp is not None and tstamp == self.last_tstamp: + continue + self.last_tstamp = tstamp + self.display_image(fname, data) + self.save_image(fname, tstamp, data) + + def in_history(self, latlon): + '''check if latlon in the history''' + autoflag_dist = self.siyi.siyi_settings.autoflag_dist + for (lat1,lon1) in self.marker_history: + dist = mp_util.gps_distance(lat1,lon1,latlon[0],latlon[1]) + if dist < autoflag_dist: + return True + + # add to history + self.marker_history.append(latlon) + if len(self.marker_history) > self.siyi.siyi_settings.autoflag_history: + self.marker_history.pop(0) + return False + + + def handle_auto_flag(self): + if not self.siyi.siyi_settings.autoflag_enable: + return + if self.tmax < self.siyi.siyi_settings.autoflag_temp: + return + + map = self.siyi.module('map') + if not map: + return + + width = self.res[0] + height = self.res[1] + latlonalt = self.xy_to_latlon(width//2, height//2) + if latlonalt is None: + return + slices = self.siyi.siyi_settings.autoflag_slices + slice_width = width // slices + slice_height = height // slices + + data = self.last_data.reshape(height, width) + + for sx in range(slices): + for sy in range(slices): + sub = data[sx*slice_width:(sx+1)*slice_width, sy*slice_height:(sy+1)*slice_height] + maxv = sub.max() - C_TO_KELVIN + if maxv < self.siyi.siyi_settings.autoflag_temp: + continue + X = (sx*slice_width) + slice_width//2 + Y = (sy*slice_height) + slice_height//2 + latlonalt = self.xy_to_latlon(X, Y) + if latlonalt is None: + continue + latlon = (latlonalt[0], latlonalt[1]) + if self.in_history(latlon): + continue + map.cmd_map_marker(["flame"], latlon=latlon) + + def display_image(self, fname, data): + '''display an image''' + a = np.frombuffer(data, dtype='>u2') + if len(a) != 640 * 512: + print("Bad size %u" % len(a)) + return + # get in Kelvin + a = (a / 64.0) + + maxv = a.max() + minv = a.min() + + self.tmin = minv - C_TO_KELVIN + self.tmax = maxv - C_TO_KELVIN + if maxv <= minv: + maxv = minv + 1 + + self.last_data = a + + self.handle_auto_flag() + + # convert to 0 to 255 + a = (a - minv) * 255 / (maxv - minv) + + # convert to uint8 greyscale as 640x512 image + a = a.astype(np.uint8) + a = a.reshape(512, 640) + + a = cv2.cvtColor(a, cv2.COLOR_GRAY2RGB) + if self.im is None: + return + self.im.set_image(a) + self.image_count += 1 + self.update_title() + + def fetch_latest(self): + '''fetch a thermal image''' + tcp = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + timeout = self.siyi.siyi_settings.fetch_timeout + if timeout >= 0: + tcp.settimeout(2) + try: + tcp.connect(self.uri) + except Exception: + return None + buf = bytearray() + + while True: + try: + b = tcp.recv(1024) + except Exception: + break + if not b: + break + buf += b + + if len(buf) != 128 + 8 + EXPECTED_DATA_SIZE: + return None + + fname = buf[:128].decode("utf-8").strip('\x00') + tstamp, = struct.unpack("red colormap for a given threshold''' + def pixel(c): + p = np.zeros((1,1,3), np.uint8) + p[:] = c + return p + + lightyellow = pixel((255,255,0)) + red = pixel((255,0,0)) + white = pixel((threshold,threshold,threshold)) + black = pixel((0,0,0)) + + threshold = mp_util.constrain(threshold, 1, 255) + + lut1 = cv2.resize(np.concatenate((black,white), axis=0), (1,threshold), interpolation=cv2.INTER_CUBIC) + lut2 = cv2.resize(np.concatenate((lightyellow,red), axis=0), (1,256-threshold), interpolation=cv2.INTER_CUBIC) + lut = np.concatenate((lut1, lut2), axis=0) + return lut + + def create_colormap_dict(self): + '''create a yellow->red colormap for all thresholds''' + ret = dict() + for i in range(256): + ret[i] = self.create_colormap_threshold(i) + return ret + + def set_threshold(self, threshold_value): + '''set pixel value for thermal colormap''' + if self.im is not None: + self.im.set_colormap_index(threshold_value) + + def set_title(self, title): + """set image title""" + if self.im is None: + return + self.im.set_title(title) + + def get_pixel_temp(self, event): + """get temperature of a pixel""" + x = event.x + y = event.y + if self.last_data is None: + return -1 + try: + p = self.last_data[y*640+x] + return p - C_TO_KELVIN + except Exception: + return -1 + + def end_tracking(self): + '''end object tracking''' + self.tracking = False + if self.im is not None: + self.im.end_tracking() + + def update_title(self): + """update thermal view title""" + self.set_title("RawThermal(%u): (%.1fC to %.1fC) %.1fC (mode %s)" % (self.image_count, self.tmin, self.tmax, + self.mouse_temp, self.siyi.click_mode)) + + def xy_to_latlon(self, x, y): + '''convert x,y pixel coordinates to a latlon tuple''' + (xres, yres) = self.res + x = (2 * x / float(xres)) - 1.0 + y = (2 * y / float(yres)) - 1.0 + aspect_ratio = float(xres) / yres + FOV = self.FOV + slant_range = self.siyi.get_slantrange(x, y, FOV, aspect_ratio) + if slant_range is None: + return None + return self.siyi.get_latlonalt(slant_range, x, y, FOV, aspect_ratio) + + + def check_events(self): + """check for image events""" + if self.im is None: + return + if not self.im.is_alive(): + self.im = None + return + for event in self.im.events(): + if isinstance(event, MPMenuItem): + if event.returnkey.startswith("Mode:"): + self.siyi.click_mode = event.returnkey[5:] + print("ViewMode: %s" % self.siyi.click_mode) + elif event.returnkey.startswith("Marker:"): + self.siyi.handle_marker(event.returnkey[7:]) + elif event.returnkey == "fitWindow": + self.im.fit_to_window() + elif event.returnkey == "fullSize": + self.im.full_size() + continue + if isinstance(event, MPImageTrackPos): + if not self.tracking: + continue + latlonalt = self.xy_to_latlon(event.x, event.y) + if latlonalt is None: + return + self.siyi.set_target(latlonalt[0], latlonalt[1], latlonalt[2]) + continue + if isinstance(event, MPImageFrameCounter): + self.frame_counter = event.frame + continue + + if event.ClassName == "wxMouseEvent": + self.mouse_temp = self.get_pixel_temp(event) + self.update_title() + if ( + event.ClassName == "wxMouseEvent" + and event.leftIsDown + and self.siyi is not None + ): + latlonalt = self.xy_to_latlon(event.x, event.y) + if latlonalt is None: + continue + if event.shiftDown: + (xres,yres) = self.res + twidth = int(yres*0.01*self.siyi.siyi_settings.track_size_pct) + self.siyi.end_tracking() + self.im.start_tracker(event.x, event.y, twidth, twidth) + self.tracking = True + elif event.controlDown: + self.siyi.end_tracking() + else: + self.siyi.camera_click(self.siyi.click_mode, latlonalt) + +if __name__ == '__main__': + from optparse import OptionParser + parser = OptionParser("camera_view.py [options]") + (opts, args) = parser.parse_args() + + c = RawThermal(None, (640, 512)) + + while True: + time.sleep(0.1) + c.check_events() diff --git a/MAVProxy/modules/mavproxy_SIYI/tools/temp_dir.py b/MAVProxy/modules/mavproxy_SIYI/tools/temp_dir.py index 7eff6de393..0092e79033 100755 --- a/MAVProxy/modules/mavproxy_SIYI/tools/temp_dir.py +++ b/MAVProxy/modules/mavproxy_SIYI/tools/temp_dir.py @@ -1,5 +1,7 @@ #!/usr/bin/env python3 +# save images in therm cap + import sys import numpy as np import cv2 @@ -7,10 +9,37 @@ import glob import os from MAVProxy.modules.lib.mp_image import MPImage +from pymavlink import mavutil + +from argparse import ArgumentParser +parser = ArgumentParser(description=__doc__) + +parser.add_argument("--min-temp", default=None, type=float, help="min temperature") +parser.add_argument("--siyi-log", default=None, type=float, help="SIYI binlog") +parser.add_argument("dirname", default=None, type=str, help="directory") +args = parser.parse_args() -DNAME=sys.argv[1] +DNAME=args.dirname +mouse_temp=-1 +tmin = -1 +tmax = -1 +last_data = None +C_TO_KELVIN = 273.15 + +def update_title(): + global tmin, tmax, mouse_temp + cv2.setWindowTitle('Thermal', "Thermal: (%.1fC to %.1fC) %.1fC" % (tmin, tmax, mouse_temp)) + +def click_callback(event, x, y, flags, param): + global last_data, mouse_temp + if last_data is None: + return + p = last_data[y*640+x] + mouse_temp = p - C_TO_KELVIN + update_title() def display_file(fname): + global mouse_temp, tmin, tmax, last_data print('Importing: ', fname) a = np.fromfile(fname, dtype='>u2') if len(a) != 640 * 512: @@ -21,15 +50,22 @@ def display_file(fname): C_TO_KELVIN = 273.15 - #a = np.clip(a, C_TO_KELVIN+50, C_TO_KELVIN+200) - maxv = a.max() minv = a.min() - print("Max=%.3fC Min=%.3fC" % (maxv-273.15, minv-273.15)) + + tmin = minv - C_TO_KELVIN + tmax = maxv - C_TO_KELVIN + + if args.min_temp is not None and tmax < args.min_temp: + return + + print("Max=%.3fC Min=%.3fC" % (tmax, tmin)) if maxv <= minv: print("Bad range") return + last_data = a + # convert to 0 to 255 a = (a - minv) * 65535.0 / (maxv - minv) @@ -37,17 +73,13 @@ def display_file(fname): a = a.astype(np.uint16) a = a.reshape(512, 640) - cv2.imshow('Grey16', a) + cv2.imshow('Thermal', a) + cv2.setMouseCallback('Thermal', click_callback) + cv2.setWindowTitle('Thermal', "Thermal: (%.1fC to %.1fC) %.1fC" % (tmin, tmax, mouse_temp)) -def find_newest(dname): - flist = glob.glob(dname + "/siyi*bin") - return max(flist, key=os.path.getctime) + cv2.waitKey(0) -last_file = None +flist = sorted(glob.glob(DNAME + "/*bin")) -while True: - fname = find_newest(DNAME) - if fname != last_file: - last_file = fname - display_file(fname) - cv2.waitKey(1) +for f in flist: + display_file(f) diff --git a/MAVProxy/modules/mavproxy_kmlread.py b/MAVProxy/modules/mavproxy_kmlread.py index 594132a2ee..b1c19435be 100644 --- a/MAVProxy/modules/mavproxy_kmlread.py +++ b/MAVProxy/modules/mavproxy_kmlread.py @@ -7,6 +7,8 @@ ''' import time, math, random +import re + from pymavlink import mavutil, mavwp from MAVProxy.modules.lib import mp_module @@ -46,7 +48,7 @@ def __init__(self, mpstate): def cmd_param(self, args): '''control kml reading''' - usage = "Usage: kml | snapfence | snapwp" + usage = "Usage: kml | snapfence | snapwp" if len(args) < 1: print(usage) return @@ -66,6 +68,8 @@ def cmd_param(self, args): print("Found layer: " + layer) elif args[0] == "toggle": self.togglekml(args[1]) + elif args[0] == "colour" or args[0] == "color": + self.cmd_colour(args[1:]) elif args[0] == "fence": self.fencekml(args[1]) else: @@ -137,6 +141,26 @@ def cmd_snap_fence(self, args): if changed: fencemod.send_fence() + def cmd_colour(self, args): + if len(args) < 2: + print("kml colour LAYERNAME 0xBBGGRR") + return + (layername, colour) = args + layer = self.find_layer(layername) + if layer is None: + print(f"No layer {layername}") + return + m = re.match(r"(?:0x)?(?P[0-9A-Fa-f]{2})(?P[0-9A-Fa-f]{2})(?P[0-9A-Fa-f]{2})", colour) + if m is None: + print("bad colour") + return + (red, green, blue) = (int(m.group("red"), 16), + int(m.group("green"), 16), + int(m.group("blue"), 16)) + self.mpstate.map.remove_object(layer) + layer.set_colour((red, green, blue)) + self.mpstate.map.add_object(layer) + def fencekml(self, layername): '''set a layer as the geofence''' #Strip quotation marks if neccessary @@ -232,7 +256,13 @@ def togglekml(self, layername): self.mpstate.map.add_object(alayer) self.curtextlayers.append(layername) self.menu_needs_refreshing = True - + + def find_layer(self, layername): + for layer in self.allayers: + if layer.key == layername: + return layer + return None + def clearkml(self): '''Clear the kmls from the map''' #go through all the current layers and remove them diff --git a/MAVProxy/modules/mavproxy_map/__init__.py b/MAVProxy/modules/mavproxy_map/__init__.py index 5a89fc5217..9b6f93bada 100644 --- a/MAVProxy/modules/mavproxy_map/__init__.py +++ b/MAVProxy/modules/mavproxy_map/__init__.py @@ -8,6 +8,7 @@ import sys, os, math import functools import time +import datetime from MAVProxy.modules.lib import mp_util from MAVProxy.modules.lib import mp_settings from MAVProxy.modules.lib import mp_module @@ -87,12 +88,15 @@ def __init__(self, mpstate): 'zoom', 'center', 'follow', + 'menu', + 'marker', 'clear']) self.add_completion_function('(MAPSETTING)', self.map_settings.completion) self.default_popup = MPMenuSubMenu('Popup', items=[]) self.add_menu(MPMenuItem('Fly To', 'Fly To', '# guided ', - handler=MPMenuCallTextDialog(title='Altitude (m)', default=self.mpstate.settings.guidedalt))) + handler=MPMenuCallTextDialog(title='Altitude (FLYTOFRAMEUNITS)', default=self.mpstate.settings.guidedalt, + settings=self.settings))) self.add_menu(MPMenuItem('Terrain Check', 'Terrain Check', '# terrain check')) self.add_menu(MPMenuItem('Show Position', 'Show Position', 'showPosition')) self.add_menu(MPMenuItem('Google Maps Link', 'Google Maps Link', 'printGoogleMapsLink')) @@ -105,6 +109,10 @@ def __init__(self, mpstate): MPMenuItem('Set Origin (with height)', 'Set Origin', '# confirm "Set ORIGIN with height?" map setorigin '), ])) + self.cmd_menu_add(["Marker:Flag", "map", "marker", "flag"]) + self.cmd_menu_add(["Marker:Barrell", "map", "marker", "barrell"]) + self.cmd_menu_add(["Marker:Flame", "map", "marker", "flame"]) + self._colour_for_wp_command = { # takeoff commands mavutil.mavlink.MAV_CMD_NAV_TAKEOFF: (255,0,0), @@ -136,6 +144,25 @@ def add_menu(self, menu): self.default_popup.add(menu) self.map.add_object(mp_slipmap.SlipDefaultPopup(self.default_popup, combine=True)) + def cmd_menu_add(self, args): + '''add to map menus''' + if len(args) < 2: + print("Usage: map menu add MenuPath command") + return + menupath = args[0].strip('"').split(':') + name = menupath[-1] + cmd = '# ' + ' '.join(args[1:]) + self.default_popup.add_to_submenu(menupath[:-1], MPMenuItem(name, name, cmd)) + self.map.add_object(mp_slipmap.SlipDefaultPopup(self.default_popup, combine=True)) + + def cmd_menu(self, args): + '''control console menus''' + if len(args) < 2: + print("Usage: map menu ") + return + if args[0] == 'add': + self.cmd_menu_add(args[1:]) + def remove_menu(self, menu): '''add to the default popup menu''' from MAVProxy.modules.mavproxy_map import mp_slipmap @@ -162,11 +189,91 @@ def print_google_maps_link(self): pos = self.mpstate.click_location print("https://www.google.com/maps/search/?api=1&query=%f,%f" % (pos[0], pos[1])) + + def write_JSON(self, fname, template, vardict): + '''write a JSON file in log directory''' + fname = os.path.join(self.logdir, fname) + json = template + for k in vardict.keys(): + value = vardict[k] + json = json.replace("{{" + k + "}}", str(value)) + open(fname, 'w').write(json) + + def cmd_map_marker(self, args, latlon=None): + '''add a map marker''' + usage = "Usage: map marker " + if latlon is None: + latlon = self.mpstate.click_location + if latlon is None: + print("Need click position for marker") + return + (lat, lon) = latlon + marker = 'flag' + text = '' + if len(args) > 0: + marker = args[0] + + if len(args) > 1: + text = ' '.join(args[1:]) + flag = marker + '.png' + + icon = self.map.icon(flag) + self.map.add_object(mp_slipmap.SlipIcon( + 'icon - %s [%u]' % (str(flag),self.icon_counter), + (float(lat),float(lon)), + icon, layer=3, rotation=0, follow=False)) + self.icon_counter += 1 + now = time.time() + tstr = datetime.datetime.fromtimestamp(now).strftime("%Y_%m_%d_%H_%M_%S") + subsec = now - math.floor(now) + millis = int(subsec * 1000) + fname = "marker_%s_%03u.json" % (tstr, millis) + + gpi = self.master.messages.get('GLOBAL_POSITION_INT',None) + att = self.master.messages.get('ATTITUDE',None) + + self.write_JSON(fname,'''{ +"marker" : {{MARKER}}, +"text" : "{{TEXT}}", +"tsec" : {{TIMESEC}}, +"mlat" : {{LAT}}, +"mlon" : {{LON}}, +"vlat" : {{VLAT}}, +"vlon" : {{VLON}}, +"valt" : {{VALT}}, +"gspeed" : {{GSPEED}}, +"ghead" : {{GHEAD}}, +"roll" : {{ROLL}}, +"pitch" : {{PITCH}}, +"yaw" : {{YAW}}, +} +''', { + "TIMESEC" : now, + "MARKER" : marker, + "TEXT" : text, + "LAT" : lat, + "LON" : lon, + "VLAT" : "%.9f" % (gpi.lat*1.0e-7), + "VLON" : "%.9f" % (gpi.lon*1.0e-7), + "VALT" : gpi.alt*1.0e-3, + "GSPEED" : math.sqrt(gpi.vx**2+gpi.vy**2)*0.01, + "GHEAD" : gpi.hdg*0.01, + "ROLL" : math.degrees(att.roll), + "PITCH" : math.degrees(att.pitch), + "YAW" : math.degrees(att.yaw) + }) + + print("Wrote marker %s" % fname) + + + def cmd_map(self, args): '''map commands''' from MAVProxy.modules.mavproxy_map import mp_slipmap if len(args) < 1: - print("usage: map ") + print("usage: map ") + elif args[0] == "menu": + self.cmd_menu(args[1:]) elif args[0] == "icon": usage = "Usage: map icon " flag = 'flag.png' @@ -193,6 +300,8 @@ def cmd_map(self, args): icon, layer=3, rotation=0, follow=False)) self.icon_counter += 1 + elif args[0] == "marker": + self.cmd_map_marker(args[1:]) elif args[0] == "vehicletype": if len(args) < 3: print("Usage: map vehicletype SYSID TYPE") diff --git a/MAVProxy/modules/mavproxy_map/data/flame.png b/MAVProxy/modules/mavproxy_map/data/flame.png new file mode 100644 index 0000000000..b3780d7589 Binary files /dev/null and b/MAVProxy/modules/mavproxy_map/data/flame.png differ diff --git a/MAVProxy/modules/mavproxy_map/mp_slipmap_util.py b/MAVProxy/modules/mavproxy_map/mp_slipmap_util.py index 315b177c6e..019b2d9dd2 100644 --- a/MAVProxy/modules/mavproxy_map/mp_slipmap_util.py +++ b/MAVProxy/modules/mavproxy_map/mp_slipmap_util.py @@ -226,6 +226,9 @@ def __init__(self, key, points, layer, colour, linewidth, arrow = False, popup_m self._has_timestamps = False self._showlines = showlines + def set_colour(self, colour): + self.colour = colour + def bounds(self): '''return bounding box''' if self.hidden: diff --git a/MAVProxy/modules/mavproxy_mode.py b/MAVProxy/modules/mavproxy_mode.py index 8b715ae938..99029ef7a6 100644 --- a/MAVProxy/modules/mavproxy_mode.py +++ b/MAVProxy/modules/mavproxy_mode.py @@ -83,13 +83,17 @@ def cmd_guided(self, args): return altitude = float(args[0]) - print("Guided %s %s" % (str(latlon), str(altitude))) + altitude = self.height_convert_from_units(altitude) + + frame = self.flyto_frame() + + print("Guided %s %s frame %u" % (str(latlon), str(altitude), frame)) if self.settings.guided_use_reposition: self.master.mav.command_int_send( self.settings.target_system, self.settings.target_component, - self.module('wp').get_default_frame(), + frame, mavutil.mavlink.MAV_CMD_DO_REPOSITION, 0, # current 0, # autocontinue @@ -107,7 +111,7 @@ def cmd_guided(self, args): self.settings.target_system, self.settings.target_component, 0, - self.module('wp').get_default_frame(), + frame, mavutil.mavlink.MAV_CMD_NAV_WAYPOINT, 2, 0, 0, 0, 0, 0, int(latlon[0]*1.0e7), diff --git a/MAVProxy/modules/mavproxy_wp.py b/MAVProxy/modules/mavproxy_wp.py index 063fbfaa9d..50961c39df 100644 --- a/MAVProxy/modules/mavproxy_wp.py +++ b/MAVProxy/modules/mavproxy_wp.py @@ -161,6 +161,7 @@ def commands(self): ret.update({ 'add': self.cmd_add, "changealt": self.cmd_changealt, + "changeframe": self.cmd_changeframe, 'draw': self.cmd_draw, 'editor': self.cmd_editor, 'loop': self.cmd_loop,