Reimplement control logic

Move logic into its own module and implement it using nicer programming
patterns

src/fieldbus.rs

@@ -66,6 +66,15 @@ impl Fieldbus {
         pii.copy_from_slice(&data.pii);
         data.piq.copy_from_slice(piq);
     }
+
+    pub fn with_process_images<F, R>(&mut self, f: F) -> R
+    where
+        F: FnOnce(&[u8; PII_SIZE], &mut [u8; PIQ_SIZE]) -> R,
+    {
+        let mut data = self.inner.lock().unwrap();
+        let data = &mut *data;
+        f(&data.pii, &mut data.piq)
+    }
 }
 
 struct PeripheralInfo {