[paparazzi-commits] [5601] clone pprz_protocol to use with an other uart

[Gautier Hattenberger]

Revision: 5601
          http://svn.sv.gnu.org/viewvc/?view=rev&root=paparazzi&revision=5601
Author:   gautier
Date:     2010-08-23 11:08:21 +0000 (Mon, 23 Aug 2010)
Log Message:
-----------
clone pprz_protocol to use with an other uart

Added Paths:
-----------
    paparazzi3/trunk/conf/modules/booz_extra_dl.xml
    paparazzi3/trunk/sw/airborne/modules/core/extra_pprz_dl.c
    paparazzi3/trunk/sw/airborne/modules/core/extra_pprz_dl.h

Added: paparazzi3/trunk/conf/modules/booz_extra_dl.xml
===================================================================
--- paparazzi3/trunk/conf/modules/booz_extra_dl.xml                             
(rev 0)
+++ paparazzi3/trunk/conf/modules/booz_extra_dl.xml     2010-08-23 11:08:21 UTC 
(rev 5601)
@@ -0,0 +1,17 @@
+<!DOCTYPE module SYSTEM "module.dtd">
+
+<module name="core">
+  <header>
+    <file name="extra_pprz_dl.h"/>
+  </header>
+  <event fun="ExtraDatalinkEvent()"/>
+  <makefile>
+    <flag name="EXTRA_DOWNLINK_DEVICE" value="Uart0"/>
+    <flag name="EXTRA_PPRZ_UART" value="Uart0"/>
+    <flag name="USE_UART0"/>
+    <flag name="UART0_BAUD" value="B57600"/>
+    <flag name="UART0_VIC_SLOT" value="5"/>
+    <file name="extra_pprz_dl.c"/>
+  </makefile>
+</module>
+

Added: paparazzi3/trunk/sw/airborne/modules/core/extra_pprz_dl.c
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/core/extra_pprz_dl.c                   
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/core/extra_pprz_dl.c   2010-08-23 
11:08:21 UTC (rev 5601)
@@ -0,0 +1,33 @@
+/*
+ * Paparazzi mcu0 $Id: pprz_transport.c 929 2006-06-02 12:11:37Z poine $
+ *  
+ * Copyright (C) 2006  Pascal Brisset, Antoine Drouin
+ * Copyright (C) 2010  ENAC
+ *
+ * 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. 
+ *
+ */
+
+#include <inttypes.h>
+#include "extra_pprz_dl.h"
+#include "uart.h"
+
+volatile bool_t extra_pprz_msg_received = FALSE;
+uint8_t extra_pprz_ovrn, extra_pprz_error;
+volatile uint8_t extra_pprz_payload_len;
+uint8_t extra_pprz_payload[PPRZ_PAYLOAD_LEN];

Added: paparazzi3/trunk/sw/airborne/modules/core/extra_pprz_dl.h
===================================================================
--- paparazzi3/trunk/sw/airborne/modules/core/extra_pprz_dl.h                   
        (rev 0)
+++ paparazzi3/trunk/sw/airborne/modules/core/extra_pprz_dl.h   2010-08-23 
11:08:21 UTC (rev 5601)
@@ -0,0 +1,226 @@
+/*
+ * Paparazzi $Id: pprz_transport.h 4870 2010-04-24 03:02:39Z poine $
+ *  
+ * Copyright (C) 2010  ENAC
+ *
+ * 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 pprz_transport.h
+ *  \brief Extra datalink using PPRZ protocol
+ *
+ *  Pprz frame:
+ *
+ *   |STX|length|... payload=(length-4) bytes ...|Checksum A|Checksum B|
+ *
+ *   where checksum is computed over length and payload:
+ *     ck_A = ck_B = length
+ *     for each byte b in payload
+ *       ck_A += b; ck_b += ck_A
+ */
+
+#ifndef EXTRA_PPRZ_DL_H
+#define EXTRA_PPRZ_DL_H
+
+#include "pprz_transport.h"
+
+#define __ExtraLink(dev, _x) dev##_x
+#define _ExtraLink(dev, _x)  __ExtraLink(dev, _x)
+#define ExtraLink(_x) _ExtraLink(EXTRA_DOWNLINK_DEVICE, _x)
+
+/** 4 = STX + len + ck_a + ck_b */
+#define ExtraPprzTransportSizeOf(_payload) (_payload+4)
+
+#define ExtraPprzTransportCheckFreeSpace(_x) ExtraLink(CheckFreeSpace(_x))
+
+#define ExtraPprzTransportPut1Byte(_x) ExtraLink(Transmit(_x))
+#define ExtraPprzTransportSendMessage() ExtraLink(SendMessage())
+
+#define ExtraPprzTransportHeader(payload_len) { \
+  ExtraPprzTransportPut1Byte(STX);                             \
+  uint8_t msg_len = ExtraPprzTransportSizeOf(payload_len);     \
+  ExtraPprzTransportPut1Byte(msg_len);                 \
+  ck_a = msg_len; ck_b = msg_len;                      \
+}
+
+#define ExtraPprzTransportTrailer() { \
+  ExtraPprzTransportPut1Byte(ck_a);    \
+  ExtraPprzTransportPut1Byte(ck_b);    \
+  ExtraPprzTransportSendMessage() \
+}
+
+#define ExtraPprzTransportPutUint8(_byte) { \
+    ck_a += _byte;                       \
+    ck_b += ck_a;                        \
+    ExtraPprzTransportPut1Byte(_byte);           \
+ }
+
+#define ExtraPprzTransportPutNamedUint8(_name, _byte) 
ExtraPprzTransportPutUint8(_byte)
+
+#define ExtraPprzTransportPut1ByteByAddr(_byte) {       \
+    uint8_t _x = *(_byte);              \
+    ExtraPprzTransportPutUint8(_x);     \
+  }
+
+#define ExtraPprzTransportPut2ByteByAddr(_byte) { \
+    ExtraPprzTransportPut1ByteByAddr(_byte);   \
+    ExtraPprzTransportPut1ByteByAddr((const uint8_t*)_byte+1); \
+  }
+
+#define ExtraPprzTransportPut4ByteByAddr(_byte) { \
+    ExtraPprzTransportPut2ByteByAddr(_byte);   \
+    ExtraPprzTransportPut2ByteByAddr((const uint8_t*)_byte+2); \
+  }
+
+#ifdef __IEEE_BIG_ENDIAN /* From machine/ieeefp.h */
+#define ExtraPprzTransportPutDoubleByAddr(_byte) { \
+    ExtraPprzTransportPut4ByteByAddr((const uint8_t*)_byte+4); \
+    ExtraPprzTransportPut4ByteByAddr((const uint8_t*)_byte);   \
+  }
+#else
+#define ExtraPprzTransportPutDoubleByAddr(_byte) { \
+    ExtraPprzTransportPut4ByteByAddr((const uint8_t*)_byte);   \
+    ExtraPprzTransportPut4ByteByAddr((const uint8_t*)_byte+4); \
+  }
+#endif
+
+
+#define ExtraPprzTransportPutInt8ByAddr(_x) 
ExtraPprzTransportPut1ByteByAddr(_x)
+#define ExtraPprzTransportPutUint8ByAddr(_x) 
ExtraPprzTransportPut1ByteByAddr((const uint8_t*)_x)
+#define ExtraPprzTransportPutInt16ByAddr(_x) 
ExtraPprzTransportPut2ByteByAddr((const uint8_t*)_x)
+#define ExtraPprzTransportPutUint16ByAddr(_x) 
ExtraPprzTransportPut2ByteByAddr((const uint8_t*)_x)
+#define ExtraPprzTransportPutInt32ByAddr(_x) 
ExtraPprzTransportPut4ByteByAddr((const uint8_t*)_x)
+#define ExtraPprzTransportPutUint32ByAddr(_x) 
ExtraPprzTransportPut4ByteByAddr((const uint8_t*)_x)
+#define ExtraPprzTransportPutFloatByAddr(_x) 
ExtraPprzTransportPut4ByteByAddr((const uint8_t*)_x)
+
+#define ExtraPprzTransportPutArray(_put, _n, _x) { \
+  uint8_t _i; \
+  ExtraPprzTransportPutUint8(_n); \
+  for(_i = 0; _i < _n; _i++) { \
+    _put(&_x[_i]); \
+  } \
+}
+
+#define ExtraPprzTransportPutFloatArray(_n, _x) 
ExtraPprzTransportPutArray(ExtraPprzTransportPutFloatByAddr, _n, _x)
+#define ExtraPprzTransportPutDoubleArray(_n, _x) 
ExtraPprzTransportPutArray(ExtraPprzTransportPutDoubleByAddr, _n, _x)
+
+#define ExtraPprzTransportPutInt16Array(_n, _x) 
ExtraPprzTransportPutArray(ExtraPprzTransportPutInt16ByAddr, _n, _x)
+#define ExtraPprzTransportPutUint16Array(_n, _x) 
ExtraPprzTransportPutArray(ExtraPprzTransportPutUint16ByAddr, _n, _x)
+
+#define ExtraPprzTransportPutInt32Array(_n, _x) 
ExtraPprzTransportPutArray(ExtraPprzTransportPutInt32ByAddr, _n, _x)
+#define ExtraPprzTransportPutUint32Array(_n, _x) 
ExtraPprzTransportPutArray(ExtraPprzTransportPutUint32ByAddr, _n, _x)
+
+#define ExtraPprzTransportPutUint8Array(_n, _x) 
ExtraPprzTransportPutArray(ExtraPprzTransportPutUint8ByAddr, _n, _x)
+
+
+/** Receiving pprz messages */
+
+extern uint8_t extra_pprz_payload[PPRZ_PAYLOAD_LEN];
+
+extern volatile bool_t extra_pprz_msg_received;
+extern uint8_t extra_pprz_ovrn, extra_pprz_error;
+extern volatile uint8_t extra_pprz_payload_len;
+
+#include "led.h"
+//#include "uart.h"
+//#include "messages.h"
+//#include "downlink.h"
+static inline void parse_extra_pprz( uint8_t c ) {
+  static uint8_t pprz_status = UNINIT;
+  static uint8_t _ck_a, _ck_b, payload_idx;
+
+  //uint8_t tab[] = { c };
+  //DOWNLINK_SEND_DEBUG(DefaultChannel,1,tab);
+  switch (pprz_status) {
+  case UNINIT:
+    if (c == STX)
+      pprz_status++;
+    break;
+  case GOT_STX:
+    if (extra_pprz_msg_received) {
+      extra_pprz_ovrn++;
+      goto error;
+    }
+    extra_pprz_payload_len = c-4; /* Counting STX, LENGTH and CRC1 and CRC2 */
+    _ck_a = _ck_b = c;
+    pprz_status++;
+    payload_idx = 0;
+    break;
+  case GOT_LENGTH:
+    extra_pprz_payload[payload_idx] = c;
+    _ck_a += c; _ck_b += _ck_a;
+    payload_idx++;
+    if (payload_idx == extra_pprz_payload_len)
+      pprz_status++;
+    break;
+  case GOT_PAYLOAD:
+    if (c != _ck_a)
+      goto error;
+    pprz_status++;
+    break;
+  case GOT_CRC1:
+    if (c != _ck_b)
+      goto error;
+    extra_pprz_msg_received = TRUE;
+    goto restart;
+  default:
+    goto error;
+  }
+  return;
+ error:
+  extra_pprz_error++;
+ restart:
+  pprz_status = UNINIT;
+  return;
+}
+
+static inline void extra_pprz_parse_payload(void) {
+  uint8_t i;
+  for(i = 0; i < extra_pprz_payload_len; i++) 
+    dl_buffer[i] = extra_pprz_payload[i];
+  dl_msg_available = TRUE;
+}
+
+#define __ExtraPprzLink(dev, _x) dev##_x
+#define _ExtraPprzLink(dev, _x)  __ExtraPprzLink(dev, _x)
+#define ExtraPprzLink(_x) _ExtraPprzLink(EXTRA_PPRZ_UART, _x)
+
+#define ExtraPprzBuffer() ExtraPprzLink(ChAvailable())
+#define ReadExtraPprzBuffer() { while 
(ExtraPprzLink(ChAvailable())&&!extra_pprz_msg_received) 
parse_extra_pprz(ExtraPprzLink(Getch())); }
+
+
+/* Datalink Event */
+
+#define ExtraDatalinkEvent() {                 \
+  if (ExtraPprzBuffer()) {                             \
+    ReadExtraPprzBuffer();                             \
+    if (extra_pprz_msg_received) {                     \
+      extra_pprz_parse_payload();                      \
+      extra_pprz_msg_received = FALSE;         \
+    }                                          \
+  }                                            \
+  if (dl_msg_available) {                      \
+    dl_parse_msg();                            \
+    dl_msg_available = FALSE;                  \
+  }                                            \
+}
+
+
+#endif /* EXTRA_PPRZ_DL_H */
+