From 63f9907f92f1190d2f3a5c89f5830c67c6e3e2bd Mon Sep 17 00:00:00 2001 From: Mark Jessop Date: Sat, 7 Dec 2024 11:07:02 +1030 Subject: [PATCH] Send sensor temperature and focusFoM data via gps telemetry. Add options for using FocusFoM to select images --- rx/WenetPackets.py | 16 +++++++++----- rx/templates/index.html | 8 +++++++ rx/wenetserver.py | 6 ++++++ start_tx.sh | 6 ++++++ tx/PacketTX.py | 19 +++++++++++++++-- tx/WenetPiCamera2.py | 47 ++++++++++++++++++++++++++++++----------- tx/tx_picamera2_gps.py | 9 ++++++-- 7 files changed, 90 insertions(+), 21 deletions(-) diff --git a/rx/WenetPackets.py b/rx/WenetPackets.py index bffc690..8514c52 100644 --- a/rx/WenetPackets.py +++ b/rx/WenetPackets.py @@ -19,7 +19,7 @@ # Check if we are running in Python 2 or 3 PY3 = sys.version_info[0] == 3 -WENET_VERSION = "1.2.0" +WENET_VERSION = "1.2.1" WENET_IMAGE_UDP_PORT = 7890 WENET_TELEMETRY_UDP_PORT = 55672 @@ -36,7 +36,7 @@ class WENET_PACKET_TYPES: class WENET_PACKET_LENGTHS: - GPS_TELEMETRY = 65 + GPS_TELEMETRY = 73 ORIENTATION_TELEMETRY = 43 IMAGE_TELEMETRY = 80 @@ -208,7 +208,7 @@ def gps_telemetry_decoder(packet): # Wrap the next bit in exception handling. try: # Unpack the packet into a list. - data = struct.unpack(">BHIBffffffBBBffHfffff", packet) + data = struct.unpack(">BHIBffffffBBBffHfffffff", packet) gps_data['week'] = data[1] gps_data['iTOW'] = data[2]/1000.0 # iTOW provided as milliseconds, convert to seconds. @@ -231,6 +231,8 @@ def gps_telemetry_decoder(packet): gps_data['load_avg_15'] = round(data[18],3) gps_data['disk_percent'] = round(data[19],3) gps_data['lens_position'] = round(data[20],4) + gps_data['sensor_temp'] = round(data[21],1) + gps_data['focus_fom'] = int(data[22]) # Check to see if we actually have real data in these new fields. # If its an old transmitter, it will have 0x55 in these spots, which we can detect if gps_data['cpu_speed'] == 21845: @@ -244,6 +246,8 @@ def gps_telemetry_decoder(packet): gps_data['load_avg_15'] = 0 gps_data['disk_percent'] = -1.0 gps_data['lens_position'] = -999.0 + gps_data['sensor_temp'] = -999.0 + gps_data['focus_fom'] = -999.0 # Perform some post-processing on the data, to make some of the fields easier to read. @@ -305,7 +309,7 @@ def gps_telemetry_string(packet): if gps_data['error'] != 'None': return "GPS: ERROR Could not decode." else: - gps_data_string = "GPS: %s Lat/Lon: %.5f,%.5f Alt: %dm, Speed: H %dkph V %.1fm/s, Heading: %d deg, Fix: %s, SVs: %d, DynModel: %s, Radio Temp: %.1f, CPU Temp: %.1f, CPU Speed: %d, Load Avg: %.2f, %.2f, %.2f, Disk Usage: %.1f%%, Lens Pos: %.4f" % ( + gps_data_string = "GPS: %s Lat/Lon: %.5f,%.5f Alt: %dm, Speed: H %dkph V %.1fm/s, Heading: %d deg, Fix: %s, SVs: %d, DynModel: %s, Radio Temp: %.1f, CPU Temp: %.1f, CPU Speed: %d, Load Avg: %.2f, %.2f, %.2f, Disk Usage: %.1f%%, Lens Pos: %.4f, Sensor Temp: %.1f, FocusFoM: %d" % ( gps_data['timestamp'], gps_data['latitude'], gps_data['longitude'], @@ -323,7 +327,9 @@ def gps_telemetry_string(packet): gps_data['load_avg_5'], gps_data['load_avg_15'], gps_data['disk_percent'], - gps_data['lens_position'] + gps_data['lens_position'], + gps_data['sensor_temp'], + int(gps_data['focus_fom']) ) return gps_data_string diff --git a/rx/templates/index.html b/rx/templates/index.html index 8b64a09..b79f1e6 100644 --- a/rx/templates/index.html +++ b/rx/templates/index.html @@ -129,6 +129,14 @@ if(msg.lens_position > -900){ _new_gps_detailed += ", Lens Position: " + msg.lens_position } + if(msg.sensor_temp > -900){ + _new_gps_detailed += ", Sensor Temp: " + msg.sensor_temp + } + if(msg.focus_fom > -900){ + _new_gps_detailed += ", FocusFoM: " + msg.focus_fom + } + + $('#detail_gps_telem_data').html(_new_gps_detailed); } diff --git a/rx/wenetserver.py b/rx/wenetserver.py index 8c4bdc1..308e5fd 100644 --- a/rx/wenetserver.py +++ b/rx/wenetserver.py @@ -163,6 +163,12 @@ def handle_gps_telemetry(gps_data): if gps_data['lens_position'] > -999.0: _extra_fields['lens_position'] = gps_data['lens_position'] + if gps_data['sensor_temp'] > -999.0: + _extra_fields['sensor_temp'] = gps_data['sensor_temp'] + + if gps_data['focus_fom'] > -999.0: + _extra_fields['focus_fom'] = gps_data['focus_fom'] + sondehub.add_telemetry( current_callsign + "-Wenet", gps_data['timestamp'] + "Z", diff --git a/start_tx.sh b/start_tx.sh index 17dd79d..c39005d 100755 --- a/start_tx.sh +++ b/start_tx.sh @@ -116,8 +116,14 @@ sleep 10 # --afwindow 0.25,0.25,0.5,0.5 \ # # Set a fixed lens offset for the PiCam v3, in dioptres. May help with autofocus in cold temperatures. +# The PiCam v3 can be offset by a maximum of -3 dioptres before hitting a hard-stop +# Set this to -99.0 to set the PiCam v3 to use the entire lens travel range. # e.g. to offset by 1 dioptre: # --afoffset -1.0 \ +# +# Use the Focus Figure-of-merit metadata to select the transmitted image, instead of selecting on file size +# Only works for lenses with autofocus (PiCam v3) +# --use_focus_fom python3 tx_picamera2_gps.py \ --rfm98w $SPIDEVICE \ diff --git a/tx/PacketTX.py b/tx/PacketTX.py index 92aded9..f35bcbf 100644 --- a/tx/PacketTX.py +++ b/tx/PacketTX.py @@ -290,14 +290,27 @@ def transmit_gps_telemetry(self, gps_data, cam_metadata=None): _disk_percent = -1.0 _lens_position = -999.0 + _sensor_temperature = -999.0 + _focus_fom = -999.0 if cam_metadata: + # {'SensorTimestamp': 390269427000, 'ScalerCrop': (0, 0, 4608, 2592), 'ScalerCrops': [(0, 0, 4608, 2592)], 'AfPauseState': 0, + # 'AfState': 2, 'ExposureTime': 59994, 'FocusFoM': 21380, 'AnalogueGain': 2.081300735473633, + # 'AeLocked': True, 'ColourCorrectionMatrix': (1.7214878797531128, -0.46079355478286743, -0.26070430874824524, -0.3001042306423187, 1.5704208612442017, -0.27031660079956055, 0.150499626994133, -1.1309722661972046, 1.9804826974868774), + # 'FrameDuration': 69669, 'SensorTemperature': 65.0, 'DigitalGain': 1.0001286268234253, 'LensPosition': 1.701196312904358, + # 'Lux': 107.27578735351562, 'ColourTemperature': 2927, 'ColourGains': (1.459670066833496, 2.9101195335388184), 'SensorBlackLevels': (4096, 4096, 4096, 4096)} if 'LensPosition' in cam_metadata: _lens_position = cam_metadata['LensPosition'] + if 'SensorTemperature' in cam_metadata: + _sensor_temperature = cam_metadata['SensorTemperature'] + + if 'FocusFoM' in cam_metadata: + _focus_fom = float(cam_metadata['FocusFoM']) + # Construct the packet try: - gps_packet = struct.pack(">BHIBffffffBBBffHfffff", + gps_packet = struct.pack(">BHIBffffffBBBffHfffffff", 1, # Packet ID for the GPS Telemetry Packet. gps_data['week'], int(gps_data['iTOW']*1000), # Convert the GPS week value to milliseconds, and cast to an int. @@ -319,7 +332,9 @@ def transmit_gps_telemetry(self, gps_data, cam_metadata=None): _load_avg_5, _load_avg_15, _disk_percent, - _lens_position + _lens_position, + _sensor_temperature, + _focus_fom ) self.queue_telemetry_packet(gps_packet) diff --git a/tx/WenetPiCamera2.py b/tx/WenetPiCamera2.py index 9b0ca68..8974e88 100644 --- a/tx/WenetPiCamera2.py +++ b/tx/WenetPiCamera2.py @@ -56,9 +56,10 @@ def __init__(self, af_window = None, af_offset = 0, exposure_value = 0.0, + use_focus_fom = False, temp_filename_prefix = 'picam_temp', debug_ptr = None, - init_retries = 10 + init_retries = 10, ): """ Instantiate a WenetPiCam Object @@ -88,6 +89,8 @@ def __init__(self, h: Height of rectangle, as fraction of frame height If not provided, the default windowing (approx centre third of width/height) will be used. af_offset: Offset the lens by a fixed dioptre. May help with autofocus during flights. + exposure_value: Add a exposure compensation. Defaults to 0. + use_focus_fom: Set to True to use FocusFoM data to select the best image instead of file size. temp_filename_prefix: prefix used for temporary files. debug_ptr: 'pointer' to a function which can handle debug messages. @@ -108,6 +111,7 @@ def __init__(self, self.af_window = af_window self.af_offset = af_offset self.exposure_value = exposure_value + self.use_focus_fom = use_focus_fom self.af_window_rectangle = None # Calculated during init self.autofocus_mode = False @@ -304,6 +308,7 @@ def capture(self, filename='picam.jpg', quality=90): # Attempt to capture a set of images. img_metadata = [] + focus_fom = [] for i in range(self.num_images): self.debug_message("Capturing Image %d of %d" % (i+1,self.num_images)) # Wrap this in error handling in case we lose the camera for some reason. @@ -314,6 +319,9 @@ def capture(self, filename='picam.jpg', quality=90): metadata = self.cam.capture_file("%s_%d.jpg" % (self.temp_filename_prefix,i)) # Save metadata for this frame img_metadata.append(metadata.copy()) + # Separately store the focus FoM so we can look for the max easily. + if 'FocusFoM' in metadata: + focus_fom.append(metadata['FocusFoM']) self.capture_in_progress = False print(f"Image captured: {time.time()}") @@ -329,25 +337,40 @@ def capture(self, filename='picam.jpg', quality=90): self.capture_in_progress = True self.cam.stop() + if len(focus_fom)>0: + self.debug_message(f"Focus FoM Values: {str(focus_fom)}") + # Otherwise, continue to pick the 'best' image based on filesize. self.debug_message("Choosing Best Image.") - pic_list = glob.glob("%s_*.jpg" % self.temp_filename_prefix) - pic_sizes = [] - # Iterate through list of images and get the file sizes. - for pic in pic_list: - pic_sizes.append(os.path.getsize(pic)) - _largest_pic_idx = pic_sizes.index(max(pic_sizes)) - largest_pic = pic_list[_largest_pic_idx] + + if self.use_focus_fom and len(focus_fom) > 0: + # Use FocusFoM data to pick the best image. + _best_pic_idx = focus_fom.index(max(focus_fom)) + best_pic = "%s_%d.jpg" % (self.temp_filename_prefix,_best_pic_idx) + + else: + # Otherwise use the filesize of the resultant JPEG files. + # Bigger JPEG = Sharper image + pic_list = glob.glob("%s_*.jpg" % self.temp_filename_prefix) + pic_sizes = [] + # Iterate through list of images and get the file sizes. + for pic in pic_list: + pic_sizes.append(os.path.getsize(pic)) + _best_pic_idx = pic_sizes.index(max(pic_sizes)) + best_pic = pic_list[_best_pic_idx] # Report the image pick results. - if 'LensPosition' in img_metadata[_largest_pic_idx]: - self.debug_message(f"Best Image was #{_largest_pic_idx}, Lens Pos: {img_metadata[_largest_pic_idx]['LensPosition']:.4f}") + if 'LensPosition' in img_metadata[_best_pic_idx]: + if self.use_focus_fom: + self.debug_message(f"Best Image was #{_best_pic_idx}, Lens Pos: {img_metadata[_best_pic_idx]['LensPosition']:.4f}, FocusFoM: {img_metadata[_best_pic_idx]['FocusFoM']}") + else: + self.debug_message(f"Best Image was #{_best_pic_idx}, Lens Pos: {img_metadata[_best_pic_idx]['LensPosition']:.4f}") else: - self.debug_message(f"Best Image was #{_largest_pic_idx}") + self.debug_message(f"Best Image was #{_best_pic_idx}") # Copy best image to target filename. self.debug_message("Copying image to storage with filename %s" % filename) - os.system("cp %s %s" % (largest_pic, filename)) + os.system("cp %s %s" % (best_pic, filename)) # Clean up temporary images. os.system("rm %s_*.jpg" % self.temp_filename_prefix) diff --git a/tx/tx_picamera2_gps.py b/tx/tx_picamera2_gps.py index 0f1ed0a..aec8205 100644 --- a/tx/tx_picamera2_gps.py +++ b/tx/tx_picamera2_gps.py @@ -37,6 +37,9 @@ parser.add_argument("--afwindow", type=str, default=None, help="For PiCam v3 Autofocus mode, set the AutoFocus window, x,y,w,h , in fractions of frame size. (Default: None = default)") parser.add_argument("--afoffset", type=float, default=0.0, help="For PiCam v3 Autofocus mode, offset the lens by this many dioptres (Default: 0 = No offset)") parser.add_argument("--exposure", type=float, default=0.0, help="Exposure compensation. -8.0 to 8.0. Sets the ExposureValue control. (Default: 0.0)") +parser.add_argument("--use_focus_fom", action='store_true', default=False, help="Use Focus FoM data instead of file size for image selection.") +parser.add_argument("--num_images", type=int, default=5, help="Number of images to capture on each cycle. (Default: 5)") +parser.add_argument("--image_delay", type=float, default=1.0, help="Delay time between each image capture. (Default: 1 second)") parser.add_argument("-v", "--verbose", action='store_true', default=False, help="Show additional debug info.") args = parser.parse_args() @@ -209,14 +212,16 @@ def post_process_image(filename): picam = WenetPiCamera2.WenetPiCamera2( tx_resolution=args.resize, callsign=callsign, - num_images=5, + num_images=args.num_images, + image_delay=args.image_delay, debug_ptr=tx.transmit_text_message, vertical_flip=args.vflip, horizontal_flip=args.hflip, whitebalance=args.whitebalance, lens_position=args.lensposition, af_window=args.afwindow, - af_offset=args.afoffset + af_offset=args.afoffset, + use_focus_fom=args.use_focus_fom ) # .. and start it capturing continuously. picam.run(destination_directory="./tx_images/",