Send sensor temperature and focusFoM data via gps telemetry. Add opti…

…ons for using FocusFoM to select images

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

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);
 
                 }

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",

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 \

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)

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)

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/",