[paparazzi-commits] [4960]

[antoine drouin]

Revision: 4960
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=4960
Author:   poine
Date:     2010-06-14 17:07:06 +0000 (Mon, 14 Jun 2010)
Log Message:
-----------


Modified Paths:
--------------
    paparazzi3/trunk/sw/airborne/lisa/lisa_overo_link.c

Modified: paparazzi3/trunk/sw/airborne/lisa/lisa_overo_link.c
===================================================================
--- paparazzi3/trunk/sw/airborne/lisa/lisa_overo_link.c 2010-06-14 17:05:14 UTC 
(rev 4959)
+++ paparazzi3/trunk/sw/airborne/lisa/lisa_overo_link.c 2010-06-14 17:07:06 UTC 
(rev 4960)
@@ -7,3 +7,35 @@
   overo_link_arch_init();
 }
 
+#ifdef USE_OVERO_LINK_TELEMETRY
+#include <string.h>
+
+uint8_t overo_link_telemetry_insert_idx;
+uint8_t overo_link_telemetry_extract_idx;
+uint8_t overo_link_telemetry_buf[OVERO_LINK_TELEMETRY_BUF_SIZE];
+
+uint8_t overo_link_telemetry_get(char* buf, int len) {
+  int8_t available = overo_link_telemetry_insert_idx - 
overo_link_telemetry_extract_idx;
+  int8_t nb_put;
+  /* we don't cross circular buffer end */
+  if (available >= 0) {
+    nb_put = Min(available, len);
+    memcpy(buf, &overo_link_telemetry_buf[overo_link_telemetry_extract_idx], 
nb_put);
+    overo_link_telemetry_extract_idx += nb_put;
+  }
+  /* we cross circular buffer end */
+  else {
+    available += OVERO_LINK_TELEMETRY_BUF_SIZE;
+    nb_put = Min(available, len);
+    uint8_t first_put =  OVERO_LINK_TELEMETRY_BUF_SIZE - 1 - 
overo_link_telemetry_extract_idx;
+    memcpy(buf, &overo_link_telemetry_buf[overo_link_telemetry_extract_idx], 
first_put);
+    overo_link_telemetry_extract_idx += nb_put;
+    if (first_put < nb_put) {
+      memcpy(&buf[first_put], &overo_link_telemetry_buf[0], nb_put - 
first_put);
+      overo_link_telemetry_extract_idx -= OVERO_LINK_TELEMETRY_BUF_SIZE;
+    }
+  }
+  return nb_put;
+}
+
+#endif /* USE_OVERO_LINK_TELEMETRY */