From dc6b01a274c43757a6df6c30ad1d1915b57e0864 Mon Sep 17 00:00:00 2001
From: dewagter <dewagter@gmail.com>
Date: Fri, 26 Sep 2014 06:29:28 +0200
Subject: [PATCH] [FBW] Option to have telemetry through the FBW for outback
 safety (so it can decode flight termination modes even when AP is down)

---
 sw/airborne/fbw_datalink.c | 59 ++++++++++++++++++++++++++++++++++++++
 sw/airborne/fbw_datalink.h | 33 +++++++++++++++++++++
 2 files changed, 92 insertions(+)
 create mode 100644 sw/airborne/fbw_datalink.c
 create mode 100644 sw/airborne/fbw_datalink.h

diff --git a/sw/airborne/fbw_datalink.c b/sw/airborne/fbw_datalink.c
new file mode 100644
index 00000000000..1d7caafa3e6
--- /dev/null
+++ b/sw/airborne/fbw_datalink.c
@@ -0,0 +1,59 @@
+
+
+
+
+
+#include "fbw_datalink.h"
+#include "mcu_periph/uart.h"
+#include "led.h"
+
+
+#define __ModemLink(dev, _x) dev##_x
+#define _ModemLink(dev, _x)  __ModemLink(dev, _x)
+#define ModemLink(_x) _ModemLink(MODEM_LINK, _x)
+#define ModemBuffer() ModemLink(ChAvailable())
+
+
+#define __AutopilotLink(dev, _x) dev##_x
+#define _AutopilotLink(dev, _x)  __AutopilotLink(dev, _x)
+#define AutopilotLink(_x) _AutopilotLink(AUTOPILOT_LINK, _x)
+
+#define AutopilotBuffer() AutopilotLink(ChAvailable())
+
+static inline void autopilot_parse(char c)
+{
+  ModemLink(Transmit(c));
+}
+
+static inline void modem_parse(char c)
+{
+  AutopilotLink(Transmit(c));
+}
+
+#define ReadAutopilotBuffer() {                  \
+    while (AutopilotLink(ChAvailable()))         \
+      autopilot_parse(AutopilotLink(Getch()));      \
+  }
+
+#define ReadModemBuffer() {                  \
+    while (ModemLink(ChAvailable()))         \
+      modem_parse(ModemLink(Getch()));      \
+  }
+
+void FbwDataLinkPeriodic(void)
+{
+  LED_OFF(2);
+  LED_OFF(4);
+}
+
+void FbwDataLinkEvent(void)
+{
+  if (ModemLink(ChAvailable()))
+    LED_ON(2);
+
+  if (AutopilotLink(ChAvailable()))
+    LED_ON(4);
+
+  ReadModemBuffer();
+  ReadAutopilotBuffer();
+}
diff --git a/sw/airborne/fbw_datalink.h b/sw/airborne/fbw_datalink.h
new file mode 100644
index 00000000000..3dd061cb5df
--- /dev/null
+++ b/sw/airborne/fbw_datalink.h
@@ -0,0 +1,33 @@
+/*
+ * Copyright (C) 2014 CDW
+ *
+ * This file is part of paparazzi.
+ *
+ * paparazzi is free software; you can redistribute it and/or modify
+ * it under the terms of the GNU General Public License as published by
+ * the Free Software Foundation; either version 2, or (at your option)
+ * any later version.
+ *
+ * paparazzi is distributed in the hope that it will be useful,
+ * but WITHOUT ANY WARRANTY; without even the implied warranty of
+ * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
+ * GNU General Public License for more details.
+ *
+ * You should have received a copy of the GNU General Public License
+ * along with paparazzi; see the file COPYING.  If not, write to
+ * the Free Software Foundation, 59 Temple Place - Suite 330,
+ * Boston, MA 02111-1307, USA.
+ *
+ */
+/** \file fbw_datalink.h
+ *  \brief Handling of messages coming from ground in FTD
+ *
+ */
+
+#ifndef FBW_DATALINK_EVENT
+#define FBW_DATALINK_EVENT
+
+extern void FbwDataLinkPeriodic(void);
+extern void FbwDataLinkEvent(void);
+
+#endif