[vision] Add image switch function

sw/airborne/modules/computer_vision/lib/vision/image.c

@@ -82,6 +82,26 @@ void image_copy(struct image_t *input, struct image_t *output)
   memcpy(output->buf, input->buf, input->buf_size);
 }
 
+/**
+ * This will switch image *a and *b
+ * This is faster as image_copy because it doesn't copy the
+ * whole image buffer.
+ * @param[in,out] *a The image to switch
+ * @param[in,out] *b The image to switch with
+ */
+void image_switch(struct image_t *a, struct image_t *b)
+{
+  /* Remember everything from image a */
+  struct image_t old_a;
+  memcpy(&old_a, a, sizeof(struct image_t));
+
+  /* Copy everything from b to a */
+  memcpy(a, b, sizeof(struct image_t));
+
+  /* Copy everything from the remembered a to b */
+  memcpy(b, &old_a, sizeof(struct image_t));
+}
+
 /**
  * Convert an image to grayscale.
  * Depending on the output type the U/V bytes are removed

sw/airborne/modules/computer_vision/lib/vision/image.h

@@ -67,6 +67,7 @@ struct flow_t {
 void image_create(struct image_t *img, uint16_t width, uint16_t height, enum image_type type);
 void image_free(struct image_t *img);
 void image_copy(struct image_t *input, struct image_t *output);
+void image_switch(struct image_t *a, struct image_t *b);
 void image_to_grayscale(struct image_t *input, struct image_t *output);
 uint16_t image_yuv422_colorfilt(struct image_t *input, struct image_t *output, uint8_t y_m, uint8_t y_M, uint8_t u_m, uint8_t u_M, uint8_t v_m, uint8_t v_M);
 void image_yuv422_downsample(struct image_t *input, struct image_t *output, uint16_t downsample);

sw/airborne/modules/computer_vision/opticflow/opticflow_calculator.c

@@ -172,7 +172,7 @@ void opticflow_calc_frame(struct opticflow_t *opticflow, struct opticflow_state_
   // *************************************************************************************
   free(corners);
   free(vectors);
-  image_copy(&opticflow->img_gray, &opticflow->prev_img_gray);
+  image_switch(&opticflow->img_gray, &opticflow->prev_img_gray);
 }
 
 /**